A*

// A* is Dijkstra with a hunch. Dijkstra spreads out in all directions equally,
// because it knows nothing about where the goal lies. A* adds an estimate of what
// is still to come and sorts the queue by "spent so far plus guessed remainder".
// That pulls the search towards the goal instead of letting it grow in a circle.
//
// The estimate has to be careful in one respect: it may never be too high. As long
// as it stays below the true remaining cost, A* finds exactly the same cheapest way
// as Dijkstra - it only looks at far fewer places on the way there. An estimate that
// is always zero turns A* back into Dijkstra.
public static class AStar
{
    public static Route Search(IGraph graph, int start, int goal, Func<int, double> estimate)
    {
        var cheapest = new Dictionary<int, double> { [start] = 0 };
        var cameFrom = new Dictionary<int, int>();
        var settled = new HashSet<int>();
        var seen = new List<int>();

        // The queue is sorted by the guessed total, the dictionary keeps the real
        // price. Mixing those two up is the classic mistake with this algorithm.
        var queue = new PriorityQueue<int, double>();

        queue.Enqueue(start, estimate(start));

        while (queue.TryDequeue(out var node, out _))
        {
            if (!settled.Add(node))
                continue;

            seen.Add(node);

            var cost = cheapest[node];

            if (node == goal)
                return Route.Backwards(cameFrom, start, goal, cost, [.. seen]);

            foreach (var step in graph.Neighbours(node))
            {
                var price = cost + step.Cost;

                if (cheapest.TryGetValue(step.Node, out var known) && known <= price)
                    continue;

                cheapest[step.Node] = price;
                cameFrom[step.Node] = node;

                queue.Enqueue(step.Node, price + estimate(step.Node));
            }
        }

        return Route.Nothing([.. seen]);
    }
}

// '#' is a wall, '.' is grass, ',' is sand and '~' is mud. S is where the way
// starts, G is where it should end.
public static readonly string[] Terrain =
[
    "############################################################",
    "#.....................................##...................#",
    "#...........##........................##...................#",
    "#...........##................######..##...................#",
    "#...........##................######..##...................#",
    "#...........##........................##...................#",
    "#...........##.,,,,,,,,...............##...................#",
    "#...........##.~~~~~~~~...............##...................#",
    "#...........##.~~~~~~~~.##............##...................#",
    "#...........##.~~~~~~~~.##............##...................#",
    "#S..........##.~~~~~~~~.##............##..................G#",
    "#...........##.~~~~~~~~.##............##...................#",
    "#...........##.~~~~~~~~.##...............,,,,,,,,,,........#",
    "#..............~~~~~~~~.##..,,,,,,,,,....~~~~~~~~~~........#",
    "#..............,,,,,,,,.##..~~~~~~~~~....~~~~~~~~~~##......#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~##......#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~........#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~........#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~........#",
    "#.......................##.................................#",
    "############################################################",
];

// What a step onto a square costs. Zero means there is no way through.
public static GridGraph Ground() => GridGraph.Parse(Terrain, square => square switch
{
    '#' => 0,
    ',' => 2,
    '~' => 5,
    _ => 1,
});

// Where the S and the G stand.
public static int Find(char square)
{
    for (var y = 0; y < Terrain.Length; y++)
    {
        var x = Terrain[y].IndexOf(square);

        if (x >= 0)
            return y * Terrain[0].Length + x;
    }

    throw new ArgumentException($"The map has no '{square}'.", nameof(square));
}

// The map again, with a way drawn into it.
public static string Draw(GridGraph map, int[] path)
{
    var drawn = Terrain.Select(row => row.ToCharArray()).ToArray();

    foreach (var node in path)
    {
        if (drawn[map.Y(node)][map.X(node)] is not ('S' or 'G'))
            drawn[map.Y(node)][map.X(node)] = 'o';
    }

    return string.Join(Environment.NewLine, drawn.Select(row => new string(row)));
}

var map = Maps.Ground();
var start = Maps.Find('S');
var goal = Maps.Find('G');

// Dijkstra knows nothing about where the goal lies and grows in a circle.
// A* is told to estimate what is left as the distance in steps, walls and
// mud ignored. That estimate is never too high, so both find the same way.
var blind = Dijkstra.Search(map, start, goal);
var guided = AStar.Search(map, start, goal, node => map.Manhattan(node, goal));

Console.WriteLine(Maps.Draw(map, guided.Path));
Console.WriteLine($"Dijkstra  cost {blind.Cost,3}  {blind.Visited,4} squares looked at");
Console.WriteLine($"A*        cost {guided.Cost,3}  {guided.Visited,4} squares looked at");

var map = Maps.Ground();
var start = Maps.Find('S');
var goal = Maps.Find('G');

// The estimate can be taken more or less seriously. Weight 0 ignores it
// completely - that is Dijkstra. Weight 1 is the honest estimate. Anything
// above that pulls harder towards the goal than is allowed, and the way
// found may no longer be the cheapest one.
Console.WriteLine("weight  cost  squares looked at");

foreach (var weight in new[] { 0, 1, 1.5, 3, 8 })
{
    var route = AStar.Search(map, start, goal, node => map.Manhattan(node, goal) * weight);

    Console.WriteLine($"{weight,6}  {route.Cost,4}  {route.Visited,17}");
}