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;

        New_G_Score : G_Score;
        New_F_Score : F_Score;
    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;
                    New_G_Score := G_Scores.Element (Current) + 1;
                    New_F_Score := New_G_Score + Heuristic (N, Goal);
                    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 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;