PathFinder

program PathFinder;

{$APPTYPE CONSOLE}

uses
  SysUtils,
  Windows,
  Graphics;

type
  TNode = record
    E, L: integer;
    X, Y: integer;
    S, D: PColor;
  end;

  THeap = array of TNode;

const
  MinW = 200;
  dx: array [0 .. 7] of integer = (-1, 1, 0, 0, -1, -1, 1, 1);
  dy: array [0 .. 7] of integer = (0, 0, -1, 1, -1, 1, 1, -1);
  dL: array [0 .. 7] of integer = (65536, 65536, 65536, 65536, 92682, 92682, 92682, 92682);
  DirCounts: array [0 .. 1] of integer = (4, 8);

var
  x1,y1,x2,y2: integer;
  W,H: integer;
  i: integer;
  DirC: integer;
  ColorShift: array [0 .. 7] of integer;       // для указателеёбства с пикселами битмапов
  S: string;
  bmP, bmL, bmW, bmA: TBitmap;
  T: cardinal;
  LW: integer;

  procedure Push(var H: THeap; const N: TNode);
  var
    i, ai: integer;
    tmp: TNode;
  begin
    i := Length(H);
    SetLength(H, i+1);
    H[i] := N;

    if i>0 then begin
      tmp := H[i];
      repeat
        ai := (i-1) div 2;
        if H[ai].E < tmp.E then break;
        H[i] := H[ai];
        i := ai;
      until i=0;
      H[i] := tmp;
    end;
  end;

  procedure Pop(var H: THeap; var N: TNode);
  var
    E,i,ai,j1,j2: integer;
    tmp: TNode;
  begin

    E := Length(H);
    N := H[0];
    H[0] := H[E-1];
    SetLength(H, E-1);

    i := 0;
    ai :=0;
    dec(E);

    if E>0 then begin
      tmp := H[i];
      repeat
        j1 := i*2+1;
        j2 := j1+1;

        if j1>=E then break
        else if j2>=E then ai := j1
        else if H[j1].E < H[j2].E then ai := j1
        else ai := j2;

        if tmp.E > H[ai].E then H[i] := H[ai]
        else break;
        i := ai;
      until false;
      H[i] := tmp;
    end;
  end;

  procedure SetShifts;
  var
    i: integer;
  begin
    for i := Low(ColorShift) to High(ColorShift) do ColorShift[i] := (dx[i] + dy[i]*W) * SizeOf(TColor);
  end;

  procedure SetWeights(S: TBitmap; var D: TBitmap);
  var
    i,j,k: integer;
    SC, DC: PColor;
  begin
    if D = nil then D := TBitmap.Create;
    D.PixelFormat := pf32bit;
    D.Width := W;
    D.Height := H;
    SC := S.ScanLine[S.Height-1];
    DC := D.ScanLine[D.Height-1];
    for j := 0 to H-1 do for i := 0 to W-1 do begin
      if SC^ = $FF then begin
        x1 := i;
        y1 := j;
        SC^ := $FFFFFF;
      end else if SC^ = $FF00 then begin
        x2 := i;
        y2 := j;
        SC^ := $FFFFFF;
      end;

      k := (SC^ shr 16 and $FF) + (SC^ shr 8 and $FF) + (SC^ and $FF);
      // k : 0 .. 255*3
      if k=0 then DC^ := MaxInt
      else DC^ := (255*3) - k + MinW;
      // max -> 10; min -> 255*3
      Inc(SC);
      Inc(DC);
    end;
  end;

  function FastDist(x1,y1,x2,y2: integer): integer;
  var
    dx, dy: integer;
  begin
    if DirC = 4 then result := (abs(x1-x2) + abs(y1-y2)) * MinW
    else begin
      dx := abs(x1-x2);
      dy := abs(y1-y2);
      if dx>dy then result := dx+dy*27146 div 65536 else result := dy+dx*27146 div 65536;
      result := result * MinW;
    end;
  end;

  procedure Fill(S: TBitmap; var D: TBitmap; x1,y1,x2,y2: integer);
  var
    i: integer;
    SC, DC: PColor;
    tmpD: PColor;
    N: TNode;
    T: THeap;
    newl,newe: integer;
    ax,ay: integer;

    procedure PushP(x,y,L,E: integer;S,D: PColor);
    var
      N: TNode;
    begin
      N.X := x;
      N.Y := y;
      N.L := L;
      N.E := E;
      N.S := S;
      N.D := D;
      Push(T, N);
      D^ := L;   // dlinna
    end;

  begin
    if D = nil then D := TBitmap.Create;
    D.PixelFormat := pf32bit;
    D.Width := W;
    D.Height := H;
    SC := S.ScanLine[S.Height-1];
    DC := D.ScanLine[D.Height-1];
    tmpD := DC;
    for i := 0 to S.Width*S.Height-1 do begin
      tmpD^ := MaxInt;
      Inc(tmpD);
    end;
    Inc(SC, x1+y1*W);
    Inc(DC, x1+y1*W);

    PushP(x1,y1,0,FastDist(x1,y1,x2,y2),SC,DC); // evreistika

    while Length(T) > 0 do begin
      Pop(T, N);
      for i := 0 to DirC-1 do begin
        ax := N.X + dx[i];
        ay := N.Y + dy[i];
        if ((ax>=0) and (ax<W)) and ((ay>=0) and (ay<H)) then begin
          PChar(SC) := PChar(N.S) + ColorShift[i];
          PChar(DC) := PChar(N.D) + ColorShift[i];
          if SC^ < MaxInt then begin
            newl := N.L + SC^ * DL[i] div 65536;
            newe := newl + FastDist(ax,ay,x2,y2);      // evreistika
            if newl < DC^ then PushP(ax,ay,newl,newe,SC,DC);
            if (ax=x2) and (ay=y2) then exit;
          end;
        end;
      end;
    end;
  end;

  procedure DrawPath(P, S: TBitmap; var D: TBitmap; x1,y1,x2,y2: integer; var LW: integer);
  var
    SC, DC: PColor;
    SN, DN: array [Low(ColorShift) .. High(ColorShift)] of PColor;
    m: integer;
    i, dir: integer;
    ax, ay: integer;
  begin
    if D = nil then D := TBitmap.Create;
    D.PixelFormat := pf32bit;
    D.Width := W;
    D.Height := H;
    SC := P.ScanLine[P.Height-1];
    DC := D.ScanLine[D.Height-1];
    for i := 0 to W*H-1 do begin
      DC^ := SC^;
      Inc(DC);
      Inc(SC);
    end;

    SC := S.ScanLine[S.Height-1];
    DC := D.ScanLine[D.Height-1];
    Inc(SC, x2+y2*W);
    Inc(DC, x2+y2*W);

    LW := 0;

    while(x2<>x1) or (y2<>y1) do begin
      DC^ := $FF0000;

      m := MaxInt;
      dir := High(ColorShift)+1;

      for i := 0 to DirC-1 do begin
        ax := x2 + dx[i];
        ay := y2 + dy[i];
        if ((ax>=0) and (ax<W)) and ((ay>=0) and (ay<H)) then begin
          PChar(SN[i]) := PChar(SC) + ColorShift[i];
          PChar(DN[i]) := PChar(DC) + ColorShift[i];
          if SN[i]^ < m then begin
            m := SN[i]^;
            dir := i;
          end;
        end;
      end;

      if dir > High(ColorShift) then break;
      if LW = 0 then LW := SC^;
      SC^ := MaxInt;

      Inc(x2, dx[dir]);
      Inc(y2, dy[dir]);
      SC := SN[dir];
      DC := DN[dir];
    end;
    DC^ := $FF0000;
  end;

  function CutDim(S: string): string;
  var
    i: integer;
  begin
    i := Length(S);
    while (i>0) and (S[i] <> '.') do Dec(i);
    if i=0 then i := Length(S)+1;
    Result := Copy(S, 1, i-1);
  end;

begin
  if ParamCount = 0 then begin
    Write('Enter file name: ');
    ReadLn(S);
  end else S := ParamStr(1);
  WriteLn('Loading...');
  bmP := TBitmap.Create;
  bmP.LoadFromFile(S);
  bmP.PixelFormat := pf32bit;

  W := bmP.Width;
  H := bmP.Height;

  x1 := 0;
  y1 := 0;
  x2 := W-1;
  y2 := H-1;


  SetShifts;
  WriteLn('weights...');
  SetWeights(bmP, bmL);

  for i := 0 to 1 do begin
    DirC := DirCounts[i];
    T := GetTickCount;                    // timer begin
    WriteLn('finding path, ', DirC, ' directions...');
    Fill(bmL, bmW, x1, y1, x2, y2);
    WriteLn('drawing path...');
    DrawPath(bmP, bmW, bmA, x1, y1, x2, y2, LW);
    T := GetTickCount - T;                // timer end
    WriteLn('saving file...');
    bmW.SaveToFile(CutDim(S) + '_wave_' + IntToStr(DirC) + '.bmp');
    bmA.SaveToFile(CutDim(S) + '_answer_' + IntToStr(DirC) + '.bmp');
    WriteLn('complete. Total (finding+drawing) time = ', T, ' ms, length = ', LW/MinW:0:5);
  end;
  WriteLn('Press ENTER to exit.');
  ReadLn;
end.