@name Breadth First Search
@inputs #Nodes:table 
@inputs Search [Origin Destination]:vector Reset
@outputs N S E [NodeList VectorList]:array Completed SEARCHING DT:table Error
@persist [N S E Begin Next Ready CurNode GoalNode StartNode]:number
@persist [Open Start StartK End EndK]:array 
@persist [Closed Nodes]:table
@persist [Offset Self Origin Goal Destination CNV]:vector
@persist Filename:string
@model models/props_junk/PopCan01a.mdl
@trigger

#errors
#Error=0 It worked! 
#Error=1 No end node found
#Error=2 No start node found
#Error=3 Start and End nodes are on separate nodegraph fragments
if(first()){
    SEARCHING=0
    Filename="nodegraphs/Nodegraph_5_"+map()+".txt"
    O=owner()
    Offset=vec(0,0,10) #How high off the ground should a node be placed
    runOnFile(1)
    fileLoad(Filename)
}
if(fileLoaded()&!Nodes:count()){
    Nodes=glonDecodeTable(fileRead())
    print("ops "+opcounter())
}
if(Reset){reset()}
if(Nodes:count()&Search&(changed(Search)|~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
    Completed=0
    Ready=0
    Next=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<=Nodes:count()){
            timer("start",20)
            
        }else{
            Begin=0 
            Next=1
        }
        
        #Finds the nearest nodes to the start and your goal vectors
        while(N<=Nodes:count()&perf()){
                #print("ops "+opcounter())
                V=Nodes[N,array][1,vector]
                #print("V "+V:toString())
                if(V){
                    Start:pushNumber(round(V:distance(Self)))
                    StartK:pushNumber(N)
                    End:pushNumber(round(V:distance(Goal)))
                }
                N++
        }
             
    }
    if(Next){
        #Time to do ranger tests! 
        EndK=StartK:clone()
        rangerIgnoreWorld(0)
        rangerHitEntities(0)
        rangerHitWater(0)


        if(S<=Start:count()&!perf()){timer("start",20)}
	
        while(S<=Start:count()&perf()){
 
            Min=StartK[Start:minIndex(),number]
            Trace=rangerOffset(Self,Nodes[Min,array][1,vector]):hit()
            
            if(!Trace){
                CurNode=Min
                print("Start "+Min)
                break
            }else{
                StartK:remove(Start:minIndex())
                Start:remove(Start:minIndex())
            }
        S++
        }
 
        if(E<=End:count()&!perf()){timer("start",20)}
	       while(E<End:count()&perf()){
            Min=EndK[End:minIndex(),number]
           
            if(!rangerOffset(Goal,Nodes[Min,array][1,vector]):hit()){
                GoalNode=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. 
        
        if(!CurNode){print("Cannot locate a node within sight of the start position") Error=2}
        if(!GoalNode){print("Cannot locate a node within sight of the end position") Error=1}
        if(CurNode){
            
            StartNode=CurNode #This is used when reconstructing the path
            Open:pushNumber(CurNode)  
            #print("CurNode "+CurNode+" "+"GoalNode "+GoalNode)
            Depth=1 #This sets the search's starting depth
            Closed[CurNode,vector2]=vec2(CurNode,Depth)
            NodeList:pushNumber(CurNode)
        }
        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
        #print("CurNode "+CurNode)
        if(!CurNode){print("Error preforming search, open list depleted early.") Error=3 break}
        Depth=Closed[CurNode,vector2]:y() #Current Node's depth
        if(CurNode==GoalNode){
            #print("search complete")
            Completed=1
            SEARCHING=0
            Ready=0
            timer("Parse",20)
            NodeList=array()
            VectorList=array()
            NodeList:unshiftNumber(GoalNode)
            #VectorList:unshiftVector(Nodes[GoalNode,vector])
            VectorList:pushVector(Goal)
            break
            
        }else{
            Links=array()
            Links=Nodes[CurNode,array]
            #print("CurNode "+CurNode)
            #printTable(Links)
            for(C=3,Links:count()){
                LN=Links[C,number] 
                #print("LN "+LN)
                #print("closed "+LN+" "+Closed[CurNode,vector2]:toString())
                if(!Closed[LN,vector2]){ #keeps us from duplicating node info
                    Open:pushNumber(LN) #pushes a nodeID to be scanned
                    Closed[LN,vector2]=vec2(CurNode,Depth+1) #vec2(Parent,Depth of parent +1)
                    
                    
            
                }
            }
            #print("Open")
            #printTable(Open)
        }
    }
}
if(clk("Parse")){ 
    if(CurNode!=StartNode){timer("Parse",1)} #enable this when you got it working right!
    while(CurNode!=StartNode&perf()){
        if(CurNode){
            CurNode=Closed[CurNode,vector2]:x() #set the next node to the parent of the current
            Depth=Closed[CurNode,vector2]:y() #get the depth of the current node
            Links=array()
            Links=Nodes[CurNode,array]
            NodeList:unshiftNumber(CurNode)
            VectorList:unshiftVector(Nodes[CurNode,array][1,vector])     
        }
    }
    if(CurNode==StartNode){
        #NodeList:unshiftNumber(CurNode)
        #VectorList:unshiftVector(Nodes[CNS,vector])
        stoptimer("Parse")
        #print("SUCCESS! PATH FOUND!")
        #printTable(NodeList)
        Error=0
    }
    
}
