@name Cronoid pathfinder mk 3
@inputs [GPS1 GPS2]:vector
@outputs Path:array [Dest Point Start]:vector Comp

@persist Deg P Post SC Cla
@persist File:string
@persist [Point V Hull]:vector
@persist [Open Scores S Vecs PP]:array
@persist [Closed Parents Pop Shorts]:table

if(first()){
        #Scale value of the pathfinder's grid
    Deg=32
    Hull=vec(1,1,1)*Deg
    
        #Degrees of movement
    #Vecs:pushVector(vec(1,-1,1))
    Vecs:pushVector(vec(1,0,1))
    #Vecs:pushVector(vec(1,1,1))
    #Vecs:pushVector(vec(1,-1,0))
    Vecs:pushVector(vec(1,0,0))
    #Vecs:pushVector(vec(1,1,0))
    #Vecs:pushVector(vec(1,-1,-1))
    Vecs:pushVector(vec(1,0,-1))
    #Vecs:pushVector(vec(1,1,-1))
    Vecs:pushVector(vec(0,-1,1))
    Vecs:pushVector(vec(0,0,1))
    Vecs:pushVector(vec(0,1,1))
    Vecs:pushVector(vec(0,-1,0))
    Vecs:pushVector(vec(0,1,0))
    Vecs:pushVector(vec(0,-1,-1))
    Vecs:pushVector(vec(0,0,-1))
    Vecs:pushVector(vec(0,1,-1))
    #Vecs:pushVector(vec(-1,-1,1))
    Vecs:pushVector(vec(-1,0,1))
    #Vecs:pushVector(vec(-1,1,1))
    Vecs:pushVector(vec(-1,-1,0))
    Vecs:pushVector(vec(-1,0,0))
    Vecs:pushVector(vec(-1,1,0))
    #Vecs:pushVector(vec(-1,-1,-1))
    Vecs:pushVector(vec(-1,0,-1))
    #Vecs:pushVector(vec(-1,1,-1))
        #Because of automatic throttling, this array can be
        #expanded to much greater ranges of movement, but doing so
        #can slow the chip down considerably as it searches through
        #each vector for each individual Point.
    
        #Map shortcut loading
    File="cpath-"+map()
    fileLoad(File)
    
        #Ranger settings
    rangerPersist(1)
    rangerFilter(entity():getConstraints())
    rangerFilter(owner())
    
        #Start and Destination points rounded in to Deg grid
            #Start
    Start=round(GPS2/Hull)*Hull
    
        #Start added to the Open and Scores lists as the earliest
        #vector point to check
    Open[1,vector]=Start
    Scores[1,number]=1
    Pop[Start:toString(),number]=1
    Point=Start
    
        #Destination indicator holo
    holoCreate(1,Dest)
    holoModel(1,"hqcone")
    holoScaleUnits(1,Hull)
    
    Cla=50
    
    runOnTick(1)
}
if(!SC & fileLoaded(File)){
    if(fileRead(File)){
        S=glonDecode(fileRead(File))
        
        while(S[1,vector] & S[2,vector]){
            V1=S[1,vector]
            V2=S[2,vector]
            
            V1=round(V1/Hull)*Hull
            V2=round(V2/Hull)*Hull
            
            Shorts[V1:toString(),vector]=V2
            Shorts[V2:toString(),vector]=V1
            
            S:remove(2)
            S:remove(1)
        }
    }
    SC=1
}
elseif(!Comp){
            #Destination
                Off=GPS1-Start
        Dest=round(GPS1/Hull)*Hull
        holoPos(1,Dest)
    
        Des=Point:distance(Dest)
    Sight=rangerOffset(Point,Dest):distance() == Des
    Sight=Sight & Des <= Deg
    
    if(Open:count() & !Sight){
        while(perf()){
            #New Point is the vector in Open with the lowest value 
            #in Scores (K)
        K=Scores:minIndex()
        Point=Open[K,vector]
        
        if(!Closed[Point:toString(),number]){
            while(perf()){
                P++
                    An=(Dest-Point):toAngle():setPitch(0)
                Ba=Vecs[P,vector]
                V=Point+Ba*Hull
                
                if(!Closed[V:toString(),number]){
                        #Access check
                            #Can V be reached from Point?
                            #(R:hit()==0)
                    HR=rangerOffset(Point,V)
                    R=!HR:hit()
                    
                        #Drop check
                            #Is there a dangerous drop below V?
                    A=rangerOffset(500,V,vec(0,0,-1)):distance()
                    A=(A<=400)
                    
                        #Jump check
                            #If V is above Point, is there a step?
                    J=rangerOffset(Deg,V,vec(0,0,-1)):hit()
                    J=(V:z()>Point:z() ? J : 1)
                    
                    if(R & A & J){
                            #Movement cost from Point to V
                        G = Deg
                        
                            #Heuristic, guess of distance to Dest 
                            #from V
                        H = V:distance(Dest)
                        
                            #Altitude change, distance vertical 
                            #from Point to V
                        A = Ba:z()
                        
                            #F value, the Score
                        F = G + H + A
                        
                        if(!Pop[V:toString(),number]){
                            Str=V:toString()
                            
                                #Add V to the Open list
                            Open:pushVector(V)
                            Pop[Str,number]=1
                                #Add V's F score to the list
                                #so we can determine it's priority later
                            Scores[Open:count(),number]=F
                            
                            Parents[Str,vector]=Point
                                #Keeping track of Parents lets us trace
                                #the route when it completes
                            
                            #Shortcut transmission
                            if(Shorts[Str,vector]){
                                Sis=Shorts[Str,vector]
                                
                                Open:pushVector(Sis)
                                Scores[Open:count(),number]=F
                                Parents[Sis:toString(),vector]=V
                            }
                        }
                    }
                    else{
                            #If it doesn't meet those criteria, 
                            #V is blacklisted and added to the
                            #Closed list
                        Closed[V:toString(),number]=1
                    }
                }
            }
        }
        if(P>=Vecs:count()|Closed[Point:toString(),number]){
            P=0
            
                #Since we have now checked Point and it's 
                #surroundings, Point is added to the Closed list 
                #(to prevent repeat checks) and removed from the 
                #Open & Scores lists
            Closed[Point:toString(),number]=1
            Open:remove(K)
            Pop[Point:toString(),number]=0
            Scores:remove(K)
        }
        }
    }
}

        #Path extrapolation and post-processing
if(Sight & !Comp){
    if(!Path:count()){
        V=Point
        Post=1
    }
        #Create full point route from Parents list
    if(Post==1){
        while(perf() & Path[1,vector]:distance(Start)>Deg){
            V=Parents[V:toString(),vector]
            
            Path:insertVector(1,V)
        }
        if(Path[1,vector]:distance(Start)<=Deg){
            Post++
        }
    }
        #Remove unnecessary points
    elseif(Post==2){
            #Create an array of the vector directions
        if(!PP:count()){
            for(N=1,Path:count()){
                if(!perf()){break}
                
                V=Path[N,vector]
                V2=Path[N-1,vector]
                Re=V-V2
                PP:pushVector(Re)
            }
        }
            #Remove all points whose directions do not change
            #from the previous point
        else{
            for(N=3,PP:count()){
                if(!perf()){break}
                
                V=PP[N,vector]
                V2=PP[N-1,vector]
                
                if(V==V2){
                    PP:remove(N-1)
                    Path:remove(N-1)
                    
                    Ch=1
                }
            }
            if(!Ch){Post++}
        }
    }
        #Average out points within Deg of eachother
        #This eliminates zigzagging
    elseif(Post==3){
        PP=Path:clone()
        for(N=2,PP:count()){
            V1=PP[N-1,vector]
            V2=PP[N,vector]
            
            Dis=V1:distance(V2)
            
            A=Dis-(Deg/2)
            B=Dis+(Deg/2)
            
            if(inrange(Deg,A,B)){
                Av=V1
                Av+=V2
                Av/=2
                
                Path[N-1,vector]=Av
                Path:remove(N)
                
                PP:remove(N-1)
                
                Ch=1
            }
        }
        if(!Ch){Post++}
    }
        #Indicate path and remove excess hologram
    else{
        Comp=1
        
        for(N=1,Path:count()){
            if(!holoEntity(N+1)){
                holoCreate(N+1,Path[N,vector])
                holoScaleUnits(N+1,Hull)
            }
            else{
                holoPos(N+1,Path[N,vector])
            }
            holoColor(N+1,vec(0,1,0)*255,255)
        }
        for(M=1,Cla-N+1){
            holoDelete(N+M+1)
        }
    }
}
if($GPS1|$GPS2){
    DV=round(GPS1/Hull)*Hull
    SV=round(GPS2/Hull)*Hull
    
    if(DV!=Dest|SV!=Start){
        holoDeleteAll()
        Start=round(GPS2/Hull)*Hull
        
        Closed=table()
        Parents=table()
        Pop=table()
        
        Path=array()
        Open=array()
        Scores=array()
        
        Open[1,vector]=Start
        Scores[1,number]=1
        Pop[Start:toString(),number]=1
        Point=Start
        
            #Destination indicator holo
        holoCreate(1,Dest)
        holoModel(1,"hqcone")
        holoScaleUnits(1,Hull)
        
        Comp=0
    }
}