@name Breadth First Search
@inputs Nodes:table Search [Origin Destination]:vector
@outputs [NodeList VectorList]:array Completed SEARCHING
#@outputs [Start StartK End EndK]:array 
@persist [N S E Begin Next Ready CurNode GoalNode StartNode]:number
@persist [Open Start StartK End EndK]:array 
@persist [Closed DT]:table
@persist [Co Brk CNS]:string
@persist [Offset Self Origin Goal Destination CNV]:vector
@trigger
if(first()){
    Co=":" #Colon, this is a link pair divider
    Brk="<" #Break, this is a link separation character
    O=owner()
    Offset=vec(0,0,10) #How high off the ground should a node be placed
}

if(Search&changed(Search)){
    SEARCHING=1 #trigger Search
    Begin=1 #this only runs once when searching to prevent the goal from moving.
    Open=array() #Numerical Node ID list of nodes to visit
    Closed=table() #numerical table of node's visited
    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
    NodeList=array() #final path by nodes
    VectorList=array() #final path by vectors 
    timer("start",1)
    CNS=""
    CurNode=0
    N=1
    S=1
    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("Starting search")
}
if(SEARCHING&(clk("next")|clk("start")|clk("continue"))){

    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
        }
        #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)))
                }
                N++
        }
        #if(N>round(Nodes:count()/2)){printTable(Start)}        
    }
    if(Next){
        #Time to do ranger tests! 
        EndK=StartK:clone()
        rangerIgnoreWorld(0)
        rangerHitEntities(0)
        rangerHitWater(0)
        #print("next")

        if(S<=Start:count()&!perf()){timer("start",20)}
	
        while(S<=Start:count()&perf()){
        #for(C=1,Start:count()){
            Min=StartK[Start:minIndex(),number]
            Trace=rangerOffset(Self,Nodes[toString(Min),vector]):hit()
            #print("S "+S+" Trace "+Trace+" Min "+Min+" Origin "+Origin+" Goal "+Goal)
            
            if(!Trace){
                CurNode=Min
                print("Start "+Min)
                break
            }else{
                StartK:remove(Start:minIndex())
                Start:remove(Start:minIndex())
                print(StartK)
            }
        S++
        }
        
	
        if(E<=End:count()&!perf()){timer("start",20)}
        #for(C=1,End:count()){
	       while(E<End:count()&perf()){
            Min=EndK[End:minIndex(),number]
           
            if(!rangerOffset(Goal,Nodes[toString(Min),vector]):hit()){
                GoalNode=Min
                print("End "+Min)
                break
            }else{
                EndK:remove(End:minIndex())
                End:remove(End:minIndex())
            }
	E++
        }#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")}
        if(CurNode){
            CNS=toString(CurNode)
            StartNode=CurNode #This is used when reconstructing the path
            Open:pushNumber(CurNode)   
            Depth=1 #This sets the search's starting depth
            Closed[CNS,vector2]=vec2(CurNode,Depth)
            NodeList:pushNumber(CurNode)
        } #else{print("hard wiring enabled") CurNode=StartNode=9 GoalNode=16}
        if(CurNode&GoalNode){
            Ready=1 
            Next=0
        }else{Ready=0 SEARCHING=0}

        
        
    }#End Next
    BFS=Open:count()&Ready&!Next
    if(BFS){timer("continue",50) print("resuming search")}
    while(BFS&perf()){
        CurNode=Open:shiftNumber() #moves the first item off the list
        CNS=toString(CurNode) #Current Node String
        #print("Cur "+CurNode+" Goal "+GoalNode)
        Depth=Closed[CNS,vector2]:y() #Current Node's depth
        Closed[CNS,vector2]=vec2(CurNode,Depth)
        if(CurNode==GoalNode){
            print("search complete")
            Completed=1
            SEARCHING=0
            Ready=0
            DT=table() #depth Table
            DT[Depth,number]=1
            Closed[CNS,vector2]=vec2(CurNode,Depth)
            timer("Parse",20)
            NodeList=array()
            #printTable(Closed)
            VectorList=array()
            #NodeList:unshiftNumber(StartNode)
            #VectorList:unshiftVector(Nodes[toString(StartNode),vector])
            break
            
        }else{
            
            #ClosedString=glonEncode(Closed)#used to search the whole array at once
            
            Links=array()
            Links=Nodes["S"+CNS,string]:replace(CNS+Co,""):explode(Brk) #This converts the Link from string format to an array of strings
            
            for(C=2,Links:count()){
                LS=Links[C,string]
                LN=LS:toNumber() #converts each index from string to number
               # Find=!ClosedString:find(LS)
               # print("LS "+LS+" Find "+Find)
                if(!Closed[LS,vector2]){ #keeps us from duplicating node info
                    Open:pushNumber(LN) #pushes a nodeID to be scanned
                    Closed[LS,vector2]=vec2(CurNode,Depth+1) #vec2(Parent,Depth of parent +1)
                    #print("closed "+Closed[LS,vector2]:toString()+" Open "+LN)
                }
            }
            #print(Open:concat(","))
        }
    }
}


if(clk("Parse")){ 
   #printTable(Closed)
    #printTable(Open)
    if(CurNode!=StartNode){timer("Parse",1)} #enable this when you got it working right!
    while(CurNode!=StartNode&perf()){
        if(CurNode){
            CNS=toString(CurNode)
            CurNode=Closed[CNS,vector2]:x() #set the next node to the parent of the current
            Depth=Closed[CNS,vector2]:y() #get the depth of the current node
           # print("CurNode="+CurNode)
            Links=array()
            Links=Nodes["S"+CNS,string]:replace(CNS+Co,""):explode(Brk) #This converts the Link from string format to an array of strings
            
            for(C=2,Links:count()){
                LS=Links[C,string]
                LD=Closed[LS,vector2]:y() #Link Depth
                #print("LS "+LS+" Link Depth "+Closed[LS,vector2]:y()+" Depth "+Depth)
                LN=LS:toNumber() #converts each index from string to number
                if((LD==(Depth-1))&!DT[Depth-1,number]){
                    DT[Depth-1,number]=1
                    NodeList:unshiftNumber(CurNode)  #record the current node's ID
                    VectorList:unshiftVector(Nodes[CNS,vector]) #record the current node's location
                    CurNode=Closed[LS,vector2]:x()
                    

                }
            }
            
            
        }
    }
    if(CurNode==StartNode){
        NodeList:unshiftNumber(CurNode)
        VectorList:unshiftVector(Nodes[CNS,vector])
        stoptimer("Parse")
        print("SUCCESS! PATH FOUND!")
    }
    
}
