Rotating Piston Drill Controller

public string pistonKeyword = "drill";


public float rotationVelocity = 0.5f,
             angleVariance = 0.25f,
             pistonExtensionSpeed = 0.5f, rotorAngleStepDistance = 0.5f,
             nextRotorAngle = 0f, currentDistance = 0f, pistonExtensionDistance = 0.5f,
             cargoCapacityLimit = 0.9f, autoRange = 0f,
             maxPistonDistance = 0f, distanceVariance = 0.12f,
             returnSpeed = 0.5f, pistonMaximum = 10f, currentAngle = 0f;

List<IMyPistonBase>
    downPistons = new List<IMyPistonBase>(),
    upPistons = new List<IMyPistonBase>();

List<IMyShipDrill> drills = new List<IMyShipDrill>();

List<IMyCargoContainer> containers = new List<IMyCargoContainer>();

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 IMyTextSurface mySurface;

public int outTicks = 10;

public Program()
{
    Runtime.UpdateFrequency = UpdateFrequency.Update1;
    if (Me.CustomData == "") SaveData();
    else LoadData();
    Scan();
    try
    {
        IMyTextSurfaceProvider provider = (IMyTextSurfaceProvider)Me;
        if (provider.SurfaceCount > 0)
        {
            mySurface = provider.GetSurface(0);
            mySurface.ContentType = ContentType.TEXT_AND_IMAGE;
        }
    }
    catch { }
    ResetPistons();
}

public void Main(string argument, UpdateType updateSource)
{
    outCapacity = false;
    if (Me.CustomData != settingBackup) LoadData();
    if (argument != "") try { Commands(argument.ToLower()); } catch { }
    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 + upPistons.Count));
        Echo("Camera: " + (cameraBlock != null).ToString());
        Echo("Connector: " + (connector != null).ToString());
        Echo($"Extension: {ExtensionRemaining():N1}m");
        SetDrills(false);
        SetRotor(0f);
        ResetPistons();
        currentDistance = 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();
    OutStatus();
}

public void Scan()
{
    //Variables
    List<IMyPistonBase> pistonList = new List<IMyPistonBase>();
    //Get base piston
    GridTerminalSystem.GetBlocksOfType<IMyPistonBase>(pistonList, b => b.CubeGrid == Me.CubeGrid && b.CustomName.ToLower().Contains(pistonKeyword));
    if (pistonList.Count == 0) return;
    //Reset variables
    maxPistonDistance = 0f;
    upPistons.Clear();
    downPistons.Clear();
    drills.Clear();
    containers.Clear();
    cameraBlock = null;
    connector = null;
    savedRotor = null;
    //Set base piston
    upPistons.Add(pistonList[0]);
    maxPistonDistance += upPistons[0].HighestPosition - (upPistons[0].HighestPosition - upPistons[0].MaxLimit);
    //Set pistons by up/down
    IMyPistonBase currentPiston = upPistons[0];
    pistonList.Clear();
    GridTerminalSystem.GetBlocksOfType<IMyPistonBase>(pistonList, b => b.CubeGrid == currentPiston.TopGrid);
    while (pistonList.Count > 0)
    {
        if (SameDirection(upPistons[0], pistonList[0]))
        {
            upPistons.Add(pistonList[0]);
            maxPistonDistance += upPistons[upPistons.Count - 1].HighestPosition - (upPistons[upPistons.Count - 1].HighestPosition - upPistons[upPistons.Count - 1].MaxLimit);
        }
        else if (OppositeDirection(upPistons[0], pistonList[0]))
        {
            downPistons.Add(pistonList[0]);
            maxPistonDistance += downPistons[downPistons.Count - 1].HighestPosition - downPistons[downPistons.Count - 1].MinLimit;
        }
        currentPiston = pistonList[0];

        pistonList.Clear();
        GridTerminalSystem.GetBlocksOfType<IMyPistonBase>(pistonList, b => b.CubeGrid == currentPiston.TopGrid);
    }
    //Get rotor
    List<IMyMotorAdvancedStator> rotors = new List<IMyMotorAdvancedStator>();
    GridTerminalSystem.GetBlocksOfType<IMyMotorAdvancedStator>(rotors, b => b.CubeGrid == currentPiston.TopGrid);
    if (rotors.Count == 0) return;
    //Set rotor
    savedRotor = new SavedRotor();
    savedRotor.rotor = rotors[0];
    savedRotor.angleVariance = angleVariance;
    savedRotor.desiredAngle = savedRotor.rotor.Angle * 180f / (float)Math.PI;
    nextRotorAngle = savedRotor.desiredAngle;
    savedRotor.rotationVelocity = rotationVelocity;
    savedRotor.rotor.TargetVelocityRPM = 0f;
    //Try to get connector
    List<IMyShipConnector> connectorList = new List<IMyShipConnector>();
    if (connector == null)
    {
        GridTerminalSystem.GetBlocksOfType<IMyShipConnector>(connectorList, b => b.CubeGrid == savedRotor.rotor.TopGrid || b.CubeGrid == currentPiston.TopGrid);
        if (connectorList.Count > 0) connector = connectorList[0];
    }
    //Get drills
    GridTerminalSystem.GetBlocksOfType<IMyShipDrill>(drills, b => b.CubeGrid == savedRotor.rotor.TopGrid);
    //Get camera
    List<IMyCameraBlock> cameraList = new List<IMyCameraBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyCameraBlock>(cameraList, b => b.CubeGrid == savedRotor.rotor.TopGrid);
    if (cameraList.Count > 0) cameraBlock = cameraList[0];
    //Get containers
    GridTerminalSystem.GetBlocksOfType<IMyCargoContainer>(containers, b => b.CubeGrid == Me.CubeGrid);
}

public void ResetPistons()
{
    SetPistonVelocity(downPistons, -returnSpeed);
    SetPistonMax(downPistons, 0f);
    SetPistonVelocity(upPistons, returnSpeed);
    SetPistonMin(upPistons, -1f);
}

public bool SameDirection(IMyTerminalBlock originBlock, IMyTerminalBlock block)
{
    if (Vector3D.Distance(originBlock.WorldMatrix.Up, block.WorldMatrix.Up) <= distanceVariance) return true;
    return false;
}

public bool OppositeDirection(IMyTerminalBlock originBlock, IMyTerminalBlock block)
{
    if (Vector3D.Distance(originBlock.WorldMatrix.Up, block.WorldMatrix.Down) <= distanceVariance) return true;
    return false;
}

public void OutStatus()
{
    if (mySurface != null)
    {
        outTicks++;
        if (outTicks >= 10)
        {
            outTicks = 0;

            StringBuilder builder = new StringBuilder();
            if (drilling) builder.Append("Drilling");
            if (retracting) builder.AppendLine($"{(drilling ? "Drilling" : retracting ? "Retracting" : "Idle")}{(auto ? " : Auto" : "")}");
            if (drilling)
            {
                builder.Append(($"Progress: {Math.Round(Progress() * 100.0, 2)}%").PadRight(20));
                builder.AppendLine(($"Current Angle: {(Math.Round(currentAngle, 2)).ToString().PadLeft(6)}").PadLeft(20));
                builder.Append(($"Depth: {Math.Round(currentDistance, 1)}m").PadRight(20));
                builder.AppendLine(($"Next Angle: {(Math.Round(nextRotorAngle, 2)).ToString().PadLeft(6)}").PadLeft(20));
                builder.AppendLine($"Extension: {ExtensionRemaining():N1}m");
            }
            if (drilling || auto)
            {
                builder.AppendLine($"Fill Level: {Math.Round(FillLevel() * 100f, 2)}%");
            }
            builder.AppendLine($"Extending Pistons: {downPistons.Count}");
            builder.AppendLine($"Retracting Pistons: {upPistons.Count}");
            builder.AppendLine($"Drills: {drills.Count}");
            builder.AppendLine($"Rotor: {savedRotor != null && savedRotor.rotor != null}");
            builder.AppendLine($"Camera: {cameraBlock != null}");
            builder.AppendLine($"Connector: {connector != null}");

            mySurface.WriteText(builder.ToString());
        }
    }
}

public void Commands(string arg)
{
    if (arg.StartsWith("set "))
    {
        try
        {
            float distance = float.Parse(arg.Replace(" ", "").Substring(3));
            if (distance > 0f)
            {
                StepDistance(distance - currentDistance);
            }
        }
        catch { }
    }
    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;
            case "measure":
                AutoRange();
                break;
        }
}

static string[] SplitLines(string data)
{
    return data.Split(new[] { "\r\n", "\r", "\n" }, StringSplitOptions.RemoveEmptyEntries);
}

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

public void LoadData()
{
    string[] settingArray = SplitLines(Me.CustomData);
    for (int i = 0; i < settingArray.Length; i++)
        if (settingArray[i].Contains("=")) ProcessSetting(settingArray[i].TrimEnd(';'));
    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 "auto":
            auto = boolSetting;
            break;
    }
}

public void AutoRange()
{
    if (connector != null) connector.Enabled = false;
    double scanDistance = maxPistonDistance;
    currentDistance = 0f;
    if (!scanningRange)
    {
        cameraBlock.EnableRaycast = true;
        scanningRange = true;
    }
    if (cameraBlock.CanScan(scanDistance))
    {
        MyDetectedEntityInfo info = cameraBlock.Raycast(scanDistance, 0f, 0f);
        if (info.HitPosition.HasValue)
        {
            Progress();
            float hitDistance = (float)Vector3D.Distance(cameraBlock.GetPosition(), info.HitPosition.Value) - 7.25f + currentDistance;
            if (info.Type == MyDetectedEntityType.Planet || info.Type == MyDetectedEntityType.Asteroid)
            {
                scanningRange = false;
                SetPistonVelocity(downPistons, pistonExtensionSpeed);
                SetPistonVelocity(upPistons, -pistonExtensionSpeed);
                SetPistonMax(downPistons, 0f);
                SetPistonMin(upPistons, -1f);
                StepDistance(hitDistance);
                drilling = true;
                retracting = false;
            }
        }
    }
}

public void DrillScript()
{
    if (FillLevel() < cargoCapacityLimit)
    {
        SetDrills(true);
        if (RotorPlaced() && ExtensionRemaining() < 1.5) 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(downPistons) || !FullyRetracted(upPistons))
                StepDistance(pistonExtensionDistance);
            else
            {
                drilling = false;
                retracting = true;
            }
        }
        Echo($"Progress: {Math.Round(Progress() * 100.0, 2)}% 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;
    }
    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()
{
    currentAngle = savedRotor.CurrentAngle();
    Echo("Current Angle: " + currentAngle);
    Echo("Next Angle: " + nextRotorAngle);
    Echo($"Current Depth: {currentDistance}m");
    return Math.Abs((savedRotor.desiredAngle % 360f) - (currentAngle % 360f)) <= angleVariance;
}

public void StepDistance(float distance)
{
    SetPistonVelocity(downPistons, pistonExtensionSpeed);
    SetPistonVelocity(upPistons, -pistonExtensionSpeed);

    float remainingDistance = distance;

    for (int i = 0; i < downPistons.Count && remainingDistance > 0f; i++)
    {
        IMyPistonBase piston = (IMyPistonBase)downPistons[i];
        if (piston.MaxLimit < piston.HighestPosition)
        {
            if (piston.MaxLimit + remainingDistance <= piston.HighestPosition)
            {
                piston.MaxLimit = piston.MaxLimit + remainingDistance;
                remainingDistance = 0f;
            }
            else
            {
                remainingDistance -= piston.HighestPosition - piston.MaxLimit;
                piston.MaxLimit = piston.HighestPosition;
            }
        }
    }

    for (int i = 0; i < upPistons.Count && remainingDistance > 0f; i++)
    {
        IMyPistonBase piston = (IMyPistonBase)upPistons[i];
        if (piston.MinLimit > 0f)
        {
            if (piston.MinLimit - remainingDistance >= 0f)
            {
                piston.MinLimit = piston.MinLimit - remainingDistance;
                remainingDistance = 0f;
            }
            else
            {
                remainingDistance -= piston.MinLimit;
                piston.MinLimit = 0f;
            }
        }
    }
}

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

public double ExtensionRemaining()
{
    double remainder = 0;

    foreach (IMyPistonBase piston in upPistons)
        remainder += Math.Max(0, piston.CurrentPosition - piston.MinLimit);
    foreach (IMyPistonBase piston in downPistons)
        remainder += Math.Max(0, piston.MaxLimit - piston.CurrentPosition);

    return remainder;
}

public double Progress()
{
    double max = maxPistonDistance, curr = 0;
    for (int i = 0; i < upPistons.Count; i++)
        curr += upPistons[i].HighestPosition - (upPistons[i].CurrentPosition + (upPistons[i].HighestPosition - upPistons[i].MaxLimit));
    for (int i = 0; i < downPistons.Count; i++)
        curr += downPistons[i].CurrentPosition - downPistons[i].MinLimit;
    currentDistance = (float)curr;
    return curr / max;
}

public void SetPistonMax(List<IMyPistonBase> pistons, float distance)
{
    for (int i = 0; i < pistons.Count; i++)
        pistons[i].MaxLimit = distance == -1f || distance > pistons[i].MaxLimit ? pistons[i].HighestPosition : distance;
}

public void SetPistonMin(List<IMyPistonBase> pistons, float distance)
{
    for (int i = 0; i < pistons.Count; i++)
        pistons[i].MinLimit = distance == -1f || distance > pistons[i].HighestPosition ? pistons[i].HighestPosition : distance;
}

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

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

public bool FullyExtended(List<IMyPistonBase> pistons)
{
    for (int i = 0; i < pistons.Count; i++)
    {
        IMyPistonBase pistonBase = pistons[i];
        if (pistonBase.CurrentPosition < pistonBase.HighestPosition)
            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 IMyMotorAdvancedStator 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 (rotor.Angle * 180f / (float)Math.PI) % 360f;
    }

    public void CheckRotor()
    {
        float currentAngle = 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) rotor.TargetVelocityRPM = rotationVelocity;
            else rotor.TargetVelocityRPM = -rotationVelocity;
        }
        else rotor.TargetVelocityRPM = 0f;
    }
}