Graphics 800,800,0,2
SetBuffer BackBuffer()

;Map
Global A_X, A_Y, B_X, B_Y, Map_W=24, Map_H=24
Dim Map(Map_W, Map_H)

Type Map_OpenList
   Field X,Y
End Type

Type Map_ClosedList
   Field X,Y
End Type

;Map2
FreeMap()
SetMap_A(1, 1)
SetMap_B(23, 23)
SetMap_Line(0,0,24,0)
SetMap_Line(0,24,24,24)
SetMap_Line(0,0,0,24)
SetMap_Line(24,0,24,24)

For Dots = 1 To 50      ;Place 10 Random Dots
   SetMap_Dot(Rand(2,22),Rand(2,22))
Next

;no 100%
mfps = CreateTimer(30)

;Main Code
.main
ClsColor 51,153,51
While Not KeyHit(1)
   Cls
   msec=MilliSecs()
   DrawMap()
   msec=MilliSecs()-msec
   
   If KeyHit(57)
      msec2 = MilliSecs()
      cost=AStar()
      msec2 = MilliSecs() - msec2
   EndIf
   Color 255,255,255
   Text 0,0,"Draw MSec: " + msec
   Text 0,15,"A*   MSec: " + msec2
   Text 0,30,"A*   Cost: " + Cost
   
   Flip 0
   WaitTimer mfps
Wend
End

Function Line(X, Y, X2, Y2, R=255, G=255, B=255)
   LockBuffer
   W2 = GraphicsWidth()
   H2 = GraphicsHeight()
   X3 = X2 - X
   Y3 = Y2 - Y
   If X3 > 0 XA = 1
   If X3 < 0 XA = -1
   If Y3 > 0 YA = 1
   If Y3 < 0 YA = -1
   RGB = 255 * $1000000 + R * $10000 + G * $100 + B
   
   If Abs(Y3) < Abs(X3)
      AddY = -Abs(X3)
      Delta = 2 * Abs(Y3)
      CouY = 2 * AddY
      While X+X5 <> X2
         X5 = X5 + XA
         AddY = AddY + Delta
         If AddY > 0 Y5 = Y5 + YA : AddY = AddY + CouY
         ;If X+X5 < W2 And X+X5 >= 0 And Y+Y5 < H2 And Y+Y5 >= 0
         ;   WritePixelFast X+X5, Y+Y5, RGB
         ;EndIf
         WritePixel X+X5, Y+Y5, RGB
      Wend
   Else
      AddX = -Abs(Y3)
      Delta = 2 * Abs(X3)
      CouX = 2 * AddX
      While Y+Y5 <> Y2
         Y5 = Y5 + YA
         AddX = AddX + Delta
         If AddX > 0 X5 = X5 + XA : AddX = AddX + CouX
         ;If X+X5 < W2 And X+X5 >= 0 And Y+Y5 < H2 And Y+Y5 >= 0
         ;   WritePixelFast X+X5, Y+Y5, RGB
         ;EndIf
         WritePixel X+X5, Y+Y5, RGB
      Wend
   EndIf
   UnlockBuffer
End Function

;Functions
Function FreeMap()
   For X = 0 To Map_W:For Y = 0 To Map_H
      Map(X,Y) = 0
   Next:Next
End Function

Function DrawMap(Way=1)
   Color 51, 102, 51
   For X = 0 To Map_W:For Y = 0 To Map_H         ;Draw Free fields
      If Map(X,Y) = 0
         Rect X*32+1,Y*32+1,30,30
      EndIf
   Next:Next
   Color 51, 51, 102
   For X = 0 To Map_W:For Y = 0 To Map_H         ;Draw Blocked fields
      If Map(X,Y) = 1
         Rect X*32+1,Y*32+1,30,30
      EndIf
   Next:Next
   Color 255,0,0:Rect A_X*32+4,A_Y*32+4,24,24
   Color 0, 0, 255:Rect B_X*32+4,B_Y*32+4,24,24
   If Way=1
      DrawWay()
   EndIf
End Function

Function DrawWay()
   For MCL.Map_ClosedList = Each Map_ClosedList
      If Firs = 1
         Color 255,255,255
         Line LX*32+16,LY*32+16,MCL\X*32+16,MCL\Y*32+16
         LX = MCL\X:LY = MCL\Y
      Else
         LX = MCL\X:LY = MCL\Y
      EndIf
      Firs = 1
   Next
End Function

Function AStar()
   Delete Each Map_OpenList
   Delete Each Map_ClosedList
   Local LastCost = 2147483647, LastFieldCost = 0, FieldCost, CurCost, EstCost, FullCost, ReachedB, CostHV = 10, CostD = 14, Retry = 2
   MOL.Map_OpenList = New Map_OpenList
   MOL\X = A_X
   MOL\Y = A_Y
   
   EstCost = Abs(B_X-A_X + B_Y-A_Y)
   
   
   For MOL.Map_OpenList = Each Map_OpenList
      X = MOL\X:Y = MOL\Y
      MCL.Map_ClosedList = New Map_ClosedList
      MCL\X = X:MCL\Y = Y
      Delete MOL.Map_OpenList
      
      If X = B_X And Y = B_Y
         Return FullCost
      EndIf
      
      For X2 = 1 To -1 Step -1:For Y2 = 1 To -1 Step -1
         If X+X2 >= 0 And X+X2 <= Map_W And Y+Y2 >= 0 And Y+Y2 <= Map_H
            If Collide(X+X2,Y+Y2) = 0
               EstCost = Dist(X+X2,Y+Y2,B_X,B_Y)*10
               If X+X2 <> X And Y+Y2 <> Y
                  CurCost = LastFieldCost + CostD + EstCost
                  FieldCost = CostD
               Else
                  CurCost = LastFieldCost + CostHV + EstCost
                  FieldCost = CostHV
               EndIf
               If CurCost <= LastCost
                  LastCost = CurCost
                  LastX = X+X2
                  LastY = Y+Y2
               EndIf
            EndIf
         EndIf
      Next:Next
      If LastCost <> 2147483647
         MOL2.Map_OpenList = New Map_OpenList
         MOL2\X = LastX
         MOL2\Y = LastY
         LastFieldCost = LastFieldCost + FieldCost
      EndIf
      LastCost = 2147483647
      FullCost = LastFieldCost
   Next
End Function

Function Collide(X,Y)
   If Map(X,Y) = 0 And IsInMCL(X,Y) = 0
      Return 0
   EndIf
   Return 1
End Function

Function IsInMCL(X,Y)
   For MCL.Map_ClosedList = Each Map_ClosedList
      If MCL\X = X And MCL\Y = Y
         Return 1
      EndIf
   Next
   For MOL.Map_OpenList = Each Map_OpenList
      If MOL\X = X And MOL\Y = Y
         Return 1
      EndIf
   Next
   Return 0
End Function

Function Dist(X1,Y1,X2,Y2)
   Return Abs(X1-X2)+Abs(Y1-Y2)
End Function

Function SetMap_A(X,Y)
   Map(X,Y) = 0:A_X = X:A_Y = Y
End Function

Function SetMap_B(X,Y)
   Map(X,Y) = 0:B_X = X:B_Y = Y
End Function

Function SetMap_Dot(X,Y)
   Map(X,Y) = 1
End Function

Function SetMap_Line(X,Y,X2,Y2)
   SetMap_Dot(X,Y)
   X3 = X2 - X
   Y3 = Y2 - Y
   If X3 > 0 XA = 1
   If X3 < 0 XA = -1
   If Y3 > 0 YA = 1
   If Y3 < 0 YA = -1
   
   If Abs(Y3) < Abs(X3)
      AddY = -Abs(X3)
      Delta = 2 * Abs(Y3)
      CouY = 2 * AddY
      While X+X5 <> X2
         X5 = X5 + XA
         AddY = AddY + Delta
         If AddY > 0 Y5 = Y5 + YA : AddY = AddY + CouY
         Map(X5+X,Y5+Y) = 1
      Wend
   Else
      AddX = -Abs(Y3)
      Delta = 2 * Abs(X3)
      CouX = 2 * AddX
      While Y+Y5 <> Y2
         Y5 = Y5 + YA
         AddX = AddX + Delta
         If AddX > 0 X5 = X5 + XA : AddX = AddX + CouX
         Map(X5+X,Y5+Y) = 1
      Wend
   EndIf
End Function