Pathfinding using waypoints help

;HEAP FUNCTIONS
Global HEAPSIZE=0
Global HMAXSIZE=100			;this is your heaps size
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)

Function HEAP_INSERT(n,key#)
HEAPSIZE=HEAPSIZE+1
HEAPITEM(HEAPSIZE) = n

m = HEAPSIZE
HEAPKEY(HEAPITEM(m))=key

  While m <> 1 ;While item hasn't bubbled to the top (m=1)

     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else

        Exit ;exit the while/wend loop
     End If
  Wend
End Function

Function HEAP_REMOVELOWEST()
;If HEAP_EMPTY()=0
L = HEAPITEM(1)

;GET THE LAST ITEM AND PUT IT IN THE FIRST POSITION
;(FIRST GET*S DELETED)
HEAPITEM(1) = HEAPITEM(HEAPSIZE)
HEAPSIZE = HEAPSIZE - 1

v = 1
;Repeat the following until the item sinks to its proper spot in the binary heap.
Repeat
  u = v
  If 2*u+1 <= HEAPSIZE;if both children exist
    ;Select the lowest of the two children.
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u)) Then v = 2*u 
    If HEAPKEY(HEAPITEM(v)) >= HEAPKEY(HEAPITEM(2*u+1)) Then v = 2*u+1

  Else If 2*u <= HEAPSIZE ;if only child #1 exists
    ;Check if the F cost is greater than the child
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u))Then v = 2*u
  End If

  If u <> v Then ; If parent's F > one or both of its children, swap them
    temp = HEAPITEM(u)
    HEAPITEM(u) = HEAPITEM(v)
    HEAPITEM(v) = temp
  Else
    Exit ;if item <= both children, exit repeat/forever loop
  End If
Forever

Return L
;End If
End Function

Function UPDATE_HEAPITEM(item,newkey#,HEAPPOS=0)
If HEAPPOS=0
For i=1 To HEAPSIZE
If HEAPITEM(i) = item Then m=i:Exit
Next
Else 
m=HEAPPOS
End If

HEAPKEY(HEAPITEM(m)) = newkey

  While m <> 1 ;While item hasn't bubbled to the top (m=1)
     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else
        Exit ;exit the while/wend loop
     End If
  Wend
End Function

Function HEAP_EMPTY()
If HEAPSIZE>0 Return 0 Else Return 1
End Function
Function CLEAR_HEAP()
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)
HEAPSIZE=0
End Function

Function P_ASTAR(node1%,GOAL%,o%=0)
Local CLOSED%[P_MAXNODES]
Local PARENT%[P_MAXNODES]
Local G#[P_MAXNODES]					;GENERATED COST
Local H#[P_MAXNODES]					;ESTIMATED COST (to target)
goal_found=0

If o=0 Then P_GOAL_NODE(o) = 0


If NODE1 <> GOAL
CLEAR_HEAP()
HEAP_INSERT(node1,0)


;P_ELAPSEDTIME=MilliSecs()
Repeat
;GET THE MOST LOWEST COST NODE FROM THE HEAP
current = HEAP_REMOVELOWEST()

;PUT IT TO THE CLOSED LIST
CLOSED[current] = 1

	;CHECK ALL THE NODES CONNECTED TO CURRENT NODE
	For i=1 To P_NODE_CONNECTIONS(current)
	nn = P_NODE_CON(current,i)

	;IF NOT ON THE CLOSED LIST PROCEED
	If CLOSED[nn]=0
	
	;SEARCH THE OPEN LIST -> PUT THE HEAP POS IN found ELSE found = 0
	found=0
	For ii=1 To HEAPSIZE
	If HEAPITEM(ii) = nn Then found=ii:Exit
	Next

	;NOT ON OPENLIST ---> CALCULATE COST AND PUT IT IN!
	If found=0
	PARENT[nn] = current
	H[nn] = P_MANHATTAN_DISTANCE(nn,GOAL)					;ESTIMATE THE DISTANCE TO GOAL
	G[nn] = P_NODE_ARCLENGTH(current,i) + G[current]		;G = distance to this node + G of the parent node
	F# = G[nn] + H[nn]																	;CALCULATE FINAL COST

	;PUT TO OPENLIST
	HEAP_INSERT(nn,F)
	End If

	;IS ON THE OPEN LIST --- > CHECK IF THIS IS BETTER PATH TO  IT
	If found>0
	NG# = P_NODE_ARCLENGTH(current,i) + G[current]

		;IF THIS PATH*S G SCORE = LOWER THAN OLD G --> UPDATE
		If NG<G[nn]
		PARENT[nn] = current
		G[nn] = NG																				;REPLACE THE OLD G WITH THE CURRENT G
		F# = G[nn] + H[nn]																	;CALCULATE FINAL COST
		UPDATE_HEAPITEM(nn,F,found)											;UPDATE THIS NODE
		End If
		
	End If

	End If
	Next

If current=GOAL Then goal_found=1
Until  HEAP_EMPTY()=1 Or goal_found=1 
End If

	P_PATHNODES(o)=0
	P_PATHCOST(o)=0
	
	If P_GOAL_NODE(o)>0
	P_PATHNODES(o) = P_PATHNODES(o) + 1
	P_PATHNODE(o,P_PATHNODES(o)) = P_GOAL_NODE(o)
	End If

;TRACE THE PATH!
If goal_found=1
current=GOAL
P_PATHCOST(o) = G[current]

	Repeat
	P_PATHNODES(o) = P_PATHNODES(o)+1
	P_PATHNODE(o,P_PATHNODES(o)) = current
	current = PARENT[current]
	Until current=node1
	P_PATHNODES(o) = P_PATHNODES(o)+1
	P_PATHNODE(o,P_PATHNODES(o)) = current

End If



;P_ELAPSEDTIME=MilliSecs()-P_ELAPSEDTIME

Return P_PATHNODES(o)
End Function