A*

// A* ist Dijkstra mit einer Ahnung. Dijkstra breitet sich gleichmäßig in alle Richtungen
// aus, weil er nicht weiß, wo das Ziel liegt. A* nimmt eine Schätzung des Restwegs dazu
// und sortiert die Warteschlange nach "bisher ausgegeben plus geschätzter Rest".
// Das zieht die Suche zum Ziel, statt sie im Kreis wachsen zu lassen.
//
// Die Schätzung muss in einem Punkt vorsichtig sein: Sie darf nie zu hoch liegen. Solange
// sie unter den echten Restkosten bleibt, findet A* genau denselben günstigsten Weg
// wie Dijkstra - es schaut unterwegs nur weit weniger Orte an. Eine Schätzung, die
// immer null ist, macht aus A* wieder 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>();

        // Die Warteschlange ist nach der geschätzten Summe sortiert, das Wörterbuch hält den
        // echten Preis. Beides zu verwechseln ist der klassische Fehler bei diesem Algorithmus.
        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]);
    }
}

// '#' ist Wand, '.' Wiese, ',' Sand und '~' Sumpf. Bei S beginnt der Weg,
// bei G soll er enden.
public static readonly string[] Terrain =
[
    "############################################################",
    "#.....................................##...................#",
    "#...........##........................##...................#",
    "#...........##................######..##...................#",
    "#...........##................######..##...................#",
    "#...........##........................##...................#",
    "#...........##.,,,,,,,,...............##...................#",
    "#...........##.~~~~~~~~...............##...................#",
    "#...........##.~~~~~~~~.##............##...................#",
    "#...........##.~~~~~~~~.##............##...................#",
    "#S..........##.~~~~~~~~.##............##..................G#",
    "#...........##.~~~~~~~~.##............##...................#",
    "#...........##.~~~~~~~~.##...............,,,,,,,,,,........#",
    "#..............~~~~~~~~.##..,,,,,,,,,....~~~~~~~~~~........#",
    "#..............,,,,,,,,.##..~~~~~~~~~....~~~~~~~~~~##......#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~##......#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~........#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~........#",
    "#.......................##..~~~~~~~~~....~~~~~~~~~~........#",
    "#.......................##.................................#",
    "############################################################",
];

// Was ein Schritt auf ein Feld kostet. Null heißt: kein Durchkommen.
public static GridGraph Ground() => GridGraph.Parse(Terrain, square => square switch
{
    '#' => 0,
    ',' => 2,
    '~' => 5,
    _ => 1,
});

// Wo das S und das G stehen.
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));
}

// Die Karte noch einmal, mit einem Weg hineingezeichnet.
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 weiß nichts darüber, wo das Ziel liegt, und wächst im Kreis.
// A* bekommt als Schätzung des Rests die Anzahl der Schritte, ohne Wände und
// ohne Sumpf. Sie liegt nie zu hoch, deshalb finden beide denselben Weg.
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');

// Die Schätzung kann man mehr oder weniger ernst nehmen. Gewicht 0 lässt sie ganz
// weg - das ist Dijkstra. Gewicht 1 ist die ehrliche Schätzung. Alles darüber zieht
// stärker zum Ziel, als erlaubt ist, und der gefundene Weg ist dann
// vielleicht nicht mehr der günstigste.
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}");
}