@name Ruad Multi Seg IK
@inputs 
@outputs Angs:array SumAngles
@persist Base:entity Inc Ang Dist TotLength [EndEffectorLoc TargetLoc]:vector
interval(100)

function holoData (Index,Model:string,Scale:vector,Pos:vector,Ang:angle,Material:string,Color:vector,Parent:entity)
{
    holoCreate(Index)
    holoModel(Index,Model)
    holoScale(Index,Scale)
    holoPos(Index,Pos)
    holoAng(Index,Ang)
    holoMaterial(Index,Material)
    holoColor(Index,Color)
    holoParent(Index,Parent)
}

if(first())
{
    Base = entity():isWeldedTo()
    timer("hinges",1200*0)    
    timer("bars",1200*1)    
}

Seg1Length = 15
Seg2Length = 15
Seg3Length = 15
Seg4Length = 15
Seg5Length = 15
Seg6Length = 15
Seg7Length = 15
Seg8Length = 15
TotLength = Seg1Length + Seg2Length + Seg3Length + Seg4Length + Seg5Length + Seg6Length + Seg7Length

TargetLoc = owner():aimPos()

if(clk("hinges"))
{
    holoData(10,"hqsphere",vec(1,1,1)*0,Base:toWorld(vec(0,0,0)),Base:toWorld(ang()),"",vec(1,0.5,0.5)*255,Base)    # Marker
    holoData(9,"hqsphere",vec(1,1,1)*0.25,Base:toWorld(vec(0,0,0)),Base:toWorld(ang()),"",vec(1,0.25,0.25)*255,Base)    # Marker
    holoData(0,"hqsphere",vec(1,1,1)*0.3,Base:toWorld(vec(0,0,0)),Base:toWorld(ang()),"",vec(1,0.5,0.5)*255,Base)    # end effector
    holoData(1,"cube",vec(1,1,1)*0.3,Base:toWorld(vec(-40,-40,3)),Base:toWorld(ang()),"",vec(255),Base)    #base joint
    holoData(2,"cube",vec(1,1,1)*0.3,holoEntity(1):toWorld(vec(Seg1Length,0,0)),holoEntity(1):toWorld(ang()),"",vec(255),holoEntity(1))    
    holoData(3,"cube",vec(1,1,1)*0.3,holoEntity(2):toWorld(vec(Seg2Length,0,0)),holoEntity(2):toWorld(ang()),"",vec(255),holoEntity(2))    
    holoData(4,"cube",vec(1,1,1)*0.3,holoEntity(3):toWorld(vec(Seg3Length,0,0)),holoEntity(3):toWorld(ang()),"",vec(255),holoEntity(3))    
    holoData(5,"cube",vec(1,1,1)*0.3,holoEntity(4):toWorld(vec(Seg4Length,0,0)),holoEntity(4):toWorld(ang()),"",vec(255),holoEntity(4))    
    holoData(6,"cube",vec(1,1,1)*0.3,holoEntity(5):toWorld(vec(Seg5Length,0,0)),holoEntity(5):toWorld(ang()),"",vec(255),holoEntity(5))    
    holoData(7,"cube",vec(1,1,1)*0.3,holoEntity(6):toWorld(vec(Seg6Length,0,0)),holoEntity(6):toWorld(ang()),"",vec(255),holoEntity(6))    
    
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
    Angs:pushNumber(0)
}

SumAngles = Angs:sum()

if(clk("bars"))
{
    holoData(11,"cube",vec(Seg1Length/12,0.25,0.25)*1,holoEntity(1):toWorld(vec(Seg1Length/2,0,0)),holoEntity(1):toWorld(ang()),"",vec(1,0,0)*255,holoEntity(1))    #base joint
    holoData(12,"cube",vec(Seg2Length/12,0.25,0.25)*1,holoEntity(2):toWorld(vec(Seg2Length/2,0,0)),holoEntity(2):toWorld(ang()),"",vec(1,0,0)*255,holoEntity(2))    #base joint
    holoData(13,"cube",vec(Seg3Length/12,0.25,0.25)*1,holoEntity(3):toWorld(vec(Seg3Length/2,0,0)),holoEntity(3):toWorld(ang()),"",vec(1,0,0)*255,holoEntity(3))    #base joint
    holoData(14,"cube",vec(Seg4Length/12,0.25,0.25)*1,holoEntity(4):toWorld(vec(Seg4Length/2,0,0)),holoEntity(4):toWorld(ang()),"",vec(1,0,0)*255,holoEntity(4))    #base joint
    holoData(15,"cube",vec(Seg5Length/12,0.25,0.25)*1,holoEntity(5):toWorld(vec(Seg5Length/2,0,0)),holoEntity(5):toWorld(ang()),"",vec(1,0,0)*255,holoEntity(5))    #base joint
    holoData(16,"cube",vec(Seg6Length/12,0.25,0.25)*1,holoEntity(6):toWorld(vec(Seg6Length/2,0,0)),holoEntity(6):toWorld(ang()),"",vec(1,0,0)*255,holoEntity(6))    #base joint
    holoData(17,"cube",vec(Seg7Length/12,0.25,0.25)*1,holoEntity(7):toWorld(vec(Seg7Length/2,0,0)),holoEntity(7):toWorld(ang()),"",vec(1,0,0)*255,holoEntity(7))    #base joint
}

EndEffectorLoc = holoEntity(7):toWorld(vec(Seg8Length,0,0))
holoPos(0,EndEffectorLoc)
holoPos(9,TargetLoc)
if((holoEntity(1):pos() - TargetLoc):length() < TotLength + 5)
{
    Dist = (EndEffectorLoc - TargetLoc):length()
    Mul = (($Dist*150)+(Dist*2))/100000
    while(EndEffectorLoc:distance(TargetLoc) > 5 & perf())
    {
        for(I=1,7)
        {
            holoPos(10,(holoEntity(I):pos() + EndEffectorLoc)/2)
            holoAng(10,((holoEntity(I):pos() - EndEffectorLoc)/2):toAngle())
            LocDist = (holoEntity(I):pos() - EndEffectorLoc):length()/50
            Dir = holoEntity(10):bearing(TargetLoc)/500 - (sign(holoEntity(I):bearing(TargetLoc))*0.25)
            Angs[I,number] = Angs[I,number] + ((Dir * (0.5 + I/3)) / LocDist) * (Dist/5)
            holoAng(I,Base:toWorld(ang(0,Angs[I,number],0)))
        }
    }
}
