static IMyGridTerminalSystem MyGTS;
static List<IMyGyro> Gyros = new List<IMyGyro>();
static IMyCockpit MyCockpit;
static Vector3D G;
static double g;
static double maxrpm = 60;
static double tickspersecond = 6.0;
static PID PitchControl;
static double gyrobias = 0;
static List<IMyTextSurface> CockpitScreens = new List<IMyTextSurface>();
static List<IMyCockpit> Cockpits = new List<IMyCockpit>();
Matrix CockpitOr;

public Program()
{
MyGTS = GridTerminalSystem;
MyGTS.GetBlocksOfType(Gyros,block => block.CubeGrid==Me.CubeGrid);
MyGTS.GetBlocksOfType(Cockpits,block => block.CubeGrid==Me.CubeGrid);
MyCockpit = Cockpits[0];
// Rotation matrix relative to grid
MyCockpit.Orientation.GetMatrix(out CockpitOr);

CockpitScreens.Add( MyCockpit.GetSurface(0));
CockpitScreens.Add(MyCockpit.GetSurface(1));

// Configure the gyros for override/manual control at recompile
foreach (IMyGyro gyro in Gyros){
if (!gyro.CustomData.Contains("leave"))gyro.GyroOverride = true;
else gyro.GyroOverride=false;
}
// Get new pitch conroller
PitchControl = new PID(50,0.0,0);

// Update every 10 ticks
Runtime.UpdateFrequency = UpdateFrequency.Update10;
}

public void Main(string argument, UpdateType updateSource)
{

// Get the gravity vector and convert to unit quantity
G = MyCockpit.GetNaturalGravity();
G.Normalize();

// Get the component of gravity acting in the forward direction of the cockpit
g = G.Dot(MyCockpit.WorldMatrix.Forward);

// if the forward direction gravity is 0, then the vehicle is level in pitch. regulate the gravity to 0 = regulate pitch to 0
double output = PitchControl.GetSignal(-g,tickspersecond);
// if the forward gravity component is negative (pitching up) then make sure the output rotation is positve (pitching down)
if (g >0)output = -Math.Abs(output);
// if the forward gravity component is positive (pitching down) then make sure the output rotation is negative (pitching up)
else output = Math.Abs(output);

// bound the commanded angular velocity magnitude to 60 RPM
output = Math.Max(output,-maxrpm*2*Math.PI/60);
output = Math.Min(output,maxrpm*2*Math.PI/60);
// if there is a bias add it in here (radians/sec)
output+=gyrobias;

// create the bodyfixed angular rotation vector
Vector3D bodyrot = new Vector3D(0.0,output,0.0);

// make sure the magnitude is stored
double bodymag = bodyrot.Length();

// scale the bodyrotation vector to unit
bodyrot.Normalize();

// decompose commanded body angular velocity into each gyroscope
foreach(IMyGyro gyro in Gyros) {

Echo(gyro.DisplayNameText);

// get the rotation of the gyroscope
Matrix gyroor;
gyro.Orientation.GetMatrix(out gyroor);

// for some reason, I don't know why, the gyroscope orientation is presented as PitchRollYaw Euler Transformation...
Vector3 XYZ;
Matrix.GetEulerAnglesXYZ(ref gyroor, out XYZ);
String test = string.Format("TF {0:0.00} {1:0.00} {2:0.00}", XYZ.X, XYZ.Y, XYZ.Z);
Echo(test);

// Get the commanded rotation relative to the gyroscope's local axes
Vector3D localCmd = Vector3D.Transform(bodyrot,gyroor);

// Scale the command vector to radian/s
localCmd = Vector3D.Multiply(localCmd,bodymag*2*Math.PI/60.0);

gyro.Yaw = (float)(localCmd.GetDim(0));
gyro.Pitch = (float)(localCmd.GetDim(1));
gyro.Roll = (float)(localCmd.GetDim(2));

string OutputStr = string.Format("CMD {0:0.00} {1:0.00} {2:0.00}\n",localCmd.GetDim(0),localCmd.GetDim(1),localCmd.GetDim(2));
Echo(OutputStr);
}

}

class PID
    {
    public double Kp {get;set;}
    public double Ki {get;set;}
    public double Kd {get;set;}
    public double Signal {get;set;}
    double Iscale = 1;
    double ValuePrev;
    double Integral;
    public PID(double kp=1,double ki=1,double kd=1)
    {
    Kp=kp;
    Ki=ki;
    Kd=kd;
    }
    public void Reset()
    {
    ValuePrev=0;
    Integral=0;
    Signal=0;
    }
    public double GetSignal(double value,double dt=1.0/60.0)
    {
    if(Ki!=0)
    Integral += value*Iscale;
    Signal=Kp*value+Ki*Integral*dt+Kd*(value-ValuePrev)/dt;
    ValuePrev=value;
    return Signal;
    }
    }