remstart *******************************************************************************

Copyright (C) 2006  Jess Telford - http://jt0.org

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

******************************************************************************* remend

remstart

/************************** TYPE Declarations *****************/
Put these somewhere at the start of your code.

`For the A*
Type openclosed
   traversable As Boolean
   parent As Integer
   parentCount As Integer
   open As Integer
   closed As Integer
   f As Integer
   g As Integer
   h As Integer
EndType

Type floodfill
   accessable As Boolean
   location As Integer
EndType

/************************* FUNCTIONS ***************************/

_AStar_init(x As Integer, y As Integer)
   x and y represent the Width and Height of map respectively
   If you had a zero-based array from 0 to 4 in both directions,
    the x = 5 and y = 5, etc.

_AStar_find_path(x1 As Integer, y1 As Integer, x2 As Integer, y2 As Integer)`Will return a pointer to memory containing the x/y combinations of the path in this format:
   [No Nodes Ex. Start Node Inc. End Node]
   For each Node:
     Integer [XPos in Map]
     Integer [YPos in Map]

_AStar_Uninit()
   undim the array's

_AStar_set_terrain(x As Integer, y As Integer, terrain As Boolean)
   Value of 1 for 'terrain' is 'impassable'

_AStar_get_terrain(x As Integer, y As Integer)
   returns 1 for impassable

_AStar_enable_diagonal_cutting(diag As Boolean)
   Enable units to cut corners instead of going around an object in a square fashion


remend


Function ________A*-Functions_________
   `Just a little bit of a nicety within the editor when it's listing the available functions
EndFunction


`x and y represent the Width and Height respectively
`If you had a zero-based array from 0 to 4 in both directions,
` the x = 5 and y = 5, etc.
Function _AStar_init(x As Integer, y As Integer)

   Local totalSize As Integer
   totalSize = x * y

   `The world map holding the passable terrain info
   Global Dim AStar_map(totalSize) As openclosed

   `Obvious
   Global AStar_width As Integer
   Global AStar_height As Integer

   `Used for the offset within the array to find the correct 'tile'
   Global AStar_off1 As Integer
   Global AStar_off2 As Integer

   `This is the cost for moving horizontal/vertical and diagonally
   Global AStar_g_perp As Integer
   Global AStar_g As Integer

   `Corner Cutting across obsticals is allowed?
   Global AStar_diag_Pass As Boolean

   `How many items are in the Open List?
   Global AStar_total_Open As Integer

   `How many items are in the Closed List?
   Global AStar_total_Closed As Integer

   `Penality for turns whilst pathing ( keeps 'em straighter, and looking nicer )
   Global AStar_penalty_turn As Integer

   AStar_width = x
   AStar_height = y

   AStar_off1 = 0 - (AStar_width + 1)
   AStar_off2 = AStar_width - 1

   AStar_g_perp = 10
   AStar_g = 14

   AStar_total_Open = 0
   AStar_total_Closed = 0

   AStar_penalty_turn = 0

   `These arrays have to start at 1,1 rather than 0,0
   Global Dim AStar_Open(totalSize) As Integer
   Global Dim AStar_Closed(totalSize) As Integer

   Global debug As Boolean
   debug = 0

EndFunction


Function _AStar_Uninit()

   `The world map holding the passable terrain info
   UnDim AStar_map(0)

   `These arrays have to start at 1,1 rather than 0,0
   UnDim AStar_Open(0)
   UnDim AStar_Closed(0)

EndFunction


`Value of 1 for 'terrain' is 'impassable'
`
Function _AStar_set_terrain(x As Integer, y As Integer, terrain As Boolean)

   If x >= 0 And x < AStar_width And y >= 0 And y < AStar_height
      AStar_map(y * AStar_width + x).traversable = terrain
   EndIf

EndFunction



Function _AStar_get_terrain(x As Integer, y As Integer)

   Local result As Integer
   result = -1

   If x >= 0 And x < AStar_width And y >= 0 And y < AStar_height
      result = AStar_map(y * AStar_width + x).traversable
   EndIf

EndFunction result



Function _AStar_enable_diagonal_cutting(diag As Boolean)

   AStar_diag_Pass = diag

EndFunction



`Will return a pointer to memory containing the x/y combinations of the path in this format:

`[No Nodes Ex. Start Node Inc. End Node]
`For each Node:
`  Integer [XPos in Map]
`  Integer [YPos in Map]
Function _AStar_find_path(x1 As Integer, y1 As Integer, x2 As Integer, y2 As Integer)
`x1/y1 & x2/y2 Must be 0-based array indecies

   `Memory pointer to the Area holding info on the Path
   Local result As DWord
   Local tmpMem As DWord

   `Have we reached x2/y2 yet?
   Local goal As Integer

   `The current parent node we're at
   Local location As Integer
   Local newLocation As Integer


   Local doIt As Boolean
   Local masterCut As Boolean

   Local tmpf As Integer
   Local tmpg As Integer
   Local tmph As Integer

   goal = y2 * AStar_width + x2
   location = y1 * AStar_width + x1

   `Add our initial parent to the Open List
   AStar_binary_new(location, 0)

   Local loopNum As Integer

   `Keep going till we get to the goal
   Repeat

      Inc loopNum

remstart ######################
If debug = 1
If (location > AStar_width * AStar_height - 1) Or (location < 0)
   Exit Prompt "Urm... Location (" + Str$(location) + ") is too big, wtf? On Loop Number " + Str$(loopNum), "Whoops!"
   End
EndIf

Set Cursor 0,0
Cls
Print "Current Location = " + Str$(location)
Print "Total Open = " + Str$(AStar_total_Open)
Sync : Sync
Wait Key
EndIf
remend `######################

      `Row above current node
      For i = AStar_off1 To AStar_off1 + 2

remstart ######################
If debug = 1
If (location + i > AStar_width * AStar_height - 1) Or (location + i < 0)
   Exit Prompt "Urm... Location + i (" + Str$(location + i) + ") is too big, wtf? On Loop Number " + Str$(loopNum) + ". sx = " + Str$(x1) + ", sy = " + Str$(y1) + ", ex = " + Str$(x2) + ", ey = " + Str$(y2), "Whoops!"
   End
EndIf
EndIf
remend `######################

         newLocation = location + i

         If AStar_map(newLocation).traversable = 0


            `Reset the value(s)
            doIt = 0
            masterCut = 0

            `Work out the F, G, H values
            If i - AStar_off1 = 1
               tmpg = AStar_map(location).g + AStar_g_perp
            Else
               `Figure out the weighting for this one
               tmpg = AStar_map(location).g + AStar_g

               `Can we use it, though? Does it cut a corner?
               If AStar_diag_Pass = 0
                  If i - AStar_off1 = 0
                     If AStar_map(location - 1).traversable = 1 Or AStar_map(location + AStar_off1 + 1).traversable = 1 Then masterCut = 1
                  Else
                     If AStar_map(location + 1).traversable = 1 Or AStar_map(location + AStar_off1 + 1).traversable = 1 Then masterCut = 1
                  EndIf
               EndIf

            EndIf

            `Don't allow it if it's corner cutting
            If masterCut = 0

               tmph = AStar_get_heuristic(((newLocation) MOD AStar_width) , ((newLocation) - ((newLocation) MOD AStar_width)) / AStar_width , x2 , y2 )

               tmpf = tmpg + tmph

               `Need a check for if it's on the closed list already
               If AStar_map(newLocation).closed > 0
                  `If it is on the closed list, we need to check if the new values
                  ` Are better than those previously in it. If so, take it off the
                  ` closed list and put it back onto the Open list with the new values
                  If AStar_map(newLocation).f > tmpf
                     `_AStar_closed_remove(newLocation)
                     `doIt = 1
                  EndIf
               Else
                  `Check if it's already on the Open List
                  If AStar_map(newLocation).open > 0
                     `If it is, then we compare the values as for the closed list
                     If AStar_map(newLocation).f > tmpf
                        `_AStar_closed_remove(newLocation)
                        doIt = 1
                     EndIf
                  Else
                     doIt = 1
                  EndIf
               EndIf



               If doIt = 1
                  AStar_map(newLocation).parent = location
                  AStar_map(newLocation).parentCount = AStar_map(location).parentCount + 1
                  AStar_map(newLocation).g = tmpg
                  AStar_map(newLocation).h = tmph
                  AStar_map(newLocation).f = tmpf
                  `Penalties for turns
                  If location - AStar_map(location).parent <> i Then AStar_map(newLocation).f = AStar_map(newLocation).f + AStar_penalty_turn
                  If AStar_map(newLocation).open = 0
                     AStar_binary_new(newLocation, 0)
                  Else
                     AStar_binary_new(newLocation, AStar_map(newLocation).open)
                  EndIf

                  If newLocation = goal Then location = goal : Exit

remstart ######################
If debug = 1
Print Str$(newLocation) + ", f = " + Str$(AStar_map(newLocation).f) + ", g = " + Str$(AStar_map(newLocation).g) + ", h = " + Str$(AStar_map(newLocation).h) + ", XPos = " + Str$(((newLocation) MOD AStar_width)) + ", YPos = " + Str$(((newLocation) - ((newLocation) MOD AStar_width)) / AStar_width) + ", Open = " + Str$(AStar_map(newLocation).open)
Sync : Sync
Wait Key
EndIf
remend `######################

               EndIf

            EndIf

         EndIf
      Next i

      `Did we find the goal this time?
      If location = goal Then Exit


      `Left and Right of current Node
      For i = -1 To 1 Step 2

remstart ######################
If debug = 1
If location + i > AStar_width * AStar_height - 1
   Exit Prompt "Urm... Location (" + Str$(location + i) + ") is too big, wtf? On Loop Number " + Str$(loopNum), "Whoops!"
   End
EndIf
EndIf
remend `######################

         newLocation = location + i

         If AStar_map(newLocation).traversable = 0


            `Reset the value
            doIt = 0

            `Work out the F, G, H values
            tmpg = AStar_map(location).g + AStar_g_perp
            tmph = AStar_get_heuristic(((newLocation) MOD AStar_width) , ((newLocation) - ((newLocation) MOD AStar_width)) / AStar_width , x2 , y2 )
            tmpf = tmpg + tmph

            `Need a check for if it's on the closed list already
            If AStar_map(newLocation).closed > 0
               `If it is on the closed list, we need to check if the new values
               ` Are better than those previously in it. If so, take it off the
               ` closed list and put it back onto the Open list with the new values
               If AStar_map(newLocation).f > tmpf
                  `_AStar_closed_remove(newLocation)
                  `doIt = 1
               EndIf
            Else
               `Check if it's already on the Open List
               If AStar_map(newLocation).open > 0
                  `If it is, then we compare the values as for the closed list
                  If AStar_map(newLocation).f > tmpf
                     `_AStar_closed_remove(newLocation)
                     doIt = 1
                  EndIf
               Else
                  doIt = 1
               EndIf
            EndIf



            If doIt = 1
               AStar_map(newLocation).parent = location
               AStar_map(newLocation).parentCount = AStar_map(location).parentCount + 1
               AStar_map(newLocation).g = tmpg
               AStar_map(newLocation).h = tmph
               AStar_map(newLocation).f = tmpf
               `Penalties for turns
               If location - AStar_map(location).parent <> i Then AStar_map(newLocation).f = AStar_map(newLocation).f + AStar_penalty_turn
               If AStar_map(newLocation).open = 0
                  AStar_binary_new(newLocation, 0)
               Else
                  AStar_binary_new(newLocation, AStar_map(newLocation).open)
               EndIf

remstart ######################
If debug = 1
Print Str$(newLocation) + ", f = " + Str$(AStar_map(newLocation).f) + ", g = " + Str$(AStar_map(newLocation).g) + ", h = " + Str$(AStar_map(newLocation).h) + ", XPos = " + Str$(((newLocation) MOD AStar_width)) + ", YPos = " + Str$(((newLocation) - ((newLocation) MOD AStar_width)) / AStar_width) + ", Open = " + Str$(AStar_map(newLocation).open)
Sync : Sync
Wait Key
EndIf
remend `######################

               If newLocation = goal Then location = goal : Exit
            EndIf

         EndIf
      Next i

      `Did we find the goal this time?
      If location = goal Then Exit

      `Row below current node
      For i = AStar_off2 To AStar_off2 + 2

remstart ######################
If debug = 1
If location + i > AStar_width * AStar_height - 1
   Exit Prompt "Urm... Location (" + Str$(location + i) + ") is too big, wtf? On Loop Number " + Str$(loopNum), "Whoops!"
   End
EndIf
EndIf
remend `######################

         newLocation = location + i

         If AStar_map(newLocation).traversable = 0

            `Reset the value
            doIt = 0
            masterCut = 0

            `Work out the F, G, H values
            If i - AStar_off2 = 1
               tmpg = AStar_map(location).g + AStar_g_perp
            Else
               tmpg = AStar_map(location).g + AStar_g

               `Can we use it, though? Does it cut a corner?
               If AStar_diag_Pass = 0
                  If i - AStar_off2 = 0
                     If AStar_map(location - 1).traversable = 1 Or AStar_map(location + AStar_off2 + 1).traversable = 1 Then masterCut = 1
                  Else
                     If AStar_map(location + 1).traversable = 1 Or AStar_map(location + AStar_off2 + 1).traversable = 1 Then masterCut = 1
                  EndIf
               EndIf

            EndIf

            If masterCut = 0

               tmph = AStar_get_heuristic(((newLocation) MOD AStar_width) , ((newLocation) - ((newLocation) MOD AStar_width)) / AStar_width , x2 , y2 )

               tmpf = tmpg + tmph

               `Need a check for if it's on the closed list already
               If AStar_map(newLocation).closed > 0
                  `If it is on the closed list, we need to check if the new values
                  ` Are better than those previously in it. If so, take it off the
                  ` closed list and put it back onto the Open list with the new values
                  If AStar_map(newLocation).f > tmpf
                     `_AStar_closed_remove(newLocation)
                     `doIt = 1
                  EndIf
               Else
                  `Check if it's already on the Open List
                  If AStar_map(newLocation).open > 0
                     `If it is, then we compare the values as for the closed list
                     If AStar_map(newLocation).f > tmpf
                        `_AStar_closed_remove(newLocation)
                        doIt = 1
                     EndIf
                  Else
                     doIt = 1
                  EndIf
               EndIf



               If doIt = 1
                  AStar_map(newLocation).parent = location
                  AStar_map(newLocation).parentCount = AStar_map(location).parentCount + 1
                  AStar_map(newLocation).g = tmpg
                  AStar_map(newLocation).h = tmph
                  AStar_map(newLocation).f = tmpf
                  `Penalties for turns
                  If location - AStar_map(location).parent <> i Then AStar_map(newLocation).f = AStar_map(newLocation).f + AStar_penalty_turn
                  If AStar_map(newLocation).open = 0
                     AStar_binary_new(newLocation, 0)
                  Else
                     AStar_binary_new(newLocation, AStar_map(newLocation).open)
                  EndIf

remstart ######################
If debug = 1
Print Str$(newLocation) + ", f = " + Str$(AStar_map(newLocation).f) + ", g = " + Str$(AStar_map(newLocation).g) + ", h = " + Str$(AStar_map(newLocation).h) + ", XPos = " + Str$(((newLocation) MOD AStar_width)) + ", YPos = " + Str$(((newLocation) - ((newLocation) MOD AStar_width)) / AStar_width) + ", Open = " + Str$(AStar_map(newLocation).open)
Sync : Sync
Wait Key
EndIf
remend `######################

                  If newLocation = goal Then location = goal : Exit

               EndIf

            EndIf

         EndIf
      Next i

      `Did we find the goal this time?
      If location = goal Then Exit


remstart ######################
If debug = 1
Ink 0,0
Box Screen Width () - 80, 0, Screen Width(), Screen Height()
Ink RGB(255,255,255),0
Text Screen Width() - 40, 0, "(" + Str$(AStar_total_Open) + ")"
For i = 1 To AStar_total_Open
   Text Screen Width() - 40, i * 20, Str$(AStar_Open(i)) + "/" + Str$(AStar_map(AStar_Open(i)).open)
Next i
Text Screen Width() - 85, 0, "(" + Str$(AStar_total_Closed) + ")"
For i = 1 To AStar_total_Closed
   Text Screen Width() - 80, i * 20, Str$(AStar_Closed(i)) + "/" + Str$(AStar_map(AStar_Closed(i)).closed)
Next i
Sync : Sync
Wait Key
EndIf
remend `######################

      `Put this parent onto the Closed Heap
      _AStar_closed_add(location)

      `Now, find our next Parent node by grabbing the lowest from the Open list
      location = AStar_binary_get()

remstart ######################
If debug = 1
Ink 0,0
Box Screen Width () - 80, 0, Screen Width(), Screen Height()
Ink RGB(255,255,255),0
Text Screen Width() - 40, 0, "(" + Str$(AStar_total_Open) + ")"
For i = 1 To AStar_total_Open
   Text Screen Width() - 40, i * 20, Str$(AStar_Open(i)) + "/" + Str$(AStar_map(AStar_Open(i)).open)
Next i
Text Screen Width() - 85, 0, "(" + Str$(AStar_total_Closed) + ")"
For i = 1 To AStar_total_Closed
   Text Screen Width() - 80, i * 20, Str$(AStar_Closed(i)) + "/" + Str$(AStar_map(AStar_Closed(i)).closed)
Next i
Sync : Sync
Wait Key
EndIf
remend `######################

   Until (location = goal) Or (AStar_total_Open = 0)


   `Ok, so we've found the final child, and as such we just track back till we get to our parent
   `[No Nodes Ex. Start Node Inc. End Node]
   `For each Node:
   `  Integer [XPos in Map]
   `  Integer [YPos in Map]


   result = Make Memory(4 + (8 * AStar_map(location).parentCount))

remstart ######################
If debug = 1
Cls
Text 0,0,"Total Tiles Passed: " + Str$(AStar_map(location).parentCount)

Sync : Sync
Wait Key
EndIf
remend `######################

   tmpMem = result

   `The header
   (*tmpMem) = AStar_map(location).parentCount : Inc tmpMem, 4

   If AStar_map(location).parentCount > 0
      `The X/Y Info
remstart
      Inc tmpMem, (8 * AStar_map(location).parentCount)

      Repeat
         Dec tmpMem, 8
         (*tmpMem) = (location MOD AStar_width) : Inc tmpMem, 4
         (*tmpMem) = (location - (location MOD AStar_width)) / AStar_width : Dec tmpMem, 4
         location = AStar_map(location).parent
      `Keep going till the next parent is the start square, we don't need that one
      Until location = y1 * AStar_width + x1
remend
      newLocation = location
      Repeat
         (*tmpMem) = (newLocation MOD AStar_width) : Inc tmpMem, 4
         (*tmpMem) = (newLocation - (newLocation MOD AStar_width)) / AStar_width : Inc tmpMem, 4
         newLocation = AStar_map(newLocation).parent
      `Keep going till the next parent is the start square, we don't need that one
      Until newLocation = y1 * AStar_width + x1

   EndIf


   `Clean up so there's no conflict's next time we try to run it
   For i = 1 To AStar_total_Open
      AStar_map(AStar_Open(i)).f = 0
      AStar_map(AStar_Open(i)).g = 0
      AStar_map(AStar_Open(i)).h = 0
      AStar_map(AStar_Open(i)).parentCount = 0
      AStar_map(AStar_Open(i)).open = 0
      AStar_Open(i) = 0
   Next i

   For i = 1 To AStar_total_Closed
      AStar_map(AStar_Closed(i)).f = 0
      AStar_map(AStar_Closed(i)).g = 0
      AStar_map(AStar_Closed(i)).h = 0
      AStar_map(AStar_Closed(i)).parentCount = 0
      AStar_map(AStar_Closed(i)).closed = 0
      AStar_Closed(i) = 0
   Next i

   AStar_total_Closed = 0
   AStar_total_Open = 0

EndFunction result



Function _AStar_closed_remove(index As Integer)

   AStar_map(AStar_Closed(index)).closed = 0
   AStar_map(AStar_Closed(index)).open = 0
   AStar_Closed(index) = 0

   If AStar_total_Closed < 0
      If File Exist("Error.txt") Then Delete File "Error.txt"
      Open To Write 1, "Error.txt"
      Write String 1, "It's below BEFORE (" + Str$(AStar_total_Closed) + ") decrementing and doing its stuff"
      Close File 1
      End
   EndIf

   Dec AStar_total_Closed, 1

   If index <= AStar_total_Closed
      For i = index To AStar_total_Closed
         AStar_Closed(i) = AStar_Closed(i + 1)
         AStar_map(AStar_Closed(i)).closed = i
      Next i
   EndIf

   If AStar_total_Closed < 0
      If File Exist("Error.txt") Then Delete File "Error.txt"
      Open To Write 1, "Error.txt"
      Write String 1, "It's below AFTER (" + Str$(AStar_total_Closed) + ") decrementing and doing its stuff"
      Close File 1
      End
   EndIf

EndFunction



Function _AStar_closed_add(value As Integer)

   Inc AStar_total_Closed

remstart ######################
If debug = 1
If (AStar_total_Closed > Array Count(AStar_Closed(0))) Or (AStar_total_Closed < 0) Then Exit Prompt "Total Closed = " + Str$(AStar_total_Closed) + ", Array size = " + Str$(Array Count(AStar_Closed(0))), "Not Good!" : End
EndIf
remend `######################

   AStar_Closed(AStar_total_Closed) = value
   AStar_map(value).closed = AStar_total_Closed
   AStar_map(value).open = 0

EndFunction


`reorderIndex should be zero if it's a new item,
` if it's an already existing item, it should be its index
Function AStar_binary_new(value As Integer, reorderIndex As Integer)

   Local count As Integer
   Local temp As Integer

   `Keep track of where in the list it has bubbled to so far
   If reorderIndex > 0
      count = reorderIndex
      `AStar_Open(count) = value
   Else
      `Reflect the new size
      Inc AStar_total_Open

remstart ######################
If debug = 1
If AStar_total_Open <= 0 Or AStar_total_Open > AStar_width * AStar_height
   If File Exist("Error.txt") Then Delete File "Error.txt"
   Open To Write 1, "Error.txt"
   For i = 0 To AStar_total_Closed
      Write String 1, Str$(AStar_Closed(i))
   Next i
   Close File 1
   Exit Prompt "Urm... AStar_total_Open (" + Str$(AStar_total_Open) + ") is too low, wtf? Info saved to Error.txt", "Whoops!"
   End
EndIf
EndIf
remend `######################


      `Add the new value at the 'end' of the list
      AStar_Open(AStar_total_Open) = value

      count = AStar_total_Open
   EndIf

   `Repeat until it reaches the top of the list
   ` ( Or EXITs as in the 'ELSE' )
   While count >= 4

      `If This one is less than its parent, then swap them
      If AStar_map(AStar_Open(count)).f <= AStar_map(AStar_Open(Int(count / 2))).f

remstart ######################
If AStar_Open(Int(count / 2)) = 0 Or AStar_Open(count) = 0 Or count < 4
   Sync : Sync
   Wait Key
   Exit Prompt "U-oh!, Value = " + Str$(value) + " No. " + Str$(Int(count / 2)) + " = " + Str$(AStar_Open(Int(count / 2))) + " No. " + Str$(count) + " = " + Str$(AStar_Open(count)) + " reorderIndex = " + Str$(reorderIndex), ":("
   End
EndIf
remend `######################

         `Do the swapping
         temp = AStar_Open(Int(count / 2))
         AStar_Open(Int(count / 2)) = AStar_Open(count)
         AStar_Open(count) = temp

         AStar_map(AStar_Open(count)).open = count
         AStar_map(AStar_Open(Int(count / 2))).open = Int(count / 2)

         `Now we're at the 'parent's location
         count = Int(count / 2)
      Else
         `Nope, not lower, it's as high as it can go, so exit the loop
         Exit
      EndIf

   EndWhile

   `set the tracking variable
   AStar_map(AStar_Open(count)).open = count


EndFunction




Function AStar_binary_get()


remstart ######################
If debug = 1
If AStar_total_Open <= 0 Or AStar_total_Open > AStar_width * AStar_height
   If File Exist("Error.txt") Then Delete File "Error.txt"
   Open To Write 1, "Error.txt"
   For i = 0 To AStar_total_Closed
      Write String 1, Str$(AStar_Closed(i))
   Next i
   Close File 1
   Exit Prompt "Urm... AStar_total_Open (" + Str$(AStar_total_Open) + ") is too low, wtf? Info saved to Error.txt", "Whoops!"
   End
EndIf
EndIf
remend `######################


   Local result As Integer

   Local temp As Integer

   Local v As Integer
   Local u As Integer

   `Reset the top-most's Open track to 0 as it is about to be put on the Closed List
   AStar_map(AStar_Open(1)).open = 0


   `Now, start the removal process so that our heap isn't totally screwed up
   `Move the last-most to the front
   AStar_Open(1) = AStar_Open(AStar_total_Open)
   AStar_map(AStar_Open(1)).open = 1

   AStar_Open(AStar_total_Open) = 0

   `Reflect the removal of one item
   Dec AStar_total_Open

   `Initialize the starting place
   v = 1

   `Repeat until we want to exit
   Do
      u = v

      `If both the children exist ( ie, it CAN go further down the list )
      If (2*u) + 1 <= AStar_total_Open

         `Select the lowest of the two children ( Notice the use of u and v )
         If AStar_map(AStar_Open(u)).f >= AStar_map(AStar_Open(2*u)).f Then v = 2*u
         If AStar_map(AStar_Open(v)).f >= AStar_map(AStar_Open((2*u) + 1)).f Then v = (2*u) + 1

      Else
         If (2*u) <= AStar_total_Open
            `Check if the cost is greater than the child
            If AStar_map(AStar_Open(u)).f >= AStar_map(AStar_Open(2*u)).f Then v = 2*u
         EndIf
      EndIf

      `Now, if we've found a child smaller than the parent, swap them
      If u <> v
         temp = AStar_Open(u)
         AStar_Open(u) = AStar_Open(v)
         AStar_Open(v) = temp
         `And swap the tracking variables also
         AStar_map(AStar_Open(u)).open = u
         AStar_map(AStar_Open(v)).open = v
      Else
         `Whoops, the parent is in the right spot, let's exit!
         Exit
      EndIf
   Loop

   `Get the top-most from the heap
   result = AStar_Open(1)

EndFunction result



Function AStar_get_heuristic(x1 As Integer, y1 As Integer, x2 As Integer, y2 As Integer)

   result As Integer
   dx As Integer
   dy As Integer
   min As Integer

   dx = Abs(x2 - x1)
   dy = Abs(y2 - y1)

   If dx < dy Then min = dx Else min = dy

   `diagdistance*min(|dx|,|dy|) + orthodistance*||dx|-|dy||

   result = (AStar_g * min) + (AStar_g_perp * Abs(dx - dy))

`   result = (dx + dy) * AStar_g_perp

EndFunction result