#@name gait system 2
@inputs 
@outputs GaitRate
@persist Gaits:table Entity:entity 
@trigger none

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

    function gaitAdd( ID:string, Offset:vector, Height ) {
        local Temp = rangerOffset(Height, Entity:toWorld(Offset), -Entity:up()):pos()
        Gaits[ID, table] = table(
            "Offset" = Offset,
            "Height" = Height,
            "Status" = "Calculate",
            "Hit" = "No",
            
            "PointA" = Temp,
            "PointB" = Temp,
            "PointC" = Temp,
            "Output" = Temp
        )
    }
    
    function gaitSet( ID:string, Start, Size ) {
        local Start = Start - floor(Start)
        Gaits[ID, table]["A", number] = Start
        Gaits[ID, table]["B", number] = Start + Size    
    }
    
    function ranger gaitTrace( Gait:table, Delay ) {
        local RA = rangerOffset(Entity:pos(), Entity:toWorld(Gait["Offset", vector]))
        local RB = rangerOffset(RA:pos(), RA:pos() + Entity:vel() / Delay)
        local RC = rangerOffset(Gait["Height", number], RB:pos(), -Entity:up())
        return RB:hit() ? RB : RC
    }

    function gaitSequence( ID:string, Rate, Delay ) {
        #--------------------
        #-- Gait Data
        local Gait = Gaits[ID, table]

        Gait["Cycle", number] = Gait["Cycle", number] + Rate

        local Cycle = Gait["Cycle", number]
        local A = Gait["A", number]
        local B = Gait["B", number]  

        local Frac = 0
        local Move = 0

        if (Cycle > 1) { Gait["Cycle", number] = 0 }

        #--------------------
        #-- Gait Sequence
        if (Cycle >= A & Cycle <= B) {
            Frac = (Cycle - A) / (B - A)
            Move = 1    
        }

        if (A < 0) {
            if (Cycle >= abs(A) & Cycle <= 1) {
                Frac = (Cycle - A) / (B - A)
                Move = 1
            }
        } elseif (B > 1) {
            if (Cycle + 1 >= A & Cycle + 1 <= B) {
                Frac = (Cycle + 1 - A) / (B - A)
                Move = 1
            }
        }

        #--------------------
        #-- Steps
        if (Move) {
            switch (Gait["Status", string]) {
                #-- Status: Calculate
                #-- Purpose: Store any data relevant to the planned step
                case "Calculate",
                    local Trace =  gaitTrace(Gait, Delay)

                    if (Gait["Output", vector]:distance(Trace:pos()) >= 1) {
                        Gait["PointA", vector] = Gait["Output", vector]
                        Gait["Status", string] = "Execute"
                    }
                break

                #-- Status: Execute
                #-- Purpose: Use stored data to perform step                 
                case "Execute", 
                    local Trace =  gaitTrace(Gait, Delay)

                    Gait["PointC", vector] = Trace:pos()
                    Gait["PointB", vector] = (Gait["PointA", vector] + Gait["PointC", vector])/2 + vec(0, 0, Gait["PointA", vector]:distance(Gait["PointC", vector])/3)
                    Gait["Output", vector] = bezier(Gait["PointA", vector], Gait["PointB", vector], Gait["PointC", vector], Frac)
                    
                    Gait["Hit", string] = "Yes"
                break
            }
        } else {
            if (Gait["Hit", string] == "Yes") {
                Gait["Output", vector] = Gait["PointC", vector]
                Gait["Status", string] = "Calculate"
                Gait["Hit", string] = "No"
            }
        }       
    }
    
    function number lerp( N0, N1, T ) {
        return (1-T)*N0 + T*N1
    }

    gaitAdd( "FL", vec(150,150,0), 100 )
    gaitAdd( "FR", vec(150,-150,0), 100 )
    gaitAdd( "RL", vec(-50,50,0), 100 )
    gaitAdd( "RR", vec(-50,-50,0), 100 )

    for (I = 1, 4) { holoCreate(I) }
}

interval(60)

GaitRate = lerp(GaitRate, Entity:vel():length(), 0.25)
local GaitMod = GaitRate/100
local GaitSize = clamp(0.4 + GaitMod*0.03, 0, 0.9)

gaitSet("FL", 1/4, GaitSize)
gaitSet("FR", 3/4, GaitSize)
gaitSet("RL", 2/4, GaitSize) 
gaitSet("RR", 4/4, GaitSize)

gaitSequence("FL", 0.02 + GaitMod*0.01, 6)
gaitSequence("FR", 0.02 + GaitMod*0.01, 6)
gaitSequence("RL", 0.04 + GaitMod*0.025, 8)
gaitSequence("RR", 0.04 + GaitMod*0.025, 8)

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

Entity:propGravity(0)
Entity:applyForce(Entity:toWorldAxis(vec(200,0,0) - Entity:velL())*Entity:mass())


#[3:23 PM - Metamist: To get longer steps, increase the size of the gait
3:23 PM - Metamist: longer bar means more time in air
3:23 PM - Metamist: More time in air means longer steps
3:23 PM - Metamist: So instead of having it between 0.5 - 1.0, put it at 0.2 - 1.0
3:23 PM - Metamist: Just change hte size varaible
3:23 PM - Metamist: The function has a start pos and a size
3:23 PM - Metamist: Increase size to get longer steps]#


