@name Inverse Kinematics
@inputs Prop:entity
@outputs Target:vector Yaw Q1 Q2 Q3 Angle1 Angle2
@persist Pos0:vector B L
#runOnTick(1)
interval(100)

if(first()) {
    holoCreate(1)
    holoCreate(2)
    holoCreate(3)
    holoCreate(4)
    holoCreate(5)
    Pos0 = entity():pos()
}

L = 65
Pos = entity():pos()
Size1 = vec(1/6, 1/6, 1/6)
Size2 = vec(L, 1, 1)

Target = (Pos - Prop:pos())
Yaw = Target:toAngle():yaw() + 180

B = Target:length()
Q1 = -atan(Target:z()/vec2(Target):length())
Q2 = acos((L^2 - L^2 - B^2)/(2*L*B))
Q3 = acos((B^2 - L^2 - L^2)/(2*L*L))
Angle1 = Q1 + Q2
Angle2 = Angle1 + Q3

ArmPos1 = Prop:pos() + vec(L, 0, 0):rotate(ang(-Angle1, Yaw, 0))
ArmPos2 = ArmPos1 + vec(L, 0, 0):rotate(ang(-Angle2, Yaw, 0))


holoPos(1, Prop:pos())
holoScale(1, Size1)

holoPos(2, (Prop:pos() + ArmPos1)/2)
holoAng(2, ang(-Angle1, Yaw, 0))
holoScaleUnits(2, Size2)

holoPos(3, ArmPos1)
holoScale(3, Size1)
holoModel(3,"hq_sphere")

holoPos(4, (ArmPos1 + ArmPos2)/2)
holoAng(4, ang(-Angle2, Yaw, 0))
holoScaleUnits(4, Size2)

holoPos(5, ArmPos2)
holoScale(5, Size1)
