@name robotic_movement_2
@inputs 
@outputs TR
@persist Entity:entity [MoveDir WorldDown]:vector [Mass] [Inertia]:angle
@persist StepData:table [StepMode StepFoot]:string [StepCycle StepDelay] [StepPointA StepPointB StepPointC]:vector
@trigger none

if (first()) {
    
    Entity = entity()
    
    Entity:setMass(5000)
    Entity:propGravity(0)
    
    Mass    = Entity:mass()
    Inertia = shiftL(ang(Entity:inertia()))
    
    StepData = table(
        "F_Left"  = table(
            "Output" = Entity:pos(),
            "Offset" = vec(50,50,0),
            "Next"   = "R_Right"
        ),
        "F_Right" = table(
            "Output" = Entity:pos(),
            "Offset" = vec(50,-50,0),
            "Next"   = "R_Left"
        ),
        "R_Left"  = table(
            "Output" = Entity:pos(),
            "Offset" = vec(-50,50,0),
            "Next"   = "F_Left"
        ),
        "R_Right" = table(
            "Output" = Entity:pos(),
            "Offset" = vec(-50,-50,0),
            "Next"   = "F_Right"
        )
    )

    StepMode = "Calculate"
    StepFoot = "F_Left"
    
    StepCycle = curtime()
    StepDelay = 0
    
    WorldDown = vec(0,0,-1)
    
    for (I = 1, 4) { holoCreate(I+10000) }
    
}

interval(90)

MoveDir = vec(50,0,0)

if (StepMode == "Calculate") {
    if (curtime() - StepCycle > StepDelay) {
        local Temp = StepData[StepFoot, table]
    
        StepPointA = Temp["Output", vector]
        StepPointC = rangerOffset(200, Entity:toWorld(Temp["Offset", vector]) + MoveDir, WorldDown):position()
        StepPointB = (StepPointA + StepPointC)/2 + vec(0,0,StepPointA:distance(StepPointC)/3)
            
        StepCycle = 0
        StepMode  = "Animate"
    }
}
elseif (StepMode == "Animate") {
    StepCycle = min(StepCycle + 0.1, 1)
    
    local T = 10*StepCycle^3 - 15*StepCycle^4 + 6*StepCycle^5
    StepData[StepFoot, table]["Output", vector] = mix(mix(StepPointC, StepPointB, T), mix(StepPointB, StepPointA, T), T)

    if (StepCycle == 1) {
        StepFoot  = StepData[StepFoot, table]["Next", string]
        StepCycle = curtime()
        StepMode  = "Calculate"
    }
}

local Index  = 10000
local AvgPos = vec()

foreach (K, Foot:table = StepData) {
    Index++
    holoPos(Index, Foot["Output", vector]) 

    AvgPos = AvgPos + Foot["Output", vector]
}

AvgPos = AvgPos/4 + vec(0,0,50) - Entity:pos()

TR++

Entity:applyForce((AvgPos*10 - Entity:vel()) * Mass)
Entity:applyAngForce((Entity:toLocal(ang(0,TR,0))*200 - Entity:angVel()*20) * Inertia)

