;A* Pathfinder by Patrick Lester. Used by permission.
;==================================================================
;Last updated 4/7/04

;This version of the aStar library has been modified to handle 
;pathfinding around other units.

;An article describing A* and this code in particular can be found at:
;http://www.policyalmanac.org/games/aStarTutorial.htm

;If you want to use this AStar Library, you may do so free of charge so 
;long as the author byline (above) is retained. Thank you to CaseyC 
;at the Blitz Forums for suggesting the use of binary heaps for the open 
;list. Email comments and questions to Patrick Lester at 
;pwlester@policyalmanac.org.

;==========================================================
;DECLARE VARIABLES
.declareVariables

	;Adjust these variables to match your map dimensions
	Global tileSize = 25, mapWidth = 32, mapHeight = 24

	;Create needed arrays
	Dim walkability(mapWidth+1,mapHeight+1) ;array that holds wall/obstacle information	
	Dim openList(mapWidth*mapHeight+2) ;1 dimensional array holding ID# of open list items
	Dim whichList(mapWidth+1,mapHeight+1)  ;2 dimensional array used to record 
		;whether a cell is on the open list or on the closed list.
	Dim openX(mapWidth*mapHeight+2) ;1d array stores the x location of an item on the open list
	Dim openY(mapWidth*mapHeight+2) ;1d array stores the y location of an item on the open list
	Dim parentX(mapWidth+1,mapHeight+1) ;2d array to store parent of each cell (x)
	Dim parentY(mapWidth+1,mapHeight+1) ;2d array to store parent of each cell (y)
	Dim Fcost(mapWidth*mapHeight+2)	;1d array to store F cost of a cell on the open list
	Dim Gcost(mapWidth+1,mapHeight+1) 	;2d array to store G cost for each cell.
	Dim Hcost(mapWidth*mapHeight+2)	;1d array to store H cost of a cell on the open list		
	
	Dim tempUnwalkability(mapWidth+1,mapHeight+1) ;array that holds info about adjacent units
	Dim nearByPath(mapWidth+1,mapHeight+1) 		
	Dim claimedNode.unit(mapWidth+1,mapHeight+1); array that stores claimed nodes	
	Dim gGroupUnit.unit(1) ;used to sort a selected group of units		
	Dim island(mapWidth+1,mapHeight+1) 
		
	;Declare constants
	Global onClosedList = 10 ;openList variable	
	Const notfinished = 0, notStarted = 0, found = 1 ; pathStatus constants 
	Const nonexistent = 2, tempStopped = 3; pathStatus constants 
	Const walkable = 0, unwalkable = 1 ; walkability array constants	
	Global gDiagonalBlockage, gPathCost
	
	;FindPath() mode constants
	Const normal = 0, randomMove = 1

;==========================================================
;FIND PATH: This function finds the path and saves it. Non-Blitz users please note,
;the first parameter is a pointer to a user-defined object called a unit, which contains all
;relevant info about the unit in question (its current location, speed, etc.). As an
;object-oriented data structure, types are similar to structs in C.
;	Please note that targetX and targetY are pixel-based coordinates relative to the
;upper left corner of the map, which is 0,0.
Function FindPath(unit.unit,targetX,targetY,mode=normal)

	;Random move mode is used when a unit's main target is
	;currently unreachable. After the random move, the unit
	;will try to pathfind to its original target again. Random
	;moves can break up units that happen to be approaching
	;each other from opposite directions down tight corridors.
	;See the UpdatePath() function.
	If mode = randomMove Then useDijkstras = True	

;1.	Convert location data (in pixels) to coordinates in the walkability array.
	startX = Floor(unit\xLoc/tileSize) : startY = Floor(unit\yLoc/tileSize)	
	targetX = Floor(targetX/tileSize) : targetY = Floor(targetY/tileSize)

;2.	Check for redirects	
	result = CheckRedirect(unit,targetX,targetY)	
	If result = failed Then Goto noPath ;target is unwalkable and could not find a redirect.
	If result = succeeded Then targetX = gInt1 : targetY = gInt2		

	;If starting location and target are in the same location...
	If startX = targetX And startY = targetY 
		unit\pathLength = 0 : unit\pathLocation = 0	
		PokeShort unit\pathBank,0,startX ;store starting x value	
		PokeShort unit\pathBank,2,startY ;store starting y value		
		Return found
	End If

;3.	a. Reset some variables that need to be cleared
	If onClosedList > 1000000 ;occasionally redim whichList
		Dim whichList(mapWidth,mapHeight) : onClosedList = 10
	End If	
	onClosedList = onClosedList+5 ;changing the values of onOpenList and onClosed list is faster than redimming whichList() array
	onOpenList = onClosedList-1
	tempUnwalkable = onClosedList-2
	penalized = onClosedList-3 
	unit\pathLength = notstarted ;i.e, = 0
	unit\pathLocation = notstarted ;i.e, = 0
	Gcost(startX,startY) = 0 ;reset starting square's G value to 0

	;b. Create a footprint for any nearby unit that the pathfinding unit 
	;may be about to collide with. Such nodes are designated as tempUnwalkable.	
	CreateFootPrints(unit.unit)

;4.	Add the starting location to the open list of squares to be checked.
	numberOfOpenListItems = 1
	openList(1) = 1 ;assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
	openX(1) = startX : openY(1) = startY

;5.	Do the following until a path is found or deemed nonexistent.
	Repeat
	
;6.	If the open list is not empty, take the first cell off of the list.
	;This is the lowest F cost cell on the open list.
	If numberOfOpenListItems <> 0 Then

	;Pop the first item off the open list.
	parentXval = openX(openList(1)) : parentYVal = openY(openList(1)) ;record cell coordinates of the item
	whichList(parentXval,parentYVal) = onClosedList ;add the item to the closed list

	;Open List = Binary Heap: Delete this item from the open list, which
	;is maintained as a binary heap. For more information on binary heaps, see:
	;http://www.policyalmanac.org/games/binaryHeaps.htm
	numberOfOpenListItems = numberOfOpenListItems - 1 ;reduce number of open list items by 1	
	openList(1) = openList(numberOfOpenListItems+1) ;move the last item in the heap up to slot #1
	v = 1	
	Repeat ;Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
		u = v	
		If 2*u+1 <= numberOfOpenListItems ;if both children exist
		 	;Check if the F cost of the parent is greater than each child.
			;Select the lowest of the two children.	
			If Fcost(openList(u)) >= Fcost(openList(2*u)) Then v = 2*u
			If Fcost(openList(v)) >= Fcost(openList(2*u+1)) Then v = 2*u+1		
		Else
			If 2*u <= numberOfOpenListItems ;if only child #1 exists
			 	;Check if the F cost of the parent is greater than child #1	
				If Fcost(openList(u)) >= Fcost(openList(2*u)) Then v = 2*u
			End If	
		End If
		If u<>v ;if parent's F is > one of its children, swap them
			temp = openList(u)
			openList(u) = openList(v)
			openList(v) = temp				
		Else
			Exit ;otherwise, exit loop
		End If	
	Forever

	
;7.	Check the adjacent squares. (Its "children" -- these path children
	;are similar, conceptually, to the binary heap children mentioned
	;above, but don't confuse them. They are different. Path children
	;are portrayed in Demo 1 with grey pointers pointing toward
	;their parents.) Add these adjacent child squares to the open list
	;for later consideration if appropriate (see various if statements
	;below).
	For b = parentYVal-1 To parentYVal+1
	For a = parentXval-1 To parentXval+1

	;If not off the map (do this first to avoid array out-of-bounds errors)
	If a <> -1 And b <> -1 And a <> mapWidth And b <> mapHeight

	;If not already on the closed list (items on the closed list have
	;already been considered and can now be ignored).			
	If whichList(a,b) <> onClosedList 
	
	;If not a wall/obstacle square
	If walkability(a,b) <> unwalkable
	
	;If not an adjacent node that is temporarily unwalkable
	;as defined by CreateFootprints()
	If tempUnwalkability(a,b) <> tempUnwalkable
	
	;If not occupied by a stopped unit
	node = unwalkable
	If claimedNode(a,b) = Null
		node = walkable
	Else If claimedNode(a,b)\pathStatus <> stopped 
		node = walkable	
	End If
	If node = walkable
			
	;Don't cut across corners (this is optional)
	corner = walkable	
	If a = parentXVal-1 
		If b = parentYVal-1 
			If walkability(parentXval-1,parentYval) = unwalkable Then corner = unwalkable
			If walkability(parentXval,parentYval-1) = unwalkable Then corner = unwalkable						
		Else If b = parentYVal+1 
			If walkability(parentXval,parentYval+1) = unwalkable Then corner = unwalkable 
			If walkability(parentXval-1,parentYval) = unwalkable Then corner = unwalkable 							
		End If
	Else If a = parentXVal+1 
		If b = parentYVal-1 
			If walkability(parentXval,parentYval-1) = unwalkable Then corner = unwalkable 
			If walkability(parentXval+1,parentYval) = unwalkable Then corner = unwalkable 							
		Else If b = parentYVal+1 
			If walkability(parentXval+1,parentYval) = unwalkable Then corner = unwalkable 
			If walkability(parentXval,parentYval+1) = unwalkable Then corner = unwalkable 			
		End If
	End If			
	If corner = walkable
	
	;If not already on the open list, add it to the open list.			
	If whichList(a,b) <> onOpenList	

		;Create a new open list item in the binary heap.
		newOpenListItemID = newOpenListItemID + 1; each new item has a unique ID #
		m = numberOfOpenListItems+1
		openList(m) = newOpenListItemID	 ;place the new open list item (actually, its ID#) at the bottom of the heap
		openX(newOpenListItemID) = a : openY(newOpenListItemID) = b ;record the x and y coordinates of the new item

		;Figure out its base G cost
		If Abs(a-parentXval) = 1 And Abs(b-parentYVal) = 1 Then
			addedGCost = 14 ;cost of going to diagonal squares	
		Else	
			addedGCost = 10 ;cost of going to non-diagonal squares				
		End If
		Gcost(a,b) = Gcost(parentXval,parentYVal)+addedGCost
		
		;If the node lies along the path of a nearby unit, add a penalty G cost.		
		If nearByPath(a,b) = penalized
			Gcost(a,b) = Gcost(a,b)+20
		Else If a<>parentXval And b<>parentYval
			If nearByPath(a,parentYval) = penalized
				If nearByPath(parentXval,b) = penalized
					Gcost(a,b) = Gcost(a,b)+28
				End If
			End If	
		End If		
			
		;Figure out its H and F costs and parent
		If useDijkstras = False
			Hcost(openList(m)) = 10*(Abs(a - targetx) + Abs(b - targety)) ; record the H cost of the new square
		Else
			Hcost(openList(m)) = 0	
		End If		
		Fcost(openList(m)) = Gcost(a,b) + Hcost(openList(m)) ;record the F cost of the new square
		parentX(a,b) = parentXval : parentY(a,b) = parentYVal	;record the parent of the new square	
		
		;Move the new open list item to the proper place in the binary heap.
		;Starting at the bottom, successively compare to parent items,
		;swapping as needed until the item finds its place in the heap
		;or bubbles all the way to the top (if it has the lowest F cost).
		While m <> 1 ;While item hasn't bubbled to the top (m=1)	
			;Check if child's F cost is < parent's F cost. If so, swap them.	
			If Fcost(openList(m)) <= Fcost(openList(m/2)) Then
				temp = openList(m/2)
				openList(m/2) = openList(m)
				openList(m) = temp
				m = m/2
			Else
				Exit
			End If
		Wend 
		numberOfOpenListItems = numberOfOpenListItems+1 ;add one to the number of items in the heap

		;Change whichList to show that the new item is on the open list.
		whichList(a,b) = onOpenList


;8.	If adjacent cell is already on the open list, check to see if this 
	;path to that cell from the starting location is a better one. 
	;If so, change the parent of the cell and its G and F costs.	
	Else; If whichList(a,b) = onOpenList
	
		;Figure out the base G cost of this possible new path
		If Abs(a-parentXval) = 1 And Abs(b-parentYVal) = 1 Then
			addedGCost = 14;cost of going to diagonal tiles	
		Else	
			addedGCost = 10 ;cost of going to non-diagonal tiles				
		End If
		tempGcost = Gcost(parentXval,parentYVal)+addedGCost
		
		;If the node lies along the path of a nearby unit, add a penalty G cost.			
		If nearByPath(a,b) = penalized
			tempGcost  = tempGcost+20
		Else If a<>parentXval And b<>parentYval
			If nearByPath(a,parentYval) = penalized
				If nearByPath(parentXval,b) = penalized
					tempGcost  = tempGcost+28
				End If
			End If	
		End If				
		
		;If this path is shorter (G cost is lower) then change
		;the parent cell, G cost and F cost. 		
		If tempGcost < Gcost(a,b) Then 	;if G cost is less,
			parentX(a,b) = parentXval 	;change the square's parent
			parentY(a,b) = parentYVal
			Gcost(a,b) = tempGcost 	;change the G cost			

			;Because changing the G cost also changes the F cost, if
			;the item is on the open list we need to change the item's
			;recorded F cost and its position on the open list to make
			;sure that we maintain a properly ordered open list.