@name Drone Only Flight

@inputs RPod:entity LPod:entity APod:entity

@outputs RThrust LThrust AThrust


@persist RPitch LPitch APitch TargetZ Altitude Roll Pitch Stablize
@persist LTorque:vector RTorque:vector ATorque:vector
@persist AngVel ForwardVel SideVel SpeedCounter SpinCounter UpsideDown
@persist Bearing Follow:vector Active
@persist Go BearingToFollow FlippedOff

    runOnChat(1)
    interval(20)

    function number getGroundPosition(){
        rangerHitWater(1)
        rangerFilter(entity():isWeldedTo())
        RD = rangerOffset(9999999,entity():pos(),vec(0,0,-1))

        return RD:position():z()+0
        }

    if(first()|duped()){
        Torque=120 #120 normally
        Altitude=entity():pos():z()
        }

    Follow=entity():owner():pos()
    BearingToFollow=entity():bearing(Follow)

    ###Maintain Altitude

    Altitude=clamp(Follow:z()+200,getGroundPosition(),999999999)
    TargetZ = (-entity():pos():z() + Altitude)
    HoverThrust=(TargetZ+$TargetZ*15)*2

    DistanceOfTarget = entity():pos():setZ(0):distance(Follow:setZ(0))
    if(TargetZ>100){
        }
    elseif(DistanceOfTarget>300){
        Go=clamp((DistanceOfTarget-300)*1.5/300,-1,1)
        }
    else {
        Go=-0.02
        }

    ###Angle of Thruster Pods

    if (Pitch>75 || abs(Roll)>90) {
        UpsideDown=-1
        FlippedOff=0
        APitch=entity():angles():pitch()
        }
    elseif (Pitch<-30) {
        APitch=-entity():angles():pitch()
        Stablize=0
        }
    else {
        UpsideDown=1
        FlippedOff=1
        Stablize=1
        APitch=clamp((FlippedOff*Stablize*(SpeedCounter)),-90,90)
        }

    #RPitch=clamp((FlippedOff*(SpeedCounter+SpinCounter)),-90,90)
    #LPitch=clamp((FlippedOff*(SpeedCounter-SpinCounter)),-90,90)

    Rotation = ang(-RPitch,0,0)
    Local = RPod:toLocal(vec(0,0,-1) + RPod:pos())
    RTorque = (vec(0,0,1):rotate(Rotation):cross(Local) * 400 - RPod:angVelVector() * 1/3) * vec(0,RPod:inertia():y(),0)
    RPod:applyTorque(RTorque*130)

    Rotation = ang(-LPitch,0,0)
    Local = LPod:toLocal(vec(0,0,-1) + LPod:pos())
    LTorque = (vec(0,0,1):rotate(Rotation):cross(Local) * 400 - LPod:angVelVector() * 1/3) * vec(0,LPod:inertia():y(),0)
    LPod:applyTorque(LTorque*130)

    Rotation = ang(-APitch,0,0)
    Local = APod:toLocal(vec(0,0,-1) + APod:pos())
    ATorque = (vec(0,0,1):rotate(Rotation):cross(Local) * 400 - APod:angVelVector() * 1/3) * vec(0,APod:inertia():y(),0)
    APod:applyTorque(ATorque*130)

    ###Hover Balance

    Angles=entity():angles()
    Roll=-Angles:roll()
    Pitch=Angles:pitch()
    RollCounter=(Roll+$Roll*3)*10
    PitchCounter=(Pitch+$Pitch*4)*3.5-25
    StrafeCounter=(SideVel+$SideVel*0.25)/3

    RThrust=clamp(FlippedOff*((HoverThrust/cos(RPitch))-RollCounter-StrafeCounter),0,50)
    LThrust=clamp(FlippedOff*((HoverThrust/cos(LPitch))+RollCounter+StrafeCounter),0,50)
    AThrust=clamp(FlippedOff*((HoverThrust/cos(APitch)*0-PitchCounter))+30*!FlippedOff,-40*!FlippedOff,40)

    ###Counter Movement

    ForwardVel=entity():velL():x()
    SideVel=entity():velL():y()
    AngVel=entity():angVel():yaw()
    SpeedCounter=(ForwardVel+$ForwardVel*1)*(1-Go)/10
    SpinCounter=(AngVel+$AngVel*5)/6