GASP Missile Systems 12-20-15

/*
Whip's GASP Missile System v23 - revised: 12/19/15
/// PERSONAL VERSION ///
/// Stable ///
    Missile 1
___________________________________________________________________
Description:

This script handles the launch of the GASP Missile System

- Whiplash141 :)
*/

/*
___________________________________________________________________

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

//---Missile name
    static string missileNumber = "1";
    string missileTag = "Missile " + missileNumber; //unique id of the missile
    string sideThrustTag = "Side"; //tag on side thrusters
    string mainThrustTag = "Main"; //tag on main forward thrusters

//---Reference names
    string shooterReferenceName = "Shooter Reference"; //notice how this has no number

//---Runtime vars
    double guidance_delay = 1; //time (in seconds) that the missile will delay guidance activation by
    double main_ignition_delay = .5; //time (in seconds) that the missile will delay main engine activation
    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 10km is super far
    static int tick_limit = 1; //change to higher for less precision (I do not reccomend this)

//---Control system
    int controlMode = 4; //1 = bang bang, 2 = proportional, 3 = proportional with lower bound
    double lowerAngleBoundDeg = 30;

/*
___________________________________________________________________

============= Don't touch anything below this :) ==================
___________________________________________________________________
*/
//---So many lists...
    List<IMyTerminalBlock> missileBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> mainThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> artMasses = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> mergeBlocks = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> sideThrusters = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> batteries = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> remotes = new List<IMyTerminalBlock>();
    List<IMyTerminalBlock> shooterRefrenceList = 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>();

    Vector3D targetVectorNorm;
    Vector3D shooterLeftNorm;
    Vector3D shooterUpNorm;
    Vector3D originPos;
    Vector3D currentMissilePos;
    Vector3D headingVector;
    Vector3D destination;
    IMyRemoteControl shooterRefrence;
    IMyGyro missileRefrence;
    bool hasRun = false;
    bool isFired = false;
    bool shouldKill = false;
    bool firstGuidance = true;
    bool hasPassed = false;
    double distanceFromShooter;
    double lowerAngleBoundRad;
    int max_kill_time = 60 / tick_limit;
    int kill_time = 0;
    int current_tick = 0;
    int duration = 0;
    int timeElapsed = 0;

void Main(string arg)
{
    if(arg == "kill" && isFired == true)
    {
        shouldKill = true;
        max_distance = double.PositiveInfinity;
    }


    Echo("Tick: " + current_tick.ToString());
    if (!hasRun)
    {
        GrabBlocks();
    }
    else //will not run or release missile until has run setup succesfully
    {
        MissileSystems();
        StatusCheck();
        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 GrabBlocks()
{
    GridTerminalSystem.SearchBlocksOfName( missileTag, missileBlocks );
    GridTerminalSystem.SearchBlocksOfName( shooterReferenceName, shooterRefrenceList );

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

    for( int i = 0; i < missileBlocks.Count; i++ )
    {
        var thisBlock = missileBlocks[i] as IMyTerminalBlock;

        if( thisBlock is IMyThrust )
        {
            if( thisBlock.CustomName.Contains( sideThrustTag ) )
            {
                sideThrusters.Add( thisBlock );
            }
            else if( thisBlock.CustomName.Contains( mainThrustTag ) )
            {
                mainThrusters.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 );
            thisBlock.ApplyAction("OnOff_On");
            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 );
        }
    }

//---Check if we do not have an shooter remote
    if( shooterRefrenceList.Count == 0 )
    {
        Echo("No shooter refrence block found");
        hasRun = false;
        return;
    }

//---Check for control gyros
    else if( gyros.Count == 0 )
    {
        Echo("No control gyros found");
        hasRun = false;
        return;
    }
    else if( mainThrusters.Count == 0)
    {
        Echo("No main thrusters found");
        hasRun = false;
        return;
    }
    else if( sideThrusters.Count == 0)
    {
        Echo("No main thrusters found");
        //hasRun = false;
        //return;
    }
    else if( artMasses.Count == 0)
    {
        Echo("No artificial masses found");
        hasRun = false;
        return;
    }
    else if( batteries.Count == 0)
    {
        Echo("No batteries found");
        hasRun = false;
        return;
    }
    else if( mergeBlocks.Count == 0)
    {
        Echo("No merge blocks found");
        hasRun = false;
        return;
    }
    else
    {
        Echo("Ready to run");
        shooterRefrence = shooterRefrenceList[0] as IMyRemoteControl;
        missileRefrence = gyros[0] as IMyGyro;
        hasRun = true;
    }
}

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 MissileSystems()
{
    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" );
        }

        for( int i = 0 ; i < mergeBlocks.Count ; i++ )
        {
            var thisMerge = mergeBlocks[i] as IMyShipMergeBlock;
            thisMerge.ApplyAction( "OnOff_Off" );
        }
    }
    else if( timeElapsed == 60 + Math.Round( main_ignition_delay * 60 ) )
    {
        for( int i = 0 ; i < artMasses.Count ; i++ )
        {
            var thisMass = artMasses[i] as IMyVirtualMass;
            thisMass.ApplyAction( "OnOff_Off" );
        }
        ThrusterOverride();
        ManeuveringThrust();
        isFired = true;
    }

    if ( timeElapsed < 181 )
    {
        timeElapsed++;
    }
}

void GuideMissile()
{
//---Get positions of our blocks with relation to world center
    if( !shouldKill )
        originPos = shooterRefrence.GetPosition();

    currentMissilePos = missileRefrence.GetPosition();

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

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

//---Get orientation vectors from our shooter vessel
    if( !shouldKill )
    {
        var shooterForwardGrid = shooterRefrence.Position + Base6Directions.GetIntVector( shooterRefrence.Orientation.TransformDirection( Base6Directions.Direction.Forward ) );
        var targetVector = shooterRefrence.CubeGrid.GridIntegerToWorld( shooterForwardGrid ) - originPos;
        targetVectorNorm = Vector3D.Normalize( targetVector );

        var shooterLeftGrid = shooterRefrence.Position + Base6Directions.GetIntVector( shooterRefrence.Orientation.TransformDirection( Base6Directions.Direction.Left ) );
        var shooterLeftWorld = shooterRefrence.CubeGrid.GridIntegerToWorld( shooterLeftGrid ) - originPos;
        shooterLeftNorm = Vector3D.Normalize( shooterLeftWorld );

        var shooterUpGrid = shooterRefrence.Position + Base6Directions.GetIntVector( shooterRefrence.Orientation.TransformDirection( Base6Directions.Direction.Up ) );
        var shooterUpWorld = shooterRefrence.CubeGrid.GridIntegerToWorld( shooterUpGrid ) - originPos;
        shooterUpNorm = Vector3D.Normalize( shooterUpWorld );
    }

//---Find vector from shooter to missile
    var shooterToMissile = currentMissilePos - originPos;

//---Determine where missile is in relation to shooter
    bool isLeft = checkDotPositive( shooterToMissile, shooterLeftNorm );
    bool isUp = checkDotPositive( shooterToMissile, shooterUpNorm );
    int signLeft; int signUp;
    if( isLeft )
        signLeft = 1;
    else
        signLeft = -1;

    if( isUp )
        signUp = 1;
    else
        signUp = -1;

//---Calculate angle between shooter vector and missile vector
    double rawDevAngle = Math.Acos( dotProduct( targetVectorNorm, Vector3D.Normalize( shooterToMissile ) ) ) * 180 / Math.PI; //angle between shooter vector and missile

//---Calculate perpendicular distance from shooter vector
    var projectionVector = vectorProjection( shooterToMissile, targetVectorNorm );
    double deviationDistance = Vector3D.Distance( projectionVector, shooterToMissile );

    Echo( "Angular Dev: " + rawDevAngle.ToString() );

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

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

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

//---Determine scaling factor
    double scalingFactor;
    if( rawDevAngle < 90 )
    {
        scalingFactor = projectionVector.Length() + 250; //travel approx. 250m from current position in direction of target vector
        destination = shooterRefrence.GetPosition() + scalingFactor * targetVectorNorm;
        if( !hasPassed )
            hasPassed = true;
    }
    else if( hasPassed )
    {
        scalingFactor = -projectionVector.Length() + 250;
        destination = shooterRefrence.GetPosition() + scalingFactor * targetVectorNorm + signLeft * 50 * shooterLeftNorm + signUp * 50 * shooterUpNorm;
    }
    else
    {
        scalingFactor = -projectionVector.Length() + 250;
        destination = shooterRefrence.GetPosition() + scalingFactor * targetVectorNorm;
    }

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

//---Calc our new heading based upon our travel vector
    headingVector = shipToTarget;

//---Project target vector onto our top left and up vectors
    var projTargetX = vectorProjection( headingVector, missileWorldX);
    var projTargetY = vectorProjection( headingVector, missileWorldY);
    var projTargetZ = vectorProjection( headingVector, 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
    bool isPositiveX = checkDotPositive( missileWorldX, projTargetX );
    if( !isPositiveX )
        angleYaw += Math.PI/2; //we only change one value so it doesnt spaz

//---Check if yaw angle is left or right
    bool isPositiveY = checkDotPositive( missileWorldY, projTargetY );
    if( isPositiveY ) //yaw is backwards for what ever reason
        angleYaw = -angleYaw;

//---Check if pitch angle is up or down
    bool isPositiveZ = checkDotPositive( missileWorldZ, projTargetZ );
    if( !isPositiveZ )
        anglePitch = -anglePitch;

    Echo("yaw: " + Math.Round(angleYaw,2).ToString());
    Echo("pitch: " + Math.Round(anglePitch,2).ToString());

//---Angle controller

    double max_rotation_radians = max_rotation_degrees * ( Math.PI / 180 );
    double yawSpeed = 0;
    double pitchSpeed = 0;

    switch( controlMode )
    {
        case 1:
            yawSpeed = max_rotation_radians * angleYaw / Math.Abs( angleYaw );
            pitchSpeed = max_rotation_radians * anglePitch / Math.Abs( anglePitch );
            break;

        case 2:
            yawSpeed = angleYaw / Math.PI * max_rotation_radians;
            pitchSpeed = anglePitch / Math.PI * max_rotation_radians;
            break;

        case 3:
            lowerAngleBoundRad = lowerAngleBoundDeg * ( Math.PI / 180 );
            if( Math.Abs( angleYaw ) < lowerAngleBoundRad )
            {
                yawSpeed = angleYaw / lowerAngleBoundRad * max_rotation_radians;
            }
            else
            {
                yawSpeed = max_rotation_radians * angleYaw / Math.Abs( angleYaw );
            }

            if( Math.Abs( anglePitch ) < lowerAngleBoundRad )
            {
                pitchSpeed = anglePitch / lowerAngleBoundRad * max_rotation_radians;
            }
            else
            {
                pitchSpeed = max_rotation_radians * anglePitch / Math.Abs( anglePitch );
            }
            break;

        case 4:
            lowerAngleBoundRad = lowerAngleBoundDeg * ( Math.PI / 180 );
            if( Math.Abs( angleYaw ) < lowerAngleBoundRad )
            {
                yawSpeed = max_rotation_radians / 4 * angleYaw / Math.Abs( angleYaw );
            }
            else
            {
                yawSpeed = max_rotation_radians * angleYaw / Math.Abs( angleYaw );
            }

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

        default:
            yawSpeed = max_rotation_radians * angleYaw / Math.Abs( angleYaw );
            pitchSpeed = max_rotation_radians * anglePitch / Math.Abs( anglePitch );
            break;
    }

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

    if( shouldKill )
    {
        KillGuidance( rawDevAngle, deviationDistance );
    }
}

void ManeuveringThrust()
{
    for( int i = 0 ; i < sideThrusters.Count ; i++ )
    {
        IMyThrust Thrust = sideThrusters[i] as IMyThrust;
        Thrust.ApplyAction( "OnOff_On" );
    }
}

void ThrusterOverride()
{
    for( int i = 0; i < mainThrusters.Count; i++ )
    {
        IMyThrust Thrust = mainThrusters[i] as IMyThrust;
        Thrust.ApplyAction( "OnOff_On" );
        Thrust.SetValue<float>( "Override", float.MaxValue ); //not max b/c battery can't handle any more without an overload
    }
}

void KillGuidance( double angleOfDeviation, double distanceOfDeviation )
{
    Echo("Kill command recieved");
    if( angleOfDeviation < 2.5 && distanceOfDeviation < 2.5 )
    {
        kill_time++;
    }else{
        kill_time = 0;
    }

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

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

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

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

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

bool checkDotPositive( Vector3D vec1, Vector3D vec2 )
{
    double check = dotProduct( vec1, vec2 );
    if( check > 0 )
    {
        return true;
    }else{
        return false;
    }
}

double dotProduct( Vector3D v1, Vector3D v2 )
{
    double x;
    Vector3D.Dot( ref v1, ref v2, out x );
    return x;
}