using MathHelper

// Change the names here to whatever your pistons and rotor are named
// Or change the piston and rotor names to whatever the name below is
const string HorizontalPistonName = "Piston Horizontal";
const string VerticalPistonName = "Piston Vertical";
const string RotorName = "Rotor";
const string DrillName = "Drill";

float HorizontalTarget = 0.0f;
float VerticalTarget = -2.0f; // Band-aid
double timeElapsed = 0;
bool FirstRun = true;
int sign = 1;

void Main (){

    IMyPistonBase HorizontalPiston = GridTerminalSystem.GetBlockWithName(HorizontalPistonName);
    IMyPistonBase VerticalPiston = GridTerminalSystem.GetBlockWithName(VerticalPistonName);
    IMyMotorStator Rotor = GridTerminalSystem.GetBlockWithName(RotorName);
    IMyShipDrill Drill = GridTerminalSystem.GetBlockWithName(DrillName);
    IMyInventory DrillInventory = Drill.GetInventory();

    // Pause if inventory is full
    if (DrillInventory.IsFull){
        Drill.Enabled = false;
        return;
    }
    else{
        Drill.Enabled = true;
    }

    timeElapsed += Runtime.TimeSinceLastRun.TotalSeconds;

    if (FirstRun) {
        init(HorizontalPiston, VerticalPiston, Rotor);
        FirstRun = false;
    }

    // One full revolution
    if (timeElapsed > (60 / Rotor.TargetVelocityRPM) + 2){
        if (HorizontalTarget == 10){
            sign = -1;
            VerticalTarget += 2;
        }
        if (HorizontalTarget == 0){
            if (VerticalTarget == 10){
                Drill.Enabled = false;
                MovePistonTo(HorizontalPiston, 0);
                MovePistonTo(VerticalPiston, 0);
                Rotor.TargetVelocityRPM = 0.0f;
                Me.Enabled = false;
            }
            sign = 1;
            VerticalTarget += 2;
        }
        HorizontalTarget += sign*2;
        MovePistonTo(HorizontalPiston, HorizontalTarget);
        MovePistonTo(VerticalPiston, VerticalTarget);
        timeElapsed = 0;
    }
}

void init (IMyPistonBase H, IMyPistonBase V, IMyMotorStator R){
    H.MaxLimit = 0.0f;
    H.MinLimit = 0.0f;
    H.Velocity = 0.5f;

    V.MaxLimit = 0.0f;
    V.MinLimit = 0.0f;
    V.Velocity = 0.5f;

    R.Torque = 1000.0f;
    R.BrakingTorque = 1000.0f;
    R.TargetVelocityRPM = 0.25f;

    return;
}

void MovePistonTo (IMyPistonBase Piston, float Target){
    // Set the target
    MathHelper.Clamp(Target, 0.0f, 10.0f);
    Piston.MaxLimit = Target;
    Piston.MinLimit = Target;
    // Set the direction acording to the target
    if ((Piston.CurrentPostition < Target && Piston.Velocity < 0) || (Piston.CurrentPostition > Target && Piston.Velocity > 0))
        Piston.Reverse();
}