Heap Queue - A-Star

program new;

type
  _TOpenItem = record
    Loc:TPoint;
    Weight:Integer;
  end;
  _TOpenSet = array of _TOpenItem;

  _AStarData = record
    open, closed:Boolean;
    scoreA, scoreB: Integer;
    Trace:TPoint;
    Isset:Boolean;
  end;
  _AStarDataArr = array of array of _AStarData;


// A heap-structure based on Python's heap implementation.
procedure _TOpenSet._siftUp(pos: Integer);
var
  endpos, startpos, childpos, rightpos: Integer;
  newitem: _TOpenItem;
begin
  endpos := Length(Self);
  startpos := pos;
  newitem := Self[pos];
  // Move the smaller child up until hitting a leaf.
  childpos := 2 * pos + 1;    // leftmost child
  while (childpos < endpos) do
  begin
    // Set childpos to index of smaller child.
    rightpos := childpos + 1;
    if (rightpos < endpos) and (Self[childpos].Weight >= Self[rightpos].Weight) then
      childpos := rightpos;
    // Move the smaller child up.
    Self[pos] := Self[childpos];
    pos := childpos;
    childpos := 2 * pos + 1;
  end;
  // This (`pos`) node/leaf is empty. So we can place "newitem" in here, then
  // push it up to its final place (by sifting its parents down).
  Self[pos] := newitem;
  Self._siftDown(startpos, pos);
end;

// Follow the path to the root, moving parents down until finding a place newitem `pos` fits.
procedure _TOpenSet._siftDown(startpos, pos: Integer);
var
  parentpos: Integer;
  parent,newitem: _TOpenItem;
begin
  newitem := Self[pos]
  while pos > startpos do
  begin
    parentpos := (pos - 1) shr 1;
    parent := Self[parentpos];
    if (newitem.Weight < parent.Weight) then
    begin
      Self[pos] := parent;
      pos := parentpos;
      continue;
    end;
    Break;
  end;
  Self[pos] := newitem;
end;

// Push the item onto heap, maintaining the heap invariant
procedure _TOpenSet.HeapPush(Item:_TOpenItem);
var L:Integer;
begin
  L := Length(Self);
  SetLength(Self, L+1);
  Self[L] := item;
  Self._siftDown(0, L);
end;

function _TOpenSet.Pop(): _TOpenItem;
begin
  Result := Self[High(Self)];
  SetLength(Self, High(Self));
end;

// Pop the smallest item off the heap, maintaining the heap invariant.
function _TOpenSet.HeapPop(): _TOpenItem;
var
  lastelt:_TOpenItem;
begin
  lastelt := Self.pop();
  if Length(self) > 0 then
  begin
    Result := Self[0];
    Self[0] := lastelt;
    Self._siftup(0);
  end else
    Result := lastelt;
end;



// Calculate the distance between p1, and p2 using Squared euclidean.
function SqEuclidean(p1, p2:TPoint): Integer;
begin
  Result := Round(Sqr(p1.x-p2.x) + Sqr(p1.y-p2.y));
end;


// Fills AdjArr with the 4 adjacent points
procedure GetAdjacent(var AdjArr:TPointArray; Pt:TPoint);
begin
  AdjArr[0] := Point(Pt.x-1,pt.y);
  AdjArr[2] := Point(Pt.x+1,pt.y);
  AdjArr[1] := Point(Pt.x,pt.y-1);
  AdjArr[3] := Point(Pt.x,pt.y+1);
end;



(*
 Walks the path we came from backwards from current (goal) until it can't walk
 further. When it reaches "the end" it will return the path reversed (Start->Goal).
*)
function BacktracePath(Paths:_AStarDataArr; Curr:TPoint): TPointArray;
var L:Integer;
begin
  SetLength(Result, 1);
  Result[0] := Curr;
  L := 1;
  while True do
  begin
    Curr := paths[curr.y][curr.x].Trace;
    SetLength(Result, L+1);
    Result[L] := Curr;
    Inc(L);
    if (paths[curr.y][curr.x].Isset = False) then Break;
  end;
  InvertTPA(Result); //Reverse the list (bad name)
end;



(*
 AStar search algorithm modified to explicity work with Image-maze.
 This implementation is mostly ment as an example, there is multiple ways
 to notably speed it up, and possibly simplify it.
*)
function AStar(Maze: Integer; Start, Goal:TPoint; Color:Integer): TPointArray;
var
  hsize,score,i,j,W,H: Integer;
  Data: _AStarDataArr;
  OpenSet: _TOpenSet;
  HPt: _TOpenItem;
  Adj,Pt:TPoint;
  Neighbors:TPointArray;
begin
  SetLength(Neighbors, 4);
  GetBitmapSize(Maze, W,H);
  // init 2D array to keep track of data
  SetLength(Data, H, W);
  // start distace Start -> Goal.
  Data[start.y][start.x].ScoreB := SqEuclidean(Start, Goal);
  // init the openset with Start-coord
  SetLength(OpenSet, 1);
  OpenSet[0].Loc := start;
  OpenSet[0].Weight := Data[start.y][start.x].ScoreB;
  HSize := 1;

  Dec(W); Dec(H);
  //while openset is not empty
  while (HSize > 0) do
  begin
    // pop the smallest item from openset
    HPt := OpenSet.HeapPop(); //PopMin(OpenSet);
    Pt := HPt.Loc;
    Dec(HSize);
    //if current = goal then return path.
    if (Pt.x = Goal.x) and (Pt.y = Goal.y) then
    begin
      Result := BacktracePath(Data, Goal);
      Exit;
    end;
    // "remove" current point
    Data[Pt.y][Pt.x].Open := False;
    Data[Pt.y][Pt.x].Closed := True;

    // for each neighbor check if we can/should walk it
    GetAdjacent(Neighbors, Pt)
    for j:=0 to 3 do
    begin
      Adj := Neighbors[j];
      // out of range, or wall
      if not(InRange(Adj.x, 0, W) and InRange(Adj.y, 0, H)) or
         (FastGetPixel(Maze, Adj.x, Adj.y) <> Color) then
        Continue;

      Score := (Data[Pt.y][Pt.x].ScoreA + 1);
      if (Data[Adj.y][Adj.x].Closed = True) and (Score >= Data[Adj.y][Adj.x].ScoreA) then
        Continue;

      if ((Data[Adj.y][Adj.x].Open = False) or
         (Score < Data[Adj.y][Adj.x].ScoreA)) then
      begin
        Data[Adj.y][Adj.x].Trace := Pt;     //Used when we backtrace.
        Data[Adj.y][Adj.x].Isset := True;   //Used when we backtrace.
        // keep track over scores || used to evaluate best direction.
        Data[Adj.y][Adj.x].ScoreA := Score;
        Data[Adj.y][Adj.x].ScoreB := (Data[Adj.y][Adj.x].ScoreA + SqEuclidean(Adj, Goal));
        // if not already added to the openset then add it and Increase HSize
        if (Data[Adj.y][Adj.x].Open = False) then
        begin
          HPt.Loc := Adj;
          HPt.Weight := Data[Adj.y][Adj.x].ScoreB;
          OpenSet.HeapPush(HPt);
          Data[Adj.y][Adj.x].Open := True;
          Inc(HSize);
        end;
      end;
    end;
  end;
end;



//------------

var
  bmp, W, H: Integer;
  Path:TPointArray;
begin
  bmp := LoadBitmap(AppPath + 'tests/maze.png');
  Path := AStar(bmp, Point(0,44), Point(148,0), $FFFFFF);
  DrawTPABitmap(bmp, path, $FF);

  GetBitmapSize(BMP, W,H);
  DisplayDebugImgWindow(W,H);
  DrawBitmapDebugImg(bmp);
  FreeBitmap(bmp);
end.