@name 3siktest
@inputs Target:vector
@outputs [V1 V2]:string
@persist LengthA LengthB LengthC
@persist [Prev Next Cur]:vector T
@trigger none
@model models/sprops/rectangles/size_1_5/rect_6x18x3.mdl


if ( first() ) {
    
    holoCreate( 999 )

    LengthA = 50
    LengthB = 75
    LengthC = 100
    
    #-- Can have different LengthA/LengthC
    function ik_typeA ( Base:entity, Target:vector, [HoloA HoloB HoloC HoloD], [LengthA LengthB LengthC], Foot ) {
        local DirN = Target-holoEntity( HoloA ):pos()
        
        local DirY = Base:toLocalAxis( DirN )
        holoAng( HoloA, Base:toWorld( ang( 0, atan( DirY[2], DirY[1] ), 0 ) ) )
        
        local DirP = holoEntity( HoloA ):toLocalAxis( DirN )
        local DisP = min( DirP:length()-LengthB, LengthA + LengthC)
        
        holoAng( HoloB, holoEntity( HoloA ):toWorld( ang( atan( DirP[1], DirP[3] )-acos( ( LengthC^2-LengthA^2-DisP^2 )/( -2*LengthA*DisP ) ), 0, 0 ) ) )

        local DirP = holoEntity( HoloB ):toLocalAxis( Target-holoEntity( HoloC ):pos() )
        local DisP = min( DirP:length(), LengthB + LengthC )
        
        holoAng( HoloC, holoEntity( HoloB ):toWorld( ang( atan( DirP[1], DirP[3] )-acos( ( LengthC^2-LengthB^2-DisP^2 )/( -2*LengthB*DisP ) ), 0, 0 ) ) )
        holoAng( HoloD, holoEntity( HoloC ):toWorld( ang( acos( ( DisP^2-LengthB^2-LengthC^2 )/( 2*LengthB*LengthC ) ), 0, 0 ) ) )
    }

    function ik_typeB ( Base:entity, Target:vector, [HoloA HoloB HoloC], [LengthA LengthB LengthC], Foot ) {
        local DirQ = Base:toLocalAxis( Target-holoEntity( HoloA ):pos() )
        local DisQ = min( DirQ:length()-LengthB, LengthA + LengthC)
        local Quat = quat( DirQ:toAngle() )*qRotation( vec( 0, 90 - acos( clamp( ( LengthC^2-LengthA^2-DisQ^2 )/( -2*LengthA*DisQ ), -1, 1 ) ) , 0 ) ) # <---- acos returning Nan

        holoAng( HoloA, Base:toWorld( Quat:toAngle() ) )
        
        local DirP = holoEntity( HoloA ):toLocalAxis( Target-holoEntity( HoloB ):pos() )
        local DisP = min( DirP:length(), LengthB + LengthC )
        
        holoAng( HoloB, holoEntity( HoloA ):toWorld( ang( atan( DirP[1], DirP[3] )-acos( ( LengthC^2-LengthB^2-DisP^2 )/( -2*LengthB*DisP ) ), 0, 0 ) ) )
        holoAng( HoloC, holoEntity( HoloB ):toWorld( ang( acos( ( DisP^2-LengthB^2-LengthC^2 )/( 2*LengthB*LengthC ) ), 0, 0 ) ) )  
    }

    # Type A
    holoCreate( 1 ) #Yaw
    holoCreate( 2 ) #Pitch1
    holoCreate( 3 ) #Pitch2
    holoCreate( 4 ) #Pitch3
  
    holoParent( 1, entity() )
    for ( I = 2, 4 ) { holoModel( I, "models/props_c17/oildrum001.mdl" ), holoParent( I, I - 1 ) }
    
    holoPos( 3, holoEntity( 2 ):toWorld( vec( 0, 0, LengthA ) ) )
    holoPos( 4, holoEntity( 3 ):toWorld( vec( 0, 0, LengthB ) ) )
 
    holoScaleUnits( 2, vec( 8, 8, LengthA ) )
    holoScaleUnits( 3, vec( 8, 8, LengthB ) )
    holoScaleUnits( 4, vec( 8, 8, LengthC ) )
    
    for ( I = 1, 4 ) { holoAlpha( I, 155 ), holoAng( I, entity():angles() ) }


    # Type B
    holoCreate( 10 ) #Yaw/Pitch1
    holoCreate( 11 ) #Pitch2
    holoCreate( 12 ) #Pitch3

  
    holoParent( 10, entity() )
    holoParent( 11, 10 )
    holoParent( 12, 11 )
    for ( I = 10, 12 ) { holoModel( I, "models/props_c17/oildrum001.mdl" ) }
    
    holoPos( 10, entity():toWorld( vec( 0, 0, 0 ) ) )
    holoPos( 11, holoEntity( 10 ):toWorld( vec( 0, 0, LengthA ) ) )
    holoPos( 12, holoEntity( 11 ):toWorld( vec( 0, 0, LengthB ) ) )
 
    holoScaleUnits( 10, vec( 5, 5, LengthA ) )
    holoScaleUnits( 11, vec( 5, 5, LengthB ) )
    holoScaleUnits( 12, vec( 5, 5, LengthC ) )

}

interval( 60 )


#ik_typeA( entity(), Target, 1, 2, 3, 4, LengthA, LengthB, LengthC, 999 )
ik_typeB( entity(), Target, 10, 11, 12, LengthA, LengthB, LengthC, 999 )


