double forwardSpeed = 0;
double horizontalSpeed = 0;
double verticalSpeed = 0;

Vector3D position = new Vector3D(0,0,0);
Vector3D oldPosition = new Vector3D(0,0,0);
long oldTick=0;
double frameTime = 0;
double simSpeed = 1d;

bool isSetup = false;

float keyThrustForward = 0;
float keyThrustRight = 0;
float keyThrustUp = 0;

float dampForward = 0;
float dampRight = 0;
float dampUp = 0;

List<IMyGravityGenerator> gravsForward = new List<IMyGravityGenerator>();
List<IMyGravityGenerator> gravsBackward = new List<IMyGravityGenerator>();
List<IMyGravityGenerator> gravsRight = new List<IMyGravityGenerator>();
List<IMyGravityGenerator> gravsLeft = new List<IMyGravityGenerator>();
List<IMyGravityGenerator> gravsUp = new List<IMyGravityGenerator>();
List<IMyGravityGenerator> gravsDown = new List<IMyGravityGenerator>();

bool isInput = false;
bool isMoving = false;

IMyShipController controller = null;


public void Main(string argument)
{

    if(!isSetup)
    {
        Setup();
    }

    InitializeVectors();
    ReadInputs();

    isInput = (keyThrustForward+keyThrustRight+keyThrustUp != 0);
    isMoving = (Math.Abs(forwardSpeed+horizontalSpeed+verticalSpeed) > 0.1f);

    if(isMoving || isInput)
    {
        GetDamps();
        AdjustGravs();
    }

    SwitchGravs();

}



private void ReadInputs()
{
    keyThrustForward = -controller.MoveIndicator.Z * 9.81f;
    keyThrustRight = controller.MoveIndicator.X * 9.81f;
    keyThrustUp = controller.MoveIndicator.Y * 9.81f;
}


private void GetDamps()
{
    dampForward = 0;
    dampRight = 0;
    dampUp = 0;

    if(keyThrustForward == 0)
    {
        dampForward = (float)forwardSpeed/(float)simSpeed * 0.5f;
    }
    if(keyThrustRight == 0)
    {
        dampRight = (float)horizontalSpeed/(float)simSpeed * 0.5f;
    }
    if(keyThrustUp == 0)
    {
        dampUp = (float)verticalSpeed/(float)simSpeed * 0.5f;
    }
}


private void AdjustGravs()
{
    for (int i=0; i<gravsForward.Count; ++i)
    {
        gravsForward[i].GravityAcceleration = -dampForward + keyThrustForward;
    }
    for (int i=0; i<gravsBackward.Count; ++i)
    {
        gravsBackward[i].GravityAcceleration = dampForward - keyThrustForward;
    }
    for (int i=0; i<gravsRight.Count; ++i)
    {
        gravsRight[i].GravityAcceleration = -dampRight + keyThrustRight;
    }
    for (int i=0; i<gravsLeft.Count; ++i)
    {
        gravsLeft[i].GravityAcceleration = dampRight - keyThrustRight;
    }
    for (int i=0; i<gravsUp.Count; ++i)
    {
        gravsUp[i].GravityAcceleration = -dampUp + keyThrustUp;
    }
    for (int i=0; i<gravsDown.Count; ++i)
    {
        gravsDown[i].GravityAcceleration = dampUp - keyThrustUp;
    }
}


private void SwitchGravs()
{
    List<IMyTerminalBlock> list = new List<IMyTerminalBlock>();

    if(isInput || isMoving)
    {
        GridTerminalSystem.GetBlocksOfType<IMyVirtualMass>(list);
        for (int i=0; i<list.Count; ++i)
        if(list[i].CubeGrid == Me.CubeGrid)
        {
            list[i].ApplyAction("OnOff_On");
        }
        GridTerminalSystem.GetBlocksOfType<IMyGravityGenerator>(list);
        for (int i=0; i<list.Count; ++i)
        {
            list[i].ApplyAction("OnOff_On");
        }
    }
    else
    {
        GridTerminalSystem.GetBlocksOfType<IMyVirtualMass>(list);
        for (int i=0; i<list.Count; ++i)
        if(list[i].CubeGrid == Me.CubeGrid)
        {
            list[i].ApplyAction("OnOff_Off");
        }
        GridTerminalSystem.GetBlocksOfType<IMyGravityGenerator>(list);
        for (int i=0; i<list.Count; ++i)
        {
            list[i].ApplyAction("OnOff_Off");
        }
    }
}


private void Setup()
{
    isSetup = true;
    controller = MyController();

    gravsForward.Clear();
    gravsBackward.Clear();
    gravsRight.Clear();
    gravsLeft.Clear();
    gravsUp.Clear();
    gravsDown.Clear();

    List<IMyTerminalBlock> list = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyGravityGenerator>(list);
    for (int i=0; i<list.Count; ++i)
    {
        IMyGravityGenerator gravgen = list[i] as IMyGravityGenerator;
        gravgen.FieldSize = new Vector3(30,30,30);
        Vector3D gravDirection = gravgen.WorldMatrix.Down;
        if(gravDirection == controller.WorldMatrix.Forward)
        {
            gravsForward.Add(gravgen);
        }
        if(gravDirection == controller.WorldMatrix.Backward)
        {
            gravsBackward.Add(gravgen);
        }
        if(gravDirection == controller.WorldMatrix.Right)
        {
            gravsRight.Add(gravgen);
        }
        if(gravDirection == controller.WorldMatrix.Left)
        {
            gravsLeft.Add(gravgen);
        }
        if(gravDirection == controller.WorldMatrix.Up)
        {
            gravsUp.Add(gravgen);
        }
        if(gravDirection == controller.WorldMatrix.Down)
        {
            gravsDown.Add(gravgen);
        }
    }
}


private IMyShipController MyController()
{
    List<IMyTerminalBlock> list = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyShipController>(list);
    for(int i=0; i<list.Count; i++)
    {
        if(list[i].CubeGrid == Me.CubeGrid && !list[i].CustomName.ToLower().Contains("cryo") && !list[i].CustomName.ToLower().Contains("passenger"))
        {
            IMyShipController controller = list[i] as IMyShipController;
            return controller;
        }
    }
    return null;
}


private void InitializeVectors()
{
    position = controller.GetPosition();
    long tick = DateTime.Now.Ticks /10000;
    frameTime = frameTime*0.9d + Math.Max(1,(double)(tick-oldTick)) * 0.1d;
    simSpeed = (1000d/60d) / frameTime;
    oldTick = tick;
    forwardSpeed = (position-oldPosition).Dot(controller.WorldMatrix.Forward) / frameTime * 1021d;
    horizontalSpeed = (position-oldPosition).Dot(controller.WorldMatrix.Right) / frameTime * 1021d;
    verticalSpeed = (position-oldPosition).Dot(controller.WorldMatrix.Up) / frameTime * 1021d;
    oldPosition = position;
}