/* http://forums.keenswh.com/post/automated-turrets-how-programmable-block-7307510 */

string sensorTag = "Sensor Zone";
void Main()
{
    //fire is origin
    //Loop is front
    //programmable is up
    //RotorY is right
    List<IMyTerminalBlock> turretBlocks = new List<IMyTerminalBlock>();
    GridTerminalSystem.SearchBlocksOfName("Turret1", turretBlocks);

    List<IMyTerminalBlock> sensorZones = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMySensorBlock>(sensorZones);

    //Grab the first target from the sensors.
    IMySensorBlock sensorBlock = null;
    for(int i=0;i<sensorZones.Count;i++)
    {
        if(sensorZones[i].CustomName.Contains(sensorTag) && (sensorZones[i] as IMySensorBlock).LastDetectedEntity != null)
        {
            sensorBlock = sensorZones[i] as IMySensorBlock;
            i = sensorZones.Count;
        }
    }

    IMyTimerBlock loopTimer = null;
    IMyTimerBlock fireTimer = null;
    IMyProgrammableBlock programmable = null;
    IMyMotorStator rotorx = null;
    IMyMotorStator rotory = null;

    for(int i=0;i<turretBlocks.Count;i++)
    {
        if(turretBlocks[i].CustomName.Contains("Loop"))
            loopTimer = turretBlocks[i] as IMyTimerBlock;
        else if(turretBlocks[i].CustomName.Contains("Fire"))
            fireTimer = turretBlocks[i] as IMyTimerBlock;
        else if(turretBlocks[i].CustomName.Contains("Program"))
            programmable = turretBlocks[i] as IMyProgrammableBlock;
        else if(turretBlocks[i].CustomName.Contains("Rotor X"))
            rotorx = turretBlocks[i] as IMyMotorStator;
        else if(turretBlocks[i].CustomName.Contains("Rotor Y"))
            rotory = turretBlocks[i] as IMyMotorStator;
    }

    Vector3D sensorTarget = new Vector3D(0,0,0);
    if(sensorBlock != null && sensorBlock.LastDetectedEntity != null)
    {
        sensorTarget = sensorBlock.LastDetectedEntity.GetPosition();

            var x = Math.Floor(sensorTarget.GetDim(0)).ToString();
            var y = Math.Floor(sensorTarget.GetDim(1)).ToString();
            var z = Math.Floor(sensorTarget.GetDim(2)).ToString();

        float range = (float)((fireTimer.GetPosition() - sensorTarget).Length());

        float gyroPitch = 0;
        float gyroYaw = 0;

        GetDirectionTo(sensorTarget,
                                 fireTimer as IMyTerminalBlock,
                                 loopTimer as IMyTerminalBlock,
                                 programmable as IMyTerminalBlock,
                                 rotory as IMyTerminalBlock,
                                 ref gyroPitch, ref gyroYaw);

        rotorx.SetValueFloat("Velocity", gyroYaw);
        rotory.SetValueFloat("Velocity", gyroPitch);

        List<IMyTerminalBlock> thrusters = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyThrust>(thrusters);

        if((gyroPitch > -5)&&(gyroPitch < 5)&&
           (gyroYaw > -5)&&(gyroYaw < 5))
        {
            fireTimer.GetActionWithName("TriggerNow").Apply(fireTimer);
        }
    }
    else
    {
        //turn off rotors.
        rotorx.SetValueFloat("Velocity", 0.0f);
        rotory.SetValueFloat("Velocity", 0.0f);
    }
    loopTimer.GetActionWithName("TriggerNow").Apply(loopTimer);
}

/*
VRageMath.Vector3D TV         This is the target vector.
IMyTerminalBlock Origin       Origin block.
IMyTerminalBlock Forward      Block directly in front of the origin.
IMyTerminalBlock Up           Block directly above the origin.
IMyTerminalBlock Right        Block directly to the right of the origin.
ref double Pitch              Reference to Pitch.
ref double Yaw                Reference to Yaw.
*/
void GetDirectionTo(VRageMath.Vector3D TV,
                    IMyTerminalBlock Origin,
                    IMyTerminalBlock Forward,
                    IMyTerminalBlock Up,
                    IMyTerminalBlock Right,
                    ref float Pitch, ref float Yaw)
{
  VRageMath.Vector3D OV = Origin.GetPosition();//Get positions of reference blocks.
  VRageMath.Vector3D FV = Forward.GetPosition();
  VRageMath.Vector3D UV = Up.GetPosition();
  VRageMath.Vector3D RV = Right.GetPosition();

  float TVOV = (float)((OV - TV).Length());//Get magnitudes of vectors.

  float TVFV = (float)((FV - TV).Length());
  float TVUV = (float)((UV - TV).Length());
  float TVRV = (float)((RV - TV).Length());

  float OVFV = (float)((FV - OV).Length());
  float OVUV = (float)((UV - OV).Length());
  float OVRV = (float)((RV - OV).Length());

  float ThetaP = (float)(Math.Acos((TVUV * TVUV - OVUV * OVUV - TVOV * TVOV) / (-2 * OVUV * TVOV)));
  //Use law of cosines to determine angles.
  float ThetaY = (float)(Math.Acos((TVRV * TVRV - OVRV * OVRV - TVOV * TVOV) / (-2 * OVRV * TVOV)));

  float RPitch = (float)(90 - (ThetaP * 180 / Math.PI));//Convert from radians to degrees.
  float RYaw = (float)(90 - (ThetaY * 180 / Math.PI));

  if (TVOV < TVFV) RPitch = 180 - RPitch;//Normalize angles to -180 to 180 degrees.
  if (RPitch > 180) RPitch = -1 * (360 - RPitch);

  if (TVOV < TVFV) RYaw = 180 - RYaw;
  if (RYaw > 180) RYaw = -1 * (360 - RYaw);

  Pitch = RPitch;//Set Pitch and Yaw outputs.
  Yaw = RYaw;
}