Maze solver Simba 2.0 - ffast

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: single;
    rdist: single;
  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: single): 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)
// For this usage we are looking at time complexity in range of
// average O(log n)
function TSearchArray.Insert(_node:TPoint; _hdist, _rdist: single): Int32;
var
  idx,i: Int32;
begin
  if (Self.hi = -1) or (Self.Data[Self.hi].hdist >= _hdist) then
    idx := self.hi + 1
  else if (Self.hi >= 5) and (_hdist <= Self.Data[Self.hi-5].hdist) then
  begin
    for idx := self.hi+1 downto Self.hi-5 do
    begin
      Self.data[idx] := Self.data[idx-1];
      if Self.data[idx].hdist >= _hdist then break;
    end;
  end else 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)));
  end;

  with Self.data[idx] do
  begin
    hdist := _hdist;
    rdist := _rdist;
    node  := _node;
  end;

  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: single);
begin
  with self.data[self.hi] do
  begin
    _node  := node;
    _hdist := hdist;
    _rdist := rdist;
  end;

  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;


function SolveMaze(Start, Stop: TPoint; Img: TImage): TPointArray;
var
  score,hdist,score1: single;
  i,w,h: Int32;
  arr: TSearchArray;
  map: TSingleMatrix;
  p,q: TPoint;
  bmp: TImage;
  adj: TPointArray = [[-1,0],[1,0],[0,-1],[0,1],[1,-1],[1,1],[-1,-1],[-1,1]];
  t: Double;
begin
  arr.Init(img.Width * img.Height);
  arr.Insert(start, Sqr(start.x-stop.x)+Sqr(start.y-stop.y), 0);

  w := img.Width - 1;
  h := img.Height - 1;
  map.SetSize(w+1, h+1);

  SetLength(adj, 4);
  // heuristical forward search for goal
  while (arr.hi >= 0) do
  begin
    arr.Pop(p, hdist, score); //inline for higher performance
    if (p = stop) then break;
    for q in adj do
      with q do
      begin
        x += p.x;
        y += p.y;
        if img.InImage(x,y) and (img.Pixel[x,y] = $FFFFFF)  then
        begin
          img.Pixel[x,y] := $666666;
          arr.Insert(q, score + Sqr(x-stop.x)+Sqr(y-stop.y), score+1);
          map[y,x] := score+1; //<--- for simple backtrace
        end;
      end;
  end;

  // backtrace towards 0 distance from stop using the distance map
  score := map[stop.y,stop.x];
  Result += p; //pre-allocate result size for another boost SetLen(res, Ceil(score))
  while score > 1 do
  begin
    for q in adj do
    begin
      q.x += p.x;
      q.y += p.y;
      if img.InImage(q.x,q.y) 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;
    end;
    Result += p;
  end;
  Result += start;
end;


var
  start := Point(5,0); stop  := Point(635,480);
  //stop := Point(428,510); start := Point(83,2);

  img, im2: TImage;
  TPA,white,black,path1,path2: TPointArray;
  t:Double;
begin
  img := TImage.Create('images\maze.png');
  //Swap(start,stop);

  for 0 to 100 do
  begin
    img.Free();
    img := TImage.Create('images\maze.png');

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

  img.Fill($FFFFFF);
  img.DrawColor := 0;
  img.DrawTPA(black);

  img.DrawColor := $FF00FF;
  img.DrawTPA(path1.Grow(1));

  img.Show();
  img.Free();
end.