Whip's OGM Code - JDAM functionality

/*
Whip's Optical Missile Guidance System v61 - revised: 10/20/16
/// PUBLIC RELEASE ///
/// Stable ///
_______________________________________________________________________
///DESCRIPTION///
    This script allows the user to manually guide a missile with the
    heading of his/her ship! The missiles will behave similar to a TOW missile.
_______________________________________________________________________
///FEATURES///
    * Configurable spiral trajectory to avoid turrets! (can be turned off)
    * Configurable detach sequence parameters!
    * Automatic thruster orientation detection! No more pesky directional naming needed :D
    * Works in atmosphere!
    * Runs at 10 Hz (old was 60 Hz) so it is much less laggy! SIX new missiles lag less than ONE of my old ones!
    * Compensates for unwanted drift!
    * Automatically shuts off and goes ballistic if vital components are damaged
    * Safety mechanisms to stop the missile from flying up your tail pipe!
____________________________________________________________
///MISSILE SETUP INSTRUCTIONS///

===REQUIRED BLOCKS===
    * A timer
    * A Remote Control pointing FORWARD
    * A program block with this code
    * A battery or reactor (for power)
    * A merge, connector, or rotor (for attachment points)
    * A gyroscope pointing forward

    OPTIONAL BLOCKS:
    * An artificial mass block
    * A beacon
    * An antenna

    ALL BLOCKS on the missile should have the name tag "Missile 1" added in their names.

===ADDITIONAL NAME TAGS===
    Some specific blocks need ADDITIONAL name tags

    * Any detach thrusters need the name tag "Detach" in addition to the missile tag

===TIMER SETUP===
    The timer should be set to:
    * "Start" itself
    * "Trigger Now" itself
    * "Run" the missile program with the argument "fire"
______________________________________________________________________
///SHOOTER SHIP SETUP///

    The shooter ship needs:
    * A remote with name tag "Shooter Reference" pointing in the direction you eant the missile to fly.
    * A method to connect to the missile (merge, rotor, or connector)
______________________________________________________________________
///FIRING INSTRUCTIONS///

    The missile must be physically attached to the shooter ship before firing.
    To fire the missile, simply trigger the missile timer :)

______________________________________________________________________
///TROUBLESHOOTING///

    Find the program block in the ship terminal to check if the code detects any errors.

    Before firing, type "setup" in the argument field of the program block and hit "run".
    This will tell you if any vital components are missing!

______________________________________________________________________
///AUTHOR'S NOTES///

    Make sure to look at the configurable variables in the section below and tweak them to your liking!

    I have spent a ton of time trying to make this code better than the previous iteration.
    I hope y'all enjoy :D

Code by Whiplash141 :)
*/

/*
___________________________________________________________________

///////////////CONFIGURABLE VARIABLES/////////////////

========== You can edit these variables to your liking ============
___________________________________________________________________
*/

//---Missile Name Tags
    const string missileNumber = "1";
    string missileTag = "Missile " + missileNumber; //(MANDATORY) unique id of the missile
    const string detachThrustTag = "Detach"; //(Optional) tag on detach thrust

//---Reference Name Tags
    const string shooterReferenceName = "Shooter Reference"; //name of the remote on the shooter vessel

//---Runtime variables
    const double updatesPerSecond = 10; // self explanatory :P

//---Missile Detach Parameters
    const double disconnectDelay = 1; //time (in seconds) that the missile will delay disconnection from firing ship

    const double guidanceDelay = 2; // time (in seconds) that the missile will delay guidance activation by

    const double detachDuration = 1;
        // time that the missile will execute detach. During the detach function, the missile
        // will use its detach thrusters and any artificial mass it detects to detach from the ship.
        // Setting this to ZERO will skip this feature and move on to the main ignition delay stage.

    const double mainIgnitionDelay = 1;
        // time (in seconds) that the missile will delay main engine activation AFTER the
        // detach function has finished. During this time, the missile will drift without
        // any thrusters firing. Set this to ZERO if you do not want this :)

//---Drift Compensation Parameters
    const bool driftCompensation = true;
        // this determines if the missile will negate unwanted drift. This allows you to make a
        // missile using just forward thrust!

//---Spiral Trajectory Parameters
    bool enableSpiralTrajectory = true; //determines if missiles will spiral to avoid turret fire
    const double spiralDegrees = 5; // angular deviation of the spiral pattern
    const double timeMaxSpiral = 3; // time it takes the missile to complete a full spiral cycle

//---Rotation speed control system
    const double proportionalConstant = 5; // proportional gain of gyroscopes
    const double derivativeConstant = 2; // derivative gain of gyroscopes

//---Missile impact point offsets
    const double offsetUp = 0; // (in meters) Positive numbers offset up, negative offset down
    const double offsetLeft = 0; // (in meters) Positive numbers offset left, negative offset right

//---Missile spin parameters
    const double missileSpinRPM = 0; //this specifies how fast the missile will spin when flying(only in space)

//---Missile guidance parameters
    bool lockVector = true; //this will lock the vector on launch

/*
___________________________________________________________________

============= Don't touch anything below this :) ==================
___________________________________________________________________
*/
//---So many lists...
    List<IMyTerminalBlock> missileBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> unsortedThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> mainThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> sideThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> detachThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> artMasses = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> mergeBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> batteries = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> remotes = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> shooterReferenceList = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> shooterTurretReferenceList = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> gyros = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> timers = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> programs = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> importantBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> connectors = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> rotors = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> reactors = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> antennas = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> beacons = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> sensors = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> warheads = new List<IMyTerminalBlock>();

//---Yo dawg... I heard u like vectors...
    Vector3D shooterForwardVec;
    Vector3D shooterLeftVec;
    Vector3D shooterUpVec;
    Vector3D originPos;
    Vector3D missilePos;
    Vector3D headingVec;
    Vector3D destinationVec;
    Vector3D gravVec;

//---These are kinda super important
    IMyTerminalBlock shooterReference = null;
    IMyRemoteControl missileReference = null;

//---These booleans track the status of the missile
    bool isSetup = false;
    bool setupFailed = false;
    bool firstRun = true;
    bool shouldKill = false;
    bool firstGuidance = true;
    bool hasPassed = false;
    bool killAllowed = false;
    bool inGravity = false;
    bool controlGyros = true;
    bool missileStage1 = false;
    bool missileStage2 = false;
    bool missileStage3 = false;
    bool missileStage4 = false;

//---Store all this important stuff for computations between methods
    double distanceFromShooter;
    double max_kill_time = 3;
    double kill_time = 0;
    double timeElapsed = 0; //time elapsed over current iterations
    double timeTotal = 0; //total time program has been running
    double lastYawAngle = 0;
    double lastPitchAngle = 0;
    double lastRollAngle = 0;

    static double max_distance = 10000; //static b/c we change it on kill command
    const double degToRad = Math.PI / 180;
    const double radToDeg = 180 / Math.PI;
    const double max_time_to_guide = 300; //in seconds
    const double timeLimit = 1 / updatesPerSecond;

void Main(string arg)
{
    if (arg.ToLower() == "fire")
    {
        if (!isSetup)
        {
            GrabBlocks();
        }
        else //will not run or release missile until has run setup succesfully
        {
            if (firstRun)
            {
                timeElapsed = 0;
                timeTotal = 0;
                firstRun = false;
            }
            else
            {
                timeElapsed += Runtime.TimeSinceLastRun.TotalSeconds;
                timeTotal += Runtime.TimeSinceLastRun.TotalSeconds;
                timeSpiral += Runtime.TimeSinceLastRun.TotalSeconds;
            }

            LaunchMissile();
            StatusCheck();

            if (timeTotal >= guidanceDelay && timeElapsed >= timeLimit)
            {
                Echo("WMI Optical Missile Guidance System Active..."); Echo("Run Time: " + Math.Round(timeTotal).ToString());
                GuideMissile();
                timeElapsed = 0;
            }
        }
    }
    else if (arg.ToLower() == "setup")
    {
        GrabBlocks();
    }
    else if (arg.ToLower() == "kill" && killAllowed)
    {
        shouldKill = true;
        max_distance = double.PositiveInfinity;
    }

    if (timeTotal > max_time_to_guide)
    {
        shouldKill = true;
        max_distance = double.PositiveInfinity;
    }
}

bool isRemote(IMyTerminalBlock block)
{
    var remoteTest = block as IMyRemoteControl;
    return remoteTest != null;
}

bool isTurret(IMyTerminalBlock block)
{
    var testTurret = block as IMyLargeTurretBase;
    return testTurret != null;
}

void ClearLists()
{
    missileBlocks.Clear();
    unsortedThrusters.Clear();
    mainThrusters.Clear();
    sideThrusters.Clear();
    detachThrusters.Clear();
    artMasses.Clear();
    mergeBlocks.Clear();
    batteries.Clear();
    remotes.Clear();
    shooterReferenceList.Clear();
    shooterTurretReferenceList.Clear();
    gyros.Clear();
    timers.Clear();
    programs.Clear();
    importantBlocks.Clear();
    connectors.Clear();
    rotors.Clear();
    reactors.Clear();
    antennas.Clear();
    beacons.Clear();
    sensors.Clear();
    warheads.Clear();
}

void GrabBlocks()
{
    ClearLists();

    setupFailed = false;

    GridTerminalSystem.SearchBlocksOfName(missileTag, missileBlocks);
    GridTerminalSystem.SearchBlocksOfName(shooterReferenceName, shooterReferenceList, isRemote);
    GridTerminalSystem.SearchBlocksOfName(shooterReferenceName, shooterTurretReferenceList, isTurret);

    for (int i = 0; i < shooterReferenceList.Count; i++)
    {
        importantBlocks.Add(shooterReferenceList[i]);
    }

    for (int i = 0; i < shooterTurretReferenceList.Count; i++)
    {
        importantBlocks.Add(shooterTurretReferenceList[i]);
    }

    //---Sort through all blocks with the missile tag
    for (int i = 0; i < missileBlocks.Count; i++)
    {
        var thisBlock = missileBlocks[i] as IMyTerminalBlock;

        if (thisBlock is IMyThrust)
        {
            if (thisBlock.CustomName.Contains(detachThrustTag))
            {
                detachThrusters.Add(thisBlock);
                unsortedThrusters.Add(thisBlock);
            }
            else {
                unsortedThrusters.Add(thisBlock);
            }
        }
        else if (thisBlock is IMyVirtualMass)
        {
            artMasses.Add(thisBlock);
        }
        else if (thisBlock is IMyBatteryBlock)
        {
            batteries.Add(thisBlock);
        }
        else if (thisBlock is IMyGyro)
        {
            gyros.Add(thisBlock);
            importantBlocks.Add(thisBlock);
        }
        else if (thisBlock is IMyShipMergeBlock)
        {
            mergeBlocks.Add(thisBlock);
        }
        else if (thisBlock is IMyTimerBlock)
        {
            timers.Add(thisBlock);
            importantBlocks.Add(thisBlock);
        }
        else if (thisBlock is IMyProgrammableBlock)
        {
            programs.Add(thisBlock);
            importantBlocks.Add(thisBlock);
        }
        else if (thisBlock is IMyRemoteControl)
        {
            remotes.Add(thisBlock);
        }
        else if (thisBlock is IMyShipConnector)
        {
            connectors.Add(thisBlock);
        }
        else if (thisBlock is IMyMotorStator)
        {
            rotors.Add(thisBlock);
        }
        else if (thisBlock is IMyReactor)
        {
            reactors.Add(thisBlock);
        }
        else if (thisBlock is IMyRadioAntenna)
        {
            antennas.Add(thisBlock);
        }
        else if (thisBlock is IMyBeacon)
        {
            beacons.Add(thisBlock);
        }
        else if (thisBlock is IMySensorBlock)
        {
            sensors.Add(thisBlock);
        }
        else if (thisBlock is IMyWarhead)
        {
            warheads.Add(thisBlock);
        }
    }

    Echo("Setup results for Missile " + missileNumber);
    //---Check what we have and display any missing blocks
    if (artMasses.Count == 0)
    {
        Echo("[OPTIONAL] No artificial masses found");
    }

    if (sensors.Count == 0)
    {
        Echo("[OPTIONAL] No sensors found");
    }

    if (warheads.Count == 0)
    {
        Echo("[OPTIONAL] No warheads found");
    }

    if (beacons.Count == 0)
    {
        Echo("[OPTIONAL] No beacons found");
    }

    if (antennas.Count == 0)
    {
        Echo("[OPTIONAL] No antennas found");
    }

    if (shooterReferenceList.Count == 0 && shooterTurretReferenceList.Count == 0)
    {
        Echo("[FAILED] No remote or turret named '" + shooterReferenceName + "'found");
        setupFailed = true;
    }

    if (gyros.Count == 0)
    {
        Echo("[FAILED] No control gyros found");
        setupFailed = true;
    }

    if (remotes.Count == 0)
    {
        Echo("[FAILED] No remotes found");
        setupFailed = true;
    }
    else
    {
        GetThrusterOrientation(remotes[0]);
    }

    if (sideThrusters.Count == 0)
    {
        Echo("[OPTIONAL] No side thrusters found");
    }

    if (detachThrusters.Count == 0)
    {
        Echo("[OPTIONAL] No detach thrusters found");
    }

    if (mainThrusters.Count == 0)
    {
        if (!lockVector)
        {
            Echo("[FAILED] No main thrusters found");
            setupFailed = true;
        }
        else
            Echo("[OPTIONAL] No main thrusters found");
    }

    if (batteries.Count == 0 && reactors.Count == 0)
    {
        Echo("[FAILED] No batteries or reactors found");
        setupFailed = true;
    }

    if (mergeBlocks.Count == 0 && rotors.Count == 0 && connectors.Count == 0)
    {
        Echo("[FAILED] No merge blocks, rotors, or connectors found");
        setupFailed = true;
    }

    if (!setupFailed)
    {
        Echo("[SUCCESS] Ready to run");

        if (shooterReferenceList.Count != 0)
            shooterReference = shooterReferenceList[0];
        else
            shooterReference = shooterTurretReferenceList[0];

        //---Get orientation vectors of our shooter vessel
        if (lockVector)
        {
            if (shooterReference is IMyLargeTurretBase)
            {
                var turret = shooterReference as IMyLargeTurretBase;
                double azimuth = (double)turret.Azimuth;
                double elevation = (double)turret.Elevation;

                var turretHeading = VectorAzimuthElevation(azimuth, elevation);
                shooterForwardVec = shooterReference.WorldMatrix.Forward * turretHeading.X + shooterReference.WorldMatrix.Left * turretHeading.Y + shooterReference.WorldMatrix.Up * turretHeading.Z;
                shooterForwardVec = Vector3D.Normalize( shooterForwardVec );

                shooterLeftVec = shooterForwardVec.Cross(turret.WorldMatrix.Down);
                shooterUpVec = shooterForwardVec.Cross(shooterLeftVec);
            }
            else
            {
                shooterForwardVec = shooterReference.WorldMatrix.Forward;
                shooterLeftVec = shooterReference.WorldMatrix.Left;
                shooterUpVec = shooterReference.WorldMatrix.Up;
            }

            //---Get positions of our origin with offsets added in
            originPos = shooterReference.GetPosition() + offsetLeft * shooterLeftVec + offsetUp * shooterUpVec;
        }

        missileReference = remotes[0] as IMyRemoteControl;
        GetGyroOrientation(missileReference, gyros);
        isSetup = true;
    }
}

void GetThrusterOrientation(IMyTerminalBlock refBlock)
{
    var forwardDirn = refBlock.WorldMatrix.Forward;

    foreach (IMyThrust thisThrust in unsortedThrusters)
    {
        var thrustDirn = thisThrust.WorldMatrix.Backward;
        bool sameDirn = thrustDirn == forwardDirn;

        if (sameDirn)
        {
            mainThrusters.Add(thisThrust);
        }
        else {
            sideThrusters.Add(thisThrust);
        }
    }
}

void StatusCheck()
{
    for (int k = 0; k < importantBlocks.Count; k++)
    {
        IMyTerminalBlock block = importantBlocks[k];
        IMySlimBlock slim = block.CubeGrid.GetCubeBlock(block.Position);
        if (slim.CurrentDamage > 0)
        {
            Echo("Damage");
            kill_time = max_kill_time;
            KillGuidance(0, 0);
            return;
        }
    }
}

void LaunchMissile()
{
    //Stage 1: Discharge
    if (!missileStage1) //set battery to discharge
    {
        foreach (IMyBatteryBlock thisBattery in batteries)
        {
            thisBattery?.ApplyAction("OnOff_On"); //make sure our battery is on
            thisBattery?.SetValue("Recharge", false);
            thisBattery?.SetValue("Discharge", true);
        }

        foreach (IMyReactor thisReactor in reactors)
        {
            thisReactor?.ApplyAction("OnOff_On"); //make sure our reactors are on
        }

        foreach (IMySensorBlock thisSensor in sensors)
        {
            thisSensor?.ApplyAction("OnOff_Off");
        }

        foreach (IMyWarhead thisWarhead in warheads)
        {
            thisWarhead?.SetValue<bool>("Safety", true);
        }

        missileStage1 = true;
    }
    //Stage 2: Release
    else if (timeTotal >= disconnectDelay && !missileStage2) //detach missile
    {
        foreach (IMyVirtualMass thisMass in artMasses)
        {
            thisMass?.ApplyAction("OnOff_On");
        }

        foreach (IMyGyro thisGyro in gyros)
        {
            thisGyro?.ApplyAction("OnOff_On");
        }

        foreach (IMyShipMergeBlock thisMerge in mergeBlocks)
        {
            thisMerge?.ApplyAction("OnOff_Off");
        }

        foreach (IMyShipConnector thisConnector in connectors)
        {
            thisConnector?.ApplyAction("Unlock");
        }

        foreach (IMyMotorStator thisRotor in rotors)
        {
            thisRotor?.SetValue<bool>("Force weld", false);
            thisRotor?.ApplyAction("Detach");
        }

        foreach (IMyRadioAntenna thisAntenna in antennas)
        {
            thisAntenna?.SetValue<float>("Radius", 800f);
            thisAntenna?.ApplyAction("OnOff_Off");
            thisAntenna?.SetValue<bool>("EnableBroadCast", true);
            thisAntenna?.ApplyAction("OnOff_On");
            thisAntenna?.SetCustomName("");
        }

        foreach (IMyBeacon thisBeacon in beacons)
        {
            thisBeacon?.SetValue<float>("Radius", 800f);
            thisBeacon?.ApplyAction("OnOff_On");
            thisBeacon?.SetCustomName("");
        }

        ManeuveringThrust(false);

        foreach (IMyThrust thisThrust in detachThrusters)
        {
            thisThrust?.ApplyAction("OnOff_On");
            thisThrust?.SetValue<float>("Override", float.MaxValue);
        }

        killAllowed = true;
        missileStage2 = true;

    }
    //Stage 3: Drift
    else if (timeTotal >= disconnectDelay + detachDuration && !missileStage3)
    {
        ManeuveringThrust(true);

        foreach (IMyThrust thisThrust in detachThrusters)
        {
            thisThrust?.SetValue<float>("Override", float.MinValue);
        }
        missileStage3 = true;
    }
    //Stage 4: Ignition
    else if (timeTotal >= disconnectDelay + mainIgnitionDelay + detachDuration && !missileStage4) //fire missile
    {
        foreach (IMyVirtualMass thisMass in artMasses)
        {
            thisMass?.ApplyAction("OnOff_Off");
        }

        foreach (IMySensorBlock thisSensor in sensors)
        {
            thisSensor?.ApplyAction("OnOff_On");
        }

        foreach (IMyWarhead thisWarhead in warheads)
        {
            thisWarhead?.SetValue<bool>("Safety", false); //arms warheads
        }

        MainThrustOverride();
        missileStage4 = true;
    }
}

void GuideMissile()
{
    //---Get orientation vectors of our shooter vessel
    if (!lockVector)
    {
        if (shooterReference is IMyLargeTurretBase)
        {
            var turret = shooterReference as IMyLargeTurretBase;
            double azimuth = (double)turret.Azimuth;
            double elevation = (double)turret.Elevation;

            var turretHeading = VectorAzimuthElevation(azimuth, elevation);
            shooterForwardVec = shooterReference.WorldMatrix.Forward * turretHeading.X + shooterReference.WorldMatrix.Left * turretHeading.Y + shooterReference.WorldMatrix.Up * turretHeading.Z;
            shooterForwardVec = Vector3D.Normalize( shooterForwardVec );

            shooterLeftVec = shooterForwardVec.Cross(turret.WorldMatrix.Down);
            shooterUpVec = shooterForwardVec.Cross(shooterLeftVec);
        }
        else
        {
            shooterForwardVec = shooterReference.WorldMatrix.Forward;
            shooterLeftVec = shooterReference.WorldMatrix.Left;
            shooterUpVec = shooterReference.WorldMatrix.Up;
        }

        //---Get positions of our origin with offsets added in
        originPos = shooterReference.GetPosition() + offsetLeft * shooterLeftVec + offsetUp * shooterUpVec;
    }

    missilePos = missileReference.GetPosition();

    //---Find current distance from shooter to missile
    distanceFromShooter = Vector3D.Distance(originPos, missilePos);
    ScaleAntennaRange(distanceFromShooter);

    //---Check if we are in range
    if (distanceFromShooter > max_distance)
    {
        Echo("Out of range");
        shouldKill = true;
    }

    //---Get orientation vectors of our missile
    Vector3D missileFrontVec = missileReference.WorldMatrix.Forward;
    Vector3D missileLeftVec = missileReference.WorldMatrix.Left;
    Vector3D missileUpVec = missileReference.WorldMatrix.Up;

    //---Find vector from shooter to missile
    var shooterToMissileVec = missilePos - originPos;

    //---Calculate angle between shooter vector and missile vector
    double rawDevAngle = VectorAngleBetween(shooterForwardVec, shooterToMissileVec) * radToDeg;

    //---Calculate perpendicular distance from shooter vector
    var projectionVec = VectorProjection(shooterToMissileVec, shooterForwardVec);
    double deviationDistance = Vector3D.DistanceSquared(projectionVec, shooterToMissileVec);

    //---Check if we have gravity
    double rollAngle = 0; double rollSpeed = 0;

    var remote = remotes[0] as IMyRemoteControl;
    inGravity = false;

    gravVec = missileReference.GetNaturalGravity();
    double gravMagSquared = gravVec.LengthSquared();
    if (gravMagSquared != 0)
    {
        if (gravVec.Dot(missileUpVec) < 0)
        {
            rollAngle = Math.PI / 2 - Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1));
        }
        else
        {
            rollAngle = Math.PI + Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1));
        }

        if (firstGuidance) lastRollAngle = rollAngle;

        rollSpeed = Math.Round(proportionalConstant * rollAngle + derivativeConstant * (rollAngle - lastRollAngle) / timeLimit, 2);

        inGravity = true;
    }
    else
    {
        if (missileStage4)
            rollSpeed = missileSpinRPM * Math.PI / 30; //converts RPM to rad/s
    }

    var missileSpeed = missileReference.GetShipSpeed();

    //---Determine scaling factor
    double scalingFactor;
    if (rawDevAngle < 90)
    {
        scalingFactor = projectionVec.Length() + Math.Max(2 * missileSpeed, 200); //travel approx. 200m from current position in direction of target vector
        destinationVec = originPos + scalingFactor * shooterForwardVec;
        if (!hasPassed)
            hasPassed = true;
    }
    else if (hasPassed)
    {
        //---Determine where missile is in relation to shooter
        int signLeft = Math.Sign(shooterToMissileVec.Dot(shooterLeftVec));
        int signUp = Math.Sign(shooterToMissileVec.Dot(shooterUpVec));

        scalingFactor = -projectionVec.Length() + Math.Max(2 * missileSpeed, 200); //added the Math.Max part for modded speed worlds
        destinationVec = originPos + scalingFactor * shooterForwardVec + signLeft * 50 * shooterLeftVec + signUp * 50 * shooterUpVec;
    }
    else
    {
        scalingFactor = -projectionVec.Length() + Math.Max(2 * missileSpeed, 200);
        destinationVec = originPos + scalingFactor * shooterForwardVec;
    }

    //---Find vector from missile to destinationVec
    var missileToTargetVec = destinationVec - missilePos;                                                                                                                                                                                                                                                                                                     //w.H/i-P*L+a.s^H

    //---Get travel vector
    var missileVelocityVec = missileReference.GetShipVelocities().LinearVelocity;

    //---Calc our new heading based upon our travel vector
    if (missileStage4)
    {
        headingVec = CalculateHeadingVector(missileToTargetVec, missileVelocityVec, driftCompensation);
    }
    else
    {
        headingVec = CalculateHeadingVector(missileToTargetVec, missileVelocityVec, false);
    }

    //---Calc spiral trajectory
    if (enableSpiralTrajectory)
    {
        headingVec = SpiralTrajectory(headingVec, shooterForwardVec, shooterUpVec);
    }

    //---Get pitch and yaw angles
    double yawAngle; double pitchAngle;
    GetRotationAngles(headingVec, missileFrontVec, missileLeftVec, missileUpVec, out yawAngle, out pitchAngle);

    if (firstGuidance)
    {
        lastPitchAngle = pitchAngle;
        lastYawAngle = yawAngle;
        firstGuidance = false;
    }

    //---Angle controller
    double yawSpeed = Math.Round(proportionalConstant * yawAngle + derivativeConstant * (yawAngle - lastYawAngle) / timeLimit, 2);
    double pitchSpeed = Math.Round(proportionalConstant * pitchAngle + derivativeConstant * (pitchAngle - lastPitchAngle) / timeLimit, 2);

    //---Set appropriate gyro override
    if (controlGyros)
    {
        ApplyGyroOverride(pitchSpeed, yawSpeed, rollSpeed, gyros);
    }

    //---Store previous values
    lastYawAngle = yawAngle;
    lastPitchAngle = pitchAngle;
    lastRollAngle = rollAngle;

    if (shouldKill)
    {
        KillGuidance(rawDevAngle, deviationDistance);
        lockVector = true;
    }
}

Vector3D CalculateHeadingVector(Vector3D targetVec, Vector3D velocityVec, bool driftComp)
{
    if (!driftComp)
    {
        return targetVec;
    }

    if (velocityVec.LengthSquared() < 100)
    {
        return targetVec;
    }

    if (targetVec.Dot(velocityVec) > 0)
    {
        return VectorReflection(velocityVec, targetVec);
    }
    else
    {
        return -velocityVec;
    }
}

//Whip's Vector from Elevation and Azimuth
Vector3D VectorAzimuthElevation(double az, double el)
{
    el = el % (2 * Math.PI);
    az = az % (2 * Math.PI);

    if (az != Math.Abs(az))
    {
        az = 2 * Math.PI + az;
    }

    int x_mult = 1;

    if (az > Math.PI / 2 && az < Math.PI)
    {
        az = Math.PI - (az % Math.PI);
        x_mult = -1;
    }
    else if (az > Math.PI && az < Math.PI * 3 / 2)
    {
        az = 2 * Math.PI - (az % Math.PI);
        x_mult = -1;
    }

    double x; double y; double z;

    if (el == Math.PI / 2)
    {
        x = 0;
        y = 0;
        z = 1;
    }
    else if (az == Math.PI / 2)
    {
        x = 0;
        y = 1;
        z = y * Math.Tan(el);
    }
    else {
        x = 1 * x_mult;
        y = Math.Tan(az);
        double v_xy = Math.Sqrt(1 + y * y);
        z = v_xy * Math.Tan(el);
    }

    return new Vector3D(x, y, z);
}

void ScaleAntennaRange(double dist)
{
    foreach (IMyRadioAntenna thisAntenna in antennas)
    {
        thisAntenna?.SetValue("Radius", (float)dist + 100f);
    }

    foreach (IMyBeacon thisBeacon in beacons)
    {
        thisBeacon?.SetValue<float>("Radius", (float)dist + 100f);
    }
}

void ManeuveringThrust(bool turnOn)
{
    foreach (IMyThrust thisThrust in sideThrusters)
    {
        if (turnOn)
        {
            thisThrust?.ApplyAction("OnOff_On");
        }
        else
        {
            thisThrust?.ApplyAction("OnOff_Off");
        }
    }
}

void MainThrustOverride()
{
    foreach (IMyThrust thisThrust in mainThrusters)
    {
        thisThrust?.ApplyAction("OnOff_On");
        thisThrust?.SetValue<float>("Override", float.MaxValue);
    }
}

void KillGuidance(double angleOfDeviation, double distanceOfDeviation)
{
    if (enableSpiralTrajectory)
        enableSpiralTrajectory = false;

    if (kill_time >= max_kill_time || timeTotal >= max_time_to_guide)
    {
        for (int i = 0; i < gyros.Count; i++)
        {
            var thisGyro = gyros[i] as IMyGyro;
            if (thisGyro != null)
            {
                thisGyro.SetValue<float>("Yaw", 0f);
                thisGyro.SetValue<float>("Pitch", 0f);
                thisGyro.SetValue<float>("Roll", 0f);
                thisGyro.SetValue("Override", true);
            }
        }

        if (!inGravity)
        {
            for (int i = 0; i < mainThrusters.Count; i++)
            {
                var thisThruster = mainThrusters[i] as IMyThrust;
                if (thisThruster != null) thisThruster.ApplyAction("OnOff_Off");
            }

            for (int i = 0; i < sideThrusters.Count; i++)
            {
                var thisThruster = sideThrusters[i] as IMyThrust;
                if (thisThruster != null) thisThruster.ApplyAction("OnOff_Off");
            }
        }

        for (int i = 0; i < importantBlocks.Count; i++)
        {
            var thisBlock = importantBlocks[i] as IMyTerminalBlock;
            if (thisBlock != null)
            {
                if (!(thisBlock is IMyRemoteControl) || !(thisBlock is IMyProgrammableBlock))
                    thisBlock.ApplyAction("OnOff_Off");
            }
        }
    }

    if (angleOfDeviation < 5 && distanceOfDeviation < 1)
    {
        kill_time += timeElapsed;
    }
    else {
        kill_time = 0;
    }
}

Vector3D VectorProjection(Vector3D a, Vector3D b) //proj a on b
{
    Vector3D projection = a.Dot(b) / b.LengthSquared() * b;
    return projection;
}

Vector3D VectorReflection(Vector3D a, Vector3D b) //reflect a over b
{
    Vector3D project_a = VectorProjection(a, b);
    Vector3D reject_a = a - project_a;
    Vector3D reflect_a = project_a - reject_a;
    return reflect_a;
}

double VectorAngleBetween(Vector3D a, Vector3D b) //returns radians
{
    if (a.LengthSquared() == 0 || b.LengthSquared() == 0)
        return 0;
    else
        return Math.Acos(MathHelper.Clamp(a.Dot(b) / a.Length() / b.Length(), -1, 1));
}

bool CheckDotPositive(Vector3D a, Vector3D b)
{
    double check = a.Dot(b);
    if (check > 0)
    {
        return true;
    }
    else {
        return false;
    }
}

//Whip's Get Rotation Angles Method v4
void GetRotationAngles( Vector3D v_target, Vector3D v_front, Vector3D v_left, Vector3D v_up, out double yaw, out double pitch )
{
    //Dependencies: VectorProjection() | VectorAngleBetween()
    //Keen uses a stupid left hand rule coordSystem, I dont.
    var projTargetFront = VectorProjection( v_target, v_front );
    var projTargetLeft = VectorProjection( v_target, v_left );
    var projTargetUp = VectorProjection( v_target, v_up );
    var projTargetFrontLeft = projTargetFront + projTargetLeft;
    var projTargetFrontUp = projTargetFront + projTargetUp;

    yaw = VectorAngleBetween(v_front, projTargetFrontLeft);
    pitch = VectorAngleBetween(v_front, projTargetFrontUp);

    //---Check if yaw angle is left or right
    //multiplied by -1 to convert from right hand rule to left hand rule
    yaw = -1 * Math.Sign( v_left.Dot( projTargetLeft ) ) * yaw;

    //---Check if pitch angle is up or down
    pitch = Math.Sign( v_up.Dot( projTargetUp )) * pitch;

    //---Check if target vector is pointing opposite the front vector
    if (pitch == 0 && yaw == 0 && v_target.Dot(v_front) < 0)
    {
        yaw = Math.PI;
        pitch = Math.PI;
    }
}

//Whip's Gyro Orientation Method
string[] gyroRelativeYaw;
string[] gyroRelativePitch;
string[] gyroRelativeRoll;
int[] gyroYawSign;
int[] gyroPitchSign;
int[] gyroRollSign;

void GetGyroOrientation(IMyTerminalBlock reference_block, List<IMyTerminalBlock> gyro_list)
{
    gyroRelativeYaw = new string[gyro_list.Count];
    gyroRelativePitch = new string[gyro_list.Count];
    gyroRelativeRoll = new string[gyro_list.Count];

    gyroYawSign = new int[gyro_list.Count];
    gyroPitchSign = new int[gyro_list.Count];
    gyroRollSign = new int[gyro_list.Count];

    var reference_up = reference_block.WorldMatrix.Up; //assuming rot right
    var reference_right = reference_block.WorldMatrix.Right; //assuming rot up
    var reference_forward = reference_block.WorldMatrix.Forward; //assuming rot up

    for (int i = 0; i < gyro_list.Count; i++)
    {
        var gyro_forward = gyro_list[i].WorldMatrix.Forward;
        var gyro_backward = gyro_list[i].WorldMatrix.Backward;
        var gyro_up = gyro_list[i].WorldMatrix.Up;
        var gyro_down = gyro_list[i].WorldMatrix.Down;
        var gyro_left = gyro_list[i].WorldMatrix.Left;
        var gyro_right = gyro_list[i].WorldMatrix.Right;

        /// Pitch Fields ///
        if (reference_right == gyro_forward)
        {
            gyroRelativePitch[i] = "Roll";
            gyroPitchSign[i] = 1;
        }
        else if (reference_right == gyro_backward)
        {
            gyroRelativePitch[i] = "Roll";
            gyroPitchSign[i] = -1;
        }
        else if (reference_right == gyro_right)
        {
            gyroRelativePitch[i] = "Pitch";
            gyroPitchSign[i] = 1;
        }
        else if (reference_right == gyro_left)
        {
            gyroRelativePitch[i] = "Pitch";
            gyroPitchSign[i] = -1;
        }
        else if (reference_right == gyro_up)
        {
            gyroRelativePitch[i] = "Yaw";
            gyroPitchSign[i] = -1;
        }
        else if (reference_right == gyro_down)
        {
            gyroRelativePitch[i] = "Yaw";
            gyroPitchSign[i] = 1;
        }

        /// Yaw Fields ///
        if (reference_up == gyro_forward)
        {
            gyroRelativeYaw[i] = "Roll";
            gyroYawSign[i] = -1;
        }
        else if (reference_up == gyro_backward)
        {
            gyroRelativeYaw[i] = "Roll";
            gyroYawSign[i] = 1;
        }
        else if (reference_up == gyro_right)
        {
            gyroRelativeYaw[i] = "Pitch";
            gyroYawSign[i] = -1;
        }
        else if (reference_up == gyro_left)
        {
            gyroRelativeYaw[i] = "Pitch";
            gyroYawSign[i] = 1;
        }
        else if (reference_up == gyro_up)
        {
            gyroRelativeYaw[i] = "Yaw";
            gyroYawSign[i] = 1;
        }
        else if (reference_up == gyro_down)
        {
            gyroRelativeYaw[i] = "Yaw";
            gyroYawSign[i] = -1;
        }

        /// Roll Fields ///
        if (reference_forward == gyro_forward)
        {
            gyroRelativeRoll[i] = "Roll";
            gyroRollSign[i] = 1;
        }
        else if (reference_forward == gyro_backward)
        {
            gyroRelativeRoll[i] = "Roll";
            gyroRollSign[i] = -1;
        }
        else if (reference_forward == gyro_right)
        {
            gyroRelativeRoll[i] = "Pitch";
            gyroRollSign[i] = 1;
        }
        else if (reference_forward == gyro_left)
        {
            gyroRelativeRoll[i] = "Pitch";
            gyroRollSign[i] = -1;
        }
        else if (reference_forward == gyro_up)
        {
            gyroRelativeRoll[i] = "Yaw";
            gyroRollSign[i] = -1;
        }
        else if (reference_forward == gyro_down)
        {
            gyroRelativeRoll[i] = "Yaw";
            gyroRollSign[i] = 1;
        }
    }
}

void ApplyGyroOverride(double pitch_speed, double yaw_speed, double roll_speed, List<IMyTerminalBlock> gyro_list)
{
    for (int i = 0; i < gyro_list.Count; i++)
    {
        var thisGyro = gyro_list[i] as IMyGyro;
        if (thisGyro != null)
        {
            thisGyro.SetValue<float>(gyroRelativeYaw[i], (float)yaw_speed * gyroYawSign[i]);
            thisGyro.SetValue<float>(gyroRelativePitch[i], (float)pitch_speed * gyroPitchSign[i]);
            thisGyro.SetValue<float>(gyroRelativeRoll[i], (float)roll_speed * gyroRollSign[i]);
            thisGyro.SetValue("Override", true);
        }
    }
}

//Whip's Spiral Trajectory Method v2
double spiralRadius = Math.Tan(spiralDegrees * degToRad);
double timeSpiral = 0;

Vector3D SpiralTrajectory(Vector3D v_target, Vector3D v_front, Vector3D v_up)
{
    Vector3D v_targ_norm = Vector3D.Normalize(v_target);

    if (timeSpiral > timeMaxSpiral)
        timeSpiral = 0;

    double angle_theta = 2 * Math.PI * timeSpiral / timeMaxSpiral;

    if (v_front.Dot(v_targ_norm) > 0)
    {
        Vector3D v_x = Vector3D.Normalize(v_up.Cross(v_targ_norm));
        Vector3D v_y = Vector3D.Normalize(v_x.Cross(v_targ_norm));
        Vector3D v_target_adjusted = v_targ_norm + spiralRadius * (v_x * Math.Cos(angle_theta) + v_y * Math.Sin(angle_theta));
        return v_target_adjusted;
    }
    else
    {
        return v_targ_norm;
    }
}

/*
CHANGELOG:
- Redesigned pitch and yaw determination method
- Added sensor support, sensors turn on when main ignition is triggered
- Optimized math operations
- Re-added "hasPassed" boolean
- Made sure lists properly clear when setup is run for a second time
- Removed fudge number
- Added warhead support
- Fixed issue that caused missiles to spaz out.
- Clamped arccosine inputs to account for floating point errors
- Fixed missile spin being activated before main ignition fires
- Added in vector lock on launch
*/