My Missile Guidance Script - Dev Version

/*
Whip's GASP Missile Guidance Script v15 - revised: 11/2/15
/// PROPRIETARY VERSION ///
    Torpedo 1
___________________________________________________________________
Description:

This script handles the launch of the GASP Torpedo for the WMI Komodo Destroyer

- Whiplash141 :)
*/

/*
___________________________________________________________________

========== You can edit these variables to your liking ============
___________________________________________________________________
*/
    static string torpedo_no = "1";
    string forwardThrustName = "Forward Thrust - Torpedo - " + torpedo_no; //name tag of forward thrust on the missile
    string maneuveringThrustersName = "Thruster - Maneuvering - " + torpedo_no; //name tag of manevering thrusters on the missile
    string mergeName = "Merge - Missile Main - " + torpedo_no; //name tag of missile merge
    string artMassName = "Artificial Mass - Detach - " + torpedo_no; //name tag of missile mass
    string batteryName = "Battery - Torpedo - " + torpedo_no; //name tag of missile battery
    string shooterReferenceName = "[Shooter]"; //name tag of shooter's remote
    string missileReferenceName = "[Missile " + torpedo_no + "]"; //name tag of missile's remote
    string gyroName = "[Control " + torpedo_no + "]"; //name of missile gyro
    double guidance_delay = 2; //time (in seconds) that the missile will delay guidance activation by
    double max_rotation_degrees = 180; //in degrees per second (360 max for small ships, 180 max for large ships)
    double max_distance = 10000; //maximum guidance distance in meters; don't enlarge it lol, 10km is super far
    int tick_limit = 1; //change to higher for less precision

/*
___________________________________________________________________

============= Don't touch anything below this :) ==================
___________________________________________________________________
*/
//---So many lists...
    List<IMyTerminalBlock> forwardThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> artMasses = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> mergeBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> maneuveringThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> batteries = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> remotes = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> shooterRefrenceList = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> missileRefrenceList = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> gyroList = new List<IMyTerminalBlock>();

    IMyRemoteControl shooterRefrence;
    IMyRemoteControl missileRefrence;
    bool hasRun = false;
    double delta_origin;
    int current_tick = 0;
    int duration = 0;
    int timeElapsed = 0;

void Main()
{
    Echo("Tick: " + current_tick.ToString());

    if (!hasRun)
    {
        GrabRemotes();
    }
    else //will not run or release missile until has run setup succesfully
    {
        MissileSystems();

        if (duration < Math.Ceiling(guidance_delay * 60))
        {
            duration++;
            return;
        }
        else
        {
            if((current_tick % tick_limit) == 0)
            {
                Echo("Guidance Active");
                GuideMissile();
                current_tick = 0;
            }
        }
        current_tick++;
    }

    Echo("Has run?: " + hasRun);
}

void GrabRemotes()
{
    GridTerminalSystem.SearchBlocksOfName(gyroName, gyroList);
    GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(remotes);
    for(int i = 0 ; i < remotes.Count ; i++)
        {
            var thisRemote = remotes[i] as IMyRemoteControl;
            if(thisRemote.CustomName.Contains(shooterReferenceName))
            {
                shooterRefrenceList.Add(thisRemote as IMyRemoteControl);
                Echo("Found Shooter");
            }

            if(thisRemote.CustomName.Contains(missileReferenceName))
            {
                missileRefrenceList.Add(thisRemote as IMyRemoteControl);
                Echo("Found Missile");
            }
        }

//---Check if we do not have an shooter remote
    if(shooterRefrenceList.Count == 0)
    {
        Echo("No shooter refrence block found");
        hasRun = false;
        return;
    }
//---Check if we do not have a missile remote
    else if(missileRefrenceList.Count == 0)
    {
        Echo("No missile refrence block found");
        hasRun = false;
        return;
    }
    else if(gyroList.Count == 0)
    {
        Echo("No control gyro found");
        hasRun = false;
        return;
    }
    else
    {
        Echo("Ready to run");
        shooterRefrence = shooterRefrenceList[0] as IMyRemoteControl;
        missileRefrence = missileRefrenceList[0] as IMyRemoteControl;
        hasRun = true;
    }
}

void GuideMissile()
{
//---Get positions of our blocks with relation to world center
    var originPos = shooterRefrence.GetPosition();
    var missilePos = missileRefrence.GetPosition();

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

//---Check if we are in range
    if(delta_origin < max_distance) //change this later to be larger
    {
    //---Get forward vector from our shooter vessel
        var shooterForward = shooterRefrence.Position + Base6Directions.GetIntVector(shooterRefrence.Orientation.TransformDirection(Base6Directions.Direction.Forward));
        var targetVector = shooterRefrence.CubeGrid.GridIntegerToWorld(shooterForward);
        var targetVectorNorm = Vector3D.Normalize(targetVector - shooterRefrence.GetPosition());

    //---Find vector from shooter to missile
        var missileVector = Vector3D.Subtract(missilePos, originPos);

    //---Calculate angle between shooter vector and missile vector
        double dotProduct; Vector3D.Dot(ref targetVectorNorm, ref missileVector, out dotProduct);
        double x = dotProduct / missileVector.Length();
        double rawDevAngle = Math.Acos(x) * 180f / Math.PI; //angle between shooter vector and missile

    //---Calculate perpendicular distance from shooter vector
        var projectionVector = dotProduct * targetVectorNorm;
        double deviationDistance = Vector3D.Distance(projectionVector,missileVector);
        Echo("Angular Dev: " + rawDevAngle.ToString());

    //---Determine scaling factor
        double scalingFactor;
        if(rawDevAngle < 90)
        {
            if(deviationDistance > 100)
            {
                scalingFactor = delta_origin; //if we are too far from the beam, dont add any more distance till we are closer
            }
            else
            {
                scalingFactor = (delta_origin + 100); //travel approx. 100m from current position in direction of target vector
            }
        }
        else
        {
            scalingFactor = 200; //if missile is behind the shooter, goes 200m directly infront of shooter for better accuracy
        }
        var destination = shooterRefrence.GetPosition() + scalingFactor * targetVectorNorm;
        Echo(destination.ToString()); //debug

    //---Find front left and top vectors of our missile
        var missileGridX = missileRefrence.Position + Base6Directions.GetIntVector(missileRefrence.Orientation.TransformDirection(Base6Directions.Direction.Forward));
        var missileWorldX = missileRefrence.CubeGrid.GridIntegerToWorld(missileGridX) - missilePos;

        var missileGridY = missileRefrence.Position + Base6Directions.GetIntVector(missileRefrence.Orientation.TransformDirection(Base6Directions.Direction.Left));
        var missileWorldY = missileRefrence.CubeGrid.GridIntegerToWorld(missileGridY) - missilePos;

        var missileGridZ = missileRefrence.Position + Base6Directions.GetIntVector(missileRefrence.Orientation.TransformDirection(Base6Directions.Direction.Up));
        var missileWorldZ = missileRefrence.CubeGrid.GridIntegerToWorld(missileGridZ) - missilePos;

    //---Find vector from missile to destination
        var shipToTarget = Vector3D.Subtract(destination, missilePos);

    //---Project target vector onto our top left and up vectors
        double dotX; Vector3D.Dot(ref shipToTarget, ref missileWorldX, out dotX);
        double dotY; Vector3D.Dot(ref shipToTarget, ref missileWorldY, out dotY);
        double dotZ; Vector3D.Dot(ref shipToTarget, ref missileWorldZ, out dotZ);
        var projTargetX = dotX / (missileWorldX.Length() * missileWorldX.Length()) * missileWorldX;
        var projTargetY = dotY / (missileWorldY.Length() * missileWorldY.Length()) * missileWorldY;
        var projTargetZ = dotZ / (missileWorldZ.Length() * missileWorldZ.Length()) * missileWorldZ;
        var projTargetXYplane = projTargetX + projTargetY;

    //---Get Yaw and Pitch Angles
        double angleYaw = Math.Atan(projTargetY.Length() / projTargetX.Length());
        double anglePitch = Math.Atan(projTargetZ.Length() / projTargetXYplane.Length());

    //---Check if x is positive or negative
        double checkPositiveX; Vector3D.Dot(ref missileWorldX, ref projTargetX, out checkPositiveX); Echo("check x:" + checkPositiveX.ToString());
        if(checkPositiveX < 0)
        {
            angleYaw += Math.PI/2; //we only change one value so it doesnt spaz
        }

    //---Check if yaw angle is left or right
        double checkYaw; Vector3D.Dot(ref missileWorldY, ref projTargetY, out checkYaw); Echo("check yaw:" + checkYaw.ToString());
        if(checkYaw > 0) //yaw is backwards for what ever reason
            angleYaw = -angleYaw;
        Echo("yaw angle:" + angleYaw.ToString());

    //---Check if pitch angle is up or down
        double checkPitch; Vector3D.Dot(ref missileWorldZ, ref projTargetZ, out checkPitch); Echo("check pitch:" + checkPitch.ToString());
        if(checkPitch < 0)
            anglePitch = -anglePitch;
        Echo("pitch angle:" + anglePitch.ToString());

    //---Angle controller
        double max_rotation_radians = max_rotation_degrees * (Math.PI / 180);
        double yawSpeed = max_rotation_radians * angleYaw / Math.Abs(angleYaw);
        double pitchSpeed = max_rotation_radians * anglePitch / Math.Abs(anglePitch);

        //Alt method 1: Proportional Control
        //(small ship gyros too weak for this to be effective)
        /*
            double yawSpeed = angleYaw / Math.PI * max_rotation_radians;
            double pitchSpeed = anglePitch / Math.PI * max_rotation_radians;
        */

        //Alt method 2: Proportional Control with bounds
        //(Small gyros still too weak :/)
            /*if (angleYaw < Math.PI/4)
            {
                yawSpeed = angleYaw * max_rotation_radians / (Math.PI/4);
            }
            else
            {
                yawSpeed = max_rotation_radians;
            }

            if (anglePitch < Math.PI/4)
            {
                pitchSpeed = anglePitch * max_rotation_radians / (Math.PI/4);
            }
            else
            {
                pitchSpeed = max_rotation_radians;
            }*/

    //---Set appropriate gyro override
        for(int i = 0; i < gyroList.Count; i++)
        {
            var thisGyro = gyroList[i] as IMyGyro;
            thisGyro.SetValue<float>("Yaw", (float)yawSpeed);
            thisGyro.SetValue<float>("Pitch", (float)pitchSpeed);
            thisGyro.SetValue("Override", true);
        }
    }
    else
    {
        Echo("Out of range");
        for(int i = 0; i < gyroList.Count; i++)
        {
            var thisGyro = gyroList[i] as IMyGyro;
            thisGyro.SetValue<float>("Yaw", 0f);
            thisGyro.SetValue<float>("Pitch", 0f);
            thisGyro.SetValue("Override", true);
        }
    }
}

void MissileSystems()
{
    GridTerminalSystem.SearchBlocksOfName(artMassName,artMasses);
    GridTerminalSystem.SearchBlocksOfName(mergeName,mergeBlocks);
    GridTerminalSystem.SearchBlocksOfName(batteryName,batteries);

//---Check for the required launch components
    if(mergeBlocks.Count == 0)
    {
        Echo("No merges found");
    }
    else if(artMasses.Count == 0)
    {
        Echo("No masses found");
    }
    else if(batteries.Count == 0)
    {
        Echo("No batteries found");
    }

    if (timeElapsed == 0)
    {
        for(int i = 0 ; i < batteries.Count ; i++)
        {
            var thisBattery = batteries[i] as IMyBatteryBlock;
            thisBattery.ApplyAction("OnOff_On"); //make sure our battery is on
            thisBattery.SetValue("Recharge", false);
        }

    }
    else if(timeElapsed == 60)
    {
        for(int i = 0 ; i < artMasses.Count ; i++)
        {
            var thisMass = artMasses[i] as IMyVirtualMass;
            thisMass.ApplyAction("OnOff_On");
        }
        Echo("Detach Thrust On");

        for(int i = 0 ; i < mergeBlocks.Count ; i++)
        {
            var thisMerge = mergeBlocks[i] as IMyShipMergeBlock;
            thisMerge.ApplyAction("OnOff_Off");
        }
    }
    else if(timeElapsed == 180)
    {
        for(int i = 0 ; i < artMasses.Count ; i++)
        {
            var thisMass = artMasses[i] as IMyVirtualMass;
            thisMass.ApplyAction("OnOff_Off");
        }
        ThrusterOverride();
        ManeuveringThrust();
        Echo("Detach Thrust Off");
        Echo("Thruster Override On");
    }

    if (timeElapsed < 180)
        timeElapsed++;
}

void ManeuveringThrust()
{
    if(maneuveringThrusters.Count == 0)
        Echo("No maneivering thrust found");

    GridTerminalSystem.SearchBlocksOfName(maneuveringThrustersName,maneuveringThrusters);
    for(int i = 0 ; i < maneuveringThrusters.Count ; i++)
    {
        IMyThrust Thrust = maneuveringThrusters[i] as IMyThrust;
        Thrust.ApplyAction("OnOff_On");
    }
}

void ThrusterOverride()
{
    if(forwardThrusters.Count == 0)
        Echo("No main thrust found");

    GridTerminalSystem.SearchBlocksOfName(forwardThrustName,forwardThrusters);
    for(int i = 0; i < forwardThrusters.Count;i++)
    {
        IMyThrust Thrust = forwardThrusters[i] as IMyThrust;
        Thrust.ApplyAction("OnOff_On");
        Thrust.SetValue<float>("Override", float.MaxValue);
    }
}