enum ProjectConfig {
    POWER_HEAD    = A4,
    POWER_FEET    = A0,
    RELAY_HEAD    = 4,
    RELAY_FEET    = 5,
    BTN_HEAD_UP   = 6,
    BTN_HEAD_DOWN = 7,
    BTN_FEET_UP   = 8,
    BTN_FEET_DOWN = 9,
    LED_PIN       = 13,
    MinPressTime  = 34,
    MaxMotorTime  = 45000,
    MinRelayTime  = 1
};

enum MotorDirection { NoMotors, HeadDown, HeadUp, FeetDown, FeetUp };
enum FaultState { NoFault, MotorFault, RelayFault, ButtonFault };

FaultState currentFault = NoFault;

// Function prototypes
void setPinState(int pin, int state);
bool checkButton(int const pin);
void engageMotor(MotorDirection const whichMotor, bool (* const continueFunc)());
void initializePins();
void ledHeartbeat(MotorDirection const state = NoMotors);
void failClosed();

void setup() {
    initializePins();
}

void loop() {
    if (currentFault != NoFault) {
        failClosed();
        return;
    }
    
    ledHeartbeat();
    
    if (checkButton(BTN_HEAD_DOWN)) {
        engageMotor(HeadDown, [](){ return checkButton(BTN_HEAD_DOWN); });
    }
    else if (checkButton(BTN_HEAD_UP)) {
        engageMotor(HeadUp, [](){ return checkButton(BTN_HEAD_UP); });
    }
    else if (checkButton(BTN_FEET_DOWN)) {
        engageMotor(FeetDown, [](){ return checkButton(BTN_FEET_DOWN); });
    }
    else if (checkButton(BTN_FEET_UP)) {
        engageMotor(FeetUp, [](){ return checkButton(BTN_FEET_UP); });
    }
}

void failClosed() {
    setPinState(POWER_HEAD, LOW);
    setPinState(POWER_FEET, LOW);
    setPinState(RELAY_HEAD, LOW);
    setPinState(RELAY_FEET, LOW);
    ledHeartbeat(NoMotors);
}

void setPinState(int pin, int state) {
    digitalWrite(pin, state);
}

void initializePins() {
    pinMode(LED_PIN, OUTPUT);
    setPinState(LED_PIN, LOW);

    pinMode(POWER_HEAD, OUTPUT);
    setPinState(POWER_HEAD, LOW);

    pinMode(POWER_FEET, OUTPUT);
    setPinState(POWER_FEET, LOW);

    pinMode(RELAY_HEAD, OUTPUT);
    setPinState(RELAY_HEAD, LOW);

    pinMode(RELAY_FEET, OUTPUT);
    setPinState(RELAY_FEET, LOW);

    pinMode(BTN_HEAD_UP, INPUT_PULLUP);
    pinMode(BTN_HEAD_DOWN, INPUT_PULLUP);
    pinMode(BTN_FEET_UP, INPUT_PULLUP);
    pinMode(BTN_FEET_DOWN, INPUT_PULLUP);
}

bool checkButton(int const pin) {
    if (digitalRead(pin)) return false;

    unsigned long const start = millis();
    while (millis() - start < MinPressTime) {
        if (digitalRead(pin)) {
            currentFault = ButtonFault;
            return false;
        }
        ledHeartbeat();
    }
    return true;
}

void ledHeartbeat(MotorDirection const state /* = NoMotors */) {
    int ledState = (state == NoMotors) ? (millis() % 1500 >= 1350) : (millis() % 300 >= 150);
    setPinState(LED_PIN, ledState);
}

void engageMotor(MotorDirection const whichMotor, bool (* const continueFunc)()) {
    setPinState(POWER_HEAD, LOW);
    setPinState(POWER_FEET, LOW);
    setPinState(RELAY_HEAD, LOW);
    setPinState(RELAY_FEET, LOW);

    switch (whichMotor) {
        case HeadDown:
            setPinState(RELAY_HEAD, LOW);
            delay(MinRelayTime);
            setPinState(POWER_HEAD, HIGH);
            break;
        case HeadUp:
            setPinState(RELAY_HEAD, HIGH);
            delay(MinRelayTime);
            setPinState(POWER_HEAD, HIGH);
            break;
        case FeetDown:
            setPinState(RELAY_FEET, LOW);
            delay(MinRelayTime);
            setPinState(POWER_FEET, HIGH);
            break;
        case FeetUp:
            setPinState(RELAY_FEET, HIGH);
            delay(MinRelayTime);
            setPinState(POWER_FEET, HIGH);
            break;
        default:
            currentFault = MotorFault;
            return;
    }

    unsigned long const start = millis();
    while (continueFunc()) {
        if (millis() - start >= MaxMotorTime) {
            currentFault = MotorFault;
            break;
        }
        ledHeartbeat(whichMotor);
    }
}