Seff and Whip's Drone Code v2

//Code by Seff - Edited by Whiplash141 7/3/16 - v2
const double maxRangeFromOrigin = 20000; //in meters
const double evasionRange = 200; //in meters
const double standoffRange = 400; //in meters
const double updatesPerSecond = 10;
const double maxSpeed = 50;

//   NO TOUCH BELOW DIS LINE
//-----------------------------
bool originSet = false;

List<IMyTerminalBlock> remoteList = new List<IMyTerminalBlock>();
List<IMyTerminalBlock> weaponList = new List<IMyTerminalBlock>();
List<IMyTerminalBlock> gyroList = new List<IMyTerminalBlock>();
List<IMyTerminalBlock> thrusterList = new List<IMyTerminalBlock>();

Vector3D origin = new Vector3D(0, 0, 0);

const double timeMaxCycle = 1 / updatesPerSecond;
double timeCurrentCycle;

void Main(string argument)
{
    if (!originSet)
    {
        if (argument == null || argument == "")
        {
            origin = Me.GetPosition();
            originSet = true;
            //Me.TryRun( origin.ToString() );
            Echo("Self as origin");
        }
        else
        {
            originSet = Vector3D.TryParse(argument, out origin);
            Echo("Arg as origin");
        }
    }

    timeCurrentCycle += Runtime.TimeSinceLastRun.TotalSeconds;

    if(timeCurrentCycle >= timeMaxCycle)
    {
        //To do: define welders

        GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(remoteList, (x => x.CubeGrid == Me.CubeGrid));
        GridTerminalSystem.GetBlocksOfType<IMyGyro>(gyroList, (x => x.CubeGrid == Me.CubeGrid));
        if (remoteList.Count > 0)
        {
            IMyRemoteControl rcmain = null;
            rcmain = remoteList[0] as IMyRemoteControl;

            GetGyroOrientation(rcmain, gyroList); //get relative gyro orietation
            GetThrustOrientation(rcmain);

            Vector3D target = new Vector3D(0, 0, 0);
            bool success = rcmain.GetNearestPlayer(out target);

            if (success)
            {
                bool gotoOrigin = false;
                GridTerminalSystem.GetBlocksOfType<IMyLargeTurretBase>(weaponList);
                if (weaponList.Count == 0)
                {
                    gotoOrigin = true;
                }
                else
                {
                    bool hasUsableGun = false;
                    for (int i = 0; i < weaponList.Count; ++i)
                    {
                        var weapon = weaponList[i] as IMyLargeTurretBase;
                        weapon.ApplyAction("OnOff_On");
                        if (!weapon.IsFunctional)
                            continue;
                        else if (weapon.HasInventory() && !weapon.GetInventory(0).IsItemAt(0))
                            continue;
                        else
                            hasUsableGun = true;
                    }

                    if (!hasUsableGun)
                    {
                        gotoOrigin = true;
                    }
                }

                if (Vector3D.Distance(target, origin) > maxRangeFromOrigin)
                {
                    gotoOrigin = true;
                }


                Vector3D destination;
                Vector3D self = rcmain.GetPosition();
                Vector3D frontVector = rcmain.WorldMatrix.Forward;
                Vector3D leftVector = rcmain.WorldMatrix.Left;
                Vector3D upVector = rcmain.WorldMatrix.Up;

                /// Piloting Parameters ///
                if (gotoOrigin)
                {
                    destination = origin - self;
                    ThrustOverrideOn(forwardThrust);
                    ThrustOverrideOff(backwardsThrust);
                }
                else
                {
                    var directHeadingVec = target - self;
                    var range = directHeadingVec.Length();

                    if (range < evasionRange)
                    {
                        destination = directHeadingVec.Cross(leftVector);
                        ThrustOverrideOn(forwardThrust);
                        ThrustOverrideOff(backwardsThrust);
                    }
                    else if (range < standoffRange) //reverse
                    {
                        destination = directHeadingVec;
                        ThrustOverrideOn(backwardsThrust);
                        ThrustOverrideOff(forwardThrust);
                    }
                    else //forward
                    {
                        destination = directHeadingVec;
                        ThrustOverrideOn(forwardThrust);
                        ThrustOverrideOff(backwardsThrust);
                    }

                    //To do: Add Welders on
                }

                double speed = rcmain.GetShipSpeed();
                if (speed > maxSpeed)
                {
                    ThrustOverrideOff(backwardsThrust);
                    ThrustOverrideOff(forwardThrust);
                }

                double yawAngle = 0; double pitchAngle = 0;
                GetRotationAngles(destination, frontVector, leftVector, upVector, out yawAngle, out pitchAngle);

                ApplyGyroOverride(pitchAngle, yawAngle, 0, gyroList);
            }
        }
        else
        {
            Echo("Error: No remote controls found!");
        }

        timeCurrentCycle = 0;
    }
}

List<IMyTerminalBlock> backwardsThrust = new List<IMyTerminalBlock>();
List<IMyTerminalBlock> forwardThrust = new List<IMyTerminalBlock>();
void GetThrustOrientation(IMyTerminalBlock ref_block)
{
    List<IMyTerminalBlock> allThrust = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyThrust>(allThrust);
    backwardsThrust.Clear();
    forwardThrust.Clear();
    var forwardDirection = ref_block.WorldMatrix.Forward;
    foreach (IMyThrust thisThrust in allThrust)
    {
        var thrustDirection = thisThrust.WorldMatrix.Backward;
        if (thrustDirection == forwardDirection)
        {
            forwardThrust.Add(thisThrust);
        }
        else if (thrustDirection == -forwardDirection)
        {
            backwardsThrust.Add(thisThrust);
        }
    }
}

void ThrustOverrideOn(List<IMyTerminalBlock> thrust_list)
{
    foreach( IMyThrust thisThrust in thrust_list )
    {
        thisThrust.SetValue<float>("Override", float.MaxValue);
    }
}

void ThrustOverrideOff(List<IMyTerminalBlock> thrust_list)
{
    foreach( IMyThrust thisThrust in thrust_list )
    {
        thisThrust.SetValue<float>("Override", float.MinValue);
    }
}

//Whip's Get Rotation Angles Method
void GetRotationAngles(Vector3D v_target, Vector3D v_front, Vector3D v_left, Vector3D v_up, out double yaw, out double pitch)
{
    //Dependencies: VectorProjection() | VectorCompareDirection()
    var projTargetFront = VectorProjection(v_target, v_front);
    var projTargetLeft = VectorProjection(v_target, v_left);
    var projTargetUp = VectorProjection(v_target, v_up);
    var projTargetUpPlane = projTargetFront + projTargetLeft;

    //---Get Yaw and Pitch Angles
    yaw = Math.Asin(projTargetLeft.Length() / projTargetUpPlane.Length());
    pitch = Math.Atan(projTargetUp.Length() / projTargetUpPlane.Length());

    //---Check if yaw angle is left or right
    //yaw is backwards b/c keen uses some weird axis notation
    yaw = -1 * VectorCompareDirection(v_left, projTargetLeft) * yaw;

    //---Check if pitch angle is up or down
    pitch = VectorCompareDirection(v_up, projTargetUp) * pitch;
}

//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);
        }
    }
}

int VectorCompareDirection(Vector3D a, Vector3D b)
{
    double check = a.Dot(b);
    if (check < 0)
        return -1;
    else
        return 1;
}

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