@name step_test_7
@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 FOOT_Speed
@persist [MECH_MaxHeight MECH_MaxSpeed] [MECH_MoveDir MECH_MoveVel]:vector CT DT IKL:table IKR:table
@trigger none


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

    FOOT_OffsetL = vec(0, 10, 0)
    FOOT_OffsetR = vec(0, -10, 0)
    
    FOOT_Delay = 0
    FOOT_Speed = 1
    
    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"


    holoCreate(10,holoEntity(10000):pos())
    holoAng(10, holoEntity(10000):angles())
    holoParent(10,10000)

    local L1 = 50
    local L2 = 20
    local L3 = 35
    
    holoCreate(70)
    holoParent(70,holoEntity(10))
    holoPos(70,holoEntity(10):toWorld(vec(0,15,0)))
    holoModel(70,"models/props_c17/oildrum001.mdl")
    holoScaleUnits(70,vec(16,16,L1))
        
    holoCreate(71)
    holoParent(71,70)
    holoPos(71,holoEntity(70):toWorld(vec(0,0,L1)))
    holoModel(71,"models/props_c17/oildrum001.mdl")
    holoScaleUnits(71,vec(14,14,L2))
    
    holoCreate(72)
    holoParent(72,71)
    holoPos(72,holoEntity(71):toWorld(vec(0,0,L2)))
    holoModel(72,"models/props_c17/oildrum001.mdl")
    holoScaleUnits(72,vec(13,13,L3))
    
    holoCreate(73)
    holoParent(73,72)
    holoPos(73,holoEntity(72):toWorld(vec(0,0,L3)))
    holoScaleUnits(73,vec(12,12,12))
    

    holoCreate(80)
    holoParent(80,holoEntity(10))
    holoPos(80,holoEntity(10):toWorld(vec(0,-15,0)))
    holoModel(80,"models/props_c17/oildrum001.mdl")
    holoScaleUnits(80,vec(16,16,L1))
        
    holoCreate(81)
    holoParent(81,80)
    holoPos(81,holoEntity(80):toWorld(vec(0,0,L1)))
    holoModel(81,"models/props_c17/oildrum001.mdl")
    holoScaleUnits(81,vec(14,14,L2))
    
    holoCreate(82)
    holoParent(82,81)
    holoPos(82,holoEntity(81):toWorld(vec(0,0,L2)))
    holoModel(82,"models/props_c17/oildrum001.mdl")
    holoScaleUnits(82,vec(13,13,L3))
    
    holoCreate(83)
    holoParent(83,82)
    holoPos(83,holoEntity(82):toWorld(vec(0,0,L3)))
    holoScaleUnits(83,vec(12,12,12))  
    

function table setup_humanoid_digitigrade(Chain:array){
    local Entity = Chain[1, entity]
    local Joint0 = Chain[2, number] #hip
    local Joint1 = Chain[3, number] #knee
    local Joint2 = Chain[4, number] #knee2
    local Joint3 = Chain[5, number] #foot
    
    local Length0 = round((holoEntity(Joint0):pos()-holoEntity(Joint1):pos()):length(), 1)
    local Length1 = round((holoEntity(Joint1):pos()-holoEntity(Joint2):pos()):length(), 1)
    local Length2 = round((holoEntity(Joint2):pos()-holoEntity(Joint3):pos()):length(), 1)
    
    local Table = table(
        "entity" = Entity, 
        "joint0" = Joint0, 
        "joint1" = Joint1, 
        "joint2" = Joint2, 
        "joint3" = Joint3, 
        "joint0e" = holoEntity(Joint0), 
        "joint1e" = holoEntity(Joint1), 
        "joint2e" = holoEntity(Joint2), 
        "length0" = Length0, 
        "length1" = Length1, 
        "length2" = Length2
    )
    
    return Table
}

function solve_humanoid_digitigrade(IK:table, Pos:vector, Ang:angle){
    local Normal = IK["entity", entity]:toLocalAxis(Pos-IK["joint0e", entity]:pos())
    local Angle = Normal:toAngle():setRoll(-bearing(IK["joint0e", entity]:pos(), IK["entity", entity]:angles(), Pos))
    local Quat = quat(Angle)*qRotation(vec(0,1,0), (Normal:length()/IK["length0", number])*IK["length0", number])
    
    holoAng(IK["joint0", number], IK["entity", entity]:toWorld(Quat:toAngle()))
    
    local Normal = IK["joint0e", entity]:toLocalAxis(Pos-IK["joint1e", entity]:pos())
    local Dist = min(Normal:length(), IK["length1", number]+IK["length2", number])
    local Acos = acos((Dist^2+IK["length1", number]^2-IK["length2", number]^2)/(2*IK["length1", number]*Dist))
    
    holoAng(IK["joint1", number], IK["joint0e", entity]:toWorld(ang(atan(-Normal[3], Normal[1])+90+Acos, 0, 0)))
    holoAng(IK["joint2", number], IK["joint1e", entity]:toWorld(ang(180+acos((IK["length2", number]^2+IK["length1", number]^2-Dist^2)/(2*IK["length1", number]*IK["length2", number])), 0, 0)))
    holoAng(IK["joint3", number], Ang)
}

    IKL = setup_humanoid_digitigrade(array(holoEntity(10),70,71,72,73))
    IKR = setup_humanoid_digitigrade(array(holoEntity(10),80,81,82,83)) 
    #addIKinematics("fl","Humanoid.Digitigrade",array(holoEntity(10),70,71,72,73))
    #addIKinematics("fr","Humanoid.Digitigrade",array(holoEntity(10),80,81,82,83)) 
        
    CT = curtime()
}

interval(60)

DT = curtime() - CT
CT = curtime()

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 = mix(FOOT_TargetPos, FOOT_HoldPos, 0.25) + vec(0, 0, FOOT_StepDist/2)
    
    FOOT_Frac = 0
    FOOT_Mode = "perfStep"

}


if (FOOT_Mode == "perfStep") {
    
    FOOT_Frac = min(FOOT_Frac + FOOT_Speed*DT, 1)

    #local BezierPos = bezier(FOOT_HoldPos, FOOT_MiddlePos, FOOT_TargetPos, 6*FOOT_Frac^5 - 15*FOOT_Frac^4 + 10*FOOT_Frac^3)
    local BezierPos = mix(FOOT_TargetPos, FOOT_HoldPos, FOOT_Frac) + vec(0, 0, sin(FOOT_Frac*180)*30)
    
    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))
        holoAng(10000, MECH_MoveDir:toAngle())
    }
    
    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"
    }
    
}

solve_humanoid_digitigrade(IKL, FOOT_WPosL, ang())
solve_humanoid_digitigrade(IKR, FOOT_WPosR, ang())
