@name step_test_4
@inputs 
@outputs Step:string
@persist [StepNext StepPrev StepMid StepPos StepAvg]:vector [Step OppStep]:string [StepStage StepFrac StepDelay StepDist StepRate] Biped:table Entity:entity MoveDir:vector
@trigger none


if (first()) {
    
    Entity = holoCreate(1), holoColor(1, vec(0,255,0))
    
    function biped( LPos:vector, RPos:vector ) {

        Biped["lfoot", table] = table(
            "local" = LPos,
            "step"  = Entity:toWorld(LPos),
            "holo"  = 10000
        )

        Biped["rfoot", table] = table(
            "local" = RPos,
            "step"  = Entity:toWorld(RPos),
            "holo"  = 10001
        )
        
        Step     = "lfoot"
        OppStep  = "rfoot"
        StepNext = StepPrev = StepMid = StepPos = Entity:toWorld(LPos)
        
        StepAvg = (Entity:toWorld(LPos) + Entity:toWorld(RPos))/2
        
        holoCreate(10000)
        holoCreate(10001)

    }
    
    function bipedSwapStep() {
        Step = Step == "lfoot" ? "rfoot" : "lfoot"
        OppStep = Step == "lfoot" ? "rfoot" : "lfoot"
    }

    biped( vec(0,30,0), vec(0,-30,0) )
    
    
    MoveDir = vec(1,0,0)
    StepDelay = 1
    
    holoCreate(5)
    holoCreate(6)
    
    holoColor(6, vec(0,0,255))
    
    
}

interval(60)

StepSpeed = owner():keyPressed("up")*100

local Foot = Biped[Step, table]
switch (StepStage) {
    
    case 0, 
        
        StepDist = min(StepSpeed, StepPrev:distance(Foot["step", vector]))
        local StepDir = MoveDir:normalized()
        local StepVec = StepDir*StepDist
        
        print( StepDist )
        
        StepPrev = Foot["step", vector]
        StepNext = rangerOffset(100, toWorld( Foot["local", vector] + StepVec, ang(), StepAvg, StepDir:toAngle() ), vec(0, 0, -1)):position()
        StepMid  = mix(StepPrev, StepNext, 0.1) + vec(0, 0, StepPrev:distance(StepNext)/2)
        
        holoPos(5, StepNext)
        holoPos(6, StepPrev)

        #if (StepPrev:distance(StepNext) > 0) { 
            StepStage = 1 
        #} 
        ##else {
        #    bipedSwapStep()
        #}

    break

    case 1, 
    
        StepFrac = min(StepFrac + StepDist/10, StepDist)
        StepPos  = bezier(StepPrev, StepMid, StepNext, StepFrac/max(1, StepDist))
        
        Foot["step", vector] = StepPos
        holoPos(Foot["holo", number], StepPos)
        
        if (StepFrac == StepDist) {
            StepFrac  = curtime()
            StepStage = 2
        }
    
    break
    
    
    case 2, 
    
        if (min(curtime() - StepFrac, StepDelay) == StepDelay) {
            StepFrac = StepStage = 0 
            bipedSwapStep()   
        }
    
    break   
     
}

StepAvg = (Biped[Step, table]["step", vector] + Biped[OppStep, table]["step", vector])/2 + vec(0, 0, 100)

holoPos(1, StepAvg)

