Maze Challenge: Heuristic custom BFS

program MazeSolver;
{$I SRL/OSR.simba}
{$R-}


// home brewed search structure for the heuristic BFS lookup
// maintains order. High cost of insertion O(n).
type
  TPathStruct = record
    node: TPoint;
    hdist: Double;
    rdist: Double;
  end;

  TSearchArray = record
    lo, hi: Int32;
    data: array of TPathStruct;
  end;


procedure TSearchArray.Init(sz: Int32);
begin
  SetLength(Self.data, sz);

  Self.hi := -1;
  Self.lo := 0;
end;

// find the current value or the closest value in the array.
// Time complexity: O(log n)
function TSearchArray.SearchClosest(value: Double): Int32;
var
  l, h: Int32;
begin
  l := Self.Lo;
  h := Self.Hi;

  while l <= h do
  begin
    Result := (l + h) div 2;
    if Self.Data[Result].hdist < value then
      h := Result - 1
    else if Self.Data[Result].hdist > value then
      l := Result + 1
    else
      Exit(Result);
  end;
end;

// Insert the a value into it's correct position by searching.
// Time complexity: O(n)
// Average complexity for a maze-solver is O(log n).
function TSearchArray.Insert(node:TPoint; hdist, rdist: Double): Int32;
var
  idx,i: Int32;
begin
  idx := Self.SearchClosest(hdist);
  if Self.Data[idx].hdist > hdist then Inc(idx);

  if (self.hi+1-idx) > 0 then
    Move(Self.data[idx], Self.data[idx+1], (self.hi+1-idx)*(SizeOf(TPathStruct)));

  Self.data[idx].hdist := hdist;
  Self.data[idx].rdist := rdist;
  Self.data[idx].node  := node;

  Inc(Self.hi);
end;

// Pop the largest or smallest value depending on insertion order
// Time complexity: O(1)
procedure TSearchArray.Pop(out node:TPoint; out hdist, rdist: Double);
begin
  node  := self.data[self.hi].node;
  hdist := self.data[self.hi].hdist;
  rdist := self.data[self.hi].rdist;

  Dec(Self.Hi);
end;

function TSearchArray.AsString(): String;
var
  tmp: array of TPathStruct;
begin
  tmp := self.data;
  SetLength(tmp, self.hi+1);
  Result := ToString(tmp);
end;


procedure GetAdjacent(var adj:TPointArray; n:TPoint; EightWay:Boolean);
begin
  adj[0] := Point(n.x-1,n.y);
  adj[1] := Point(n.x,n.y-1);
  adj[2] := Point(n.x+1,n.y);
  adj[3] := Point(n.x,n.y+1);
  if EightWay then
  begin
    adj[4] := Point(n.x-1,n.y-1);
    adj[5] := Point(n.x+1,n.y+1);
    adj[6] := Point(n.x-1,n.y+1);
    adj[7] := Point(n.x+1,n.y-1);
  end;
end;


function SolveMaze(Start, Stop: TPoint; Img: TMufasaBitmap): TPointArray;
var
  score,hdist: Double;
  i,w,h: Int32;
  arr: TSearchArray;
  map: TSingleMatrix;
  adj: TPointArray;
  p,q: TPoint;

  bmp: TMufasaBitmap;
begin
  SetLength(adj, 4);
  arr.Init(img.GetWidth() * img.GetHeight());
  arr.Insert(start, Sqr(start.x-stop.x)+Sqr(start.y-stop.y), 0);

  w := img.GetWidth() - 1;
  h := img.GetHeight() - 1;

  map.SetSize(w+1, h+1);

  // heuristical forward search for goal
  while arr.hi >= 0 do
  begin
    arr.Pop(p, hdist, score);
    if p = stop then break;
    GetAdjacent(adj, p, Length(adj)=8);
    for q in adj do
      if img.PointInBitmap(q.x,q.y) and (img.GetPixel(q.x, q.y) = $FFFFFF)  then
      begin
        img.SetPixel(q.x,q.y, $666666);
        arr.Insert(q, score + Sqr(q.x-stop.x)+Sqr(q.y-stop.y), score+1);
        map[q.y, q.x] := score+1; //<--- for simple backtrace
      end;
  end;

  // backtrace towards 0 distance from stop using the distance map
  // I backtrace 8-way
  SetLength(adj, 8);
  score := map[stop.y,stop.x];
  Result += p;
  while score > 1 do
  begin
    GetAdjacent(adj, p, Length(adj)=8);
    for q in adj do
      if InRange(q.x, 0, w) and InRange(q.y, 0, h) and
         (map[q.y, q.x] < score) and (map[q.y, q.x] > 0) then
      begin
        score := map[q.y, q.x];
        p := q;
      end;
    Result += p;
  end;
  Result += start;

  //bmp.Init();
  //bmp.DrawMatrix(map);
  //bmp.Debug();
end;


var
  start := Point(5,0);
  stop  := Point(635,480);

  img, im2: TMufasaBitmap;
  TPA,white,black,path1,path2: TPointArray;
  t:Double;
begin
  img.Init();
  img.LoadFromFile('images\maze.png');

  t := PerformanceTimer;
  begin
    img.FindColors(black, $0);
    img.DrawTPA(black.Grow(2),0);
    path1 := SolveMaze(start, stop, img);
  end;
  WriteLn(PerformanceTimer - t, 'ms');

  WriteLn('Length: ', Length(path1));

  img.DrawClear($FFFFFF);
  img.DrawTPA(black, 0);
  img.DrawTPA(path1,$00FF00);

  img.Debug();
  img.Free();
end.