@name 3-seg ik
@inputs F:entity
@persist [O E H K A]:entity L1 L2 L3
@model models/hunter/blocks/cube025x025x025.mdl

interval(100)

if(first())
{
    function number cosine(A, B, C)
    {
        return acos((A^2 + B^2 - C^2) / (2*A*B))
    }
    
    function ik3([Ent Hip Knee Ankle]:entity, FootPos:vector, [L1 L2 L3 FirstSegmentAdjustAngle]:number)  {
        local Total = L1 + L2 + L3
        
        #-Hip
            local Bearing = -bearing(Hip:pos(), Ent:angles(), FootPos)
            local Angle = Ent:toLocalAxis(FootPos - Hip:pos()):toAngle():setRoll(Bearing)
            local Offset = Total - abs(min(Hip:pos():distance(FootPos), Total))
            local Quat = quat(Angle) * qRotation(vec(0, -Offset + FirstSegmentAdjustAngle, 0))
            holoAng(holoIndex(Hip), Ent:toWorld(slerp(quat(Hip), Quat, 1):toAngle()))
            
        #-Second
            local Distance = min(Knee:pos():distance(FootPos), L2+L3)
            local Axis = Hip:toLocalAxis(Knee:pos() - FootPos)
            local Atan = atan(-Axis:z(), Axis:x()) + cosine(L3, L2, Distance) + 90
            holoAng(holoIndex(Knee), Hip:toWorld(ang(Atan, 0, 0)))
            
        #-Third
            holoAng(holoIndex(Ankle), Knee:toWorld(ang(cosine(Distance, L2, L3), 0, 0)))
    }
    
    E = entity()
    O = owner()
    
    L1 = 100
    L2 = 100
    L3 = 100
    
    #Set 1
    H = holoCreate(1, E:toWorld(vec(0, 15, 0)))
    holoParent(1, E)
    holoColor(1, vec(0, 255, 0))
    
    K = holoCreate(2, H:toWorld(vec(L1, 0, 0)))
    holoParent(2, 1)
    holoColor(2, vec(0, 255, 0))
    
    A = holoCreate(3, K:toWorld(vec(0, 0, -L2)))
    holoParent(3, 2)
    holoColor(3, vec(0, 255, 0))
    
    holoCreate(4, A:toWorld(vec(0, 0, L3)))
    holoParent(4, 3)
}

ik3(E, H, K, A, O:aimPos(), L1, L2, L3, 125)
