#include "motor_controller.h"
#include "state.h"

#include <Arduino.h>


MotorController::MotorController(
    RotaryEncoderDriver& encoder,
    MotorDriver& motor,
    //P_controller& controller,
    //PI_controller& controller;
    Controller& controller,
    State& initialization,
    State& operational,
    State& stopped
)
    : current_state(nullptr),
      initialization_state(initialization),
      operational_state(operational),
      stopped_state(stopped),
      encoder(encoder),
      motor(motor),
      controller(controller),
      led(LED_PIN),
      reference_rpm(DEFAULT_REFERENCE_RPM)
{
    initialization_state.set_context(this);
    operational_state.set_context(this);
    stopped_state.set_context(this);
}


// void MotorController::set_states(
//     State* initialization,
//     State* operational,
//     State* stopped
// )

    // initialization_state = initialization;
    // operational_state = operational;
    // stopped_state = stopped;



void MotorController::begin()
{
    led.init();
    led.set_lo();

    transition_to(initialization_state);
}


void MotorController::transition_to(State& new_state)
{
    if (current_state != nullptr)
    {
        current_state->exit();
    }

    current_state = &new_state;

    current_state->set_context(this);

    current_state->enter();
}


void MotorController::update()
{
    if (current_state != nullptr)
    {
        current_state->update();
    }
}


void MotorController::command(char c)
{
    if (current_state != nullptr)
    {
        current_state->command(c);
    }
}


void MotorController::to_initialization()
{
    transition_to(initialization_state);
}


void MotorController::to_operational()
{
    transition_to(operational_state);
}


void MotorController::run_speed_controller()
{
    if (encoder.new_speed_available())
    {
        double actual = encoder.rpm();

        double pwm_percent =
            controller.update(reference_rpm, actual);

        double pwm =
            pwm_percent * PWM_PER_PERCENT;

        motor.set(pwm);
    }
}


void MotorController::stop_motor()
{
    motor.stop();
}


void MotorController::reset_speed_measurement()
{
    encoder.reset_speed_measurement();
}

void MotorController::to_stopped()
{
    transition_to(stopped_state);
}

bool MotorController::fault_active()
{
    return motor.fault_active();
}

void MotorController::reset_controller()
{
    controller.reset();
}

void MotorController::led_on()
{
   led.set_hi();
}


void MotorController::led_off()
{
   led.set_lo();
}