#@name octo test
@inputs 
@outputs Entity:entity
@persist Data:table Pairs:table Entity:entity Owner:entity Legs:table
@trigger none
@model models/maxofs2d/cube_tool.mdl

if (first()) {    
    function vector bezier(A:vector, B:vector, C:vector, T) {
        return mix(mix(A, B, T), mix(B, C, T), T)    
    }
    
    function mechLib_debugIK(Type:string, LengthA, LengthB, LengthC, Factor) 
    {
        if (Type == "spider") 
        {
            local BoneS = vec(0.45, 0.45, 0.45) * Factor
            local BoneC = vec(45, 45, 45)
            local BoneM = "cube"
            
            local LinkC = vec(125, 125, 125)
            local LinkM = "cube"
        
            for (I = 1, Data:count())
            {
                local Leg = Data[I, table]
                local Index = randint(500, 100000)

                local RootPos = mix(vec(), Leg["Off", vector], 0.75)
        
                local HipA = holoCreate(Index, Entity:toWorld(RootPos), BoneS, Entity:angles(), BoneC, BoneM)
                            holoParent(Index, Entity)
                
                local HipB = holoCreate(Index + 1, HipA:pos(), BoneS, HipA:angles(), BoneC, BoneM)
                            holoParent(Index + 1, HipA)
                            
                local JointA = holoCreate(Index + 2, HipB:toWorld(vec(0, 0, LengthA)), BoneS, HipB:angles(), BoneC, BoneM)
                            holoParent(Index + 2, HipB)
        
                local JointB = holoCreate(Index + 3, JointA:toWorld(vec(0, 0, LengthB)), BoneS, JointA:angles(), BoneC, BoneM)
                            holoParent(Index + 3, JointA)
        
                holoCreate(Index + 4, HipB:toWorld(vec(0, 0, LengthA / 2)), vec(3* Factor, 3 * Factor, LengthA) / 12, HipB:angles(), LinkC, LinkM), holoParent(Index + 4, HipB)
                holoCreate(Index + 5, JointA:toWorld(vec(0, 0, LengthB / 2)), vec(2 * Factor, 2 * Factor, LengthB) / 12, JointA:angles(), LinkC, LinkM), holoParent(Index + 5, JointA)
                holoCreate(Index + 6, JointB:toWorld(vec(0, 0, LengthA / 2)), vec(1 * Factor, 1 * Factor, LengthA) / 12, JointA:angles(), LinkC, LinkM), holoParent(Index + 6, JointB)
                holoCreate(Index + 7, JointB:toWorld(vec(-3, 0, LengthA + 0)), vec(1, 1, 1), JointB:toWorld(ang(180, 0, 0))), holoParent(Index + 7, JointB)
                holoModel(Index + 7, "models/props_junk/Shoe001a.mdl")

                Leg["debugIK", table] = table(
                    "type" = "ik_types_" + Type,
    
                    "lengthA" = LengthA,
                    "lengthB" = LengthB,
                    "lengthADoubleS" = 2 * LengthA ^ 2,
                    "lengthADouble" = LengthA + LengthA,
    
                    "base" = Entity,
                    "hipA" = HipA,
                    "hipB" = HipB,
                    "jointA" = JointA,
                    "jointB" = JointB,
    
                    "index" = Index,
                    "key" = I
                )
           }
        }
    }
    
    function ik_types_spider(TData:table, Target:vector)
    {
        local Index = TData["index", number]
        local YawAxis = TData["base", entity]:toLocalAxis(Target - TData["hipA", entity]:pos())
      
        holoAng(Index, TData["base", entity]:toWorld(ang(0, atan(YawAxis:y(), YawAxis:x()), 0))) #-HipA
        
        local PitchAxis = TData["hipA", entity]:toLocalAxis(Target - TData["hipB", entity]:pos())
        local PitchDist = min(PitchAxis:length() - TData["lengthB", number], TData["lengthADouble", number])
    
        holoAng(Index + 1, TData["hipA", entity]:toWorld(ang(atan(PitchAxis:x(), PitchAxis:z()) - acos( PitchDist / TData["lengthADouble", number] ), 0, 0)))
        holoAng(Index + 2, TData["hipB", entity]:toWorld(ang(asin(PitchDist / TData["lengthADouble", number]) - 90, 180, 0)))
        holoAng(Index + 3, TData["hipB", entity]:toWorld(ang(acos((PitchDist^2 - TData["lengthADoubleS", number]) / TData["lengthADoubleS", number]) * sign(PitchDist), 0, 0)))
    }
    
    function mechLib_runIK()
    {
        foreach (K, Leg:table = Legs) {
            if (Leg["debugIK", table]) {
                Leg["debugIK", table]["type", string]( Leg["debugIK", table], Leg["output", vector] ) 
            }    
        }
    }
    
    
    Entity = entity()
    Owner = owner()
    
    local Normal = rangerOffset(9999999, Entity:pos(), -Entity:up()):hitNormal()
    for (I = 1, 8) {
        local Start = Entity:toWorld(vec(100, 0, 0):rotate(0, (360 / 8) * I - 22.5, 0))
        Data[I, table] = table( "Off" = Entity:toLocal(Start), "P0" = Start, "P1" = Start, "P2" = Start, "Out" = Start, "Norm" = Normal, "Hold" = Start, "ID" = I )   
    }
    
    Pairs[1, table] = table(Data[1, table], Data[7, table], Data[3, table], Data[5, table], "wait", 0, 2)
    Pairs[2, table] = table(Data[8, table], Data[2, table], Data[6, table], Data[4, table], "calc", 0, 1)
    
    mechLib_debugIK("spider", 50, 30, 50, 1) 
    
    function number entity:bearing2( Target:vector, Mul, Condition ) {
        return -bearing(This:pos(), This:angles(), Target) / Mul * Condition
    }
 

}

interval(90)

local Average = vec()
local Normal = vec()

W = Owner:keyPressed("up")
S = Owner:keyPressed("down")
A = Owner:keyPressed("left")
D = Owner:keyPressed("right")
Space = Owner:keyPressed("g")

Fd = Owner:keyPressed("pad_8")
Bk = Owner:keyPressed("pad_2")
Lf = Owner:keyPressed("pad_4")
Rt = Owner:keyPressed("pad_6")

for (I = 1, 2) {
    local Pair = Pairs[I, table]

    switch (Pair[5, string]) {
        case "exec", 
            Pair[6, number] = min(Pair[6, number] + 0.5, 1)
            
            local Time = sin(Pair[6, number] * 90)
            for (K = 1, 4) {
                local Data = Pair[K, table]
                Data["Out", vector] = bezier(Data["P2", vector], Data["P1", vector], Data["P0", vector], Time)
            }
            
            if (Pair[6, number] >= 1) {
                Pair[5, string] = "wait"
                Pairs[Pair[7, number], table][5, string] = "calc"
                
                Entity:soundPlay("Step" + I, 0, "@^player/footsteps/concrete" + randint(1, 4) + ".wav")
            }
        break
        
        case "calc", 
            if (W | A | S | D) {
                local Forward = Entity:forward()*(W-S)*100- Entity:right()*(A-D)*100
                local Height = vec(0, 0, Forward:length() / pi())

                local Probe = rangerOffset(100, Entity:pos() + Forward * 2, -Entity:up())
                if (Probe:hit()) {
                    for (K = 1, 4) {
                        local Data = Pair[K, table]
                        local Ranger = rangerOffset(200, Entity:toWorld(Data["Off", vector]) + Forward, -Data["Norm", vector]) #Down)
    
                        #if (Ranger:hit()) {
                            Data["Norm", vector] = Ranger:hitNormal()
                            Data["P0", vector] = Data["P2", vector]
                            Data["P2", vector] = Ranger:position() + Data["Norm", vector] * 6
                            Data["P1", vector] = (Data["P0", vector] + Data["P2", vector]) / 2 + Height
                            Data["Hold", vector] = Entity:toLocal(Data["P2", vector]):setZ(0)
                        #}
                    }
                    
                    Pair[5, string] = "exec"
                    Pair[6, number] = 0
                }
            }
        break
    }
    
    for (J = 1, 4) { 
        local Temp = Pair[J, table]
        
        Average += Temp["Out", vector]
        Normal += Temp["Norm", vector]
        
        Temp["debugIK", table]["type", string]( Temp["debugIK", table], Temp["Out", vector] ) 
    }
}

local LfRt = (Lf - Rt)*35
local FdBk = (Fd - Bk)*35

Normal = Normal / 8
Average = Average / 8 + Normal*(35 + Space*35 + (LfRt | FdBk)*15) + Entity:forward()*FdBk + Entity:right()*LfRt

local Rotate = vec(-LfRt, -FdBk, Entity:bearing2(Owner:aimPos(), 2, W | S | A | D)) / 3
local Angle = Normal:cross(Entity:forward()):normalized():cross(Normal)
local Torque = Entity:toLocalAxis(rotationVector(quat(Angle, Normal) * qRotation(Rotate) / quat(Entity)))

Entity:applyForce(((Average - Entity:pos())*10 - Entity:vel()) * Entity:mass())
Entity:applyTorque((Torque*200 - Entity:angVelVector()*20) * Entity:inertia())
Entity:propGravity(0)
