AStar_test.go

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

package dijkstra

import (
	"fmt"
	"math"
	"math/rand"
	"testing"
)

// pathCost returns the cumulative cost at the destination of a path
// (the last node's Cost), or NaN for an empty path.
func pathCost(path []StPath) float64 {
	if len(path) == 0 {
		return math.NaN()
	}
	return path[len(path)-1].Cost
}

// countVisited reports how many vertices of a side x side grid have
// their Visited flag set after the most recent search.
func countVisited(g *StGraph, side int) int {
	n := 0
	for r := 0; r < side; r++ {
		for c := 0; c < side; c++ {
			if g.VertexIsVisited(fmt.Sprintf("r%dc%d", r, c)) {
				n++
			}
		}
	}
	return n
}

// buildJitterGrid is a side x side 4-connected grid whose vertices are
// nudged to random positions and whose (bidirectional) edge weights are
// the true Euclidean distances between neighbours. This keeps the
// straight-line heuristic admissible and consistent while making every
// edge weight distinct, so the optimal path is unique.
func buildJitterGrid(side int, seed int64) (*StGraph, map[string][2]float64) {
	rng := rand.New(rand.NewSource(seed))
	var g StGraph
	pos := make(map[string][2]float64, side*side)
	name := func(r, c int) string { return fmt.Sprintf("r%dc%d", r, c) }

	for r := 0; r < side; r++ {
		for c := 0; c < side; c++ {
			x := float64(c) + rng.Float64()*0.4
			y := float64(r) + rng.Float64()*0.4
			pos[name(r, c)] = [2]float64{x, y}
			g.VertexAdd(name(r, c), x, y, 0)
		}
	}
	add := func(a, b string) {
		pa, pb := pos[a], pos[b]
		w := Distance(pa[0], pa[1], pb[0], pb[1])
		g.VertexAddEdge(a, b, w, false)
		g.VertexAddEdge(b, a, w, false)
	}
	for r := 0; r < side; r++ {
		for c := 0; c < side; c++ {
			if c < side-1 {
				add(name(r, c), name(r, c+1))
			}
			if r < side-1 {
				add(name(r, c), name(r+1, c))
			}
		}
	}
	return &g, pos
}

// TestAStarParityUnitGrid: on a uniform grid A* and Dijkstra must agree
// on the cost for many start/goal pairs.
func TestAStarParityUnitGrid(t *testing.T) {
	const side = 12
	g := buildGrid(side)
	rng := rand.New(rand.NewSource(1))

	for i := 0; i < 200; i++ {
		from := fmt.Sprintf("r%dc%d", rng.Intn(side), rng.Intn(side))
		to := fmt.Sprintf("r%dc%d", rng.Intn(side), rng.Intn(side))

		dOK, dPath := g.DijkstraSearch(from, to)
		aOK, aPath := g.AStarSearch(from, to)

		if dOK != aOK {
			t.Fatalf("%s->%s: ok mismatch dijkstra=%v astar=%v", from, to, dOK, aOK)
		}
		if dOK && math.Abs(pathCost(dPath)-pathCost(aPath)) > 1e-9 {
			t.Fatalf("%s->%s: cost mismatch dijkstra=%g astar=%g",
				from, to, pathCost(dPath), pathCost(aPath))
		}
	}
}

// TestAStarParityJitterGrid: on a grid with distinct Euclidean weights
// the optimal path is unique, so A* must return the exact same path as
// Dijkstra.
func TestAStarParityJitterGrid(t *testing.T) {
	const side = 14
	g, _ := buildJitterGrid(side, 42)
	rng := rand.New(rand.NewSource(7))

	for i := 0; i < 200; i++ {
		from := fmt.Sprintf("r%dc%d", rng.Intn(side), rng.Intn(side))
		to := fmt.Sprintf("r%dc%d", rng.Intn(side), rng.Intn(side))

		dOK, dPath := g.DijkstraSearch(from, to)
		aOK, aPath := g.AStarSearch(from, to)

		if dOK != aOK {
			t.Fatalf("%s->%s: ok mismatch dijkstra=%v astar=%v", from, to, dOK, aOK)
		}
		if !dOK {
			continue
		}
		if math.Abs(pathCost(dPath)-pathCost(aPath)) > 1e-9 {
			t.Fatalf("%s->%s: cost mismatch dijkstra=%g astar=%g",
				from, to, pathCost(dPath), pathCost(aPath))
		}
		if len(dPath) != len(aPath) {
			t.Fatalf("%s->%s: unique optimal path expected, len dijkstra=%d astar=%d",
				from, to, len(dPath), len(aPath))
		}
		for k := range dPath {
			if dPath[k].Name != aPath[k].Name {
				t.Fatalf("%s->%s: path node %d differs dijkstra=%s astar=%s",
					from, to, k, dPath[k].Name, aPath[k].Name)
			}
		}
	}
}

// TestAStarZeroHeuristicEqualsDijkstra: with h == 0, A* is Dijkstra.
func TestAStarZeroHeuristicEqualsDijkstra(t *testing.T) {
	const side = 10
	g := buildGrid(side)
	zero := func(_, _, _, _ float64) float64 { return 0 }

	from, to := "r0c0", fmt.Sprintf("r%dc%d", side-1, side-1)
	dOK, dPath := g.DijkstraSearch(from, to)
	aOK, aPath := g.AStarSearchFunc(from, to, zero)

	if !dOK || !aOK {
		t.Fatalf("expected a path: dijkstra=%v astar=%v", dOK, aOK)
	}
	if math.Abs(pathCost(dPath)-pathCost(aPath)) > 1e-9 {
		t.Fatalf("cost mismatch dijkstra=%g astar=%g", pathCost(dPath), pathCost(aPath))
	}
	// h==0 means no goal-direction, so A* explores the whole reachable
	// set just like Dijkstra.
	if av, dv := countVisited(g, side), side*side; av != dv {
		t.Errorf("zero-heuristic A* visited %d, want all %d", av, dv)
	}
}

// TestAStarCustomManhattanParity: a custom (still admissible) heuristic
// returns the same optimal cost.
func TestAStarCustomManhattanParity(t *testing.T) {
	const side = 12
	g := buildGrid(side)
	manhattan := func(cx, cy, gx, gy float64) float64 {
		return math.Abs(cx-gx) + math.Abs(cy-gy)
	}
	rng := rand.New(rand.NewSource(3))
	for i := 0; i < 100; i++ {
		from := fmt.Sprintf("r%dc%d", rng.Intn(side), rng.Intn(side))
		to := fmt.Sprintf("r%dc%d", rng.Intn(side), rng.Intn(side))
		_, dPath := g.DijkstraSearch(from, to)
		_, aPath := g.AStarSearchFunc(from, to, manhattan)
		if math.Abs(pathCost(dPath)-pathCost(aPath)) > 1e-9 {
			t.Fatalf("%s->%s: manhattan A* cost %g != dijkstra %g",
				from, to, pathCost(aPath), pathCost(dPath))
		}
	}
}

// TestAStarFewerExpansions: DijkstraSearch computes the full
// single-source tree (no early stop), so it always finalises every
// reachable vertex. A* adds goal-directed early termination plus
// heuristic pruning, so for a goal that is not the graph's farthest
// vertex it expands far fewer.
func TestAStarFewerExpansions(t *testing.T) {
	const side = 32
	g := buildGrid(side)
	from, to := "r0c0", "r8c8" // a near goal, not the far corner

	if ok, _ := g.DijkstraSearch(from, to); !ok {
		t.Fatal("dijkstra: expected a path")
	}
	dVisited := countVisited(g, side)

	if ok, _ := g.AStarSearch(from, to); !ok {
		t.Fatal("astar: expected a path")
	}
	aVisited := countVisited(g, side)

	t.Logf("visited: dijkstra=%d astar=%d (of %d)", dVisited, aVisited, side*side)
	if aVisited >= dVisited {
		t.Errorf("expected A* to expand fewer vertices: dijkstra=%d astar=%d",
			dVisited, aVisited)
	}
}

// TestAStarRespectsBlocked: a blocked vertex is avoided, matching
// Dijkstra; blocking the only corridor yields no path on both.
func TestAStarRespectsBlocked(t *testing.T) {
	// A small graph: S - A - T and S - B - T, with A cheaper.
	var g StGraph
	g.VertexAdd("S", 0, 0, 0)
	g.VertexAdd("A", 1, 0, 0)
	g.VertexAdd("B", 1, 1, 0)
	g.VertexAdd("T", 2, 0, 0)
	for _, e := range [][3]interface{}{
		{"S", "A", 1.0}, {"A", "T", 1.0},
		{"S", "B", 1.0}, {"B", "T", 5.0},
	} {
		g.VertexAddEdge(e[0].(string), e[1].(string), e[2].(float64), false)
		g.VertexAddEdge(e[1].(string), e[0].(string), e[2].(float64), false)
	}

	// Unblocked: shortest goes via A (cost 2).
	ok, path := g.AStarSearch("S", "T")
	if !ok || math.Abs(pathCost(path)-2) > 1e-9 {
		t.Fatalf("unblocked: ok=%v cost=%g, want cost 2", ok, pathCost(path))
	}

	// Block A: must reroute via B (cost 6).
	g.VertexBLock("A")
	ok, path = g.AStarSearch("S", "T")
	if !ok || math.Abs(pathCost(path)-6) > 1e-9 {
		t.Fatalf("A blocked: ok=%v cost=%g, want cost 6", ok, pathCost(path))
	}
	for _, p := range path {
		if p.Name == "A" {
			t.Fatal("path crossed blocked vertex A")
		}
	}

	// Block both intermediates: no path.
	g.VertexBLock("B")
	if ok, _ := g.AStarSearch("S", "T"); ok {
		t.Fatal("A and B blocked: expected no path")
	}
}

// TestAStarOneWay: a one-way edge cannot be traversed backwards.
func TestAStarOneWay(t *testing.T) {
	var g StGraph
	g.VertexAdd("X", 0, 0, 0)
	g.VertexAdd("Y", 1, 0, 0)
	g.VertexAddEdge("X", "Y", 1, true) // one-way X->Y only

	if ok, _ := g.AStarSearch("X", "Y"); !ok {
		t.Fatal("X->Y should be reachable")
	}
	if ok, _ := g.AStarSearch("Y", "X"); ok {
		t.Fatal("Y->X should NOT be reachable across a one-way edge")
	}
}

// TestAStarNoPath: disconnected components return false.
func TestAStarNoPath(t *testing.T) {
	var g StGraph
	g.VertexAdd("A", 0, 0, 0)
	g.VertexAdd("B", 9, 9, 0) // isolated
	if ok, _ := g.AStarSearch("A", "B"); ok {
		t.Fatal("expected no path between disconnected vertices")
	}
}

// TestAStarEdgeCases: same node, and missing endpoints.
func TestAStarEdgeCases(t *testing.T) {
	g := buildGrid(5)

	ok, path := g.AStarSearch("r2c2", "r2c2")
	if !ok || len(path) != 1 || path[0].Name != "r2c2" || pathCost(path) != 0 {
		t.Fatalf("same-node: ok=%v path=%v", ok, path)
	}

	if ok, _ := g.AStarSearch("nope", "r0c0"); ok {
		t.Error("missing start should return false")
	}
	if ok, _ := g.AStarSearch("r0c0", "nope"); ok {
		t.Error("missing goal should return false")
	}
}

// BenchmarkSearchCompare puts Dijkstra and A* side by side on the same
// 32x32 grid for two query shapes: "corner" (goal is the farthest
// vertex -- A*'s worst case, where the straight-line heuristic on a
// 4-connected grid barely prunes and the goal is finalised last) and
// "mid" (a nearer goal, where A* wins big because DijkstraSearch still
// floods the whole single-source tree).
func BenchmarkSearchCompare(b *testing.B) {
	const side = 32
	g := buildGrid(side)
	queries := []struct {
		name, from, to string
	}{
		{"corner", "r0c0", fmt.Sprintf("r%dc%d", side-1, side-1)},
		{"mid", "r0c0", "r8c8"},
	}
	for _, q := range queries {
		b.Run("dijkstra/"+q.name, func(b *testing.B) {
			b.ReportAllocs()
			for i := 0; i < b.N; i++ {
				if ok, _ := g.DijkstraSearch(q.from, q.to); !ok {
					b.Fatal("no path")
				}
			}
		})
		b.Run("astar/"+q.name, func(b *testing.B) {
			b.ReportAllocs()
			for i := 0; i < b.N; i++ {
				if ok, _ := g.AStarSearch(q.from, q.to); !ok {
					b.Fatal("no path")
				}
			}
		})
	}
}