#@name gait system
@inputs 
@outputs GaitVel
@persist Gaits:table GaitCycle GaitVel ScaleFactor Entity:entity
@persist [EntVel EntPos EntUp]:vector EntVelLen AddVel
@trigger none

if (first()) {
    Entity = entity()
    ScaleFactor = 1

    function table addGait(Name:string, Offset:vector, LegLength) {
        local LegLenth = LegLength * ScaleFactor
        local Offset = Offset * ScaleFactor
        local GStartPos = rangerOffset(500, Entity:toWorld(Offset), vec(0, 0, -1)):position()
        
        Gaits[Name, table] = table(
            "Offset" = Offset,
            "Prev" = GStartPos,
            "Dest" = GStartPos,
            "Pos" = GStartPos,
            "IsMoving" = 0,
            "ShouldMove" = 0,
            "Height" = 0,
            "LegLength" = LegLength
        )
        
        return Gaits[Name, table]
    }
    
    function setGait(Name:string, Start, Length) {
        local Start = Start - floor(Start)
        Gaits[Name, table]["GStart", number] = Start
        Gaits[Name, table]["GStop", number] = Start + Length    
    }
    
    function runGaitSequence() {
        foreach (K, Gait:table = Gaits) {
            local GFract = 0
            local GStart = Gait["GStart", number]
            local GStop = Gait["GStop", number]
            local GMove = 0
            
            if (GaitCycle >= GStart & GaitCycle <= GStop) {
                GFract = (GaitCycle - GStart) / (GStop - GStart)
                GMove = 1    
            }
            
            if (GStart < 0) {
                if (GaitCycle >= abs(GStart) & GaitCycle <= 1) {
                    GFract = (GaitCycle - GStart) / (GStop - GStart)
                    GMove = 1
                }
            } elseif (GStop > 1) {
                if (GaitCycle + 1 >= GStart & GaitCycle + 1 <= GStop) {
                    GFract = (GaitCycle + 1 - GStart) / (GStop - GStart)
                    GMove = 1
                }            
            }
            
            local PreTrace = rangerOffset(EntPos, Entity:toWorld(Gait["Offset", vector])):position()
            local MidTrace = rangerOffset(PreTrace, PreTrace + EntVel/AddVel)
            local EndTrace = rangerOffset(Gait["LegLength", number]*2, MidTrace:position(), -EntUp)
            local UseTrace = MidTrace:hit() ? MidTrace : EndTrace
            
            if (GMove) {
                Gait["Dest", vector] = UseTrace:position()
                
                if (!Gait["IsMoving", number]) {
                    Gait["Prev", vector] = Gait["Pos", vector]
                    
                    if (Gait["Pos", vector]:distance(UseTrace:position()) >= 4) {
                        local Dot = 0
                        if (UseTrace:hit()) { 
                            Dot = 1 - EntUp:dot(UseTrace:hitNormal()) 
                        }
                        
                        local Dist = UseTrace:distance()
                        local Thresh = Gait["LegLength", number] + Dot*100*clamp(EntVelLen/100, 0, Gait["LegLength", number]/2)
                        
                        if (Dist <= Thresh) {
                            Gait["ShouldMove", number] = 1
                        }
                    }    
                }
                
                if (Gait["ShouldMove", number]) {
                    local Prev = Gait["Prev", vector]
                    local Dest = UseTrace:position()
                    local Mid = (Dest + Prev)/2 + UseTrace:hitNormal()*clamp(Prev:distance(Dest)/4, 0, Gait["LegLength", number]/2)
                    
                    local Bezier = bezier(Prev, Mid, Dest, GFract)
                    
                    Gait["Pos", vector] = Bezier
                    Gait["Height", number] = Bezier:z() - Dest:z()
                    Gait["LastMove", number] = 1
                } else {
                    Gait["Height", number] = 0
                }
            } 
            else {
                if (Gait["LastMove", number]) {
                    Gait["Pos", vector] = Gait["Dest", vector]
                    Gait["LastMove", number] = 0
                }
                Gait["ShouldMove", number] = 0
                Gait["Height", number] = 0
            }
            
            Gait["IsMoving", number] = GMove ? Gait["ShouldMove",  number] : 0
        }
        if (GaitCycle > 1) { GaitCycle = 0 }
    }
    
    function number lerp( N0, N1, T ) {
        return N0*T + N1*(1-T)
    }
    
    addGait("FL", vec(0, 30, 0), 50 + 50)
    addGait("FR", vec(0, -30, 0), 50 + 50)
    addGait("RL", vec(-30, 30, 0), 50 + 50)
    addGait("RR", vec(-30, -30, 0), 50 + 50)
    
    holoCreate(1)
    holoCreate(2)
    holoCreate(3)
    holoCreate(4)
}

interval(60)

EntUp = Entity:up()
EntPos = Entity:pos()
EntVel = Entity:vel()
EntVelLen = EntVel:length()

AddVel = 6
GaitVel = lerp(GaitVel, EntVelLen, 0.1)

local Mul = GaitVel/100
GaitCycle = GaitCycle + (0.05 + 0.03*Mul)
local GaitSize = clamp(0.4 + 0.03*Mul, 0, 0.9)

setGait("FL", 0, GaitSize)
setGait("FR", 0.5, GaitSize)
setGait("RL", 0.75, GaitSize)
setGait("RR", 0.25, GaitSize)
runGaitSequence()


holoPos(1, Gaits["FL", table]["Pos", vector])
holoPos(2, Gaits["FR", table]["Pos", vector])
holoPos(3, Gaits["RL", table]["Pos", vector])
holoPos(4, Gaits["RR", table]["Pos", vector])


