@name keyframer test
@inputs GPS:vector
@outputs 
@persist Bones:table B KeyFrames:table Mode:string Delta Timer FrameTime
@trigger none

if ( first() ) {
    
    holoCreate( 1 )
    holoCreate( 2 )
    holoCreate( 3 )
    
    holoParent( 1, entity() )
    holoParent( 2, 1 )
    holoParent( 3, 2 )
    
    holoPos( 2, holoEntity( 1 ):toWorld( vec( 0, 0, -30 ) ) )
    holoPos( 3, holoEntity( 2 ):toWorld( vec( 0, 0, -24 ) ) )
    
    B++ 
    Bones[B + ".Hip", table] = table( 
        "Index" = 1, 
        "Parent" = entity(), 
        "Angle" = ang()
    )
    Bones[B + ".Knee", table] = table( 
        "Index" = 2, 
        "Parent" = holoEntity( 1 ), 
        "Angle" = ang()
    )
    Bones[B + ".Foot", table] = table( 
        "Index" = 3, 
        "Parent" = holoEntity( 2 ), 
        "Angle" = ang()
    )
    
    
    function table:createKeyframe( Index, Key ) {
        #---------------------------------------------
        local Bone_Hip  = This[Index + ".Hip", table]
        local Bone_Knee = This[Index + ".Knee", table]
        local Bone_Foot = This[Index + ".Foot", table]
        
        local Bone_Index_Hip  = Bone_Hip["Index", number]
        local Bone_Index_Knee = Bone_Knee["Index", number]
        local Bone_Index_Foot = Bone_Foot["Index", number]
        
        local Bone_Entity_Hip  = holoEntity( Bone_Index_Hip )
        local Bone_Entity_Knee = holoEntity( Bone_Index_Knee )
        local Bone_Entity_Foot = holoEntity( Bone_Index_Foot )
                
        local Bone_Parent_Hip  = Bone_Hip["Parent", entity]
        local Bone_Parent_Knee = Bone_Knee["Parent", entity]
        local Bone_Parent_Foot = Bone_Foot["Parent", entity]
        
        #---------------------------------------------
        local Frame = table(
            "Angle.Hip"  = Bone_Parent_Hip:toLocal( Bone_Entity_Hip:angles() ),
            "Angle.Knee" = Bone_Parent_Knee:toLocal( Bone_Entity_Knee:angles() ),
            "Angle.Foot" = Bone_Parent_Foot:toLocal( Bone_Entity_Foot:angles() )
        )
        KeyFrames[Key, table] = Frame
        
        printTable(Frame)
    }
    
    function number cosine( A, B, C ) {
        return acos( ( A^2 + B^2 + C^2 )/( 2*A*B ) )    
    }
    
    function table:applyIK( Index, Effector:vector ) {
        #---------------------------------------------
        local Bone_Hip  = This[Index + ".Hip", table]
        local Bone_Knee = This[Index + ".Knee", table]
        local Bone_Foot = This[Index + ".Foot", table]
        
        local Bone_Index_Hip  = Bone_Hip["Index", number]
        local Bone_Index_Knee = Bone_Knee["Index", number]
        local Bone_Index_Foot = Bone_Foot["Index", number]
        
        local Bone_Entity_Hip  = holoEntity( Bone_Index_Hip )
        local Bone_Entity_Knee = holoEntity( Bone_Index_Knee )
        local Bone_Entity_Foot = holoEntity( Bone_Index_Foot )
                
        local Bone_Parent_Hip  = Bone_Hip["Parent", entity]
        local Bone_Parent_Knee = Bone_Knee["Parent", entity]
        local Bone_Parent_Foot = Bone_Foot["Parent", entity]
        
        local Bone_Length_A = abs(Bone_Entity_Hip:toLocal( Bone_Entity_Knee:pos() ):z())
        local Bone_Length_B = abs(Bone_Entity_Knee:toLocal( Bone_Entity_Foot:pos() ):z())
        
        local Direction = Effector - Bone_Entity_Hip:pos()
        local Distance = Direction:length()
        
        Effector = Bone_Entity_Hip:pos() + Direction:normalized() * min(Distance, Bone_Length_A + Bone_Length_B)


        #---------------------------------------------
        local IK_Axis  = Bone_Parent_Hip:toLocalAxis( Effector - Bone_Entity_Hip:pos() )
        local IK_Angle = IK_Axis:toAngle():setRoll( -bearing( Bone_Entity_Hip:pos(), Bone_Parent_Hip:angles(), Effector ) )

        local IK_Length_C = min( IK_Axis:length(), Bone_Length_A + Bone_Length_B )
        local IK_Hip_Acos = acos( (IK_Length_C^2 + Bone_Length_A^2 - Bone_Length_B^2) / (2*Bone_Length_A*IK_Length_C) )
        
        local IK_Quat = quat( IK_Angle )*qRotation( vec( 0, -IK_Hip_Acos - 90, 0 ) )
        

        #---------------------------------------------
        holoAng( Bone_Index_Hip, Bone_Parent_Hip:toWorld( IK_Quat:toAngle() ) )
        holoAng( Bone_Index_Knee, Bone_Parent_Knee:toWorld( ang( -acos( (Bone_Length_B^2 + Bone_Length_A^2 - IK_Length_C^2) / (2*Bone_Length_A*Bone_Length_B) ) + 180, 0, 0 ) ) )
    }
    
    Mode = "edit"
    FrameTime = 0.1

    runOnChat( 1 )
    interval( 100 )
}


if (chatClk()) {
    
    local LS = lastSaid():explode(" ")
    local CM = LS[1, string]
        

    switch ( CM ) {
        case "!frame", 
            local Index = LS[2, string]:toNumber()
            local Key = LS[3, string]:toNumber()
            Bones:createKeyframe( Index, Key )
            
            Mode = "idle"
        break 
        
        case "!play",
            
            Mode = "play"
            Timer = Delta = 0
            
            if (LS[2, string]:toNumber() > 0) {
                FrameTime = LS[2, string]:toNumber()
            }
        
        break
        
        case "!edit",
        
            Mode = "edit"
        
        break
        
        case "!reset",
             
            Mode = "idle"
            
            foreach (K, Bone:table = Bones) {
                holoAng( Bone["Index", number], Bone["Parent", entity]:toWorld(ang()) )    
            }
        break
    }
    
}

if ( clk() ) {
    interval( 15 )
    
    switch (Mode) {
        case "play",
        
            Timer = (Timer + FrameTime)
            if (Timer > 1) {
                Delta++
                Timer = 0
            
                if (Delta <= KeyFrames:count()) {
                    Index = 1
                    
                    local Bone_Hip  = Bones[Index + ".Hip", table]
                    local Bone_Knee = Bones[Index + ".Knee", table]
                    local Bone_Foot = Bones[Index + ".Foot", table]
                    
                    local Bone_Index_Hip  = Bone_Hip["Index", number]
                    local Bone_Index_Knee = Bone_Knee["Index", number]
                    local Bone_Index_Foot = Bone_Foot["Index", number]
                    
                    local Bone_Entity_Hip  = holoEntity( Bone_Index_Hip )
                    local Bone_Entity_Knee = holoEntity( Bone_Index_Knee )
                    local Bone_Entity_Foot = holoEntity( Bone_Index_Foot )
                            
                    local Bone_Parent_Hip  = Bone_Hip["Parent", entity]
                    local Bone_Parent_Knee = Bone_Knee["Parent", entity]
                    local Bone_Parent_Foot = Bone_Foot["Parent", entity]               
                    
                    
                    local Frame = KeyFrames[Delta, table]
                    local New_Angle_Hip = Bone_Parent_Hip:toWorld( Frame["Angle.Hip", angle] )
                    local New_Angle_Knee = Bone_Parent_Knee:toWorld( Frame["Angle.Knee", angle] )
                    local New_Angle_Foot = Bone_Parent_Foot:toWorld( Frame["Angle.Foot", angle] )
                    
                    
                    Bone_Hip["OAngle", quaternion] = quat(Bone_Entity_Hip:angles())
                    Bone_Knee["OAngle", quaternion] = quat(Bone_Entity_Knee:angles())
                    Bone_Foot["OAngle", quaternion] = quat(Bone_Entity_Foot:angles())
                    
                    Bone_Hip["Angle", quaternion] = quat(New_Angle_Hip)
                    Bone_Knee["Angle", quaternion] = quat(New_Angle_Knee)
                    Bone_Foot["Angle", quaternion] = quat(New_Angle_Foot)
                        
                    #holoAng( Bone_Index_Hip, New_Angle_Hip )
                    #holoAng( Bone_Index_Knee, New_Angle_Knee )
                    #holoAng( Bone_Index_Foot, New_Angle_Foot )
                } else {
                    Delta = 0
                    Timer = -1
                    #Mode = "idle"
                }
                
            }
            
            
            foreach (K, Bone:table = Bones) {

                holoAng( Bone["Index", number], slerp(Bone["OAngle", quaternion], Bone["Angle", quaternion], Timer):toAngle())    
            }
            
           # holoAng( Bone_Index_Hip, slerp( quat( )
            #holoAng( Bone_Index_Knee, New_Angle_Knee )
            #holoAng( Bone_Index_Foot, New_Angle_Foot )
                    
                    #            Bone_Hip["OAngle", angle] = Bone_Entity_Hip:angles()
                    #Bone_Knee["OAngle", angle] = Bone_Entity_Knee:angles()
                    #Bone_Foot["OAngle", angle] = Bone_Entity_Foot:angles()
                    
                    #Bone_Hip["Angle", angle] = New_Angle_Hip
                    #Bone_Knee["Angle", angle] = New_Angle_Knee
                    #Bone_Foot["Angle", angle] = New_Angle_Foot
            
        
        break
        
        case "edit",
            Bones:applyIK( 1, GPS )
        break
    }
    
}



