@name A* Pathfinding Chip (only)
@inputs Nodes:table Search [Origin Destination]:vector
@outputs Nodes:table Completed SEARCHING Begin Next
@outputs NodeList:array #follow the these nodes in sequence
@outputs VectorList:array #or follow these vectors
@persist [Self Goal]:vector #search variables
@persist [Start End StartK EndK FCost FNodes Linx]:array #Path finding arrays
@persist [Closed Open]:table 
@persist CurNode GoalNode PrevNode DPN N Ready#numbers
@persist Begin Next SEARCHING
#Scan="Scan"
Co=":"
Brk="<"
O=owner()
Offset=vec(0,0,10) #How high off the ground should a node be placed
if((Search&~Search)){
    SEARCHING=1 #trigger Search
    timer("search",50) #Trigger search timer
    #SCANNING=0  #disable node scanning
    #stoptimer(Scan)
    Begin=1 #this only runs once when searching to prevent the goal from moving.
    NodeList=array()
    #H=Estimate to reach Goal from here
    #G=exact cost to reach this node from start
    #F=G+H  
    #as this runs the F value of a node tells us how expensive it
    #will be to reach the destination from this node
    Open=table() #vec(Parent,H,G)
    Closed=table() #vec(Parent,H,G)
    Start=array() #one use to find the starting podsition's nearest node
    StartK=array() #saves node IDs for start array
    End=array()   #one use to find the ending position's nearest node
    EndK=array() #preserves the node IDs for end array
    FCost=array() #H+G=FCost used for evaluating the shorted route
    FNodes=array() #Node IDS for FCost
    NodeList=array() #final path by nodes
    VectorList=array() #final path by vectors 
    Count=round(Nodes:count()/2)
    N=0
    if(!Origin){Self=round(owner():pos())+Offset}else{Self=round(Origin+Offset)}
    if(!Destination){Goal=round(owner():aimPos())+Offset}else{Goal=round(Destination+Offset)}
    #print("Self "+Self+" Goal "+Goal)
    print("Starting search")
}



if(SEARCHING&(clk("search")|clk("next")|clk("start"))){

    if(Begin){
        
        #perpetuates the while statement if it's running "hot"
        if(N<=round(Nodes:count()/2)){
            timer("start",20)
        }else{
            Begin=0 
            Next=1
            print("Phase 2")
        }
        
        #Finds the nearest nodes to the start and your goal vectors
        while(N<=round(Nodes:count()/2)&perf()){
                K=toString(N)
                V=Nodes[K,vector]
                if(V){
                    Start:pushNumber(round(V:distance(Self)))
                    StartK:pushNumber(K:toNumber())
                    End:pushNumber(round(V:distance(Goal)))
                    
                }
                #print("N="+N+" Start "+Start[N,number])
                
                N++
        }        
        
        #finds the nearest nodes to the "you" and your "goal".
#        foreach(K,V:vector=Nodes){
#                Start:pushNumber(round(V:distance(Self)))
#                StartK:pushNumber(K:toNumber())
#                End:pushNumber(round(V:distance(Goal)))
#        }
        
       
    }

        
    if(Next){
        #Time to do ranger tests! 
        EndK=StartK:clone()
        rangerIgnoreWorld(0)
        rangerHitEntities(0)
        rangerHitWater(0)
        for(C=1,Start:count()){
            Min=StartK[Start:minIndex(),number]
            print("Start "+Min)
            if(!rangerOffset(Self,Nodes[toString(Min),vector]):hit()){
                CurNode=Min
                break
            }else{
                StartK:remove(Start:minIndex())
                Start:remove(Start:minIndex())
            }
        }
        for(C=1,End:count()){
            Min=EndK[End:minIndex(),number]
            print("End "+Min)
            if(!rangerOffset(Goal,Nodes[toString(Min),vector]):hit()){
                GoalNode=Min
                break
            }else{
                EndK:remove(End:minIndex())
                End:remove(End:minIndex())
            }
        }#Now that we've done the least ranger tests needed we move on. 
        
        CNV=Nodes[toString(CurNode),vector] #Current Node Vector
        #Node numbers for the nearest nodes to self and goal coordinates. 
        if(!CurNode){print("Cannot locate a node within sight of the start position")}
        if(!GoalNode){print("Cannot locate a node within sight of the end position")}
        Parent=CurNode
        G=round(CNV:distance(Self)) #initial G value
        H=round(CNV:distance(Goal)) #initial H value
        Open[toString(CurNode),vector]=vec(-Parent,H,G) #create the first Open value making Parent negative so you can see it's node 1 on the path.
        Open[toString(GoalNode),vector]=vec(-1,0,-1) #nonsense distances and parent ID that are not 0, nor positive. 
        NodeList:pushNumber(CurNode)
        
        if(CurNode&GoalNode){
            Ready=1 
            Next=0
        }else{Ready=0}
        PrevNode=0
        
        
    }#End Next
    
     
    #In the event of a jam this will cease to restart the while statement to terminate in case of search confusion.    
    #in the event of no search confusion, this will continue to retrigger the search until it completes.
    if((CurNode!=GoalNode)&(CurNode!=PrevNode)&!Begin){timer("search",100)} #paces the search algorythm
    
    while(CurNode!=GoalNode&perf()&!Next&Ready){
        #if(!Closed:count()){print("While loop started!")}
        DPN=PrevNode #Double Previous node
        PrevNode=CurNode #store previous node for jam detection
        
        #Check if there is only 1 link and if so, it's ok to back track. 
        if(FCost:count()==1){CurNode=FNodes[FCost:minIndex(),number]}    
        #ELSE if there is more than 1 linked node, check if the new CurNode has been visited 
        #and exclude it if it has.
        if(FCost:count()>1){
            CurNode=FNodes[FCost:minIndex(),number]
            if(DPN==CurNode){
                FNodes:remove(FCost:minIndex())
                FCost:remove(FCost:minIndex())
                CurNode=FNodes[FCost:minIndex(),number]
            }
        } 

        CNS=toString(CurNode)
        
        #this records the sequence in which nodes are processed
        #if(PrevNode==CurNode){break print("Search is pinned at "+CurNode)}
        if(PrevNode!=CurNode){
            if(DPN!=CurNode){
            NodeList:pushNumber(CurNode)
        	   VectorList:pushVector(Nodes[CNS,vector])
            print("CNS="+CNS)
            }else{
            #-----------Some debug data when path is jammed--------
                print("Search is jammed at node "+CNS+" "+FCost:min())
#                print("FCost")
#                printTable(FCost)
#                print("FNodes")
#                printTable(FNodes)
#                print("closed list:")
#                printTable(Closed) 
#                print("open list:")
#                printTable(Open)      
                Jammed=1
                #-----------
                
                
                #------A Bad Fix-----------
                #Swapping previous and current nodes. 
#                PrevNodeT=PrevNode #A temp variable for moving purposes
#                PrevNode=CurNode 
#                CurNode=PrevNodeT
#                CNS=toString(CurNode)
                #--------------------------
                print("CurNode="+CurNode+" PrevNode="+PrevNode)
                #print("moving node back from "+CNS+" to "+PrevNode)
                
            }
        }
        
        
        if(owner():lastSaid()=="stop it"){break print("search stopped")}
        Closed[CNS,vector]=Open[CNS,vector] #copy open list entry to closed list to begin the entry "move".
        Open[CNS,vector]=vec() #delete entry in open list to complete the "move".
        
        #------------
        #clear these arrays or the search will stall if the target is too distant. 
        #also the search will repetitously scan the same nodes over and over again if when progressing.
        FNodes=array()
        FCost=array()        
        #------------
        
        Completed=CurNode==GoalNode #This is just a check for external chips to know if the search completed.
                
        if(CurNode==GoalNode){
            SEARCHING=0 
            stoptimer("search")
            
            #this timer isn't needed as the path is now recorded during the search.
#            timer("path",100) #trigger the path processing function in a little bit
            print("Search completed!") 
            break
        }else{
        
            #Everything below this point pertains to heuristic analysis. This is the primary engine of A*
            Linx=array() #clear link array
            Linx=Nodes[CNS,string]:replace(CNS+Co,""):explode(Brk) #This converts the Link from string format to an array
            
            #printTable(Linx)
            #---------Another Bad fix---------
            #This if superficially removes a link in case of a jam
#            if(Jammed){
#                for(C=2,Linx:count()){
#                    #this removes the looping link of a loop is detected
#                    if(Linx[C,string]:toNumber()==PrevNode){Linx:remove(C) print("removed link from: "+CNS+" to "+Linx[C,string])}
#                    
#                }
#                Jammed=0
#            }
            #---------------------------
            
            GCN=Closed[CNS,vector]:z() #G Current Node
            GNV=Nodes[toString(GoalNode),vector] #Goal Node Vector
            print("GCN="+GCN)
            
            #Scan links to see if any are in the closed list
            for(C=2,Linx:count()){
                LS=Linx[C,string] #Links String
                 #closed vector vec(Parent,H,G)
                 #open Node vector vec(Parent,H,G)
                GO=Open[LS,vector]:z()#G Open
                GC=Closed[LS,vector]:z()#G Closed 
                LSV=Nodes[LS,vector] #Link String Vector
                if(GC&(GC<GCN)){ #if (this neighbor is in the closed list and our current g value is lower) 
                    G=round(LSV:distance(Nodes[CNS,vector]))+GCN
                    if(!H){H=round(Nodes[LS,vector]:distance(GNV))}else{H=Closed[LS,vector]:y()}
                    Closed[LS,vector]=vec(CurNode,H,G) #Parent,H,G  writes new info to closed list
                    FNodes:pushNumber(LS:toNumber()) #This tells me what node has the lowest F
                    FCost:pushNumber(H+G) #This is purely for assisting in calculating lowest F
                    if(Jammed){print("Closed "+LS+" H "+H+" G "+G)}
                }elseif(GO&(GO<GCN)){
                    G=round(LSV:distance(Nodes[CNS,vector]))+GCN
                    if(!H){H=round(Nodes[LS,vector]:distance(GNV))}else{H=Open[LS,vector]:y()}
                    Open[LS,vector]=vec(CurNode,H,G) #writes new info to Open list
                    FNodes:pushNumber(LS:toNumber()) #This tells me what node has the lowest F
                    FCost:pushNumber(H+G) #This is purely for assisting in calculating lowest F
                    if(Jammed){print("Open "+LS+" H "+H+" G "+G)}
                }elseif(!GO){
                    G=round(LSV:distance(Nodes[CNS,vector]))+GCN
                    H=round(LSV:distance(GNV))
                    Open[LS,vector]=vec(CurNode,H,G) #creates new open list entry
                    FNodes:pushNumber(LS:toNumber()) #This tells me what node has the lowest F
                    FCost:pushNumber(H+G) #This is purely for assisting in calculating lowest F
                    if(Jammed){print("New "+LS+" H "+H+" G "+G)}
                }#end if ifelse else scan
            }#end of for scan
        }#end of else
    }#End of while
}#End of search
