// Script for controlling "mechanical" doors, aka custom doors using hinges.
//
// Future features:
// * Rotor and piston support.
// * "Multi-stage doors", e.g. actuate a hinge, then a piston.
//
// Currently this operates on hinges whose names start with "DoorX", where X is an arbitrary identifier.
// Usage:
//   Script argument should be a space-separated list of identifiers and either
//   `-open` (default behavior) or `-close`.
//   Failing to pass any identifiers will cause the action to happen to all doors on the grid.
//
// By default "open" is assumed to be 90 degrees and "closed" is assumed to be 0 degrees.
//   However, each hinge can have this behavior configured via custom data. A sample config
//   (with the default values):
//
//   OpenAngle=90
//   ClosedAngle=0
//   Velocity=5


MyCommandLine cli;
List<IEnumerator<bool>> jobs;
Map<string, Door> doors;

public class DoorHinge {
    public DoorHinge(Program p, IMyMotorStator hinge) {
        P = p;
        Hinge = hinge;
    }

    // For these two functions, IMyMotorStator.Angle reports radians, but
    //   IMyMotorStator.RotateToAngle() expects degrees...
    public void OpenDoorHinge() {
        foreach(IMyMotorStator hinge in Hinges) {
            Hinge.RotorLock = false;
            TargetAngle = DegToRad(OpenAngle);
            Hinge.RotateToAngle(MyRotationDirection.AUTO, OpenAngle, Velocity);
        }
    }

    public void CloseDoorHinge() {
        Hinge.RotorLock = false;
        TargetAngle = DegToRad(ClosedAngle);
        Hinge.RotateToAngle(MyRotationDirection.AUTO, ClosedAngle, Velocity);
    }

    // Process the hinge's movement.
    // It will return true when the panel has finished moving.
    public bool Actuate() {
        if (Math.Abs(Hinge.Angle - TargetAngle) < 0.001) {
            Hinge.RotorLock = true;
        }
        return Locked();
    }

    public bool Locked() {
        return Hinge.RotorLock;
    }

    private Program P; // for access to Echo, etc.
    private IMyMotorStator Hinge { get; set; }
    private float TargetAngle { get; set; }
    private float OpenAngle { get; set; } = 90F;
    private float ClosedAngle { get; set; } = 0F;
    private float Velocity { get; set; } = 5F;

    private void ParseConfig() {
        string[] lines = Hinge.CustomData.Split('\n');
        foreach (string line in lines) {
            string[] tokens = line.Split('=');
            if (tokens.Length != 2) continue;
            switch(tokens[0]) {
                case "OpenAngle":
                    OpenAngle = float.Parse(tokens[1]);
                    break;
                case "ClosedAngle":
                    ClosedAngle = float.Parse(tokens[1]);
                    break;
                case "Velocity":
                    Velocity = float.Parse(tokens[1]);
                    break;
            }
        }
    }

    // TODO: a utility class or function would be lovely...
    //   In general, the encapsulation feels a little screwy here.
    private float DegToRad(float degrees) {
        return degrees * ((float)Math.PI / 180F);
    }
}

public class Door {
    public Door(Program p) {
        P = p;
        Hinges = new List<DoorHinge>();
    }

    // Add a hinge to the door
    public void AddHinge(IMyMotorStator hinge) {
        Hinges.Add(P, new DoorHinge(hinge));
    }

    public void OpenDoor() {
        foreach (DoorHinge hinge in Hinges) {
            hinge.OpenDoorHinge();
        }
    }

    public void CloseDoor() {
        foreach (DoorHinge hinge in Hinges) {
            hinge.CloseDoorHinge();
        }
    }

    // Process the door's movement. This will return true when the door is done moving.
    public bool Actuate() {
        bool done = true;
        foreach (DoorHinge hinge in Hinges) {
            if (hinge.Actuate()) done = false;
        }
        return done;
    }

    public bool Locked() {
        foreach (DoorHinge hinge in Hinges) {
            if (!hinge.Locked()) return false;
        }
        return true;
    }

    private List<DoorHinge> hinges;
    private Program P;
}

public Program() {
    cli = new MyCommandLine();
    jobs = new List<IEnumerator<bool>>();

    doors = new Map<string, Door>();

    List<IMyMotorStator> allHinges = new List<IMyMotorStator>();
    GridTerminalSystem.GetBlocksOfType(allHinges);
    foreach(IMyMotorStator hinge in allHinges) {
        if (hinge.CustomName.StartsWith("Door")) {
            string doorName = hinge.CustomName;
            if (!hingeMap.ContainsKey(doorName)) {
                hingeMap[doorName] = new Door(this);
            }
            hingeMap[doorName].AddHinge(hinge);
        }
    }

    Echo($"Found {doors.Keys.Count} doors.");
}

public void Main(string argument, UpdateType updateSource) {
    if (updateSource & (UpdateType.Trigger | UpdateType.Terminal)) {
        // Create a new job
        cli.TryParse(argument);

        List<Door> doorsToControl = new List<Door>();
        foreach (string arg in argument) {
            if (!doors.ContainsKey($"Door{arg}")) {
                Echo($"Door with identifier {arg} not found. Skipping.");
                continue;
            }
            if (!doors[arg].Locked()) {
                Echo($"Door {arg} already moving. Skipping.");
                continue;
            }
            doorsToControl.Add(doors[arg]);
        }

        if (doorsToControl.IsEmpty) {
            Echo("No doors found. Not creating new job.");
        } else {
            Echo("Creating new job.");
            if (cli.Switch("close")) jobs.Add(CloseDoors(doorsToControl));
            else jobs.Add(OpenDoors(doorsToControl));
            Runtime.UpdateFrequency = UpdateFrequency.Update1;
        }
    }

    // Process running jobs
    for (int i=0; i < jobs.Count; i++) {
        if (!jobs[i].MoveNext()) {
            jobs[i].Dispose();
            jobs.Remove(jobs[i]);
            i--;
        }
    }

    if (jobs.IsEmpty) {
        Runtime.UpdateFrequency = UpdateFrequency.None;
    }
}

private IEnumerator<bool> OpenDoors(List<Door> doorsToControl) {
    Echo("Opening doors.");
    foreach (Door door in doors) {
        door.OpenDoor();
    }
    return ActuateDoors(doorsToCotrol);
}

private IEnumerator<bool> CloseDoors(List<Door> doorsToControl) {
    Echo("Closing doors.");
    foreach (Door door in doorsToControl) {
        panel.CloseDoor();
    }
    return ActuateDoors(doorsToControl);
}

private IEnumerator<bool> ActuateDoors(doorsToControl) {
    while (true) {
        Echo("Actuating doors.");
        bool done = true; // assume we've finished, then falsify it below
        foreach (Door door in doorsToControl) {
            if (!door.Actuate()) {
                done = false;
            }
        }
        if (done) yield break;
        yield return true;
    }
}