@name step_test_6
@inputs 
@outputs 
@persist [FOOT_HoldPos FOOT_TargetPos FOOT_MiddlePos]:vector [FOOT_Mode FOOT_Active FOOT_Inactive]:string [FOOT_Wait FOOT_Frac FOOT_StepDist FOOT_Delay] 
@persist [FOOT_OffsetL FOOT_OffsetR FOOT_WPosL FOOT_WPosR FOOT_Average]:vector FOOT_Debug
@persist [MECH_MaxHeight MECH_MaxSpeed] [MECH_MoveDir MECH_MoveVel]:vector
@trigger none


if (first()) {
    
    #----------------------------------
    #-- Initial Biped Variables
    
    MECH_MaxHeight = 100
    MECH_MaxSpeed  = 100

    FOOT_OffsetL = vec(0, 40, 0)
    FOOT_OffsetR = vec(0, -40, 0)
    
    FOOT_Delay = 0
    
    FOOT_Debug = 1
    
    #----------------------------------
    #-- Initial Biped Setup

    FOOT_WPosL   = toWorld( FOOT_OffsetL, ang(), entity():pos(), entity():angles() )
    FOOT_WPosR   = toWorld( FOOT_OffsetR, ang(), entity():pos(), entity():angles() )
    FOOT_Average = (FOOT_WPosL + FOOT_WPosR)*0.5
    
    if (FOOT_Debug) {
        holoCreate(hash("lfoot"), FOOT_WPosL)    
        holoCreate(hash("rfoot"), FOOT_WPosR) 
        holoCreate(10000)   
    }
    
    FOOT_HoldPos = FOOT_TargetPos = FOOT_MiddlePos = FOOT_WPosL

    FOOT_Active   = "lfoot"
    FOOT_Inactive = "rfoot"
    FOOT_Mode     = "calcStep"
    
}

interval(60)


MECH_MoveDir = vec(1, 0, 0)
MECH_MoveVel = MECH_MoveDir*MECH_MaxSpeed


if (FOOT_Mode == "calcStep") {
    
    if (FOOT_Active == "lfoot") {
        FOOT_HoldPos = FOOT_WPosL
        FOOT_TargetPos = rangerOffset(MECH_MaxHeight, toWorld(FOOT_OffsetL + MECH_MoveVel, ang(), FOOT_Average + vec(0, 0, MECH_MaxHeight), MECH_MoveDir:toAngle()), vec(0, 0, -1)):position()
    }
    else {
        FOOT_HoldPos = FOOT_WPosR
        FOOT_TargetPos = rangerOffset(MECH_MaxHeight, toWorld(FOOT_OffsetR + MECH_MoveVel, ang(), FOOT_Average + vec(0, 0, MECH_MaxHeight), MECH_MoveDir:toAngle()), vec(0, 0, -1)):position() 
    }
    
    FOOT_StepDist  = FOOT_HoldPos:distance(FOOT_TargetPos)
    FOOT_MiddlePos = FOOT_TargetPos + vec(0, 0, FOOT_StepDist/2)
    
    FOOT_Frac = 0
    FOOT_Mode = "perfStep"

}


if (FOOT_Mode == "perfStep") {
    
    FOOT_Frac = min(FOOT_Frac + 0.05, 1)

    local BezierPos = bezier(FOOT_HoldPos, FOOT_MiddlePos, FOOT_TargetPos, 6*FOOT_Frac^5 - 15*FOOT_Frac^4 + 10*FOOT_Frac^3)
    
    if (FOOT_Active == "lfoot") {
        FOOT_WPosL = BezierPos
    }
    else {
        FOOT_WPosR = BezierPos
    }
    
    FOOT_Average = (FOOT_WPosL + FOOT_WPosR)*0.5 
    
    if (FOOT_Debug) {
        holoPos(hash(FOOT_Active), BezierPos)    
        holoPos(10000, FOOT_Average + vec(0, 0, MECH_MaxHeight))
    }
    
    if (FOOT_Frac == 1) {
        FOOT_Wait = curtime()
        FOOT_Mode = "waitStep"
    }
}

if (FOOT_Mode == "waitStep") {
    
    local Delay = curtime() - FOOT_Wait
    
    if (min(Delay, FOOT_Delay) == FOOT_Delay) {
        FOOT_Active   = FOOT_Active == "lfoot" ? "rfoot" : "lfoot"
        FOOT_Inactive = FOOT_Active == "lfoot" ? "rfoot" : "lfoot"
        FOOT_Mode     = "calcStep"
    }
    
}

