#include "stopped_state.h"
#include "motor_controller.h"

#include <Arduino.h>


StoppedState::StoppedState()
    : previous_time(0),
      led_state(false)
{
}


void StoppedState::enter()
{
    Serial.println("State: Stopped");

    context->stop_motor();

    led_state = false;

    digitalWrite(LED_BUILTIN, LOW);

    previous_time = millis();
}

// Controlling the blinking of the LED @ 2Hz

void StoppedState::update()
{
    unsigned long current_time = millis();

    if (current_time - previous_time >= 250)
    {
        previous_time = current_time;

        led_state = !led_state;

        digitalWrite(
            LED_BUILTIN,
            led_state ? HIGH : LOW
        );
    }
}


// Commands for Stopped [ r = reset and o for operational]

void StoppedState::command(char c)
{
    if (c == 'r' || c == 'R')
    {
        Serial.println("Command: reset");

        context->to_initialization();
    }

    else if (c == 'o' || c == 'O')
    {
        Serial.println("Command: set operational");

        context->to_operational();
    }
}


void StoppedState::exit()
{
    digitalWrite(LED_BUILTIN, LOW);
}