@name IK solver 4
@inputs Dest:vector EndRot:angle StartSolve
@outputs EndPos:vector Tempops Ops
@persist [BoneOps BonePos BoneAng Bones]:array Index IndexIK Ops TotLength CreateBones Damp BoneCount TempPos:vector Solve
@trigger StartSolve

#Bones stores the length of each bone
#BonePos stores the position of the ROOT of each bone, NOT IT'S END!
#BoneAng stores the angle of each joint.
#EndPos is the position of the END of the FINAL bone, the End Effector.

#At end of each execution, perform an FK loop to update the positions and display. VITALLY IMPORTANT it fully completes each execution.

#BoneOps contains the OPtionS for each bone.
#String format for defining bones:
#      PYR            P Y R      P Y R
#axis of rotation  min angles  max angles
#    (unused)        (Used)      (Used)

if (first())
{
#        Lengths                           Options                               Initial Angles
    Bones[1,number]=50   BoneOps[1,string]="110 -80 -200 0 105 200 0"     BoneAng[1,angle]=ang(0,0,0)
    Bones[2,number]=40   BoneOps[2,string]="110 -10 0 0 100 0 0"          BoneAng[2,angle]=ang(0,0,0)
    Bones[3,number]=30   BoneOps[3,string]="110 -10 0 0 160 0 0"          BoneAng[3,angle]=ang(0,0,0)
    #Bones[4,number]=10   BoneOps[4,string]="111 -90 -90 -360 90 90 360"   BoneAng[4,angle]=ang(0,0,0)
    
    
    Damp=1
    BoneCount=Bones:count()
    for (I=1,BoneCount){ #Rough estimate of how many ops the FK loop requires. Math based on experimentation.
        Ops+=100+(I*21)
    }
    CreateBones=1
}
Pos=entity():pos()

#create bones
if (CreateBones)
{
    while (Index<BoneCount & minquota()>Ops)
    {
        Index++
        Holo=Index*2-1
        holoCreate(Holo,Pos+vec(0,0,TotLength))
        holoModel(Holo,"sphere3")
        holoScaleUnits(Holo,vec(6,6,6))
        if (Index==1) { holoParent(Index,entity()) }
        else { holoParent(Holo,Holo-2) }
        
        
        Holo=Index*2
        Len=Bones[Index,number]
        holoCreate(Holo,Pos+vec(0,0,TotLength+(Len/2)))
        holoModel(Holo,"cylinder")
        holoScaleUnits(Holo,vec(5,5,Len))
        holoParent(Holo,Holo-1)
        TotLength+=Len
    }
    if (Index==BoneCount){ CreateBones=0 }
    else { interval(20) }
}

if (!CreateBones)
{
    if (StartSolve) { Solve=1 }
    #IK
    if (EndPos:distance(Dest)>1 & Pos:distance(Dest)<TotLength & Solve){ #Check to make sure we're not already there, and that the destination is reachable.
        while (IndexIK>0 & minquota()>Ops){
            Targ=(Dest-BonePos[IndexIK,vector]):normalized()
            Vec=(EndPos-BonePos[IndexIK,vector]):normalized()
            Cos=Targ:dot(Vec)
            
            if (Cos<0.999){
                Cross=Targ:cross(Vec)
                Turn=toDeg(acos(Cos))
                if (Damp & Turn>Damp){ Turn=Damp }
                SetAng=BoneAng[IndexIK,angle]
                Curr=quat(SetAng)
                Rot=qRotation(Cross,-Turn)
                NewRot=(Rot*Curr):toAngle()
                Vars=BoneOps[IndexIK,string]:explode(" ")
                MinAng=ang(Vars[2,string]:toNumber(),Vars[3,string]:toNumber(),Vars[4,string]:toNumber())
                MaxAng=ang(Vars[5,string]:toNumber(),Vars[6,string]:toNumber(),Vars[7,string]:toNumber())
                NewRot=clamp(NewRot,MinAng,MaxAng)
                BoneAng[IndexIK,angle]=NewRot
            }
            IndexIK--
        }
        if (IndexIK==0)
        { IndexIK=BoneCount }
        interval(10)
    }
    else
    { Solve=0 }
    
    
    Tempops=opcounter()
    #FK
    Index=0
    while (Index<BoneCount)
    {
        if (Index==0){EndPos=Pos}
        Index++
                Ang=ang()
        Index2=0
        while (Index2<Index)
        {
            Index2++
            Ang+=BoneAng[Index2,angle]
        }
        holoAng(Index*2-1,Ang)
        BonePos[Index,vector]=EndPos
        EndPos+=vec(0,0,Bones[Index,number]):rotate(Ang)
    }
    Tempops=opcounter()-Tempops
}
