@name IK Test 2
@inputs Pod:wirelink 
@persist Chair:entity [Hip Hip2]:entity Gyro:angle L1 L2 Inc 
@model models/hunter/plates/plate1x1.mdl

rangerFilter(entity())
rangerFilter(players())
Foot = rangerOffset(65,entity():toWorld(vec(1,20,0)),-entity():up())

rangerFilter(entity())
rangerFilter(players())
Foot2 = rangerOffset(65,entity():toWorld(vec(1,-20,0)),-entity():up())

rangerFilter(entity())
rangerFilter(players())
Rng = rangerOffset(100,entity():pos(),-entity():up())

if (first()) {
    L1 = 30
    L2 = 30
    
    function number cosine(A, B, C) {
        return acos((A^2 + B^2 - C^2) / (2*A*B))
    }
    function ik(L1, L2, Hip:entity, Foot:ranger, Chassis:entity, I1, I2) {    
        local Axis = Chassis:toLocalAxis(Foot:pos() - Hip:pos())
        local Angle = Axis:toAngle():setRoll(-bearing(Hip:pos(), Chassis:angles(), Foot:pos()))
        local Dist = min(Axis:length(), L1 + L2)
        local Quat = quat(Angle) * qRotation(vec(0, 90 + cosine(Dist, L1, L2), 0))
        
        holoAng(I1, Chassis:toWorld(Quat:toAngle()))
        holoAng(I2, Hip:toWorld(ang(cosine(L2, L1, Dist) + 180, 0, 0)))
    }

    Hip = holoCreate(1,entity():toWorld(vec(0,20,0)))
    holoModel(1,"hq_sphere")
    holoParent(1,entity())
    
    holoCreate(2,holoEntity(1):toWorld(vec(0,0,L1)))
    holoModel(2,"hq_sphere")
    holoParent(2,1)
    
    holoCreate(3,holoEntity(2):toWorld(vec(0,0,L2)))
    holoAlpha(3,255)
    holoParent(3,2)
    
    Hip2 = holoCreate(4, entity():toWorld(vec(0, -20, 0)))
    holoModel(4,"hq_sphere")
    holoParent(4, entity())
    
    holoCreate(5,holoEntity(4):toWorld(vec(0,0,L1)))
    holoModel(5,"hq_sphere")
    holoParent(5,4)
    
    holoCreate(6,holoEntity(5):toWorld(vec(0,0,L2)))
    holoAlpha(6,255)
    holoParent(6,5)
    
    findByClass("prop_vehicle_prisoner_pod")
    findSortByDistance(entity():pos())
    Chair = find()
    
    Chair:propFreeze(1)
    Chair:setPos(entity():pos() + vec(0,0,6))
    Chair:setAng(entity():angles() + ang(0,90,0))
    timer("attach",300)
}

if (clk("attach")) { Chair:parentTo(entity()) }

interval(25)

if(Inc < 100){Inc++}else{Inc=0}

local Driver = Chair:driver()
local W = Driver:keyForward()
local A = Driver:keyLeft()
local S = Driver:keyBack()
local D = Driver:keyRight()

holoAng(3,entity():angles())
holoAng(6,entity():angles())

ik(L1,L2,Hip,Foot,entity(),1,2)
ik(L1,L2,Hip2,Foot2,entity(),4,5)

if (changed(Foot:hit()) & Foot:hit()) {
    entity():soundPlay("footstepL", 0.3, "npc/dog/dog_footstep" + randint(1, 4) + ".wav")
}

P = entity():angles():pitch()
Y = 0
R = entity():angles():roll()

Gyro = angnorm(ang(0,A*2.4-D*2.4,0) - ang(P, Y, R))*500

if (Rng:hit()) {
    Diff = 54-Rng:distance()
    entity():applyForce((vec(0,0,Diff) * 10 - entity():vel()) * entity():mass() + entity():forward()*(S-W) * entity():mass() * 80)
    entity():applyAngForce(Gyro-entity():angVel()*entity():mass() + $Gyro*5)
}
