@name gait_test_new
@inputs
@outputs
@persist GaitTable:table GaitFrameTime GaitFrameAdvanced WorldDownVec:vector
@trigger none

if (first()) {

    WorldDownVec = vec(0,0,-1)

    function addGait( Name:string, Base:entity, RangerOffset:vector, CycleOffset, StepMaxL, StepMaxH ) {
        GaitTable[Name, table] = table(
            "cycleOffset" = CycleOffset,
            "stepMaxL" = StepMaxL,
            "stepMaxH" = StepMaxH,
            "baseEnt"    = Base,
            "rangerPosO" = RangerOffset,
            "rangerMaxL" = StepMaxH*5
        )
    }

    function runGaits( Rate, Debug ) {
        GaitFrameTime += Rate
        GaitFrameAdvanced = $GaitFrameTime != 0

        foreach (K, Gait:table = GaitTable) {
            Gait["stepCurL", number] = Gait["stepCurL", number] + (Gait["stepMaxL", number]*GaitFrameAdvanced - Gait["stepCurL", number])*0.5
            Gait["stepCurH", number] = Gait["stepCurH", number] + (Gait["stepMaxH", number]*GaitFrameAdvanced - Gait["stepCurH", number])*0.5

            # step foot pos
            Gait["outputPos", vector] = rangerOffset(Gait["rangerMaxL", number], Gait["baseEnt", entity]:toWorld(Gait["rangerPosO", vector] + vec(
                    sin(GaitFrameTime + Gait["cycleOffset", number])*Gait["stepCurL", number], 0, 0)), WorldDownVec):position() + vec(
                        0, 0, max(0, cos(GaitFrameTime + Gait["cycleOffset", number])*Gait["stepCurH", number]))

            # step foot ang
            Gait["outputAng", angle] = Gait["baseEnt", entity]:toWorld(ang(-sin(GaitFrameTime + Gait["cycleOffset", number])*Gait["stepCurL", number], 0, 0))

            # step debugging
            if (Debug) {
                if (!Gait["debugged", number]) {
                    local UniqueID = hash(K)
    
                    holoCreate(UniqueID, vec(), vec(1/2))
                    holoParent(UniqueID, Gait["baseEnt", entity])
    
                    Gait["debugged", number] = UniqueID
                    continue
                }
    
                holoPos(Gait["debugged", number], Gait["outputPos", vector])
                holoAng(Gait["debugged", number], Gait["outputAng", angle])             
            }

        }
    }

    function vector getGaitPos( Name:string ) {
        return GaitTable[Name, table]["outputPos", vector]
    }

    function angle getGaitAng( Name:string ) {
        return GaitTable[Name, table]["outputAng", angle]
    }

    addGait( "fl", entity(), vec(30, 15, 0), 0, 30, 15 )
    addGait( "fr", entity(), vec(30, -15, 0), 45, 30, 15 )
    addGait( "rl", entity(), vec(-30, 15, 0), 90, 30, 15 )
    addGait( "rr", entity(), vec(-30, -15, 0), 135, 30, 15 )

}

interval(90)

runGaits( 50, 1 )
