MinumelArmMCU/StateMachine.cpp

#include "StateMachine.hpp"

constexpr uint8_t ERROR_INDICATOR_LED_PIN = 3;

StateMachine::StateMachine() noexcept
{
    pinMode(ERROR_INDICATOR_LED_PIN, OUTPUT);
    digitalWrite(ERROR_INDICATOR_LED_PIN, LOW);
}

StateMachine& StateMachine::getInstance() noexcept
{
    static StateMachine instance;
    return instance;
}

bool StateMachine::setState(const State newState) noexcept
{
    if (canSwitchToState(newState)) {
        state = newState;
        return true;
    }

    return false;
}

bool StateMachine::canSwitchToState(const State newState) const noexcept
{
    // Error state is final
    if (state == State::ERROR) {
        return false;
    }

    // Error state only settable via raiseError()
    if (newState == State::ERROR) {
        return false;
    }

    // Allow same-to-same transition
    if (state == newState) {
        return true;
    }

    switch (state) {
        case StateMachine::State::INITIALIZING:
            switch (newState) {
                // Can go to idle
                case State::IDLING:
                    return true;
                // Can't go straight to moving
                case State::MOVING:
                    return false;

                default:
                    return false;
            }
        break;
        case StateMachine::State::IDLING:
            switch (newState) {
                // Can't initialize again
                case State::INITIALIZING:
                    return false;
                // Can enter move state
                case State::MOVING:
                    return true;

                default:
                    return false;
            }
        break;
        case StateMachine::State::MOVING:
            switch (newState) {
                // Can't initialize again
                case State::INITIALIZING:
                    return false;
                // Can go into idling
                case State::IDLING:
                    return true;

                default:
                    return false;
            }
        break;
    }
    return false;
}

bool StateMachine::raiseError(const String &reason) noexcept
{
    state = State::ERROR;
    lastError = reason;
    digitalWrite(ERROR_INDICATOR_LED_PIN, HIGH);
    if (Serial) {
        Serial.println(String("Error!: ") + reason);
    }
}