@name anim/libraries/animate
@persist FootControllers:table IKinematics:table [FrameTime FrameAdvn] 
@persist [WorldDownVec WorldRightVec]:vector

#----------------------------------------------
#-- Section: Variables

WorldDownVec = vec(0, 0, -1)
WorldRightVec = vec(0, 1, 0)


#----------------------------------------------
#-- Section: Foot Controller

function addFootController(UniqueID:string, Ent:entity, RPos:vector, [GaitOffset FootAdjust StepMaxL StepMaxH]){
    FootControllers[UniqueID, table] = table(
        "ent" = Ent, 
        "rpos" = RPos, 
        "rmax" = 50, #StepMaxH+10, 
        "stepMaxL" = StepMaxL, 
        "stepMaxH" = StepMaxH, 
        "footAdjust" = FootAdjust, 
        "gaitOffset" = GaitOffset
    )
}

function addHitSound(UniqueID:string, SoundPath:string, Duration){
    FootControllers[UniqueID, table]["sounds", number] = 1
    FootControllers[UniqueID, table]["hitSound", string] = SoundPath
    FootControllers[UniqueID, table]["hitSoundD", number] = Duration
}

function addStepSound(UniqueID:string, SoundPath:string, Duration){
    FootControllers[UniqueID, table]["sounds", number] = 1
    FootControllers[UniqueID, table]["stepSound", string] = SoundPath
    FootControllers[UniqueID, table]["stepSoundD", number] = Duration
}

function updateFootControllers(Anim, ShowDebugHolos){
    FrameTime = FrameTime+Anim
    FrameAdvn = changed(FrameTime) #$FrameTime != 0
    
    foreach(K, Controller:table = FootControllers){
        Controller["stepL", number] = Controller["stepL", number]+(Controller["stepMaxL", number]*FrameAdvn-Controller["stepL", number])*0.5
        Controller["stepH", number] = Controller["stepH", number]+(Controller["stepMaxH", number]*FrameAdvn-Controller["stepH", number])*0.5
        
        #local Start = Controller["ent", entity]:toWorld(Controller["rpos", vector]+vec(sin(FrameTime+Controller["gaitOffset", number])*Controller["stepL", number], 0, 0))
        local Trace = rangerOffset(
            Controller["rmax", number], 
            Controller["ent", entity]:toWorld(Controller["rpos", vector]+vec(sin(FrameTime+Controller["gaitOffset", number])*Controller["stepL", number], 0, 0)),  
            -Controller["ent", entity]:up()         
            #WorldDownVec
        )

        local FootAngle = Trace:hitNormal():toAngle()
            FootAngle = FootAngle:rotateAroundAxis(FootAngle:forward(), Controller["ent", entity]:angles():yaw())
            FootAngle = FootAngle:rotateAroundAxis(FootAngle:right(), sin(FrameTime+Controller["gaitOffset", number])*Controller["stepL", number]-90)

        local ZValue = cos(FrameTime+Controller["gaitOffset", number])*Controller["stepH", number]

        if(ZValue < 0){
            if(!Controller["isDown", number]){
                Controller["isDown", number] = 1

                if(Controller["hitSound", string]){
                    Controller["ent", entity]:soundPlay("hit" + K, Controller["hitSoundD", number], Controller["hitSound", string])#, Controller["hitSoundD", number])
                }
            }
        }
        else{
            if(Controller["isDown", number]){
                Controller["isDown", number] = 0

                if(Controller["stepSound", string]){
                    Controller["ent", entity]:soundPlay("step" + K, Controller["stepSoundD", number], Controller["stepSound", string])#, Controller["stepSoundD", number])
                }
            }
        }
        
        #local Target = Start + (Trace:position() - Start):normalized() * min(70, Trace:distance())

        Controller["ang", angle] = FootAngle
        Controller["pos", vector] = Trace:position()+Controller["ent", entity]:up()*max(Controller["footAdjust", number], ZValue)#Trace:hitNormal()*max(Controller["footAdjust", number], ZValue)
        Controller["zvl", number] = ZValue
        
        if(Controller["iKinematics", table]){
            Controller["iKinematics", table]["solveIK", string](Controller["iKinematics", table], Controller["pos", vector], Controller["ang", angle])
        }

        if(ShowDebugHolos){
            if(!Controller["debug", number]){
                local UniqueID = hash(K)

                holoCreate(UniqueID)
                holoScale(UniqueID, vec(1/2))
                holoModel(UniqueID, "sphere")

                holoCreate(UniqueID+1, holoEntity(UniqueID):toWorld(vec(0, 0, 6)), vec(1/2), holoEntity(UniqueID):angles())
                holoParent(UniqueID+1, UniqueID)
                holoModel(UniqueID+1, "models/sprops/misc/origin.mdl")

                Controller["debug", number] = UniqueID
                continue 
            }

            holoPos(Controller["debug", number], Controller["pos", vector])
            holoAng(Controller["debug", number], Controller["ang", angle])
        }
    }
}


#----------------------------------------------
#-- Section: Inverse Kinematics

function addIKinematics(UniqueID:string, Type:string, Chain:array){
    if(!IKinematics:exists(Type)){ error("ERROR: Invalid IK type!") }
    FootControllers[UniqueID, table]["iKinematics", table] = IKinematics[Type, table]["setupIK", string](Chain)[table]
    FootControllers[UniqueID, table]["iKinematics", table]["solveIK", string] = IKinematics[Type, table]["solveIK", string]
}

function addIKinematicsType(UniqueID:string, Setup:string, Solve:string){
    IKinematics[UniqueID, table] = table("setupIK" = Setup, "solveIK" = Solve)    
}


#----------------------------------------------
#-- Section: Inverse Kinematics
#-- Type: Humanoid.Basic

addIKinematicsType("Humanoid.Basic", "setup_humanoid_basic", "solve_humanoid_basic")

function table setup_humanoid_basic(Chain:array){
    local Entity = Chain[1, entity]
    local Joint0 = Chain[2, number] #hip
    local Joint1 = Chain[3, number] #knee
    local Joint2 = Chain[4, number] #foot
    
    local Length0 = round((holoEntity(Joint0):pos()-holoEntity(Joint1):pos()):length(), 1)
    local Length1 = round((holoEntity(Joint1):pos()-holoEntity(Joint2):pos()):length(), 1)
    
    local Table = table(
        "entity" = Entity, 
        "joint0" = Joint0, 
        "joint1" = Joint1, 
        "joint2" = Joint2, 
        "joint0e" = holoEntity(Joint0), 
        "joint1e" = holoEntity(Joint1), 
        "length0" = Length0, 
        "length1" = Length1
    )
    
    return Table
}

function solve_humanoid_basic(IK:table, Pos:vector, Ang:angle){
    local Normal = IK["entity", entity]:toLocalAxis(Pos-IK["joint0e", entity]:pos())
    local Angle = Normal:toAngle():setRoll(-bearing(IK["joint0e", entity]:pos(), IK["entity", entity]:angles(), Pos))
    local Dist = min(Normal:length(), IK["length0", number]+IK["length1", number])
    
    local Acos = acos((Dist^2+IK["length0", number]^2-IK["length1", number]^2)/(2*IK["length0", number]*Dist))
    local Quat = quat(Angle)*qRotation(WorldRightVec, Acos+90)
    
    holoAng(IK["joint0", number], IK["entity", entity]:toWorld(Quat:toAngle()))
    holoAng(IK["joint1", number], IK["joint0e", entity]:toWorld(ang(180+acos((IK["length1", number]^2+IK["length0", number]^2-Dist^2)/(2*IK["length0", number]*IK["length1", number])), 0, 0)))
    holoAng(IK["joint2", number], Ang)
}


#----------------------------------------------
#-- Section: Inverse Kinematics
#-- Type: Humanoid.Digitigrade

addIKinematicsType("Humanoid.Digitigrade", "setup_humanoid_digitigrade", "solve_humanoid_digitigrade")

function table setup_humanoid_digitigrade(Chain:array){
    local Entity = Chain[1, entity]
    local Joint0 = Chain[2, number] #hip
    local Joint1 = Chain[3, number] #knee
    local Joint2 = Chain[4, number] #knee2
    local Joint3 = Chain[5, number] #foot
    
    local Length0 = round((holoEntity(Joint0):pos()-holoEntity(Joint1):pos()):length(), 1)
    local Length1 = round((holoEntity(Joint1):pos()-holoEntity(Joint2):pos()):length(), 1)
    local Length2 = round((holoEntity(Joint2):pos()-holoEntity(Joint3):pos()):length(), 1)
    
    local Table = table(
        "entity" = Entity, 
        "joint0" = Joint0, 
        "joint1" = Joint1, 
        "joint2" = Joint2, 
        "joint3" = Joint3, 
        "joint0e" = holoEntity(Joint0), 
        "joint1e" = holoEntity(Joint1), 
        "joint2e" = holoEntity(Joint2), 
        "length0" = Length0, 
        "length1" = Length1, 
        "length2" = Length2
    )
    
    return Table
}

function solve_humanoid_digitigrade(IK:table, Pos:vector, Ang:angle){
    local Normal = IK["entity", entity]:toLocalAxis(Pos-IK["joint0e", entity]:pos())
    local Angle = Normal:toAngle():setRoll(-bearing(IK["joint0e", entity]:pos(), IK["entity", entity]:angles(), Pos))
    local Quat = quat(Angle)*qRotation(WorldRightVec, (Normal:length()/IK["length0", number])*IK["length0", number])
    
    holoAng(IK["joint0", number], IK["entity", entity]:toWorld(Quat:toAngle()))
    
    local Normal = IK["joint0e", entity]:toLocalAxis(Pos-IK["joint1e", entity]:pos())
    local Dist = min(Normal:length(), IK["length1", number]+IK["length2", number])
    local Acos = acos((Dist^2+IK["length1", number]^2-IK["length2", number]^2)/(2*IK["length1", number]*Dist))
    
    holoAng(IK["joint1", number], IK["joint0e", entity]:toWorld(ang(atan(-Normal[3], Normal[1])+90+Acos, 0, 0)))
    holoAng(IK["joint2", number], IK["joint1e", entity]:toWorld(ang(180+acos((IK["length2", number]^2+IK["length1", number]^2-Dist^2)/(2*IK["length1", number]*IK["length2", number])), 0, 0)))
    holoAng(IK["joint3", number], Ang)
}

