@name A* Routefinder
@inputs RefindPath Targ:vector
@outputs Path:array
#by Jeremydeath
@persist StartPoint:vector2 EndPoint:vector2
@persist MainEnt:entity

@persist NodeDist PathFound Sqrt2 Height HoloScale:vector Radius CurHeight LastHeight

#The Nodes Master tables
@persist Nodes:array InvertNodes:table  
@persist NodesParents:array NodesFCost:array NodesGCost:array NodesHCost:array

#End Node
@persist StartNode:vector2      StartNodeIndex

#Start Node
@persist EndTargetNode:vector2  EndTargetNodeIndex

#Current Node
@persist CurrentNode:vector2    CurrentNodeIndex

#Array deminsions
@persist NodeCountX NodeCountY

#The open list
@persist OpenListNodes:array OpenListParent:array OpenListFCost:array

#The closed list
@persist ClosedListNodes:array ClosedListParent:array ClosedListFCost:array

#looping vars and counters
@persist LoopMain LoopMainCounter

#The finished path
@persist Path:array 

@trigger all

interval(100)

if(first()| (RefindPath & ~RefindPath))
{

    #Reset everything
    PathFound = 0
    Nodes = array() 
    InvertNodes = table() 
    NodesParents = array() 
    NodesFCost = array() 
    NodesGCost = array()
    NodesHCost = array()
    StartNode = vec2(0,0)
    StartNodeIndex = 0
    EndTargetNode = vec2(0,0)
    EndTargetNodeIndex = 0
    CurrentNode = vec2(0,0)
    CurrentNodeIndex = 0
    NodeCountX = 0
    NodeCountY = 0
    OpenListNodes = array()
    OpenListParent = array()
    OpenListFCost = array()
    ClosedListNodes = array()
    ClosedListParent = array()
    ClosedListFCost = array()
    LoopMain = 0
    LoopMainCounter = 0
    Path = array() 

    
    
    MainEnt = entity()
    
    #define the square root of 2
    Sqrt2 = sqrt(2)
    
    #define start/end points
    StartPoint = vec2(MainEnt:pos())
    EndPoint = vec2(0,0)
    
    NodeDist = 100
    Radius = 15
    HoloScale = vec(NodeDist/30,NodeDist/30,NodeDist/30)
    
    #Switch the end and starting points so that every thing comes out the correct direction
    Temp = EndPoint
    EndPoint = StartPoint
    StartPoint = Temp
    
    #Define the height
    Height = MainEnt:pos():z()+5
    CurHeight = Height
    LastHeight = CurHeight
    
    #fill the node array
    
    #Round the nodes so that they fit with the node table
    RoundStart = vec2(round(StartPoint:x()/NodeDist)*NodeDist,round(StartPoint:y()/NodeDist)*NodeDist)
    RoundEnd = vec2(round(EndPoint:x()/NodeDist)*NodeDist,round(EndPoint:y()/NodeDist)*NodeDist)
    #calculate the difference/NodeDist so that the node table can be filled
    RoundDiff = vec2(round((RoundStart:x()-RoundEnd:x())/NodeDist),round((RoundStart:y()-RoundEnd:y())/NodeDist))
    
#=========== Build the node table ================

    #Set the margin
    Margin = 6
    
    
    #Set some varibles to reduce ops
    RoundEndX = RoundEnd:x()
    RoundEndY = RoundEnd:y()
    
    MinPoint = vec2(min(RoundStart:x(),RoundEnd:x()), min(RoundStart:y(),RoundEnd:y()))-vec2(Margin*NodeDist,Margin*NodeDist)
    MaxPoint = vec2(max(RoundStart:x(),RoundEnd:x()), max(RoundStart:y(),RoundEnd:y()))+vec2(Margin*NodeDist,Margin*NodeDist)
    
    print(toString(MinPoint))
    
    NodeCountX = 2*Margin+abs(RoundDiff:x())
    NodeCountY = 2*Margin+abs(RoundDiff:y())
    
    if(NodeCountX*NodeCountY > 1048576)
    {
        print("Error! The search area is too big!")
        print("Raise the NodeDist, or lower the margin")
        exit()
    }
    
    MinPointX = MinPoint:x()
    MinPointY = MinPoint:y()
    
    RoundDiffMarg = positive(RoundDiff)+vec2(Margin*2,Margin*2)
    RoundDiffXMarg = RoundDiffMarg:x()
    
    X = 0
    Y = 0
    RoundDifXMarg = RoundDiff:x()+Margin
    while( Y <= RoundDiffMarg:y() ){
        while( X <= RoundDiffXMarg ){
            Nodes:pushVector2( vec2(MinPointX+X*NodeDist,MinPointY+Y*NodeDist) )
            X++
        }
        X = 0
        Y++
    }

    
    
    
    
#============ Invert the node table ==============
    InvertNodes = invert(Nodes)
    
    #assign start node
    StartNode = RoundStart
    StartNodeIndex = InvertNodes:number(toString(StartNode))
    

    #assign end node
    EndTargetNode = RoundEnd
    EndTargetNodeIndex = InvertNodes:number(toString(EndTargetNode))
    
    
    #Calculation of H is done with the manhattan method
    #H = abs(currentX-targetX) + abs(currentY-targetY)
    #H = Current:distance(Target)

    
    #Set the start node infromation on the master tables
    NodesParents[StartNodeIndex,number] = StartNodeIndex
    NodesGCost[StartNodeIndex,number] = 0 #Distance from start along optimal path.
    NodesHCost[StartNodeIndex,number] = abs(StartNode:x()-EndTargetNode:x()) + abs(StartNode:y()-EndTargetNode:y())
    NodesFCost[StartNodeIndex,number] =  NodesHCost[StartNodeIndex,number] #Estimated total distance from start to goal through y.
    
    #push the start node into the open list
    OpenListNodes:pushNumber(StartNodeIndex)
    OpenListParent:pushNumber(StartNodeIndex) #push yourself as the parent
    OpenListFCost:pushNumber(NodesFCost[StartNodeIndex,number])
    
    #Tell it to start the main loop
    LoopMain = 1
    
#=================== Debug =======================    
    #print(toString(ops()))
    #Corners for debug
    
    Corner1 = Nodes[1,vector2]
    holoCreate(1,vec(Corner1:x(),Corner1:y(),Height)) 
    holoScale(1,HoloScale) 
    
    Corner2 = Nodes[Nodes:count(),vector2]
    holoCreate(2,vec(Corner2:x(),Corner2:y(),Height)) 
    holoScale(2,HoloScale) 
    
    Corner3 = vec2(Corner1:x(),Corner2:y())
    holoCreate(3,vec(Corner3:x(),Corner3:y(),Height)) 
    holoScale(3,HoloScale) 
    
    Corner4 = vec2(Corner2:x(),Corner1:y())
    holoCreate(4,vec(Corner4:x(),Corner4:y(),Height))  
    holoScale(4,HoloScale)
    

    holoCreate(5,vec(StartNode:x(),StartNode:y(),Height))
    holoColor(5,vec(0,255,0))
    holoScale(5,HoloScale) 
    
    
    holoCreate(6,vec(EndTargetNode:x(),EndTargetNode:y(),Height))
    holoColor(6,vec(255,0,0))
    holoScale(6,HoloScale) 
    
    holoCreate(752752,vec(0,0,0))
    holoColor(752752,vec(255,255,0))
    holoScale(752752,HoloScale) 
    
    print(toString(NodeCountX))
    print(toString(NodeCountY))

#================= Debug ==========================    

    exit()
}



#Start the main loop
if(LoopMain == 1 & clk() & OpenListNodes:count() > 0)
{
    
    #Look for the lowest F cost square on the open list. Refer to this as the current node
    LowestFIndex = OpenListFCost:minIndex()
    CurrentNodeIndex = OpenListNodes[LowestFIndex,number]
    CurrentNode = Nodes[CurrentNodeIndex,vector2]
    
    holoPos(752752, vec(CurrentNode:x(),CurrentNode:y(),CurHeight))
    
    #build some data to be used in the future
    CurrentNodeGCost = NodesGCost[CurrentNodeIndex,number]
    CurrentNodeHCost = NodesHCost[CurrentNodeIndex,number]
    CurrentNodeFCost = NodesFCost[CurrentNodeIndex,number]
    CurrentNodeParent = OpenListParent:number(LowestFIndex)

    #Stop if you Add the target square to the closed list, in which case the optimal path has been found, or
    if( CurrentNode == EndTargetNode )
    {
        PathFound = 1
        LoopMain = 0
        exit()
    }

    
    #Switch it to the closed list.
    ClosedListNodes:pushNumber( OpenListNodes[LowestFIndex,number] )
    ClosedListParent:pushNumber( OpenListParent[LowestFIndex,number] )
    ClosedListFCost:pushNumber( OpenListFCost[LowestFIndex,number] )
    
    #Remove it from the open list
    OpenListNodes:removeNumber(LowestFIndex)
    OpenListParent:removeNumber(LowestFIndex)
    OpenListFCost:removeNumber(LowestFIndex)
    
    #Center Node
    #print("Center Node: IDX: "+toString(CurrentNodeIndex)+" Value: "+toString(CurrentNode))
    
    #Find the adjacent squares
    AdjacentNodes = array()
    #North-West
        AdjacentNodes:pushNumber(CurrentNodeIndex-NodeCountX-2)
    #North
        AdjacentNodes:pushNumber(CurrentNodeIndex-NodeCountX-1)
    #North-East
        AdjacentNodes:pushNumber(CurrentNodeIndex-NodeCountX)
    #East
        AdjacentNodes:pushNumber(CurrentNodeIndex+1)
    #South-East
        AdjacentNodes:pushNumber(CurrentNodeIndex+NodeCountX+2)
    #South
        AdjacentNodes:pushNumber(CurrentNodeIndex+NodeCountX+1)
    #South-West
        AdjacentNodes:pushNumber(CurrentNodeIndex+NodeCountX)
    #West
        AdjacentNodes:pushNumber(CurrentNodeIndex-1)
    
    
    #For each of the 8 squares adjacent to this current square 
    AdjStep = 1
    while(AdjStep <= AdjacentNodes:count())
    {
        #If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
        CurAdjNodeIndex = AdjacentNodes[AdjStep,number]
        CurAdjNode = Nodes[CurAdjNodeIndex,vector2]
        

        #print("Adj Node "+toString(AdjStep)+": IDX: "+toString(CurAdjNodeIndex)+" Value: "+toString(CurAdjNode))
        
        #Check for out-of-bounds nodes
        if(AdjStep == 1){
            if(CurAdjNode != CurrentNode+vec2(-NodeDist,-NodeDist)){
                AdjStep++
                continue
            }
        }
        elseif(AdjStep == 2){
            if(CurAdjNode != CurrentNode+vec2(0,-NodeDist)){
                AdjStep++
                continue
            }
        }
        elseif(AdjStep == 3){
            if(CurAdjNode != CurrentNode+vec2(NodeDist,-NodeDist)){
                AdjStep++
                continue}
        }
        elseif(AdjStep == 4){
            if(CurAdjNode != CurrentNode+vec2(NodeDist,0)){
                AdjStep++
                continue
            }
        }
        elseif(AdjStep == 5){
            if(CurAdjNode != CurrentNode+vec2(NodeDist,NodeDist)){
                AdjStep++
                continue
            }
        }
        elseif(AdjStep == 6){
            if(CurAdjNode != CurrentNode+vec2(0,NodeDist)){
                AdjStep++
                continue
            }
        }
        elseif(AdjStep == 7){
            if(CurAdjNode != CurrentNode+vec2(-NodeDist,NodeDist)){
                AdjStep++
                continue
            }
        }
        elseif(AdjStep == 8){
            if(CurAdjNode != CurrentNode+vec2(-NodeDist,0)){
                AdjStep++
                continue
            }
        }
        #Check Closed List
        I = 1
        OnClosed = 0
        while(I <= ClosedListNodes:count())
        {
            if(ClosedListNodes[I,number] == CurAdjNodeIndex) {
                OnClosed = 1
                break
            }
            I++
        }
        if(OnClosed == 1)
        {
            AdjStep++
            continue
        }
        
#================== Insert Terain Check's Here =================

        #Simple trace ranger check
        TempVec = vec(CurrentNode:x(),CurrentNode:y(),CurHeight)
        
        CurHeight = rangerOffset(5000,TempVec+vec(0,0,100),vec(0,0,-1)):position():z()+5
        
        if(abs(LastHeight-CurHeight) > 70) #if there is a big jump, disregard the node
        {
            AdjStep++
            continue
        }
        
        
        CurrentNodePos = vec(CurrentNode:x(),CurrentNode:y(),CurHeight)
        CurAdjNodePos = vec(CurAdjNode:x(),CurAdjNode:y(),CurHeight)
        
        RD = rangerOffset(CurrentNodePos,CurAdjNodePos)
        if(RD:hit() != 0) #If the ranger hit anything, disregard the node
        {
            AdjStep++
            continue
        }
        
        #Check the edges so that the objec will fit
        
        
        Edge = (((CurAdjNode-CurrentNode)/NodeDist)):normalized()*Radius
        Edge = vec2(-Edge:y(),Edge:x())
        
        #print(toString(vec(Edge)+CurrentNodePos)+"   "+toString(AdjStep))
        
        RD1Pos = vec(Edge)+CurrentNodePos
        
        RD1 = rangerOffset(vec(Edge)+CurrentNodePos,vec(Edge)+CurAdjNodePos)
        RD2 = rangerOffset(vec(-Edge)+CurrentNodePos,vec(-Edge)+CurAdjNodePos)
        
        
        
        if(RD1:hit() != 0 | RD2:hit() != 0) #If the rangers hit anything, disregard the node
        {
            AdjStep++
            continue
        }
        
        LastHeight = CurHeight
        
#================== End of Terain Checks =======================
        
        #Update the tentative G score
        TentativeGCost = NodesGCost[CurrentNodeIndex,number] + CurrentNode:distance(CurAdjNode)
        TentativeIsBetter = 0
        
        #Check the open list
        I = 1
        OnOpen = 0
        while(I <= OpenListNodes:count())
        {
            if(OpenListNodes[I,number] == CurAdjNodeIndex) {
                OnOpen = 1
                break
            }
            I++
        }
        
        #If it isnt on the open list... 
        if(OnOpen != 1) {  
            #add it to the open list.
            OpenListNodes:pushNumber(CurAdjNodeIndex)
            OpenListParent:pushNumber(CurrentNodeIndex)
            
            
            #update the infromation on the master tables
            HCost = abs(CurAdjNode:x()-EndTargetNode:x()) + abs(CurAdjNode:y()-EndTargetNode:y())
            NodesHCost[CurAdjNodeIndex,number] = HCost
            TentativeIsBetter = 1
            
            OpenListFCost:pushNumber(TentativeGCost+HCost)
        }
        elseif( TentativeGCost < NodesGCost[CurAdjNodeIndex,number] )
        {
            TentativeIsBetter = 1
        }
        
        if( TentativeIsBetter == 1 ) 
        {
            NodesParents[CurAdjNodeIndex,number] = CurrentNodeIndex
            NodesGCost[CurAdjNodeIndex,number] = TentativeGCost
            NodesFCost[CurAdjNodeIndex,number] =  NodesHCost[CurAdjNodeIndex,number]+NodesGCost[CurAdjNodeIndex,number]
        }
              
        AdjStep++
    }
    
    
    LoopMainCounter++
}
#Stop if you fail to find the target square, and the open list is empty. In this case, there is no path.
elseif(LoopMain == 1 & clk() & OpenListNodes:count() <= 0)
{
    PathFound = -1
    LoopMain = 0
}
elseif(LoopMain == 0 & clk() & PathFound == 1)
{
    
    #Trace back from the Target square, going through each of its parents
    print("Path Found!")
    
    TraceBackCurNodeIndex = EndTargetNodeIndex
    TraceBackCurNode = EndTargetNode
    
    while( StartNode != TraceBackCurNode )
    {
        TraceBackCurNodeIndex = NodesParents[TraceBackCurNodeIndex,number]
        TraceBackCurNode = Nodes[TraceBackCurNodeIndex,vector2]
        
        TempVec = vec(TraceBackCurNode:x(),TraceBackCurNode:y(),CurHeight)
        
        CurHeight = rangerOffset(5000,TempVec+vec(0,0,100),vec(0,0,-1)):position():z()+20
        
        Path:pushVector(vec(TraceBackCurNode:x(),TraceBackCurNode:y(),CurHeight))

        holoCreate(1000+TraceBackCurNodeIndex,vec(TraceBackCurNode:x(),TraceBackCurNode:y(),CurHeight),HoloScale) 
        holoColor(1000+TraceBackCurNodeIndex,vec(0,0,255)) 
    }
    
    PathFound = 11
    
}
elseif(LoopMain == 0 & clk() & PathFound == -1)
{
    print("There is no path!!")
}