@name Mech
@inputs Pod:wirelink 
@persist [R1 R2]:vector 
@persist Hip:entity TPOSV:array 
@persist Index Height Width Offset Walk Yaw L1 L2 L3 Y Y2 Total 
@persist Mode 
@model models/hunter/blocks/cube05x05x025.mdl

local Seat = Pod["Entity",entity]
local Driver = Seat:driver()
local W = Driver:keyForward()
local A = Driver:keyLeft()
local S = Driver:keyBack()
local D = Driver:keyRight()
local Shift = Driver:keySprint()
local Space = Driver:keyJump() 
local M1 = Driver:keyAttack1()
local M2 = Driver:keyAttack2() 
local R = Driver:keyReload()

if (first() | dupefinished()) {
    Height = 65 ## Height of torso above the feet
    Width  = 25  ## Width between hipjoints and hip prop
    Offset = 0   ## Distance of hipjoint up and down

    Hip = entity()
    Pos = Hip:pos()
    
    Hip:setMass(50000)
    
    L1 = 35
    L2 = 25
    L3 = 35
    Total = L1 + L2 + L3
    Y = 1
    Y2 = -0.5
    
    R1 = rangerOffset(30,Hip:toWorld(vec(Width*Y2,-Width*Y,20)),vec(0,0,-1)):position() 
    R2 = rangerOffset(30,Hip:toWorld(vec(Width*Y2,Width*Y,20)),vec(0,0,-1)):position()
    
    Yaw = Hip:angles():yaw()
    
    timer("checkfeet",500)
    
    rangerFilter(Hip)
    rangerPersist(1)
    
    holoCreate(100,Hip:toWorld(vec(0,-Width,-Offset)))
    holoParent(100,Hip)
    holoCreate(200,holoEntity(100):toWorld( vec(L1,0,0)))
    holoParent(200,100)
    holoCreate(300,holoEntity(200):toWorld(vec(0,0,-L2)))
    holoParent(300,200)
    holoCreate(400,holoEntity(300):toWorld(vec(0,0,L3)))
    holoParent(400,300)
    holoCreate(500,Hip:toWorld( vec(0,Width,-Offset)))
    holoParent(500,Hip)
    holoCreate(600,holoEntity(500):toWorld( vec(L1,0,0)))
    holoParent(600,500)
    holoCreate(700,holoEntity(600):toWorld(vec(0,0,-L2)))
    holoParent(700,600)
    holoCreate(800,holoEntity(700):toWorld(vec(0,0,L3)))
    holoParent(800,700)
}

function number cos(A,B,C) { 
    return acos((C^2 + B^2 - A^2)/(2*B*C))
}
     
function holoanim(H,K,A,F,Ta:vector,N,D1,D2,D3) {
    if (!Mode) {
        Dis = vec2(TPOSV[N,vector]):distance(vec2(Ta))
        Ofz = clamp(vec2(Ta):distance(vec2(TPOSV[N,vector]))/5,0,100)
        Ang = ((Ta + vec(0,0,Ofz)) - TPOSV[N,vector]):toAngle() 
        TPOSV[N,vector] = (TPOSV[N,vector] + vec(clamp(Dis,0,30),0,0):rotate(Ang))
    }
    
    if (Mode) { 
        TPOSV[N,vector] = Ta
    }
    
    # Hip 
    Bea = -bearing(holoEntity(H):pos(),Hip:angles(),TPOSV[N,vector])
    Ang = Hip:toLocalAxis(TPOSV[N,vector] - holoEntity(H):pos()):toAngle():setRoll(Bea)
    Off = Total - abs(min(holoEntity(H):pos():distance(TPOSV[N,vector]),Total))
    Qua = quat(Ang) * qRotation(vec(0,-Off + 10,0))
    holoAng(H,Hip:toWorld(slerp(quat(H),Qua,1):toAngle()))
    
    #Knee 
    Distance = min(holoEntity(K):pos():distance(TPOSV[N,vector]),(D2 + D3))
    Axis = holoEntity(H):toLocalAxis(holoEntity(K):pos() - TPOSV[N,vector])
    Atan = atan(-Axis:z(), Axis:x()) + cos(D3,D2,Distance) + 90
    holoAng(K,holoEntity(H):toWorld(ang(Atan,0,0)))
    
    #Knee2 
    holoAng(A,holoEntity(K):toWorld(ang(cos(Distance,D2,D3),0,0)))
    
    #Foot
    holoAng(F,Hip:angles())
}

function applytorque(TaAn:angle,Ta:entity) { 
    Ve = Ta:toLocal(rotationVector(quat(TaAn)/quat(Ta)) + Ta:pos())
    Ta:applyTorque((150 * Ve - 25 * Ta:angVelVector()) * Ta:inertia())
}
    
interval(30)

## Hip applyforce ##
if (!Mode) { 
    Swag = clamp(vec2(Hip:pos()):distance(vec2((TPOSV[1,vector] + TPOSV[2,vector])/2))/(5+Shift*5),0,20)
    TZ = (R1:z() + R2:z())/2 + Swag
    Up = vec(0,0,TZ + Height + Offset - Hip:pos():z())
    Ac = Hip:forward() * (W-S)*(145 + 20*Shift) + Hip:right() * (D-A) * 12 * Space
    Pa = -Hip:vel()*0.1
    Hip:applyForce((Up + Ac + Pa) * Hip:mass() - Hip:vel() * Hip:mass())
}
    
## Walking triggers ##

if (clk("checkfeet")) { 
    timer("checkfeet",350)
    Walk = !Walk
}

VelX = clamp(abs(Hip:velL():x())*(W-S)/2,-500,500)
VelY = clamp(abs(Hip:velL():y())*(A-D)*Space,-80,80)
AngV = clamp(Hip:angVel():yaw()/2,-100,100)

if (!Mode) {
    if (changed(Walk == 1) & Walk == 1) {
        TPOSV[2,vector] = holoEntity(800):pos() 
        R2 = rangerOffset(1000,Hip:toWorld(vec(VelX + Width * Y2,VelY + Width * Y + (AngV * (A-D)),100)),vec(0,0,-1)):position()
    }
    
    if (changed(Walk == 0) & Walk == 0) {
        TPOSV[1,vector] = holoEntity(400):pos()
        R1 = rangerOffset(1000,Hip:toWorld(vec(VelX + Width * Y2,VelY - Width * Y - (AngV * (A-D)),100)),vec(0,0,-1)):position()
    }
}
    
## Hip and Torso rotation ##

if (Driver) { 
    if (!Mode) { 
        if ((W|S|A|D) & !Space) { 
            Yaw = (Yaw + (A-D) * 2)%360
        }
    }
}
    
if (!Driver) {
    Yaw = Hip:angles():yaw() 
}
    
if (!Mode) {
    A1 = ang(5*Shift + Swag/2,Yaw,-5*(A-D))
}
    
applytorque(A1,Hip) 

## Holographic animations ##

holoanim(100,200,300,400,R1,1,L1,L2,L3)
holoanim(500,600,700,800,R2,2,L1,L2,L3)

if ((changed(!Driver) & !Driver)|last()) { 
    soundStop(1) 
    soundStop(2) 
    soundStop(3) 
    soundPurge() 
}

holoAng(900,Hip:toWorld(ang(-clamp(abs(Hip:velL():x()/10),0,45)+15,0,15)))
holoAng(1100,Hip:toWorld(ang(-clamp(abs(Hip:velL():x()/10),0,45)+15,0,-15)))
