enum State {
  Initial,
  ZRetracted,
  YRetracted,
  XRetracted,
  DrillDown,
  Retract,
  Advance,
  DrillMove,
  Shutdown,
  Done
};

State state = State.Initial;
const float delta = 2.0f;
const float drillSpeed = 0.3f;
float x = 0.0f, y = 0.0f;

public Program() {
  Runtime.UpdateFrequency = UpdateFrequency.Update10;
}

public IMyPistonBase Piston(String name) {
  return GridTerminalSystem.GetBlockWithName("Piston " + name) as IMyPistonBase;
}

public void Main() {
  var x1 = Piston("X1"); var x2 = Piston("X2");
  var y1 = Piston("Y1"); var y2 = Piston("Y2");
  var z1 = Piston("Z1"); var z2 = Piston("Z2");
  var drill = GridTerminalSystem.GetBlockWithName("Drill") as IMyShipDrill;
  switch (state) {
    case State.Initial: {
      drill.Enabled = false;
      int progress = (int) Math.Round(
          100 * (z1.CurrentPosition + z2.CurrentPosition) / 20.0f);
      Echo("Waiting for Z to retract (" + progress.ToString() + "%)");
      z1.MinLimit = z2.MinLimit = 0.0f;
      z1.MaxLimit = z2.MaxLimit = 10.0f;
      z1.Velocity = z2.Velocity = -1.0f;
      if (z1.CurrentPosition == 0.0f && z2.CurrentPosition == 0.0f)
        state = State.ZRetracted;
      break;
    }
    case State.ZRetracted: {
      int progress = (int) Math.Round(
          100 * (y1.CurrentPosition + y2.CurrentPosition) / 20.0f);
      Echo("Waiting for Y to retract (" + progress.ToString() + "%)");
      y1.MinLimit = y2.MinLimit = 0.0f;
      y1.MaxLimit = y2.MaxLimit = 10.0f;
      y1.Velocity = y2.Velocity = -0.5f;
      if (y1.CurrentPosition == 0.0f && y2.CurrentPosition == 0.0f)
        state = State.YRetracted;
      break;
    }
    case State.YRetracted: {
      int progress = (int) Math.Round(
          100 * (x1.CurrentPosition + x2.CurrentPosition) / 20.0f);
      Echo("Waiting for X to retract (" + progress.ToString() + "%)");
      x1.MinLimit = x2.MinLimit = 0.0f;
      x1.MaxLimit = x2.MaxLimit = 10.0f;
      x1.Velocity = x2.Velocity = -1.0f;
      if (x1.CurrentPosition == 0.0f && x2.CurrentPosition == 0.0f)
        state = State.XRetracted;
      break;
    }
    case State.XRetracted: {
      Echo("Initialized. Starting drill...");
      x1.Velocity = x2.Velocity = 0.0f;
      y1.Velocity = y2.Velocity = 0.0f;
      z1.Velocity = z2.Velocity = 0.0f;
      x = 0.0f;
      y = 0.0f;
      state = State.DrillDown;
      break;
    }
    case State.DrillDown: {
      int progress = (int) Math.Round(
          100 * (z1.CurrentPosition + z2.CurrentPosition) / 20.0f);
      Echo("Drilling (" + progress.ToString() + "%)");
      drill.Enabled = true;
      z1.Velocity = z2.Velocity = 0.5f * drillSpeed;
      if (z1.CurrentPosition == 10.0f && z2.CurrentPosition == 10.0f)
        state = State.Retract;
      break;
    }
    case State.Retract: {
      int progress = (int) Math.Round(
          100 * (y1.CurrentPosition + y2.CurrentPosition) / 20.0f);
      Echo("Retracting (" + progress.ToString() + "%)");
      drill.Enabled = false;
      z1.Velocity = z2.Velocity = -0.5f;
      if (z1.CurrentPosition == 0.0f && z2.CurrentPosition == 0.0f)
        state = State.Advance;
      break;
    }
    case State.Advance: {
      Echo("Advancing drill head...");
      if (x == 20.0f && y == 20.0f) {
        state = State.Shutdown;
        break;
      } else if (x == 20.0f) {
        x = 0.0f;
        y = Math.Min(y + delta, 20.0f);
      } else {
        x = Math.Min(x + delta, 20.0f);
      }
      state = State.DrillMove;
      break;
    }
    case State.DrillMove: {
      Echo("Moving drill head...");
      x1.Velocity = x2.Velocity = 0.5f;
      y1.Velocity = y2.Velocity = 0.5f;
      x1.MinLimit = x2.MinLimit = 0.5f * x;
      x1.MaxLimit = x2.MaxLimit = 0.5f * x;
      y1.MinLimit = y2.MinLimit = 0.5f * y;
      y1.MaxLimit = y2.MaxLimit = 0.5f * y;
      float currentX = x1.CurrentPosition + x2.CurrentPosition;
      float currentY = y1.CurrentPosition + y2.CurrentPosition;
      if (Math.Abs(x - currentX) < 0.01f && Math.Abs(y - currentY) < 0.01f)
        state = State.DrillDown;
      break;
    }
    case State.Shutdown: {
      Echo("Shutting down...");
      x1.Velocity = x2.Velocity = 0.0f;
      y1.Velocity = y2.Velocity = 0.0f;
      z1.Velocity = z2.Velocity = 0.0f;
      drill.Enabled = false;
      state = State.Done;
      break;
    }
    case State.Done: {
      Echo("Done");
      break;
    }
  }
}