@name gait_test_new2
@inputs 
@outputs 
@persist FootControllers:table [FrameTime FrameAdvn] WorldDownVec:vector Interval
@trigger none

if (first()) {

    Interval = 90

    function addFootController(UniqueID:string, Ent:entity, RPos:vector, [GaitOffset FootAdjust StepMaxL StepMaxH]) {
        FootControllers[UniqueID,table] = table(
            "ent" = Ent,
            "rpos" = RPos,
            "rmax" = StepMaxH*5,
            "stepMaxL" = StepMaxL,
            "stepMaxH" = StepMaxH,
            "footAdjust" = FootAdjust,
            "gaitOffset" = GaitOffset
        )
    }

    WorldDownVec = vec(0,0,-1)

    function updateFootControllers(Anim,ShowDebugHolos){
        FrameTime = FrameTime+Anim
        FrameAdvn = $FrameTime != 0

        foreach(K,Controller:table = FootControllers){
            Controller["stepL",number] = Controller["stepL",number]+(Controller["stepMaxL",number]*FrameAdvn-Controller["stepL",number])*0.5
            Controller["stepH",number] = Controller["stepH",number]+(Controller["stepMaxH",number]*FrameAdvn-Controller["stepH",number])*0.5

            local Trace = rangerOffset(
                Controller["rmax",number],
                Controller["ent",entity]:toWorld(Controller["rpos",vector]+vec(sin(FrameTime+Controller["gaitOffset",number])*Controller["stepL",number],0,0)),
                WorldDownVec
            )

            local FootAngle = Trace:hitNormal():toAngle()
                FootAngle = FootAngle:rotateAroundAxis(FootAngle:forward(),Controller["ent",entity]:angles():yaw())
                FootAngle = FootAngle:rotateAroundAxis(FootAngle:right(),sin(FrameTime+Controller["gaitOffset",number])*Controller["stepL",number]-90)

            Controller["ang",angle] = FootAngle
            Controller["pos",vector] = Trace:position()+Trace:hitNormal()*max(Controller["footAdjust",number],cos(FrameTime+Controller["gaitOffset",number])*Controller["stepH",number])

            if (ShowDebugHolos){
                if(!Controller["debug",number]){
                    local UniqueID = hash(K)

                    holoCreate(UniqueID)
                    holoScale(UniqueID,vec(1/2))
                    holoModel(UniqueID,"sphere")

                    holoCreate(UniqueID+1,holoEntity(UniqueID):toWorld(vec(0,0,6)),vec(1/2),holoEntity(UniqueID):angles())
                    holoParent(UniqueID+1,UniqueID)
                    holoModel(UniqueID+1,"models/sprops/misc/origin.mdl")

                    Controller["debug",number] = UniqueID
                    continue 
                }

                holoPos(Controller["debug",number], Controller["pos",vector])
                holoAng(Controller["debug",number], Controller["ang",angle])
            }
        }
    }

    #addFootController("fl",entity(),vec(30,15,0),0,3,30,15)
    #addFootController("fr",entity(),vec(30,-15,0),180,3,30,15)
    #addFootController("rl",entity(),vec(-30,15,0),90,3,30,15)
    #addFootController("rr",entity(),vec(-30,-15,0),270,3,30,15)
    
    addFootController("fl",entity(),vec(30,15,0),0,3,30,15)
    addFootController("fr",entity(),vec(30,-15,0),45,3,30,15)
    addFootController("rl",entity(),vec(-30,15,0),180,3,30,15)
    addFootController("rr",entity(),vec(-30,-15,0),225,3,30,15)


}


interval(Interval)

updateFootControllers( 15, 1 )
