Spy Plane

public string sendTag = "spycoordinate", settingBackup = "", scriptName = "NDS Spy Plane";

public double gyroStrengthMultiplier = 0.8, settingsVersion = 1.0;

public IMyTerminalBlock genRemote;

public List<IMyGyro> gyroList = new List<IMyGyro>();

public bool aligning = false, resave = false, foundName = false, foundVersion = false;

public Vector3D planetCoordinate = new Vector3D(0, 0, 0);

public Program()
{
    Runtime.UpdateFrequency = UpdateFrequency.Update10;
    Echo("Booting");
    SyncRemote();
    if (genRemote == null) throw new Exception("\n No control block found (remote control or cockpit)");
    else Echo("Booted up and found control block: " + genRemote.CustomName);
    GridTerminalSystem.GetBlocksOfType<IMyGyro>(gyroList, g => g.CubeGrid == Me.CubeGrid);
    GroundCoordinate((IMyShipController)genRemote, 0);
    StopOverride();
    if (Me.CustomData == "") SaveData();
    else LoadData();
    Save();
}

public void Save()
{

}

public void Main(string argument, UpdateType updateSource)
{
    if (Me.CustomData != settingBackup) LoadData();
    if (argument.ToLower() == "align") {
        aligning = !aligning;
        if (!aligning) StopOverride();
    }
    else if (updateSource == UpdateType.Terminal || argument != "") {
        double offset = 0;
        try { offset = double.Parse(argument); } catch { }
        try { RelayCoordinate(offset); } catch {
            SyncRemote();
            Echo("Error getting coordinate");
            if (genRemote == null) Echo("Control block lost, no control block found");
        }
    }
    if (aligning) Align();
    else Echo("Not aligning");
}

public void RelayCoordinate(double offset)
{
    Vector3D groundCoordinate = GroundCoordinate((IMyShipController)genRemote, offset);
    IGC.SendBroadcastMessage<Vector3D>(sendTag, groundCoordinate, TransmissionDistance.AntennaRelay);
    Echo("Sent location: " + groundCoordinate.ToString("N0"));
}

public void SyncRemote()
{
    if (genRemote == null || Me.CubeGrid.CubeExists(genRemote.Position)) {
        genRemote = null;
        List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(blocks);
        if (blocks.Count == 0)
            GridTerminalSystem.GetBlocksOfType<IMyCockpit>(blocks);
        if (blocks.Count > 0)
            genRemote = blocks[0];
    }
}

public Vector3D GroundCoordinate(IMyShipController reference, double offset)
{
    Vector3D vector, currentPosition = reference.GetPosition();
    if (reference.TryGetPlanetPosition(out planetCoordinate)) {
        vector = Vector3D.Normalize(currentPosition - planetCoordinate);
        double distance = Vector3D.Distance(planetCoordinate, currentPosition), elevation = 0;
        reference.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
        distance -= elevation + offset;
        return planetCoordinate + (vector * distance);
    }
    return new Vector3D(0, 0, 0);
}

public void StopOverride()
{
    for (int i = 0; i < gyroList.Count; i++) {
        gyroList[i].Pitch = 0f;
        gyroList[i].Yaw = 0f;
        gyroList[i].Roll = 0f;
        gyroList[i].GyroOverride = false;
    }
}

public void Align()
{
    Echo("Aligning");
    Vector2D dir = GetDirectionTo(planetCoordinate, genRemote);
    ApplyGyroOverride(dir.X, 0, -dir.Y, genRemote);
}

//Whip's ApplyGyroOverride Method v9 - 8/19/17
void ApplyGyroOverride(double pitch_speed, double yaw_speed, double roll_speed, IMyTerminalBlock reference)
{
    var rotationVec = new Vector3D(-pitch_speed, yaw_speed, roll_speed); //because keen does some weird stuff with signs
    var shipMatrix = reference.WorldMatrix;
    var relativeRotationVec = Vector3D.TransformNormal(rotationVec, shipMatrix);
    foreach (var thisGyro in gyroList)
    {
        var gyroMatrix = thisGyro.WorldMatrix;
        var transformedRotationVec = Vector3D.TransformNormal(relativeRotationVec, Matrix.Transpose(gyroMatrix));
        thisGyro.Pitch = (float)transformedRotationVec.X;
        thisGyro.Yaw = (float)transformedRotationVec.Y;
        thisGyro.Roll = (float)transformedRotationVec.Z;
        thisGyro.GyroOverride = true;
    }
}

public Vector2D GetDirectionTo(Vector3D targetVector, IMyTerminalBlock originBlock)
{
    MatrixD matrix = originBlock.WorldMatrix;
    Vector3D originVector = originBlock.GetPosition(), forwardVector = matrix.Down * 5.0 , rightVector = matrix.Right * 5.0, upVector = matrix.Forward * 5.0;
    forwardVector = forwardVector + originVector;
    rightVector = rightVector + originVector;
    upVector = upVector + originVector;

    double
    originRange = Vector3D.Distance(originVector, targetVector),
    forwardRange = Vector3D.Distance(forwardVector, targetVector),
    upRange = Vector3D.Distance(upVector, targetVector),
    rightRange = Vector3D.Distance(rightVector, targetVector),
    upLength = Vector3D.Distance(upVector, originVector),
    rightLength = Vector3D.Distance(rightVector, originVector);

    double
    ThetaP = Math.Acos((upRange * upRange - upLength * upLength - originRange * originRange) / (-2.0 * upLength * originRange)),
    ThetaY = Math.Acos((rightRange * rightRange - rightLength * rightLength - originRange * originRange) / (-2.0 * rightLength * originRange));
    double outPitch = 90.0 - (ThetaP * 180.0 / Math.PI), outYaw = 90.0 - (ThetaY * 180.0 / Math.PI);

    if (originRange < forwardRange) outPitch = 180 - outPitch;
    if (outPitch > 180.0) outPitch = -1 * (360 - outPitch);
    if (originRange < forwardRange) outYaw = 180.0 - outYaw;
    if (outYaw > 180.0) outYaw = -1.0 * (360.0 - outYaw);

    outPitch = (outPitch / 180.0) * Math.PI * gyroStrengthMultiplier;
    outYaw = (outYaw / 180.0) * Math.PI * gyroStrengthMultiplier;

    return new Vector2D(outPitch, outYaw);
}

public void SaveData()
{
    StringBuilder builder = new StringBuilder();

    builder.AppendLine("sendTag=" + sendTag + ";");
    builder.AppendLine("gyroStrengthMultiplier=" + gyroStrengthMultiplier.ToString() + ";");
    builder.AppendLine("scriptName=" + scriptName + ";");
    builder.AppendLine("settingsVersion=" + settingsVersion.ToString("N2") + ";");

    Me.CustomData = builder.ToString().TrimEnd();

    foundVersion = false;
    foundName = false;
    resave = false;
    Storage = Me.CustomData;
    settingBackup = Me.CustomData;
}

public void LoadData()
{
    if (Me.CustomData == "" && Storage != "") Me.CustomData = Storage;

    string temp = Me.CustomData.Replace("\r\n", String.Empty).Replace("\n", String.Empty).Replace("\r", String.Empty).Replace("\t", String.Empty);

    string[] settingArray = temp.Split(new char[] { ';' }, StringSplitOptions.RemoveEmptyEntries);

    for (int i = 0; i < settingArray.Length; i++)
        ProcessSetting(settingArray[i]);

    if (resave || !foundVersion || !foundName) SaveData();
    else settingBackup = Me.CustomData;
}

public void ProcessSetting(string settingString)
{
    string settingKey = settingString.Substring(0, settingString.IndexOf("=")), settingValue = settingString.Substring(settingString.IndexOf("=") + 1);

    bool settingBool = settingValue.ToLower() == "true";

    double settingDouble = 0.0;
    try { settingDouble = double.Parse(settingValue); } catch { }

    switch (settingKey)
    {
        case "sendTag":
            sendTag = settingValue;
        break;
        case "gyroStrengthMultiplier":
            gyroStrengthMultiplier = settingDouble;
        break;
        case "scriptName":
            foundName = true;
            if (settingValue != scriptName) resave = true;
        break;
        case "settingsVersion":
            foundVersion = true;
            if (settingDouble != settingsVersion) resave = true;
        break;
    }
}