@name Commerse Bot
@inputs [CargoGrabber MoneyGrabber Sign]:wirelink
@outputs 
@persist State P Target:entity Fly Wait:vector Money Fast Beacon HoloBeacon
@persist Self
@trigger 
@outputs Search [Origin Destination]:vector P Count SearchC  
@outputs Money State Fly [XYZ Aim Wait]:vector Fast
@outputs VectorList:array StartNode EndNode CurNode Completed Error #the final path, some useful numbers
@persist Search [CurNode SearchC GoalNode StartNode]:number #some useful numbers
@persist [Vars Nodes]:table #Vars includes all internal varaibles, Nodes is the nodegraph itself
@model models/props_junk/PopCan01a.mdl
@trigger
O=owner()
G=entity():isConstrainedTo()
runOnTick(1)
runOnChat(1)

if(first()){XYZ=G:pos()}

function aimIt() {
    Aim=G:pos()+vec(100,0,0)
}


    
    
if(chatClk()&lastSpoke()==O){

    Chat=O:lastSaid()    
    Cmd="Cbot" #Robot's name
    Get="get"  #acquire cargo from location via path find
    Give="give" #drop off cargo (overrides asking price)
    Send="send" #plot path and deliver cargo with asking price
    Ask="ask" #asking price
    Brk1="$" #break character for asking price
    Brk="<" #break character for all else
    Recall="return" #path find to me. no dropping cargo
    Dropoff="dropoff" #deliver to node number or location. 
    Spot="spot" #identify the nearest node, from self or client. spot<name
    Release="release" #drops whatever it has
    Follow="follow" #follow someone around. 
    Off="off" #shut down flying mode
    Stay="stay" #cancels follow
    Take="take" #Get what someone else looks at
    
    function entity target(){
        return findPlayerByName(Chat:explode(Brk)[2,string])
    }
    
    function number ping(Target:entity){
        
        rangerHitEntities(0)
        return rangerOffset(G:pos(),Target:pos()):hit()

    }
    
    
    CMD=Chat:find(Cmd)    
    GET=Chat:find(Get)
    GIVE=Chat:find(Give)
    SEND=Chat:find(Send)
    ASK=Chat:find(Ask)
    BRK1=Chat:find(Brk1)
    BRK=Chat:find(Brk)
    RECALL=Chat:find(Recall)
    DROPOFF=Chat:find(Dropoff)
    SPOT=Chat:find(Spot)
    RELEASE=Chat:find(Release)
    FOLLOW=Chat:find(Follow)
    OFF=Chat:find(Off)
    STAY=Chat:find(Stay)
    TAKE=Chat:find(Take)
    
    if(CMD){
        if(RELEASE){
            CargoGrabber["Grab",number]=0
            MoneyGrabber["Grab",number]=0
        }
        if(SPOT){
            Origin=O:pos()
            if(BRK){Destination=target():pos()}else{Destination=Origin}
            print(target():name())
            Search=1
            timer("spotter",3000)
        }
        if(OFF){
            Fly=0
            State=0
        }
        if(STAY){
            XYZ=G:pos() 
            State=0
            Fly=1
        }
        if(FOLLOW){
        
            if(O:aimEntity()){Target=O:aimEntity()}
            if(ping(Target)){
                Origin=G:pos()
                if(BRK){
                    Destination=target():pos()+vec(0,0,80)            
                    Target=target()
                    State=-5
                    #Wait=target():pos()
                }else{
                    Target=O
                    State=6
                }
                
                Search=1
                P=1
                Fly=1
                Fast=0
            }else{
                State=6
                Fly=1
                Fast=0
                if(BRK){Target=target()}else{Target=O}   
            }
        }
        if(DROPOFF&BRK){
            Destination=Nodes[Chat:explode(Brk)[2,string]:toNumber(),vector]
            State=-3
            Origin=G:pos()
            Search=1
            Fly=1
            Fast=0
            P=1
        }
        if(BRK1&ASK){
            Money=Chat:explode(Brk1)[2,string]:toNumber()
            print("Cost is set to: "+Money)
        }
        if(GIVE&BRK){
            #Request money before dropping goods
            Origin=G:pos()
            Destination=target():pos()
            Wait=Destination
            Fly=1
            Fast=0
            P=1
            Search=1
            #if(Money){
             #   State=-2  
            #}#else{
                State=-3
            #}
        }
        if(RECALL){timer("return",1) Fast=0}
        
        
        if(SEND&BRK){
            if(Money){
                Origin=G:pos()
                Destination=target():pos()
                Wait=Destination
                Fly=1
                Fast=0
                Search=1
                P=1
                #if(Money){
                    State=-2
                #}else{
                  #  State=-3
                #}
            }else{
                print("Please declare an asking price first")
            }
        
        }
        
        if(GET|TAKE){
            if(GET){
                if(O:aimEntity()){Target=O:aimEntity()}
            }
            if(TAKE&BRK){
                if(target():aimEntity()){Target=target():aimEntity()}
            }
#            rangerHitEntities(0)
#            Ping=rangerOffset(G:pos(),Target:pos()):hit()
            Fast=1
            if(!ping(Target)){
                State=1
                Fly=1
                
                aimIt()
            }else{
                print("Attempting to find path to object")
                Origin=G:pos()
                Destination=Target:pos()
                Search=1
                State=-1
                Fly=1
                P=1 #What point of the path are you on? 
                timer("searchoff",1000)
            }
        }
        if(Chat=="path test"){
            State=-1
        }
    }
}
#using nodegraph to navigate
Count=VectorList:count()

if(clk("spotter")&Completed){
    print("Target's Node: "+EndNode)
    print("Your Node: "+StartNode)
    timer("spot",5000)
    HoloBeacon=EndNode
    Beacon=1
}
if(Beacon){
    if(!holoEntity(1)){
        holoCreate(1,Nodes[HoloBeacon,array][1,vector])
        holoEntity(1):soundPlay(1,0,"/vo/npc/male01/overhere01.wav")
    }
    if(clk("spot")){Beacon=0 holoDelete(1)}
    
}
#rangerFilter(G)
#if(rangerOffset(G:pos(),G:pos()+G:forward()*50)){
if(Search&!Completed){XYZ=G:pos()}
######   BEHAVIOR   ######
if((State<0)&VectorList:count()&Completed){ #Generic path following 
    if(!P){P=1}
    if(!XYZ){XYZ=G:pos()} #no flying off to origin if your lost!    
    XYZ=VectorList[P,vector]+vec(0,0,Target:radius()+60)
    if(XYZ:distance(G:pos())<20){P++}
    Aim=XYZ
 
    if(changed(P)){print("Reached Point "+P+"/"+VectorList:count())}
    if(XYZ==vec()){
        print("ERROR! Origin is destination")
        State=0
        Fly=0
    }
    
     ######  end path behavior change  #######
    if((P>VectorList:count())&(VectorList:count()>1)){
        print("Destination reached!")
        if(State==-1){State=1} #get object
        if(State==-2){State=5} #money involved
        if(State==-3){State=4} #no money involved
        if(State==-4){State=0} #Return home
        if(State==-5){State=6} #follow mode
        print("State Switched to: "+State)
    }
}
if(clk("searchoff")){Search=0} #Reset search flag
#### Dormant Behavior ####

if(!State){
    #CargoGrabber["Grab",number]=0
    #Fly=1
    aimIt()
}
#------ Collection an item behavior -------#
#move over target then move to state 2
if(State==1){
    if(Target==noentity()){State=3}
    XYZ=Target:massCenter()+vec(0,0,100)
    Wait=XYZ
    if(XYZ:distance(G:pos())<10){timer("State2",2000)}
    CargoGrabber["Grab",number]=0
    aimIt()
    
}
if(clk("State2")){State=2 timer("grabit",1000)}
if(clk("grabit")&State==2){State=1}

#move down to get object
if(State==2){
    XYZ=Target:pos()+vec(0,0,20)
    
    if(G:pos():distance(Target:pos())<40){
        CargoGrabber["Grab",number]=1
        CargoGrabber["Strength",number]=10000
    }
    aimIt()
    if(CargoGrabber["Grabbed Entity",entity]==Target){
        print("Cargo Acquired!")
        State=3
    }
}


#Once successfully collected, become still
if(State==3){
   Fly=1
    XYZ=Wait 
    if(CargoGrabber["Grabbed Entity",entity]==noentity()){
       CargoGrabber["Grab",number]=0 
    }
}

#-------End collection behavior-------

#drop it off while facing your target person
if(State==4){
    XYZ=Wait+vec(0,0,100)
    Aim=Target:shootPos()
    if(XYZ:distance(G:pos())<50){
        #CargoGrabber["Grab",number]=0
        State=0
        
        print("Cargo dropped")
    }
    
    
}

#deliver and ask for payment before dropping off
if(State==5){
    MoneyGrabber["String",string]="Please place: "+Money+"\non the beam to receive package."
    XYZ=Target:pos()+vec(0,0,80)
    #ifdef entity moneyAmount()
    if(MoneyGrabber["Grabbed Entity",entity]:moneyAmount()>=Money){
        MoneyGrabber["Grab",number]=1
        State=4
        MoneyGrabber["String",string]="Thank you"
        timer("return",3000)
        CargoGrabber["Grab",number]=0
    } #endif
    Aim=Target:shootPos()
}

#Follow around person, await further orders.
if(State==6){
    if(G:pos():distance(Target:pos())>200){
        XYZ=Target:pos()+vec(0,0,80)
    }else{
        XYZ=G:pos()
    }
    if(G:pos():distance(Target:pos())>500){
        Origin=G:pos()
        Destination=Target:pos()+vec(0,0,80)
        Search=1
        P=1
        State=-5
    }
}

if(clk("return")){
    Origin=G:pos()
    Destination=O:pos()
    Fly=1
    Search=1
    State=-4
}

#Fly=0
Ranger=0




if(Ranger){
    rangerFilter(G)
    Aim=rangerOffset(10000,O:pos(),O:eye()):position()
}else{
    #Aim=O:aimPos()
}
#aimIt()
if(!G&dupefinished()){selfDestruct()}
#G=Gun:entity()
Dir = G:pos()-(Aim)
Q = quat(Dir:toAngle())*qRotation(vec(0,0,1),180) #X
#Q = quat(Dir:toAngle())*qRotation(vec(0,1,0),-90) #Z
#Q = quat(Dir:toAngle())*qRotation(vec(0,0,1),90) #Y
#Q = quat(Dir:toAngle())*qRotation(vec(0,0,1),-90) #-Y
G:applyTorque((150*G:toLocal(rotationVector(Q/quat(G))+G:pos()) - 12*G:angVelVector())*G:inertia()*5)


if(Fly){
    if(Fast){MaxSpeed = 800}else{MaxSpeed=400}
    Speed = (XYZ-G:pos())/0.015
    if (Speed:length() > MaxSpeed) {Speed = Speed/Speed:length()*MaxSpeed}
    G:applyForce((Speed-G:vel())*G:mass())

}









#=====Pathfinder======
#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
#Error=4 No search made
#Error=5 No start or end node
#Error=6 Search in progress

if(first()){
    
    if(1){gSetStr("Nodes","")}
    Error=4
    Vars=table() #stores all our array variables
    Vars["SEARCHING",number]=0
    Vars["Filename",string]="nodegraphs/Nodegraph_5_"+map()+".txt"
    Vars["Offset",vector]=vec(0,0,10) #How high off the ground should a node be placed
    runOnFile(1)
    if(gGetStr("Nodes")){
        Nodes=glonDecodeTable(gGetStr("Nodes"))
        print("Ready!")
    }else{
    runOnFile(1)
    fileLoad(Vars["Filename",string])
    }
    
}
if(fileLoaded()&!Nodes:count()){
    Read=fileRead()
    gSetStr("Nodes",Read)
    Nodes=glonDecodeTable(Read)
    print("ops "+opcounter())
}

SearchC=changed(Search)
if(Nodes:count()&Search&(SearchC|~Search)){
    Vars["SEARCHING",number]=1 #trigger Search
    Vars["Begin",number]=1 #this only runs once when searching to prevent the goal from moving.
    Vars["Open",array]=array() #Numerical Node ID list of nodes to visit
    Vars["Closed",table]=table() #numerical table of node's visited
    Vars["Start",array]=array() #one use to find the starting podsition's nearest node
    Vars["StartK",array]=array() #saves node IDs for start array
    Vars["End",array]=array()   #one use to find the ending position's nearest node
    Vars["EndK",array]=array() #preserves the node IDs for end array
    Vars["NodeList",array]=array() #final path by nodes
    VectorList=array() #final path by vectors 
    timer("start",1)
    Search=0
    Vars["CurNode",number]=0
    Vars["Completed",number]=0
    Vars["Ready",number]=0
    Vars["Next",number]=0
    Vars["N",number]=1
    Vars["S",number]=1
    if(!Origin){Vars["Self",vector]=round(owner():pos())+Vars["Offset",vector]}else{Vars["Self",vector]=round(Origin+Vars["Offset",vector])}
    if(!Destination){Vars["Goal",vector]=round(owner():aimPos())+Vars["Offset",vector]}else{Vars["Goal",vector]=round(Destination+Vars["Offset",vector])}
    #print("Starting search")
    Error=6
}
if(Vars["SEARCHING",number]&(clk("next")|clk("start")|clk("continue"))){

    if(Vars["Begin",number]){
        #perpetuates the while statement if it's running "hot"
        if(Vars["N",number]<=Nodes:count()){
            timer("start",20)
        }else{
            Vars["Begin",number]=0 
            Vars["Next",number]=1
        }
        #Finds the nearest nodes to the start and your goal vectors
        while(Vars["N",number]<=Nodes:count()&perf()){
                #print("ops "+opcounter())
                Vars["V",vector]=Nodes[Vars["N",number],array][1,vector]
                #print("V "+V:toString())
                if(Vars["V",vector]){
                    Vars["Start",array]:pushNumber(round(Vars["V",vector]:distance(Vars["Self",vector])))
                    Vars["StartK",array]:pushNumber(Vars["N",number])
                    Vars["End",array]:pushNumber(round(Vars["V",vector]:distance(Vars["Goal",vector])))
                }
                Vars["N",number]=Vars["N",number]+1
        }  
    }
    if(Vars["Next",number]){
        #Time to do ranger tests! 
        Vars["EndK",array]=Vars["StartK",array]:clone()
        rangerIgnoreWorld(0)
        rangerHitEntities(0)
        rangerHitWater(0)


        if(Vars["S",number]<=Vars["Start",array]:count()&!perf()){timer("start",20)}

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

