@name Node Graph Test2 (Older)
@inputs 
@outputs Path:array OpenN:table
@outputs HE HEP M1:vector4 M2:vector4 NS:string NSA:string Nodes:table Holo:array Closed:table
@persist Nodes:table 
@persist Record Link AutoLink Linkup Link1 Link2 SAVE LOAD HE HEP Hold SCANNING 
@persist Red:vector Blue:vector White:vector Green:vector 
@persist Off:vector Size:vector
@persist Closed:table NS:string Filename:string Self:vector Goal:vector
@persist CurNode GoalDist Node
#@persist Head1:string Head23:string Head4:string
@trigger 
interval(100)
runOnChat(1)

Chat=owner():lastSaid() #owner chat

if(chatClk()|first()){
    #valid word list
    Com="Path"
    Rec="rec"
    On1="on"
    On0="off"
    Auto="auto"
    Find="find"
    Save="save"
    Load="load"
    Scan="scan"
    Help="help"
    Help1="HELP"
    Break="<"
    Search="search"
    S=" "
    
    
    #State change reports
    Greeting="AI Pathfinding Chip Online \n"+"For Help type: "+Com+S+Help
    Vis="Node link visualizer: "
    RecMode="Record Mode: "
    Automatic="Experimental Automatic Node Linker: "
    Loading="Loading Node Graph from "
    Saving="Saving Node Graph to "
    
    #Communication Checks
    COM=Chat:find(Com)
    REC=Chat:find(Rec)
    ON1=Chat:find(On1)
    ON0=Chat:find(On0)
    AUTO=Chat:find(Auto)
    FIND=Chat:find(Find)
    SAVE=Chat:find(Save)
    LOAD=Chat:find(Load)
    BRK=Chat:find(Break)
    SCAN=Chat:find(Scan)
    HELP1=Chat:find(Help1)
    HELP=Chat:find(Help)
    SEARCH=Chat:find(Search)

}
O=owner()

if(first()){
    #Glon array search variables
#    Head1="" #Scan array for first index Head1+Num
#    Head4="" #Scan array for 4th index Num+Head4
#    Head23="" #Scan array for 2nd or 3rd index Head23+Num Cannot pick out which
    #Timers
    timer(Scan,10)
    timer("record",10)
    Nodes=table()
    
    Red=vec(255,0,0)
    Blue=vec(0,0,255)
    White=vec(255,255,255)
    Green=vec(0,255,0)
    Off=vec(0,0,20)  #Offset from where you look to where a node is
    H=0.5 #Half
    Size=vec(H,H,H)
    
    #Nodegraph file name
    Filename="NodeGraph__"+map()+".txt"
    
    #prints a greeting when chip is placed
    print(Greeting)
}



#communication "if tree"
if(chatClk()&COM){
    if(REC){
        if(HELP1){
            print(Com+S+Rec+S+On1+" - this enables node recording.")   
            print(Com+S+Rec+S+On0+" - this disables node recording.")   
            print("How to use the manual norde recorder: ")
            print("A1=attack1 \nA2=attack2 Noclip=Must be noclipped\nUse=hold use key")
            print("Cannon=Must hold gravity gun \nPhys=must be holding physgun")
            print("Crouch=must be crouched \nHE=the node you are looking at \nAimEnt=the entity your looking at.")
            print("\n")
            print("AddNodeEmpty=Cannon&A1&Use&A2")
            print("AddNode=Cannon&A2&Use&!A1 \nAddLink=Phys&A1&Use&HE \nSubLink=Phys&A2&Use&HE \nSubNode=Cannon&A1&Use&!A2")
            print("MoveNode=Cannon&HE&A2&A1&Use \nDropNode=Cannon&A2&A1&!Use \nUndoLink=!AimEnt&!HE&(A1|A2)&Use&Phys")
        }
        if(ON1){
            Record=1 
            SCANNING=1
            timer(Scan,1)
            timer(Rec,1) 
            print(RecMode+On1)

        }
        if(ON0){
            Record=0 
            stoptimer(Rec) 
            print(RecMode+On0)
        }
    }
    if(SEARCH){
        if(HELP1){
            print("The path output will be the Path:array output on the chip once the search is completed. \n Just go to each point in sequence to reach your target. \n Node graph required.")
        }
        print("Locating fastest path between given points.") 
        SEARCHING=1
        Begin=1
    }
    if(SAVE){
        
        print(Saving+Filename)
        SAVING=1
    }
    if(LOAD){
        print(Loading+Filename)
        LOADING=1
    }
    if(AUTO){
        if(HELP1){print("Nothing to it, just run this and the nodes will get linked up. \nFunction is experimental.")}
        AutoLink=1        
    }
    if(FIND){
        if(HELP1){ 
            print("where you look sets the end point of the path search.")
  
        }
        
        Self=round(owner():pos()) #where ever you are
        Goal=round(owner():aimPos()) #where ever your seeking
        print("Goal set to: "+Goal+" Self set to: "+Self)
    }
        
        #set the point your looking at to be the end node and your location to be the start node
    
    if(SCAN){
        if(ON1){
            SCANNING=1 
            timer(Scan,1)
            print(Vis+On1)
        }
        if(ON0){
            SCANNING=0
            stoptimer(Scan)
            print(Vis+On0)
        }
    }
    if(HELP){
        print(Com+" - This is the chip's command word, you must say this every time you talk to the chip.")
        print(On1+" - This sets toggled functions to the on state.")
        print(On0+" - This sets toggled functions to the off state.")
        print(Rec+" - Toggles the manual node recording mode.")
        print(Auto+" - Triggers the auto linking function.")
        print(Find+" - Sets the end point to the path search, your location is the start point.")
        print(Load+" - Loads the node graph for the current map from data/e2files.")
        print(Save+" - Saves the node graph you made for the current map to data/e2files.")
        print(Search+" - Triggers the path finding function to find the fastest path between 2 points.")
        print(Help+" - This list or add "+Help1+" to with any function to get instructions on it's use.") 
    }
    
}


#this is used to tell the below function what you LOOKED at last
if(clk("wait")){
    HEP=HE #HE= Holo Entity  HEP= Holo Entity Previous
}
#This paints the node your looking at and all linked nodes
#step slowly across the holograms or the colors stick
if(clk(Scan)&SCANNING){
    timer("wait",30)
    timer(Scan,10)
    findInCone(O:shootPos(),O:eye(),500,5)
    #findIncludeClass("gmod_wire_hologram")
    Holo=findToArray()
    HC=Holo:count()
    HE=holoIndex(Holo[1,entity])
    if(HE){
        HES=toString(HE)
        holoColor(HE,Blue)
        NS=toString(HE)
        Links=vec4()
        Links=Nodes[HES,matrix4]:row(1)
        for(C=1,4){
            L1=Nodes[HES,matrix4]:column(C):x()
            L1s=toString(L1)
            holoColor(L1,Red)
            L2=Nodes[HES,matrix4]:column(C):z()
            L2s=toString(L2)
            holoColor(L2,Red)
            D1=Nodes[HES,matrix4]:column(C):y()
            D2=Nodes[toString(HE),matrix4]:column(C):z()
            D1a=round(Nodes[L1s,vector]:distance(Nodes[HES,vector]))
            D2a=round(Nodes[L2s,vector]:distance(Nodes[HES,vector]))
            if(D1!=D1a){Nodes[HES,matrix4]:column(C):setY(D1a)}
            if(D2!=D2a){Nodes[HES,matrix4]:column(C):setW(D2a)}
        }
    }elseif(HEP){
        holoColor(HEP,White)
    
        for(C=1,4){
            L=Nodes[toString(HEP),matrix4]:column(C):x()
            holoColor(L,White)
        }
    }
}
#Manual Node Recorder Control code
if(clk(Rec)&Record){
    timer(Rec,1000)
 
    Wep=O:weapon():type()
    Crouch=O:isCrouch()
    Noclip=O:inNoclip()
    A1=O:keyAttack1()
    A2=O:keyAttack2()
    Use=O:keyUse()
    AimEnt=O:aimEntity()
    AimPos=O:aimPos()
    Cannon=Wep:find("physcannon")
    Phys=Wep:find("physgun")
    #this defines the conditions to trigger the events.
    AddNodeEmpty=Cannon&Use&A1&A2
    AddNode=Cannon&A2&Use&!A1
    AddLink=Phys&A1&Use&HE
    SubLink=Phys&A2&Use&HE
    SubNode=Cannon&A1&Use&!A2
    MoveNode=Cannon&HE&A2&A1&Use
    DropNode=Cannon&A2&A1&!Use
    UndoLink=!AimEnt&!HE&(A1|A2)&Use&Phys
    MoveNode=HE&Cannon&A2
    DropNode=Hold&Cannon&A1
    if(AddNode){
        Node=1
        NSA=toString(Nodes:count()) #NSA is Node Sting A
        NS=toString(round(Nodes:count()/2)+1) #NS is Node String
        Position=AimPos+Off
        print("Node "+NS+" added!")
        
    }
    if(SubNode){
        Node=1
        NSA=toString(Nodes:count())
        NS=toString(round(Nodes:count()/2))
        Position=vec()
    }
    if(MoveNode){
        Hold=HE
        print("Node "+NS+" is being moved.")
        
    }
    if(DropNode){
        Drop=1
        Position=AimPos+Off
        print("Node "+Hold+" has been placed.")
    }
    if(AddNodeEmpty){
        Node=1
        NSA=toString(Nodes:count()) #NSA is Node Sting A
        NS=toString(round(Nodes:count()/2)+1) #NS is Node String
        Position=AimPos+Off
        print("Node "+NS+" added!")
        
    }
    if(SubLink&!Link1){
        Unlink=0
        Link1=HE
        print("First subtraction link selected: "+Link1)
    }
    if(SubLink&Link1&Link1!=HE){
        Unlink=1
        Link2=HE
        print("Link2="+Link2+" subtracted from Link1="+Link1)
    }
    
    if(AddLink&!Link1){
        Linkup=0
        Link1=HE
        print("Link1="+Link1)
    }
    if(AddLink&Link1&(Link1!=HE)){
        Linkup=1
        Link2=HE
        print("Link2="+Link2+" Added to Link1="+Link1)
    }
    if(UndoLink){
        Link1=0
        Link2=0        
    }
    if(MoveNode){
     Move=HE
    print("Moving Node: "+Move)   
    }
    if(DropNode){
        Node=1
        holoPos(Move,AimPos+Off)
        Position=AimPos+Off
        NS=toString(Move)
    }
}

#This creates a new nodes with only a link stumb or auto links to previous node
if(Node){
    M=Nodes[NS,matrix4] 
    V1=vec4()
    V2=vec4(-1,0,0,0) #prepares for a blank matrix4
    NP=NS:toNumber()-1 #Node Previous
    Dist=Nodes[NS,vector]:distance(Nodes[toString(NP),vector])
    Nodes[NS,vector]=Position
    if(!Nodes[NS,matrix4]&Position){Nodes[NS,matrix4]=matrix4(V1,V2,V1,V1)}
    Nodes[NS,matrix4]=M
    if(!AddNodeEmpty){Nodes[NS,matrix4]=M:setColumn(1,NP,Dist,0,0)} #links node to previous node
    Node=0
    holoCreate(NS:toNumber(),round(Position),Size)
    
}

#For picking up and moving nodes
if(Drop){
    NS=toString(Hold)
    Nodes[NS,vector]=round(Position)
    holoPos(Hold,round(Position))
    Drop=0
}

#Auto links the 8 nearest nodes with a direct line of sight to each node
#Optimization needed
if(AutoLink){
LinkCue=array()
    rangerFlags("Z")
    for(C=1,Nodes:count()/2){
        CS=toString(C)
        CN=Nodes[CS,vector]
            for(D=1,Nodes:count()/2){
                DS=toString(D)
                DN=Nodes[DS,vector]
                Dist=CN:distance(DN)
                if(!rangerOffset(CN,DN):hit()){
                    LinkCue:pushVector(vec(C,D,Dist))                    
                    timer("linkup",100)
                }
            }
    }
AutoLink=0
}

#part of Linkup
if(LinkCue:count()&clk("linkup")){
    timer("linkup",10)
    Vec=LinkCue:shiftVector()
    Link1=Vec:x()
    Link2=Vec:y()
    Dist=Vec:z()
}

#Subbordinate function to add links to nodes
if(Linkup|clk("linkup")){
 
    Link=Link2 
    NS=toString(Link1)
    M=Nodes[NS,matrix4]
    M1=Nodes[NS,matrix4]:row(1)
    M2=Nodes[NS,matrix4]:row(3)
    L=0
    if(!M2:w()&!M2:w()==Link){L=8 D=4 L2=Link D2=Dist}
    if(!M2:z()&!M2:z()==Link){L=7 D=3 L2=Link D2=Dist}
    if(!M2:y()&!M2:y()==Link){L=6 D=2 L2=Link D2=Dist}
    if(!M2:x()&!M2:x()==Link){L=5 D=1 L2=Link D2=Dist}
    if(!M1:w()&!M1:w()==Link){L=4 D=4 L1=Link D1=Dist}
    if(!M1:z()&!M1:z()==Link){L=3 D=3 L1=Link D1=Dist}
    if(!M1:y()&!M1:y()==Link){L=2 D=2 L1=Link D1=Dist}
    if(!M1:x()&!M1:x()==Link){L=1 D=1 L1=Link D1=Dist}
    if(L){
        print("Connection: "+L+" has been made to node: "+NS)
        Nodes[NS,matrix4]=M:setColumn(D,L1,D1,L2,D2)
  
    }
    Link1=0
    Link2=0
    Linkup=0

}


#Subbordinate function to remove links from nodes
if(Unlink|clk("unlink")){
 
    #Link=Link2 
    NS=toString(Link1)
    M=Nodes[NS,matrix4]
    M1=Nodes[NS,matrix4]:row(1)
    M2=Nodes[NS,matrix4]:row(3)
    L=0
    if(M2:w()==Link2){L=8 D=4 L2=0 D2=0}
    if(M2:z()==Link2){L=7 D=3 L2=0 D2=0}
    if(M2:y()==Link2){L=6 D=2 L2=0 D2=0}
    if(M2:x()==Link2){L=5 D=1 L2=0 D2=0}
    if(M1:w()==Link2){L=4 D=4 L1=0 D1=0}
    if(M1:z()==Link2){L=3 D=3 L1=0 D1=0}
    if(M1:y()==Link2){L=2 D=2 L1=0 D1=0}
    if(M1:x()==Link2){L=1 D=1 L1=0 D1=0}
    if(L){
        print("Connection: "+L+" has been removed from node: "+NS)
        Nodes[NS,matrix4]=M:setColumn(D,L1,D1,L2,D2)
    }
    Link1=0
    Link2=0
    UnLink=0
}


if(SAVING){
    fileWrite(Filename,glonEncode(Nodes))
    Count=round(Nodes:count()/2)
    print("Node Graph of "+Count+" Saved for "+map())
}
if(LOADING){
    if(fileRead(Filename)){
        Nodes=glonDecodeTable(fileRead(Filename))
        timer("showholo",1)
        Count=round(Nodes:count()/2)
        print("Node graph loaded with "+Count+" nodes Loaded!")
    }else{
        timer("showholo",10000)
        fileLoad(Filename)
    }
}
if(clk("showholo")){
    if(!Nodes){fileRead(Filename)}
    Count=round(Nodes:count()/2)
    for(C=1,Count){
        CS=toString(C)
        holoCreate(C,Nodes[CS,vector],Size)
    }
}
#A* pathfinding
#Search=0

MinDistance=50 #minimum distance to target
if(SEARCHING){
    if(Begin){
        print("search in progress")
        #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
        OpenN=table() #vec4(Node,Parent,H,G)
        Farray=array() #number, How to find the lowest F value
        Closed=table() #vec4(Node,Parent,H,G)
        Start=array() #one use to find the starting position's nearest node
        End=array()   #one use to find the ending position's nearest node
        FNodes=array() #vec2(Fcost,NodeID)
        Count=round(Nodes:count()/2)
        #finds the nearest nodes to the "you" and your "goal".
        for(C=1,Count){
            CS=toString(C)
            NCS=Nodes[CS,vector]
            rangerFlags("")
            if(rangerOffset(NCS,Self):hit()){Start:pushNumber(round(NCS:distance(Self)))}
            if(rangerOffset(NCS,Goal):hit()){End:pushNumber(round(NCS:distance(Goal)))}
    }
        
        #Node numbers for the nearest nodes to self and goal coordinates. 
        StartNode=Start:minIndex()
#        print("Starting table")
#        printTable(Start)
#        print("Ending table")
#        printTable(End)
        #CurNode=StartNode
        GoalNode=End:minIndex()
        Node=StartNode
        Parent=0
        G=round(Nodes[toString(StartNode),vector]:distance(Self))
        H=round(Nodes[toString(StartNode),vector]:distance(Goal))
        OpenN[toString(Node),vector]=vec(Parent,H,G)
        OpenN[toString(GoalNode),vector]=vec(-1,0,-1)
        FNodes[1,vector2]=vec2(H+G,StartNode)
        GoalDist=round(Self:distance(Goal))
        
        print("start: "+StartNode+"\nEnd: "+GoalNode+"\n"+"GoalDist="+GoalDist)
        
        Begin=0
        timer("search",100)
    }
    while(GoalDist>MinDistance&perf()){
        print("while started")
        for(C=1,FNodes:count()){
            Farray[C,number]=FNodes[C,vector2]:x()
        }
        CurNode=FNodes[Farray:minIndex(),vector2]:y()
        print("Farray completed: "+CurNode)
        print("FNodes:count() "+FNodes:count())
        printTable(Farray)
        if(CurNode==GoalNode){SEARCHING=0 print("Break met") break}
        else{
            print("continuing past break")
            CNS=toString(CurNode)       
            Closed[CNS,vector]=OpenN[CNS,vector]
            OpenN[CNS,vector]=vec()
            Linx=array()
            DistA=array()
            
            #Extracts link info from current node
            for(C=1,4){
                print("Pulling row "+C+" data from matrix")
                CS=toString(C)
                Vec4=Nodes[CS,matrix4]:column(C)
                Linx[C,number]=Vec4:x()
                Linx[C+4,number]=Vec4:z()
                DistA[C,number]=Vec4:y()
                DistA[C+4,number]=Vec4:w()
            }
            print("matrix scan completed")
            #Scan links to see if any are in the closed list
            for(C=1,Linx:count()){
                print("checking node links")
                CS=toString(C)  #count string
                LS=toString(Linx[C,number]) #Links String
                CV=Closed[LS,vector] #closed vector
                OV=OpenN[LS,vector] #open Node vector
                if(CV&(CV:z()<Closed[CNS,vector]:z())){
                    print("closed is smaller")
                    G=DistA[C,number]+Closed[CNS,vector]:z()
                    if(!CV:y()){H=Nodes[LS,vector]:distance(Goal)}
                    else{H=CV:y()}
                    CV=vec(CurNode,H,G)
                    FNodes:pushVector2(vec2(G+H,LS:toNumber()))
                    
                }elseif(OV&(OV:z()<Closed[CNS,vector]:z())){
                    print("Open is smaller")
                    G=DistA[C,number]+Closed[CNS,vector]:z()
                    if(!OV:y()){H=Nodes[LS,vector]:distance(Goal)}
                    else{H=OV:y()}
                    OV=vec(CurNode,H,G)
                    FNodes:pushVector2(vec2(G+H,LS:toNumber()))
                }else{
                    print("closed s4")
                    H=Nodes[LS,vector]:distance(Goal)
                    G=DistA[C,number]+Closed[CNS,vector]:z()
                    OpenN[LS,vector]=vec(CurNode,H,G)
                    FNodes:pushVector2(vec2(G+H,LS:toNumber()))
                }#end if ifelse else scan
            }#end of for scan
        }#end of else
        print("while finished")
        timer("search",1)
    }#End of while
    print("closed table")
    printTable(Closed)
    print("open table")
    printTable(OpenN)
}#End of search

#Converting Closed:table to Path:array
if(clk("path")){
    Path=array()
    #Path format = X,Y,Z,NodeID
    #Closed format = Parent,H,G  All we want is the parent
    Path[1,vector4]=vec4(Nodes[toString(GoalNode),vector],GoalNode)
    for(C=1,Closed:count()){
        NS=toString(Path[C,vector4]:w())
        Parent=Closed[NS,vector]:x()
        Path:unshiftVector4(vec4(Nodes[NS,vector],NS:toNumber()))
    }
}
