#include "operational_state.h"
#include "motor_controller.h"

#include <Arduino.h>


void OperationalState::enter()
{
    Serial.println("State: Operational");

    context->led_on();
}

// Part 1 

//void OperationalState::update()
//{
//    context->run_speed_controller();
//}

// Part 2

void OperationalState::update()
{
    if (context->fault_active())
    {
        Serial.println("Fault detected");

        context->to_stopped();

        return;
    }

    context->run_speed_controller();
}


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

        context->to_initialization();
    }
}


void OperationalState::exit()
{
    context->stop_motor();

    context->led_off();
}