#include "StateMachine.hpp"
#include "UartHandler.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);
    UartHandler& uart = UartHandler::getInstance();
    if (uart.isOpen()) {
        uart.send(String("Error!: ") + reason);
    }
}