unit Problems;

{$mode ObjFPC}{$H+}

interface

uses
  Classes, SysUtils, States;

function GetNeighbours(AState: TState): TStates;
function Search(InitialState: TState): TStates;
{ Можно ли передвинуть камень в указанном направлении. }
function CanPush(Element, Delta: TPosition; Walls, Boulders: TPositions): Boolean;
function IsGoal(ANode: TNode): Boolean;

function AtSomePosition(Needle: TPosition; Heap: TPositions): Boolean;
{ Дистанция между двумя позициями не учитывая стен. }
function ManhattanDistance(A, B: TPosition): Integer;
function CalculateHeuristic(AState: TState): Integer;
function ReconstructPath(CameFrom: TNode): TStates;

implementation

function GetNeighbours(AState: TState): TStates;
const
  { Вниз, вверх, вправо, влево. }
  Moves: array[1..4] of TPosition = (
    (X: 0; Y: 1),
    (X: 0; Y: -1),
    (X: 1; Y: 0),
    (X: -1; Y: 0)
  );
var
  NewState: TState;
  NewPosition, Move, Boulder: TPosition;
begin
  Result := TStates.Create;

  for Move in Moves do
  begin
    NewPosition.X := AState.Agent.X + Move.X;
    NewPosition.Y := AState.Agent.Y + Move.Y;

    if IsFree(NewPosition, AState.Walls) then
    begin
      NewState := TState.Create(AState, NewPosition);

      { Если можно передвинуть камень, добавляем новую позицию камня.
      Иначе, оставляем камень на месте. }
      for Boulder in AState.Boulders do
        if Boulder = NewPosition then
          if CanPush(Boulder, Move, AState.Walls, AState.Boulders) then
            NewState.AddBoulder(Boulder.X + Move.X, Boulder.Y + Move.Y)
          else
          begin
            FreeAndNil(NewState);
            break
          end
        else
          NewState.AddBoulder(Boulder.X, Boulder.Y);

      if NewState <> nil then
      begin
        SetLength(Result, Length(Result) + 1);
        Result[High(Result)] := NewState
      end
    end
  end
end;

function CanPush(Element, Delta: TPosition; Walls, Boulders: TPositions): Boolean;
var
  NewPosition: TPosition;
begin
  NewPosition.X := Element.X + Delta.X;
  NewPosition.Y := Element.Y + Delta.Y;
  Result := IsFree(NewPosition, Walls) and IsFree(NewPosition, Boulders)
end;

function IsGoal(ANode: TNode): Boolean;
var
  CurrentPlace: TPosition;
begin
  Result := true;

  for CurrentPlace in ANode.State.Places do
  begin
    Result := AtSomePosition(CurrentPlace, ANode.State.Boulders);
    if not Result then
      Exit
  end
end;

function Search(InitialState: TState): TStates;
var
  Frontier: TPriorityQueue;
  Heuristics, i: Integer;
  NextState, CurrentState: TState;
  NextNode, CurrentNode: TNode;
  Neighbours: TStates;
  Visited: TStates;
begin
  Visited := TStates.Create;
  Result := TStates.Create;

  Frontier := TPriorityQueue.Create;
  try
    Heuristics := CalculateHeuristic(initialState);
    { Создаем начало графа. }
    NextState := TState.Create(InitialState);
    NextNode := TNode.Create(NextState, nil, 0, Heuristics);
    Frontier.Add(NextNode);

    while not Frontier.IsEmpty do
    begin
      Heuristics := Frontier.Count;
      CurrentNode := Frontier.Poll;

      SetLength(Visited, Length(Visited) + 1);
      Visited[High(Visited)] := CurrentNode.State;

      if IsGoal(CurrentNode) then
      begin
        Result := ReconstructPath(CurrentNode);
        Exit
      end;
      Neighbours := GetNeighbours(CurrentNode.State);

      for i := 0 to High(Neighbours) do
      begin
        NextState := Neighbours[i];
        for CurrentState in Visited do
          if CurrentState.Compare(NextState) then
          begin
            FreeAndNil(NextState);
            break
          end;

        if NextState <> nil then
        begin
          Heuristics := CalculateHeuristic(NextState);
          NextNode := TNode.Create(NextState, CurrentNode, CurrentNode.Cost + 1, Heuristics);
          Frontier.Add(NextNode)
        end
      end
    end
  finally
    FreeAndNil(Frontier)
  end;
end;

function AtSomePosition(Needle: TPosition; Heap: TPositions): Boolean;
var
  CurrentPosition: TPosition;
begin
  Result := false;
  for CurrentPosition in Heap do
    if CurrentPosition = Needle then
    begin
      Result := true;
      Exit
    end
end;

function ManhattanDistance(A, B: TPosition): Integer;
begin
  Result := Abs(A.X - B.X) + Abs(A.Y - B.Y)
end;

function CalculateHeuristic(AState: TState): Integer;
var
  CurrentPlace, CurrentBoulder: TPosition;
  Distance, MinimalDistance: Integer;
begin
  Result := 0;
  for CurrentBoulder in AState.Boulders do
  begin
    MinimalDistance := Integer.MaxValue;
    for CurrentPlace in AState.Places do
    begin
      Distance := ManhattanDistance(CurrentBoulder, CurrentPlace);
      if (not AtSomePosition(CurrentPlace, AState.Boulders)) and (Distance < MinimalDistance) then
        MinimalDistance := Distance
    end;
    Inc(Result, MinimalDistance)
  end
end;

function ReconstructPath(CameFrom: TNode): TStates;
var
  Temporary: TState;
  CurrentLength, CurrentSegment: Integer;
begin
  Result := TStates.Create;
  CurrentLength := 0;

  while CameFrom <> nil do
  begin
    SetLength(Result, CurrentLength + 1);
    Result[CurrentLength] := TState.Create(CameFrom.State);
    CameFrom := CameFrom.Node;
    Inc(CurrentLength)
  end;
  Dec(CurrentLength);
  CurrentSegment := 0;

  while CurrentSegment < CurrentLength do
  begin
    Temporary := Result[CurrentSegment];
    Result[CurrentSegment] := Result[CurrentLength];
    Result[CurrentLength] := Temporary;
    Dec(CurrentLength);
    Inc(CurrentSegment)
  end
end;

end.