Radar

IMyRemoteControl remote;
IMyTextPanel lcdCoords, lcdRadar, lcdLog, lcdEstado;
IMyGyro gyro;
IMyGravityGenerator grav1, grav2;

List<IMyTerminalBlock> controllerBlocks = new List<IMyTerminalBlock>();
List<IMyTerminalBlock> forwardThrusters = new List<IMyTerminalBlock>();
List<IMyTerminalBlock> backwardThrusters = new List<IMyTerminalBlock>();

Vector3D last;
string[] GPS1, GPS2;
//bool acelerando;
int punto = 0;
double x, y, z;
double speed;
bool finalizado = false;
int ngps = 1;

public Program()
{

    // The constructor, called only once every session and
    // always before any other method is called. Use it to
    // initialize your script.
    //
    // The constructor is optional and can be removed if not
    // needed.

    //acelerando = false;
    if (gyro == null)
    {
        List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyGyro>(blocks);
        if (blocks.Count > 0)
            gyro = blocks[0] as IMyGyro;
    }
    if (remote == null)
    {
        List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(blocks);
        if (blocks.Count > 0)
            remote = blocks[0] as IMyRemoteControl;
    }
    if (lcdCoords == null)
    {
        lcdCoords = GridTerminalSystem.GetBlockWithName("LCD Coords") as IMyTextPanel;
    }
    if (lcdRadar == null)
    {
        lcdRadar = GridTerminalSystem.GetBlockWithName("LCD Radar") as IMyTextPanel;
    }
    if (lcdLog == null)
    {
        lcdLog = GridTerminalSystem.GetBlockWithName("LCD Log") as IMyTextPanel;
    }
    if (lcdEstado == null)
    {
        lcdEstado = GridTerminalSystem.GetBlockWithName("LCD Estado") as IMyTextPanel;
    }

    if ((lcdCoords == null) || (lcdRadar == null) || (lcdLog == null) || (lcdEstado == null))
    {
        Echo("Unable to find a LCD");
        return;
    }

    if (grav1 == null)
    {
        List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyGravityGenerator>(blocks);
        if (blocks.Count > 0)
            grav1 = blocks[0] as IMyGravityGenerator;
    }
    if (grav2 == null)
    {
        List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyGravityGenerator>(blocks);
        if (blocks.Count > 0)
            grav2 = blocks[1] as IMyGravityGenerator;
    }

    if (controllerBlocks.Count == 0)
    {
        GridTerminalSystem.GetBlocksOfType<IMyShipController>(controllerBlocks);
    }
    if (backwardThrusters.Count == 0)
    {
        GridTerminalSystem.SearchBlocksOfName("Backward", backwardThrusters);
    }
    if (forwardThrusters.Count == 0)
    {
        GridTerminalSystem.SearchBlocksOfName("Forward", forwardThrusters);
    }

    string[] loadData = remote.CustomName.Split(',');
    if (loadData.Length == 2)
    {
        Int32.TryParse(loadData[0], out ngps);
        Int32.TryParse(loadData[1], out punto);
    }
    if (ngps < 1) ngps = 1;
}

public void Save()
{

    // Called when the program needs to save its state. Use
    // this method to save your state to the Storage field
    // or some other means.
    //
    // This method is optional and can be removed if not
    // needed.

    remote.SetCustomName(ngps.ToString() + "," + punto.ToString());
}



public void Main(string argument)
{

    // The main entry point of the script, invoked every time
    // one of the programmable block's Run actions are invoked.
    //
    // The method itself is required, but the argument above
    // can be removed if not needed.

    if (argument != "")
    {
        punto = 0;
        if (Int32.TryParse(argument, out ngps))
            {
            lcdLog.WritePublicText("Iniciando con argumento: '"+argument+"'");
            }
        if (ngps == 0) ngps = 1;
        finalizado = false;
    }

    if (finalizado) { return; }

    Vector3D vector1 = remote.WorldMatrix.Forward;
    GetSpeed();
    if (((punto % 10) == 0) || (punto < 10))
    {
        if (((vector1 - Vector3D.Normalize(new Vector3D(x, y, z) - remote.GetPosition())).Length() > 0.1))
        {
            string[] GPS1 = lcdCoords.GetPublicText().Split(new string[] { "\n" }, StringSplitOptions.RemoveEmptyEntries);
            if (ngps > GPS1.Length)
            {
                lcdLog.WritePublicText("\nFINALIZADO", true);
                finalizado = true;
                return;
            }
            string[] GPS2 = GPS1[ngps - 1].Split(new string[] { ":" }, StringSplitOptions.RemoveEmptyEntries);
            if (GPS2.Length < 5) { ngps++; return; }
            x = Convert.ToDouble(GPS2[2]);
            y = Convert.ToDouble(GPS2[3]);
            z = Convert.ToDouble(GPS2[4]);
            remote.ClearWaypoints();
            remote.AddWaypoint(new Vector3D(x, y, z), "Objetivo: " + GPS1[ngps - 1]);
            lcdLog.WritePublicText("\nNuevo rumbo: " + GPS1[ngps - 1], true);
            remote.SetAutoPilotEnabled(true);
            if (punto < 10) punto = 10;
        }
        else
        {
            remote.SetAutoPilotEnabled(false);
            if (speed < 250)
            {
                //TurnOffDampeners();
                for (int n = 0; n < backwardThrusters.Count; n++)
                {
                    IMyThrust thrust = backwardThrusters[n] as IMyThrust;
                    thrust.GetActionWithName("OnOff_Off").Apply(thrust);
                    //lcdLog.WritePublicText("\nApagando thruster " + thrust.CustomName, true);
                }
                for (int n = 0; n < forwardThrusters.Count; n++)
                {
                    IMyThrust thrust = forwardThrusters[n] as IMyThrust;
                    thrust.SetValueFloat("Override", 12000f);
                    //lcdLog.WritePublicText("\nAcelerando thruster " + thrust.CustomName, true);
                }
            }
            punto++;
        }
    }
    else
    {
        if (speed > 299.9)
        {
            for (int n = 0; n < forwardThrusters.Count; n++)
            {
                IMyThrust thrust = forwardThrusters[n] as IMyThrust;
                thrust.SetValueFloat("Override", 0f);
                //lcdLog.WritePublicText("\nDesactivando aceleracion en thruster " + thrust.CustomName, true);
            }
        }
        else if (speed > 250)
        {
            IMyThrust thrust = forwardThrusters[0] as IMyThrust;
            thrust.SetValueFloat("Override", 4f);
        }

        if ((remote.GetPosition() - new Vector3D(x, y, z)).Length() < 5000)
        {
            lcdLog.WritePublicText("\nAlcanzado el punto " + ngps.ToString(), true);
            for (int n = 0; n < backwardThrusters.Count; n++)
            {
                IMyThrust thrust = backwardThrusters[n] as IMyThrust;
                thrust.GetActionWithName("OnOff_On").Apply(thrust);
            }
            for (int n = 0; n < forwardThrusters.Count; n++)
            {
                IMyThrust thrust = forwardThrusters[n] as IMyThrust;
                thrust.SetValueFloat("Override", 0f);
            }
            punto = ngps * 1000000;
            ngps++;
            return;
        }
        else if (punto > 10) { punto++; }
        if ((lcdRadar.GetPublicText().Trim() != "") || (lcdRadar.GetPrivateText().Trim() != ""))
        {
            lcdLog.WritePublicText("Contacto detectado en " + remote.GetPosition().ToString() + "\n", true);
            if ((lcdRadar.GetPublicText().Trim() != lcdLog.GetPrivateText().Trim()) || (lcdLog.GetPrivateText().Trim() == ""))
                lcdLog.WritePrivateText(lcdRadar.GetPublicText() + "\n", true);
            if ((lcdRadar.GetPrivateText().Trim() != lcdLog.GetPrivateText().Trim()) || (lcdLog.GetPrivateText().Trim() == ""))
                lcdLog.WritePrivateText(lcdRadar.GetPrivateText() + "\n", true);
        }
    }

    lcdEstado.WritePublicText("Punto: " + punto.ToString());
    lcdEstado.WritePublicText("\nDesviacion: " + (vector1 - Vector3D.Normalize(new Vector3D(x, y, z) - remote.GetPosition())).Length().ToString(), true);
    lcdEstado.WritePublicText("\nVelocidad = " + speed.ToString(), true);
    lcdEstado.WritePublicText("\n" + vector1.ToString(), true);
    lcdEstado.WritePublicText("\nDist: " + (new Vector3D(x, y, z) - remote.GetPosition()).Length().ToString(), true);
    lcdEstado.WritePublicText("\n" + Vector3D.Normalize(new Vector3D(x, y, z) - remote.GetPosition()).ToString(), true);
    lcdEstado.WritePublicText("\n" + (vector1 - Vector3D.Normalize(new Vector3D(x, y, z) - remote.GetPosition())).
        Length().ToString(), true);
    last = vector1;
    Save();
}

void GetSpeed()
{
    for (int i = 0; i < controllerBlocks.Count; i++)
    {
        IMyShipController controller = controllerBlocks[i] as IMyShipController;
        if (controller != null)
        {
            if (i == 0) speed = controller.GetShipSpeed();
        }
    }
}

void TurnOffDampeners()
{
    // Disable dampeners on all control blocks
    for (int i = 0; i < controllerBlocks.Count; i++)
    {
        IMyShipController controller = controllerBlocks[i] as IMyShipController;
        if (controller != null)
        {
            if (i == 0) speed = controller.GetShipSpeed();
            if (controller.DampenersOverride == true)
            {
                lcdLog.WritePublicText("\nApagando dampeners...", true);
                controller.GetActionWithName("DampenersOverride").Apply(controller);
            }
        }
    }
}