Rotating Piston Drill Controller

public float rotationVelocity = 0.5f,
angleVariance = 0.25f,
pistonExtensionSpeed = 0.5f, rotorAngleStepDistance = 0.5f,
nextRotorAngle = 0f, nextPistonDistance = 0f, pistonExtensionDistance = 0.5f,
cargoCapacityLimit = 0.9f, autoRange = 0f,
maxPistonDistance = 0f;

public string
rotorKeyword = "drill", pistonKeyword = "drill",
drillKeyword = "drill", cameraKeyword = "drill",
connectorKeyword = "drill";

public List<IMyTerminalBlock>
downPistons = new List<IMyTerminalBlock>(),
drills = new List<IMyTerminalBlock>(),
containers = new List<IMyTerminalBlock>();

public bool drilling = false, retracting = true, stepping = false,
centeredRotor = true, drillingRotor = false, auto = false,
scanningRange = false, outCapacity = false;

public SavedRotor savedRotor;

public IMyCameraBlock cameraBlock;

public IMyShipConnector connector;

public string settingBackup = "";

public TimeSpan lockableSpan = new TimeSpan(0, 0, 0);

public Program()
{
    Runtime.UpdateFrequency = UpdateFrequency.Update1;
    if (Me.CustomData == "") SaveData();
    else LoadData();
    Scan();
    SetPistonVelocity(downPistons, -0.5f);
    SetPistonDistance(downPistons, 0f);
    Save();
}

public void Scan()
{
    //Get down pistons
    GridTerminalSystem.GetBlocksOfType<IMyPistonBase>(downPistons, b => b.CustomName.ToLower().Contains(pistonKeyword.ToLower()));
    for (int i = 0; i < downPistons.Count; i++)
        maxPistonDistance += ((IMyPistonBase)downPistons[i]).HighestPosition;
    Echo("'" + pistonKeyword + "' Pistons: " + downPistons.Count);
    //Get rotor
    List<IMyTerminalBlock> rotors = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyMotorAdvancedStator>(rotors, b => rotorKeyword == "" || b.CustomName.ToLower().Contains(rotorKeyword.ToLower()));
    Echo("'" + rotorKeyword + "' Rotors Found: " + rotors.Count);
    if (rotors.Count > 0) {
        SavedRotor rotor = new SavedRotor();
        rotor.rotor = rotors[0];
        rotor.angleVariance = angleVariance;
        rotor.desiredAngle = ((IMyMotorAdvancedStator)rotors[0]).Angle * 180f / (float)Math.PI;
        nextRotorAngle = rotor.desiredAngle;
        rotor.rotationVelocity = rotationVelocity;
        ((IMyMotorAdvancedStator)rotors[0]).TargetVelocityRPM = 0f;
        savedRotor = rotor;
        Echo("Added Rotor: " + rotors[0].CustomName);
    } else throw new Exception("\nNo Rotor Found With Keyword '" + rotorKeyword + "'");
    //Get drills
    GridTerminalSystem.GetBlocksOfType<IMyShipDrill>(drills, d => d.CustomName.ToLower().Contains(drillKeyword.ToLower()));
    Echo("'" + drillKeyword + "' Drills: " + drills.Count);
    GridTerminalSystem.GetBlocksOfType<IMyCargoContainer>(containers, c => c.CubeGrid == Me.CubeGrid || c.CustomData.ToLower().Contains("drill"));
    Echo("Containers: " + containers.Count);
    //Get camera
    List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyCameraBlock>(blocks, b => b.CustomName.ToLower().Contains(cameraKeyword.ToLower()));
    if (blocks.Count > 0) {
        cameraBlock = (IMyCameraBlock)blocks[0];
        Echo("Camera Found");
    }
    //Get connector
    GridTerminalSystem.GetBlocksOfType<IMyShipConnector>(blocks, b => b.CustomName.ToLower().Contains(connectorKeyword.ToLower()));
    if (blocks.Count > 0) {
        connector = (IMyShipConnector)blocks[0];
        Echo("Connector Found");
    }
}

public void Save()
{

}

public void Main(string argument, UpdateType updateSource)
{
    outCapacity = false;
    if (Me.CustomData != settingBackup) LoadData();
    if (argument != "") try { Commands(argument.ToLower()); } catch { }
    Purge();
    if (scanningRange) AutoRange();
    if (drilling) {
        Echo("Drilling");
        if (autoRange != 0f) Echo("Auto Range: " + autoRange.ToString("N1"));
        DrillScript();
    }
    else if (retracting) {
        Echo("Inactive");
        Echo("Drills: " + drills.Count);
        Echo("Pistons: " + downPistons.Count);
        Echo("Camera: " + (cameraBlock != null).ToString());
        Echo("Connector: " + (connector != null).ToString());
        SetDrills(false);
        SetRotor(0f);
        SetPistonVelocity(downPistons, -0.5f);
        SetPistonDistance(downPistons, 0f);
        nextPistonDistance = 0f;
        if (auto && FillLevel() < cargoCapacityLimit) {
            retracting = false;
            drilling = true;
            AutoRange();
        }
        if (!drilling && connector != null)
        {
            connector.Enabled = true;
            if (connector.Status == MyShipConnectorStatus.Connectable)
            {
                if (lockableSpan.TotalSeconds >= 5.0) connector.Connect();
                else lockableSpan += Runtime.TimeSinceLastRun;
            } else lockableSpan = new TimeSpan(0, 0, 0);
        }
    }
    CheckRotors();
}

public void Commands(string arg)
{
    if (arg.StartsWith("set ")) {
        float distance = float.Parse(arg.Substring(4));
        nextPistonDistance = distance / (float)downPistons.Count;
        SetPistonDistance(downPistons, nextPistonDistance);
    }
    else switch (arg)
    {
        case "drill":
            drilling = !drilling;
            retracting = !drilling;
            if (drilling) {
                SetPistonVelocity(downPistons, pistonExtensionSpeed);
                if (connector != null) connector.Enabled = false;
            } else if (connector != null) connector.Enabled = true;
        break;
        case "scan":
            Scan();
        break;
        case "auto":
            auto = !auto;
            SaveData();
            if (auto && cameraBlock != null) AutoRange();
            else
            {
                drilling = false;
                retracting = true;
                if (connector != null) connector.Enabled = true;
            }
        break;
    }
}

public void SaveData()
{
    StringBuilder builder = new StringBuilder();
    builder.AppendLine("pistonExtensionSpeed=" + pistonExtensionSpeed + ";");
    builder.AppendLine("pistonExtensionDistance=" + pistonExtensionDistance + ";");
    builder.AppendLine("pistonKeyword=" + pistonKeyword + ";");
    builder.AppendLine("connectorKeyword=" + connectorKeyword + ";");
    builder.AppendLine("rotorAngleStepDistance=" + rotorAngleStepDistance + ";");
    builder.AppendLine("rotationVelocity=" + rotationVelocity + ";");
    builder.AppendLine("angleVariance=" + angleVariance + ";");
    builder.AppendLine("rotorKeyword=" + rotorKeyword + ";");
    builder.AppendLine("drillKeyword=" + drillKeyword + ";");
    builder.AppendLine("cameraKeyword=" + cameraKeyword + ";");
    builder.Append("auto=" + auto + ";");
    Me.CustomData = builder.ToString();
    settingBackup = Me.CustomData;
}

public void LoadData()
{
    string settingString = Me.CustomData;
    settingString = settingString.Replace("\r\n", String.Empty);
    settingString = settingString.Replace("\n", String.Empty);
    settingString = settingString.Replace("\r", String.Empty);
    settingString = settingString.Replace("\t", String.Empty);
    string[] settingArray = settingString.Split(new char[] { ';' }, StringSplitOptions.RemoveEmptyEntries);
    for (int i = 0; i < settingArray.Length; i++)
        if (settingArray[i].Contains("=")) ProcessSetting(settingArray[i]);
    settingBackup = Me.CustomData;
}

public void ProcessSetting(string setting)
{
    bool boolSetting = false;
    float floatSetting = 0f;
    int settingIndex = setting.IndexOf("=");
    string stringSetting = setting.Substring(settingIndex + 1), settingKey = setting.Substring(0, settingIndex);
    boolSetting = stringSetting.ToLower().Contains("true");
    try { floatSetting = float.Parse(stringSetting); } catch { }
    switch (settingKey)
    {
        case "pistonExtensionSpeed":
            pistonExtensionSpeed = floatSetting;
        break;
        case "pistonExtensionDistance":
            pistonExtensionDistance = floatSetting;
        break;
        case "pistonKeyword":
            pistonKeyword = stringSetting;
        break;
        case "rotorAngleStepDistance":
            rotorAngleStepDistance = floatSetting;
        break;
        case "rotationVelocity":
            rotationVelocity = floatSetting;
        break;
        case "angleVariance":
            angleVariance = floatSetting;
        break;
        case "rotorKeyword":
            rotorKeyword = stringSetting;
        break;
        case "drillKeyword":
            drillKeyword = stringSetting;
        break;
        case "connectorKeyword":
            connectorKeyword = stringSetting;
        break;
        case "cameraKeyword":
            cameraKeyword = stringSetting;
        break;
        case "auto":
            auto = boolSetting;
        break;
    }
}

public void AutoRange()
{
    if (connector != null) connector.Enabled = false;
    double scanDistance = 0, currentDistance = (double)PistonDistance(downPistons);
    for (int i = 0; i < downPistons.Count; i++)
    {
        scanDistance += (double)((IMyPistonBase)downPistons[i]).HighestPosition - (double)((IMyPistonBase)downPistons[i]).CurrentPosition;
    }
    if (!scanningRange) {
        cameraBlock.EnableRaycast = true;
        scanningRange = true;
    }
    if (cameraBlock.CanScan(scanDistance))
    {
        MyDetectedEntityInfo info = cameraBlock.Raycast(scanDistance, 0f, 0f);
        if (info.HitPosition.HasValue)
        {
            float hitDistance = (float)currentDistance + (float)Vector3D.Distance(cameraBlock.GetPosition(), info.HitPosition.Value) - 7f;
            if (info.Type == MyDetectedEntityType.Planet || info.Type == MyDetectedEntityType.Asteroid)
            {
                scanningRange = false;
                nextPistonDistance = hitDistance / (float)downPistons.Count;
                SetPistonVelocity(downPistons, pistonExtensionSpeed);
                SetPistonDistance(downPistons, nextPistonDistance);
                drilling = true;
                retracting = false;
            }
        }
    }
}

public void Purge()
{
    PurgeList(downPistons);
    PurgeList(containers);
    PurgeList(drills);
    if (cameraBlock != null && !Verify((IMyTerminalBlock)cameraBlock)) cameraBlock = null;
}

public void PurgeList(List<IMyTerminalBlock> blocks)
{
    for (int i = 0; i < blocks.Count; i += 0)
    {
        if (!Verify(blocks[i])) blocks.RemoveAt(i);
        else i++;
    }
}

public bool Verify(IMyTerminalBlock block)
{
    return block.CubeGrid.CubeExists(block.Position);
}

public void DrillScript()
{
    if (FillLevel() < cargoCapacityLimit) {
        SetDrills(true);
        if (RotorPlaced()) StepRotor();
        SetRotor(nextRotorAngle);
        bool isCentered = IsCentered();
        if (centeredRotor && !isCentered) {
            centeredRotor = false;
            drillingRotor = true;
        }
        else if (!centeredRotor && isCentered) centeredRotor = true;
        if (drillingRotor && centeredRotor)
        {
            drillingRotor = false;
            if (!FullyExtended())
                StepPiston();
            else
            {
                drilling = false;
                retracting = true;
            }
        }
        Echo((((nextPistonDistance * (float)downPistons.Count) / maxPistonDistance) * 100f).ToString("N2") + "% Done");
    } else SetDrills(false);
}

public float FillLevel()
{
    float currLevel = 0f, maxLevel = 0f;
    for (int i = 0; i < containers.Count; i++)
    {
        IMyInventory inv = containers[i].GetInventory(0);
        currLevel += (float)inv.CurrentVolume;
        maxLevel += (float)inv.MaxVolume;
    }
    if (!outCapacity && maxLevel != 0f)
    {
        outCapacity = true;
        string capacityString = "";
        if (currLevel == 0f) capacityString = "0 / ";
        else capacityString = ((currLevel / maxLevel) * 100f).ToString("N2") + " / ";
        capacityString += (cargoCapacityLimit * 100f).ToString("N2") + "% Filled";
        Echo(capacityString);
    }
    return currLevel / maxLevel;
}

public bool IsCentered()
{
    float angle = savedRotor.CurrentAngle();
    return 360f - angle <= angleVariance || angle <= angleVariance;
}

public void SetDrills(bool enabled)
{
    for (int i = 0; i < drills.Count; i++)
        ((IMyFunctionalBlock)drills[i]).Enabled = enabled;
}

public bool RotorPlaced()
{
    float angle = savedRotor.CurrentAngle();
    Echo("Current Angle: " + angle);
    Echo("Next Angle: " + nextRotorAngle);
    Echo("Current Depth: " + (nextPistonDistance * (float)downPistons.Count));
    return Math.Abs(savedRotor.desiredAngle - angle) <= angleVariance;
}

public void StepPiston()
{
    nextPistonDistance += pistonExtensionDistance / (float)downPistons.Count;
    SetPistonDistance(downPistons, nextPistonDistance);
}

public void StepRotor()
{
    nextRotorAngle += rotorAngleStepDistance;
    if (nextRotorAngle >= 360f) nextRotorAngle -= 360f;
}

public float PistonDistance(List<IMyTerminalBlock> pistons)
{
    float dist = 0f;
    for (int i = 0; i < pistons.Count; i++)
        dist += ((IMyPistonBase)pistons[i]).CurrentPosition;
    return dist;
}

public void SetPistonDistance(List<IMyTerminalBlock> pistons, float distance)
{
    for (int i = 0; i < pistons.Count; i++)
    {
        if (distance < ((IMyPistonBase)pistons[i]).HighestPosition)
            ((IMyPistonBase)pistons[i]).MaxLimit = distance;
    }
}

public void SetPistonVelocity(List<IMyTerminalBlock> pistons, float velocity)
{
    for (int i = 0; i < pistons.Count; i++)
        ((IMyPistonBase)pistons[i]).Velocity = velocity;
}

public bool FullyRetracted(List<IMyTerminalBlock> pistons)
{
    for (int i = 0; i < pistons.Count; i++) {
        IMyPistonBase pistonBase = (IMyPistonBase)pistons[i];
        if (pistonBase.CurrentPosition > pistonBase.MinLimit)
            return false;
    }
    return true;
}

public bool FullyExtended(List<IMyTerminalBlock> pistons)
{
    for (int i = 0; i < pistons.Count; i++) {
        IMyPistonBase pistonBase = (IMyPistonBase)pistons[i];
        if (pistonBase.CurrentPosition < pistonBase.MaxLimit)
            return false;
    }
    return true;
}

public bool Positive(float angleA, float angleB)
{
    if (angleA < angleB) {
        if (Math.Abs(angleA - angleB) < 180f)
            return true;
        else return false;
    } else {
        if (Math.Abs(angleA - angleB) < 180f)
            return false;
        else return true;
    }
}

public void CheckRotors()
{
    savedRotor.CheckRotor();
}

public void SetRotor(float angle)
{
    savedRotor.desiredAngle = angle;
    savedRotor.rotationVelocity = rotationVelocity;
    savedRotor.angleVariance = angleVariance;
}

public class SavedRotor
{
    public IMyTerminalBlock rotor;
    public float desiredAngle = 0f, angleVariance = 1f, rotationVelocity = 0f;

    public void IncrementRotor(float increment)
    {
        if (desiredAngle + increment > 360) desiredAngle = desiredAngle + increment - 360f;
        else if (desiredAngle + increment < 0) desiredAngle = 360 + desiredAngle + increment;
        else desiredAngle += increment;
    }

    public float CurrentAngle()
    {
        return ((IMyMotorAdvancedStator)rotor).Angle * 180f / (float)Math.PI;
    }

    public void CheckRotor()
    {
        float currentAngle = ((IMyMotorAdvancedStator)rotor).Angle * 180f / (float)Math.PI;
        if (Math.Abs(currentAngle - desiredAngle) > angleVariance)
        {
            float positiveDistance = 0f, negativeDistace = 0f;
            if (desiredAngle > currentAngle) {
                positiveDistance = desiredAngle - currentAngle;
                negativeDistace = currentAngle + 360f - desiredAngle;
            } else {
                negativeDistace = currentAngle - desiredAngle;
                positiveDistance = desiredAngle + 360f - currentAngle;
            }
            if (positiveDistance < negativeDistace || positiveDistance == negativeDistace) ((IMyMotorAdvancedStator)rotor).TargetVelocityRPM = rotationVelocity;
            else ((IMyMotorAdvancedStator)rotor).TargetVelocityRPM = -rotationVelocity;
        } else ((IMyMotorAdvancedStator)rotor).TargetVelocityRPM = 0f;
    }
}