unit States;

{$mode ObjFPC}{$H+}

interface

uses
  Classes, SysUtils, fgl;

type
  TPosition = record
    X, Y: Integer
  end;
  TPositions = array of TPosition;

  TState = class
  private
    PAgent: TPosition;
    PWalls, PPlaces, PBoulders: TPositions;

    procedure CopyArrays(AState: TState);

  public
    constructor Create;
    constructor Create(ASTate: TState; AAgent: TPosition);
    constructor Create(ASTate: TState);

    property Walls: TPositions read PWalls;
    property Places: TPositions read PPlaces;
    property Boulders: TPositions read PBoulders;
    property Agent: TPosition read PAgent write PAgent;

    procedure AddWall(AX, AY: Integer);
    procedure AddPlace(AX, AY: Integer);
    procedure AddBoulder(AX, AY: Integer);
    procedure SetAgent(AX, AY: Integer);
    function Compare(AState: TState): Boolean;
  end;
  TStates = array of TState;

  TBoard = class
  private
    PWidth, PHeight: Integer;
    PInitialState: TState;

  public
    property Width: Integer read PWidth;
    property Height: Integer read PHeight;
    property InitialState: TState read PInitialState;

    constructor Create(AWidth, AHeight: Integer; AInitialState: TState);
    destructor Destroy; override;
  end;

  INode = interface
    function GetTotalCost: Integer;
  end;

  TNode = class(TInterfacedObject, INode)
  private
    PState: TState;
    PNode: TNode;
    PCost, Heuristic: Integer;
  public
    property State: TState read PState;
    property Node: TNode read PNode;
    property Cost: Integer read PCost;

    constructor Create(AState: TState; ANode: TNode; ACost, AHeuristic: Integer);
    destructor Destroy;

    function GetTotalCost: Integer;
  end;
  TNodes = array of TNode;

  TNodeList = specialize TFPGList<TNode>;
  TPriorityQueue = class
  private
    Nodes: TNodeList;

  public
    constructor Create;

    procedure Add(ANode: TNode);
    function Poll: TNode;
    function IsEmpty: Boolean;
    function Count: Integer;
  end;

  operator = (P1, P2: TPosition) B: Boolean;

  function IsFree(NewPosition: TPosition; Walls: TPositions): Boolean;

implementation

procedure TSTate.CopyArrays(AState: TState);
begin
  PWalls := Copy(AState.Walls, 0, Length(ASTate.Walls));
  PPlaces := Copy(AState.Places, 0, Length(ASTate.Places))
end;

constructor TState.Create;
begin
end;

constructor TState.Create(ASTate: TState; AAgent: TPosition);
begin
  PAgent := AAgent;
  CopyArrays(AState);
  PBoulders := TPositions.Create
end;

constructor TState.Create(ASTate: TState);
begin
  PAgent := AState.Agent;
  CopyArrays(AState);
  PBoulders := Copy(AState.Boulders, 0, Length(AState.Boulders))
end;

procedure TState.AddWall(AX, AY: Integer);
var
  CurrentLength: Integer;
begin
  CurrentLength := Length(PWalls);
  SetLength(PWalls, CurrentLength + 1);

  PWalls[CurrentLength].X := AX;
  PWalls[CurrentLength].Y := AY
end;

procedure TState.AddPlace(AX, AY: Integer);
var
  CurrentLength: Integer;
begin
  CurrentLength := Length(PPlaces);
  SetLength(PPlaces, CurrentLength + 1);

  PPlaces[CurrentLength].X := AX;
  PPlaces[CurrentLength].Y := AY
end;

procedure TState.AddBoulder(AX, AY: Integer);
var
  CurrentLength: Integer;
begin
  CurrentLength := Length(PBoulders);
  SetLength(PBoulders, CurrentLength + 1);

  PBoulders[CurrentLength].X := AX;
  PBoulders[CurrentLength].Y := AY
end;

procedure TState.SetAgent(AX, AY: Integer);
begin
  PAgent.X := AX;
  PAgent.Y := AY
end;

function TState.Compare(AState: TState): Boolean;
var
  i: Integer;
begin
  Result := (Agent = AState.Agent)
      and (Length(Walls) = Length(AState.Walls))
      and (Length(Boulders) = Length(AState.Boulders))
      and (Length(Places) = Length(AState.Places));
  if not Result then
    Exit;
  for i := 0 to High(Walls) do
    if Walls[i] <> AState.Walls[i] then
    begin
      Result := false;
      Exit
    end;
  for i := 0 to High(Boulders) do
    if Boulders[i] <> AState.Boulders[i] then
    begin
      Result := false;
      Exit
    end;
  for i := 0 to High(Places) do
    if Places[i] <> AState.Places[i] then
    begin
      Result := false;
      Exit
    end
end;

constructor TBoard.Create(AWidth, AHeight: Integer; AInitialState: TState);
begin
  PWidth := AWidth;
  PHeight := AHeight;
  PInitialState := AInitialState
end;

destructor TBoard.Destroy;
begin
  PInitialState.Destroy;
  inherited
end;

constructor TNode.Create(AState: TState; ANode: TNode; ACost, AHeuristic: Integer);
begin
  PState := AState;
  PNode := ANode;
  PCost := ACost;
  Heuristic := AHeuristic
end;

destructor TNode.Destroy;
begin
  PState.Free;
  inherited
end;

function TNode.GetTotalCost: Integer;
begin
  Result := Cost + Heuristic
end;

constructor TPriorityQueue.Create;
begin
  Nodes := TNodeList.Create
end;

procedure TPriorityQueue.Add(ANode: TNode);
var
  Index, TotalCost: Integer;
  CurrentNode: TNode;
begin
  Index := 0;
  TotalCost := ANode.GetTotalCost;

  for CurrentNode in Nodes do
  begin
    if CurrentNode.getTotalCost >= TotalCost then
      break;
    Inc(Index)
  end;
  Nodes.Insert(Index, ANode)
end;

function TPriorityQueue.Poll: TNode;
begin
  Result := Nodes.Extract(Nodes.First)
end;

function TPriorityQueue.IsEmpty: Boolean;
begin
  Result := Count = 0
end;

function TPriorityQueue.Count: Integer;
begin
  Result := Nodes.Count
end;

operator = (P1, P2: TPosition) B: Boolean;
begin
  B := (P1.X = P2.X) and (P1.Y = P2.Y)
end;

function IsFree(NewPosition: TPosition; Walls: TPositions): Boolean;
var
  Wall: TPosition;
begin
  for Wall in Walls do
    if NewPosition = Wall then
    begin
      Result := false;
      Exit
    end;
  Result := true
end;

end.