#@name robotic_movement
@inputs 
@outputs Test
@persist Legs:table Leg:string Mode:string Cycle Delay Entity:entity
@persist [PointA PointB PointC]:vector FDir:vector
@trigger none

if ( first() ) {
    
    Entity = entity()
    
    Entity:propFreeze( 0 )
    
    #1  3
    #4  2
 
    
    Legs = table(
        "FrontLeft"   = table( #1
            "NextLeg" = "RearRight",
            "Delay"   = 0.125,
            "Output"  = Entity:pos(),
            "Offset"  = vec(50,50,0)
        ),
        
        "FrontRight"  = table( #2
            "NextLeg" = "RearLeft",
            "Delay"   = 0.125,
            "Output"  = Entity:pos(),
            "Offset"  = vec(50,-50,0)
        ),
        
        "RearLeft"   = table( #3
            "NextLeg" = "FrontLeft",
            "Delay"   = 0.125,
            "Output"  = Entity:pos(),
            "Offset"  = vec(-50,50,0)
        ),
        
        "RearRight"  = table( #4
            "NextLeg" = "FrontRight",
            "Delay"   = 0.125,
            "Output"  = Entity:pos(),
            "Offset"  = vec(-50,-50,0)
        )
    )
    
    PointA = PointB = PointC = Entity:pos()
    
    Leg   = "FrontLeft"
    Cycle = curtime()
    Delay = 1
    
    Mode = "Cycle"
    
    holoCreate(1)
    holoCreate(2)
    holoCreate(3)
    holoCreate(4)
       
}

interval(90)

W = owner():keyPressed("up")
A = owner():keyPressed("left")
S = owner():keyPressed("down")
D = owner():keyPressed("right")

MDir = entity():toWorldAxis( vec( (W-S), (A-D), 0 ):normalized()*50 )
FDir = mix( MDir, FDir, 0.25 )

switch (Mode) {
    case "Cycle",
        local Timer = curtime()
        if ( Timer - Cycle > Delay ) {
            
            local Data = Legs[Leg, table]
            
            PointA = Data["Output", vector]
            PointC = rangerOffset(200, Entity:toWorld(Data["Offset", vector]) + FDir, vec(0,0,-1)):position()
            
            Cycle = 0
            Mode  = "Run"
            #print(Leg)
        }    
    break
    
    case "Run",
        Cycle = min(Cycle + 0.1, 1)
        
        local Data = Legs[Leg, table]
        local Lerp = 10*Cycle^3 - 15*Cycle^4 + 6*Cycle^5
        
        Data["Output", vector] = mix( PointC, PointA, Lerp ) + vec(0, 0, (sin(Cycle*180)^(1/25))*PointA:distance(PointC)/4 )
        #Data["Output", vector] = mix( PointC, PointA, Lerp ) + vec(0, 0, sin(Lerp*180)*PointA:distance(PointC)/4 )
        #Data["Output", vector] = mix( PointC, PointA, Lerp ) + vec(0, 0, sign(sin(Lerp*180))*PointA:distance(PointC)/4 )
        
        if (Cycle == 1) {
            Cycle = curtime()
            Delay = 0#Data["Delay", number]
            Leg   = Data["NextLeg", string]
            Mode  = "Cycle"
        }
    break
}

local N = 0
local Vec = vec()
foreach (K, V:table = Legs) {
    Vec += V["Output", vector]
    
    N++
    holoPos(N, V["Output", vector])
}
Vec /= 4
Vec += vec(0, 0, 50)

Entity:propGravity(0)
Entity:applyForce( ((Vec - Entity:pos())*10 - Entity:vel()) * Entity:mass() )
Entity:applyAngForce( (Entity:toLocal(ang())*200 - Entity:angVel()*20) * shiftL(ang(Entity:inertia())) )

