@name mdebug
@inputs Vehicle:entity Camera:wirelink
@outputs FootZDiff
@persist FootData:table Bones:table FrontRGPos:vector FrontRGHit
@persist Height FLLength
@persist Gaits:table WalkVel WalkCycle WalkDelta FootZDiff
@persist [R_LengthA R_LengthB R_LengthC R_Total R_SqSub R_SqAdd R_Dist R_Prod]
@persist [DestHeight JumpHeight MaxSpeed Acceleration BreakFactor]
@persist [CollisionRadius FallDampeningFactor] VelOffset:vector
@persist [Position]:vector [Angle]:angle [Speed Gravity MaxGravity] [Base Player]:entity
@trigger 

if (first()) {
    
    #--------------
    #-- Settings
    
    DestHeight          = 50
    JumpHeight          = 50
    MaxSpeed            = 30
    MaxGravity          = 10
    Acceleration        = 5
    BreakFactor         = 0.8
    CollisionRadius     = 32
    FallDampeningFactor = 0.7
    
    #--------------
    #-- Setup
    
    runOnLast(1)
    rangerHitEntities(1)
    rangerFilter(Vehicle)
    rangerPersist(1)
    
    holoCreate(1)
    holoModel(1, "models/sprops/misc/origin.mdl")
    
    Base = holoEntity(1)
    
    if (Vehicle["Position", vector]) {
        Position = Vehicle["Position", vector]
        holoPos(1, Position)
    }
    else {
        Position = entity():pos() + vec(0, 0, DestHeight)
        holoPos(1, Position)
    }
    
    function setup() {
        Camera["Parent", entity]   = Base
        Camera["Position", vector] = vec(8, 0, 38)
        Camera["Distance", number] = 0
        
        rangerFilter(Vehicle)
    }
    
    setup()

 
    function setGait(ID, Start, Length) {
        local Tbl = table()
        Start -= floor(Start)
        Length -= floor(Length)
        
        Tbl["start", number] = Start
        Tbl["end", number] = Start + Length
        
        Gaits[ID, table] = Tbl
    }
    
    function number gaitGetFract(Idx, X) {
        local Gait = Gaits[Idx, table]
        
        local Start = Gait["start", number]
        local End = Gait["end", number]
        local GFract = 0
        local GMove = 0
        
        if (X >= Start & X <= End) {
            return (X - Start) / (End - Start)
        }
        
        if (Start < 0) {
            if (X >= abs(Start) & X <= 1) {
                return (X - Start) / (End - Start)
            }
        }
        elseif (End > 1) {
            if (X + 1 >= Start & X + 1 <= End) {                    
                return (X + 1 - Start) / (End - Start)
            }
        }
        
        return 0
    }
    
    function void resetLeg(Str:string) {
        FootData[Str + ".Dest", vector] = FootData[Str + ".Pos", vector]
        FootData[Str + ".Prev", vector] = FootData[Str + ".Pos", vector]
        
        FootData[Str + ".IsMoving", number] = 0
        FootData[Str + ".ShouldMove", number] = 0
    }
    
    function number lerp(A, B, X) {
        return A + (B - A) * X
    }
    
    function vector bezierCurve(A:vector, B:vector, C:vector, X) {
        local V1 = mix(B, A, X)
        local V2 = mix(C, B, X)
        
        return mix(V2, V1, X)
    }
    

    holoCreate(10, Base:toWorld(vec(-5, 23, -5))), holoParent(10, Base)
    holoCreate(11, holoEntity(10):toWorld(vec())), holoParent(11, 10)
    holoCreate(12, holoEntity(11):toWorld(vec(0,0,-36))), holoParent(12, 11)
    holoCreate(13, holoEntity(12):toWorld(vec(0,0,-30))), holoParent(13, 12)
    holoCreate(14, holoEntity(13):toWorld(vec(0,0,-30))), holoParent(14, 13)
    
    holoCreate(15, Base:toWorld(vec(-5, -23, -5))), holoParent(15, Base)
    holoCreate(16, holoEntity(15):toWorld(vec())), holoParent(16, 15)
    holoCreate(17, holoEntity(16):toWorld(vec(0,0,-36))), holoParent(17, 16)
    holoCreate(18, holoEntity(17):toWorld(vec(0,0,-30))), holoParent(18, 17)
    holoCreate(19, holoEntity(18):toWorld(vec(0,0,-30))), holoParent(19, 18)
 
    Bones["FrontLeg.1.Shoulder0", number] = 10
    Bones["FrontLeg.1.Shoulder1", number] = 11
    Bones["FrontLeg.1.Knee", number]      = 12
    Bones["FrontLeg.1.Ankle", number]     = 13
    Bones["FrontLeg.1.Wrist", number]     = 14

    Bones["FrontLeg.2.Shoulder0", number] = 15
    Bones["FrontLeg.2.Shoulder1", number] = 16
    Bones["FrontLeg.2.Knee", number]      = 17
    Bones["FrontLeg.2.Ankle", number]     = 18
    Bones["FrontLeg.2.Wrist", number]     = 19
    
    local ScaleFactor = 1
    R_LengthA = 30*ScaleFactor
    R_LengthB = 30*ScaleFactor
    R_LengthC = 36*ScaleFactor + R_LengthA + R_LengthB - 0.5
    R_Total   = R_LengthA + R_LengthB - 0.5
    R_SqSub   = R_LengthA^2 - R_LengthB^2
    R_SqAdd   = R_LengthA^2 + R_LengthB^2
    R_Dist    = R_LengthA * 2
    R_Prod    = R_LengthA * R_LengthB * 2
    
    FLLength = R_LengthA + R_LengthB + R_LengthC
    Height = DestHeight
   

    function dragon_ik_rear( Target:vector, FootAng:angle, B, C, D, E, F ) {
        local Normal = Target - holoEntity(B):pos()
        local LocalA = Base:toLocalAxis(Normal)
        local LocalB = holoEntity(B):toLocalAxis(Normal)
        local TangentB = atan(-LocalB[3], LocalB[1]) - 100 + 65
    
        holoAng(B, Base:toWorld(ang(0, 0, 90 + atan(LocalA[3], LocalA[2]))))
        holoAng(C, holoEntity(B):toWorld(ang(TangentB + (holoEntity(C):pos():distance(Target) / R_LengthA)*45 + 180, 0, 0)))
    
        local LocalC = holoEntity(C):toLocalAxis(Target - holoEntity(D):pos())
        local Distance = min(LocalC:length(), R_Total)
    
        holoAng(D, holoEntity(C):toWorld(ang(atan(-LocalC[3], LocalC[1]) - 90 + acos((Distance^2 + R_SqSub) / (R_Dist * Distance)), 0, 0)))
        holoAng(E, holoEntity(D):toWorld(ang(180 + acos((R_SqAdd - Distance^2) / R_Prod), 0, 0)))
        holoAng(F, FootAng)
    }
    
    for (I = 1, 2) {
        local Shoulder = Bones["FrontLeg." + I + ".Shoulder0", number]
        local LRG = rangerOffset(FLLength * 2, holoEntity(Shoulder):pos(), vec(0, 0, -1))
        FootData["FrontLeg." + I + ".Dest", vector] = LRG:position()
        FootData["FrontLeg." + I + ".Pos", vector] = LRG:position()
    }
}

interval(60)

#--------------
#- Vehicle
Player = Vehicle:driver()

if (!Vehicle:parent()) {
    Vehicle:propFreeze(1)
    
    local Pos = Base:toWorld(vec(0, 0, 0))
    local Ang = Base:toWorld(ang(0, -90, 0))

    Vehicle:setPos(Pos)
    Vehicle:setAng(Ang)
    
    if ((Vehicle:pos() - Pos):length() < 0.1 & vec(Vehicle:angles() - Ang):length() < 0.1) {
        Vehicle:parentTo(Base)
        
        setup()
    }
}
if (last()) { 
    Vehicle["Position", vector] = Position
    Vehicle:deparent()
}


if (Player) {

    #--------------
    #- Camera

    Camera["Activated", number] = 1
    if (Player:keyReload()) { Vehicle:ejectPod() }
    
    #--------------
    #- Directions
    
    local Trace = rangerOffsetHull(DestHeight*2, Position, vec(0, 0, -1), vec(24))
    
    local AimDir  = Vehicle:toLocalAxis(Player:eye()):setZ(0):normalized()
    local SideDir = AimDir:cross(vec(0, 0, 1))
    local Forward = Player:keyForward() - Player:keyBack()
    local Strafe  = Player:keyRight() - Player:keyLeft()
    
    if (Forward | Strafe) { Speed = min(Speed + Acceleration, MaxSpeed) } else { Speed *= BreakFactor }

    local MoveDir = (AimDir*Forward + SideDir*Strafe):normalized()
    
    #--------------
    #- Gravity

     VelOffset = MoveDir*Speed# + vec(0, 0, Gravity))
    
    if (Trace:distance() <= DestHeight + 2) {
        Gravity *= FallDampeningFactor
        
        local Diff = Trace:distance() - DestHeight
        Position += Diff*vec(0, 0, -1)
        VelOffset += Diff*vec(0, 0, -1)
    }
    elseif (Trace:distance() <= DestHeight + 20) {
        if (MoveDir:length() > 0) {
            local SlopeDot = MoveDir:dot(Trace:hitNormal())
            
            if (SlopeDot > 0.1) {
                local Diff = Trace:distance() - DestHeight
                Position += Diff*vec(0, 0, -1)
                VelOffset += Diff*vec(0, 0, -1)
            }
            
        }
    }
    else { 
        Gravity = min(Gravity - 3, Gravity)
    }
    
    VelOffset += vec(0, 0, Gravity)
    
    #--------------
    #- Movement
    
    Position += MoveDir*Speed + vec(0, 0, Gravity)
   
    
    if (MoveDir:length() > 0) {
        Angle = slerp(quat(MoveDir:toAngle()), quat(Angle), 0.5):toAngle()

        local Trace = rangerOffsetHull(CollisionRadius, Position, MoveDir, vec(CollisionRadius))
        
        local Dist = Trace:distance()
        if (Dist <= CollisionRadius) {
            local Normal = Trace:hitNormal()
            local Diff = -(Dist - CollisionRadius)
            local Approach = MoveDir * Speed
            
            local Reflection = -2 * (Approach:dot(Normal)) * Normal - Approach
            
            Position += Reflection:normalized() * Diff
        }
    }
    
    VelOffset *= (1000/60)
    
    #VelOffset = (MoveDir*Speed + vec(0, 0, Gravity))*(1000/60)
    #VelOffset = $Position*(1000/15)
    
    
    FootZDiff = clamp( FootData["FrontLeg.1.Pos", vector]:z() - FootData["FrontLeg.2.Pos", vector]:z(), -25, 25 )
    holoPos(1, mix(Position, Base:pos(), 0.5) + (Base:right()*FootZDiff/4):setZ(abs(FootZDiff)) )
    holoAng(1, Angle)
    

    
    Anim = ops()
}
else {
    Camera["Activated", number] = 0
}


local GaitSizeFront = clamp(0.5, 0, 0.8)
local GaitSizeRear = clamp(0.4, 0, 0.8)

setGait(1, 0.0, 0.5) # Right Front
setGait(2, 0.5, 0.5) # Left Front

WalkDelta = lerp(WalkDelta, 0.11, 0.1)

WalkCycle += WalkDelta

BaseForward = Base:forward()



for (I = 1, 2) {
    local Side = (I == 1 ? -1 : 1)
    local Str  = "FrontLeg." + I

    local Shoulder3    = Bones[Str + ".Shoulder0", number]
    local Wrist        = Bones[Str + ".Wrist", number]

    #local VelOffset = 0#Base:vel() / 5
    local LegOffset = BaseForward * 0

    local LRG = rangerOffset(FLLength * 2, holoEntity(Shoulder3):pos() + LegOffset + VelOffset/6, vec(0, 0, -1))
    local WristAng = Base:angles():setPitch(0)

    if (LRG:hit()) {
        #FrontRGPos += LRG:position()
        #FrontRGHit++
    
        OldNormal = holoEntity(Wrist):up()
        Normal = LRG:hitNormal()
        
        local L = acos(OldNormal:dot(Normal))
        local Angle = holoEntity(Wrist):angles():rotateAroundAxis(OldNormal:cross(Normal),L)
        local Yaw = Base:angles():yaw() - holoEntity(Wrist):angles():yaw()
        
        WristAng = Angle:rotateAroundAxis(Normal, Yaw)
        
 
    }
    
    local GFract = gaitGetFract(I, WalkCycle)
    local GMove = GFract != 0

    local ZDot = abs((LRG:position() - FootData[Str + ".Pos", vector]):normalized():dot(vec(0, 0, 1)))
    if (ZDot > 0.9) {
        FootData[Str + ".Pos", vector ] = LRG:position()
    }
    
    if (GMove) {
        FootData[Str + ".Dest", vector ] = LRG:position()
        
        if (!FootData[Str + ".IsMoving", number]) {
            FootData[Str + ".Prev", vector] = FootData[Str + ".Pos", vector]
            
            if (FootData[Str + ".Pos", vector ]:distance(LRG:position()) >= 8) {
                local Dot = 0
                if (LRG:hit()) {
                    Dot = 1 - vec( 0, 0, 1 ):dot(LRG:hitNormal())
                }

                if (LRG:distance() <= FLLength + Dot * 100 * clamp((VelOffset:setZ( 0 ):length() / 100), 0, FLLength / 2)) {
                    FootData[Str + ".ShouldMove", number ] = 1
                }
            }
        }
        
        if (FootData[Str + ".ShouldMove", number]) {
            
            local Prev = FootData[Str + ".Prev", vector]
            local Dest = LRG:position()
            
            local Mid = mix(Dest, Prev, 0.4) + vec(0, 0, clamp((Prev:distance(Dest) / 3), 0, Height / 1.5))
            
            local Pos = bezierCurve(Prev, Mid, Dest, GFract)
            FootData[Str + ".Pos", vector] = Pos
        }
    }
    else {
        FootData[Str + ".ShouldMove", number] = 0
        if (FootData[Str + ".IsMoving", number]) {
            FootData[Str + ".Pos", vector] = FootData[Str + ".Dest", vector]
        }
    }
    
    FootData[Str + ".IsMoving", number] = GMove & FootData[Str + ".ShouldMove", number]

    dragon_ik_rear( FootData[Str + ".Pos", vector], WristAng, 
        Bones[Str + ".Shoulder0", number], 
        Bones[Str + ".Shoulder1", number],
        Bones[Str + ".Knee", number],
        Bones[Str + ".Ankle", number],
        Bones[Str + ".Wrist", number]
    )
    #holoPos(Wrist, FootData[Str + ".Pos", vector] + vec(0, 0, 2))
    #holoAng(Wrist, WristAng)
}
    if (WalkCycle > 1) {
        WalkCycle = 0
    }
