@name newmech
@inputs 
@outputs NodeCycle
@persist Entity:entity Nodes:table NodeCount NodeCycle [W A S D] Mass
@trigger none
@model models/sprops/rectangles/size_5/rect_48x54x3.mdl

if (first()) {
    Entity = entity()
    
    Entity:setMass(5000)
    Mass = Entity:mass()

    function addNode( ID:string, Pos:vector, DefaultHeight, Order ) {
        local TempPos = rangerOffset(DefaultHeight, Entity:toWorld(Pos), vec(0,0,-1)):pos()
        Nodes[ID, table] = table(
            "LocalPos" = Pos,
            "StartPos" = TempPos,
            "MiddlePos" = TempPos,
            "FinalPos" = TempPos,
            "OutputPos" = TempPos,
            "Order" = Order,
            "Height" = DefaultHeight,
            "Mode" = "wait"
        )
        NodeCount++
    }

    function debugNodes() {
        foreach (K, Node:table = Nodes) {
            if (!Node:exists("Debug")) {
                local Hash = hash(K)

                Node["Debug", array] = array()

                for (I = 1, 3) {
                    holoCreate(Hash, Node["FinalPos", vector])
                    holoScale(Hash, vec(3/12))
                    
                    Node["Debug", array][I, number] = Hash
                    Hash++
                }
            } else {

            }
        }
    }
    
    function runAllNodes( Rate, Direction:vector ) {
        NodeCycle += Rate
        foreach (K, Node:table = Nodes) {
            if (Node["Mode", string] == "wait") {
                if (NodeCycle >= Node["Order", number]) {
                    
                    Node["StartPos", vector] = Node["FinalPos", vector]
                    Node["FinalPos", vector] = rangerOffset(Node["Height", number], Entity:toWorld(Node["LocalPos", vector] + Direction), vec(0,0,-1)):pos()
                    Node["MiddlePos", vector] = (Node["StartPos", vector] + Node["FinalPos", vector]) / 2 + vec(0, 0, Node["Height", number] / 4)
                     
                    Node["Lerp", number] = 0 
                    Node["Mode", string] = "calc"
                }
                continue
            } 
    
            if (Node["Mode", string] == "calc") {
                Node["Lerp", number] = min(Node["Lerp", number] + Rate, 1)
                Node["OutputPos", vector] = bezier(Node["StartPos", vector], Node["MiddlePos", vector], Node["FinalPos", vector], Node["Lerp", number])
                
                holoPos(Node["Debug", array][1, number], Node["OutputPos", vector])
                
                if (Node["Lerp", number] >= 1) {
                    Node["OutputPos", vector] = Node["FinalPos", vector]
                    Node["Wait", number] = Node["Order", number]
                    Node["Mode", string] = "delay"
                    continue
                }
            }
            
            if (Node["Mode", string] == "delay") {
                if (NodeCycle >= Node["Wait", number]) {
                    Node["Mode", string] = "wait"
                    continue
                }
            }
        }
        if (NodeCycle > 1) { NodeCycle = 0 }
    }
    
    function vector avgNodePos( Offset:vector ) {
        local TempPos = vec()
        foreach (K, Node:table = Nodes) {
            TempPos += Node["OutputPos", vector]    
        }
        return (TempPos / NodeCount) + Offset
    }

    addNode( "FL", vec(50,50,0), 100, 0 )
    addNode( "FR", vec(50,-50,0), 100, 0.5 )
    addNode( "RL", vec(-50,50,0), 100, 0.75 )
    addNode( "RR", vec(-50,-50,0), 100, 0.25 )
    
    debugNodes()
}

interval(60)

runAllNodes( 0.1, Entity:forward()*50 )


Average = avgNodePos( vec(0, 0, 50) )
Entity:applyForce(((Average - Entity:pos())*10 - Entity:vel())*Mass)
Entity:propGravity(0)
