1 files changed, 193 insertions, 0 deletions
diff --git a/src/pathfinding.adb b/src/pathfinding.adb
new file mode 100644
index 0000000..1435ecf
--- /dev/null
+++ b/src/pathfinding.adb
@@ -0,0 +1,193 @@
+
+
+with
+
+    Things,
+    Ada.Containers.Ordered_Sets,
+    Ada.Containers.Ordered_Maps;
+
+
+package body Pathfinding is
+
+
+    type Node is record
+        X, Y : Integer;
+    end record;
+
+    function "<" (A, B : in Node) return Boolean is
+    begin
+        return A.X < B.X or (A.X = B.X and A.Y < B.Y);
+    end "<";
+
+    package Node_Sets is new Ada.Containers.Ordered_Sets
+        (Element_Type => Node);
+    package Node_To_Node_Maps is new Ada.Containers.Ordered_Maps
+        (Key_Type => Node, Element_Type => Node);
+
+
+
+
+    subtype G_Score is Integer;
+    subtype F_Score is Integer;
+
+    package G_Score_Maps is new Ada.Containers.Ordered_Maps
+        (Key_Type => Node, Element_Type => G_Score);
+    package F_Score_Maps is new Ada.Containers.Ordered_Maps
+        (Key_Type => Node, Element_Type => F_Score);
+
+    G_Scores : G_Score_Maps.Map := G_Score_Maps.Empty_Map;
+    F_Scores : F_Score_Maps.Map := F_Score_Maps.Empty_Map;
+
+
+
+
+    function F_Min
+           (N_Set : in Node_Sets.Set)
+        return Node
+    is
+        Result : Node := N_Set.First_Element;
+    begin
+        for N of N_Set loop
+            if F_Scores.Contains (N) then
+                if  not F_Scores.Contains (Result) or else
+                    F_Scores.Element (N) < F_Scores.Element (Result)
+                then
+                    Result := N;
+                end if;
+            end if;
+        end loop;
+        return Result;
+    end F_Min;
+
+
+
+
+    function Reconstruct_Path
+           (Came_From : in Node_To_Node_Maps.Map;
+            Current   : in Node)
+        return Moves.Path
+    is
+        Result : Moves.Path := Moves.Empty_Path;
+        Working : Node := Current;
+        Next : Node;
+    begin
+        while Came_From.Contains (Working) loop
+            Next := Came_From.Element (Working);
+            Result.Prefix
+                ((Delta_X => Working.X - Next.X, Delta_Y => Working.Y - Next.Y, Push => False));
+            Working := Next;
+        end loop;
+        return Result;
+    end Reconstruct_Path;
+
+
+
+
+    function Heuristic
+           (A, B : in Node)
+        return Integer is
+    begin
+        return abs (A.X - B.X) + abs (A.Y - B.Y);
+    end Heuristic;
+
+
+
+
+    function Neighbours
+           (My_Grid : in Grids.Grid;
+            Current : in Node)
+        return Node_Sets.Set
+    is
+        use type Things.Thing;
+
+        function Valid (X, Y : in Integer) return Boolean is
+        begin
+            return My_Grid.In_Bounds (X, Y) and then
+                My_Grid.Get_Square (X, Y).Is_Walkable and then
+                My_Grid.Get_Square (X, Y).Get_Contents = Things.Nothing;
+        end Valid;
+
+        Result : Node_Sets.Set := Node_Sets.Empty_Set;
+    begin
+        if Valid (Current.X - 1, Current.Y) then
+            Result.Insert ((Current.X - 1, Current.Y));
+        end if;
+        if Valid (Current.X + 1, Current.Y) then
+            Result.Insert ((Current.X + 1, Current.Y));
+        end if;
+        if Valid (Current.X, Current.Y - 1) then
+            Result.Insert ((Current.X, Current.Y - 1));
+        end if;
+        if Valid (Current.X, Current.Y + 1) then
+            Result.Insert ((Current.X, Current.Y + 1));
+        end if;
+        return Result;
+    end Neighbours;
+
+
+
+
+    function A_Star
+           (My_Grid : in Grids.Grid;
+            SX, SY  : in Integer;
+            FX, FY  : in Integer)
+        return Moves.Path
+    is
+        use type Node_Sets.Set;
+
+        Start   : Node := (X => SX, Y => SY);
+        Goal    : Node := (X => FX, Y => FY);
+        Current : Node;
+
+        Closed_Set : Node_Sets.Set := Node_Sets.Empty_Set;
+        Open_Set   : Node_Sets.Set := Node_Sets.To_Set (Start);
+
+        Came_From : Node_To_Node_Maps.Map := Node_To_Node_Maps.Empty_Map;
+    begin
+        G_Scores := G_Score_Maps.Empty_Map;
+        G_Scores.Insert (Start, 0);
+        F_Scores := F_Score_Maps.Empty_Map;
+        F_Scores.Insert (Start, Heuristic (Start, Goal));
+
+        --  This is a textbook implementation of A* search.
+        while Open_Set /= Node_Sets.Empty_Set loop
+            Current := F_Min (Open_Set);
+            if Current = Goal then
+                return Reconstruct_Path (Came_From, Current);
+            end if;
+            Open_Set.Delete (Current);
+            Closed_Set.Insert (Current);
+            for N of Neighbours (My_Grid, Current) loop
+                if not Closed_Set.Contains (N) then
+                    if not Open_Set.Contains (N) then
+                        Open_Set.Insert (N);
+                    end if;
+                    declare
+                        New_G_Score : G_Score := G_Scores.Element (Current) + 1;
+                        New_F_Score : F_Score := New_G_Score + Heuristic (N, Goal);
+                    begin
+                        if  not G_Scores.Contains (N) or else
+                            New_G_Score < G_Scores.Element (N)
+                        then
+                            Came_From.Insert (N, Current);
+                            G_Scores.Insert (N, New_G_Score);
+                            F_Scores.Insert (N, New_F_Score);
+                        end if;
+                    end;
+                end if;
+            end loop;
+        end loop;
+
+        --  And this is a modification to get as close as possible
+        --  if the goal is out of reach.
+        for N of Closed_Set loop
+            if Heuristic (N, Goal) < Heuristic (Current, Goal) then
+                Current := N;
+            end if;
+        end loop;
+        return Reconstruct_Path (Came_From, Current);
+    end A_Star;
+
+
+end Pathfinding;
+