AStar.go

// SPDX-License-Identifier: MIT
// Copyright (c) 2026 MaIII Themd

package dijkstra

// HeuristicFunc estimates the remaining cost from a vertex at
// (curX, curY) to the goal at (goalX, goalY). A* explores vertices in
// order of f = g + h, where g is the cost already spent to reach the
// vertex and h is this estimate.
//
// For A* to return a provably shortest path the heuristic must be
// admissible (never overestimate the true remaining cost). The
// built-in EuclideanHeuristic is admissible -- and consistent --
// whenever edge weights are real spatial distances between the
// vertices' (X, Y) coordinates, which is the common case for the
// geometric graphs this package targets (robot / AGV maps, road
// networks). If your edge weights mean something else (time, a
// penalty, ...), supply a heuristic in the same units that does not
// overestimate, or pass one that always returns 0 -- in which case A*
// degenerates to Dijkstra and is still correct.
type HeuristicFunc func(curX, curY, goalX, goalY float64) float64

// EuclideanHeuristic is the default heuristic: the straight-line
// distance from the current vertex to the goal. It is the tightest
// admissible estimate when edge weights are Euclidean distances.
func EuclideanHeuristic(curX, curY, goalX, goalY float64) float64 {
	return Distance(curX, curY, goalX, goalY)
}

// AStarSearch finds the shortest path from fromVertex to toVertex
// using A* with the built-in straight-line (Euclidean) heuristic over
// the vertices' (X, Y) coordinates.
//
// It returns the same result as DijkstraSearch -- ok plus a slice of
// StPath nodes ordered source-to-destination with cumulative Cost --
// but, because the search is goal-directed, it usually expands far
// fewer vertices on large spatial graphs. The built-in heuristic
// assumes edge weights are spatial distances; see HeuristicFunc for
// when to supply your own via AStarSearchFunc.
func (g *StGraph) AStarSearch(fromVertex, toVertex string) (bool, []StPath) {
	return g.AStarSearchFunc(fromVertex, toVertex, EuclideanHeuristic)
}

// AStarSearchFunc is AStarSearch with a caller-supplied heuristic.
// Passing nil uses EuclideanHeuristic. A heuristic that always
// returns 0 makes A* behave exactly like Dijkstra.
//
// As with DijkstraSearch, this resets all per-vertex search state and
// holds the graph's mutex for the whole call, so concurrent searches
// on the same graph serialise (they stay race-free).
func (g *StGraph) AStarSearchFunc(fromVertex, toVertex string, h HeuristicFunc) (bool, []StPath) {
	g.mu.Lock()
	defer g.mu.Unlock()

	if h == nil {
		h = EuclideanHeuristic
	}
	if !g.vertexIsExistLocked(fromVertex) || !g.vertexIsExistLocked(toVertex) {
		return false, nil
	}
	if !g.aStarRunLocked(fromVertex, toVertex, h) {
		return false, nil
	}
	if !g.vertexIsVisitedLocked(toVertex) {
		// Goal never reached (disconnected, or only path crosses a
		// blocked vertex).
		return false, nil
	}
	return g.extractPathLocked(fromVertex, toVertex)
}

// aStarRunLocked runs the goal-directed relaxation. It mirrors
// dijkstraRunLocked but orders the priority queue by f = g + h instead
// of g, and stops as soon as the goal is finalised. The per-vertex
// Weight still holds the true g-cost (recomputed from the parent), so
// extractPathLocked and the reported Cost are unchanged.
//
// Correctness note: a vertex is finalised (Visited) the first time it
// is dequeued, and its incoming entries are ordered by f. For a fixed
// vertex h is constant, so ordering its entries by f is the same as
// ordering by g -- the first dequeue carries the minimal g. With an
// admissible-and-consistent heuristic that minimal g is optimal, so we
// never need to reopen a vertex and the early goal exit is safe.
func (g *StGraph) aStarRunLocked(start, goal string, h HeuristicFunc) bool {
	si := g.vertexFindLocked(start)
	gi := g.vertexFindLocked(goal)
	if si < 0 || gi < 0 {
		return false
	}

	goalX := g.vertex[gi].X
	goalY := g.vertex[gi].Y
	// heuristic adapts the public HeuristicFunc to the integer-indexed
	// engine: it reads the candidate vertex's coordinates by index, so the
	// hot loop avoids the name lookup vertexXYLocked used to do per edge.
	heuristic := func(idx int32) float64 {
		v := &g.vertex[idx]
		return h(v.X, v.Y, goalX, goalY)
	}

	g.searchBegin(len(g.vertex))
	g.runSearchLocked(int32(si), int32(gi), heuristic) // early-exit at goal
	g.reconcileLocked()
	return true
}