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

MotorController::MotorController(
    RotaryEncoderDriver &encoder,
    MotorDriver &motor,
    P_controller &p_controller,
    P_controller &pi_controller,
    State &initialization,
    State &pre_operational,
    State &operational,
    State &stopped
)
    : current_state(nullptr),
      initialization_state(initialization),
      pre_operational_state(pre_operational),
      operational_state(operational),
      stopped_state(stopped),
      encoder(encoder),
      motor(motor),
      p_controller(p_controller),
      pi_controller(pi_controller),
      controller(&pi_controller),   // PI er sjálfgefin stýring
      led(LED_PIN),
      target_rpm(DEFAULT_TARGET_RPM),
      reference_rpm(DEFAULT_TARGET_RPM),
      profile_acceleration(0),
      profile_deceleration(0),
      last_actual_rpm(0.0),
      last_pwm(0.0)
{
    initialization_state.set_context(this);
    pre_operational_state.set_context(this);
    operational_state.set_context(this);
    stopped_state.set_context(this);
}


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->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_pre_operational()
{
    transition_to(pre_operational_state);
}


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


bool MotorController::run_speed_controller()
{
    if (!encoder.new_speed_available())
    {
        return false;
    }

    reference_rpm = target_rpm;

    const double actual = encoder.rpm();

    const double pwm_percent =
        controller->update(reference_rpm, actual);

    double pwm =
        pwm_percent * PWM_PER_PERCENT;

    // Klippt hér svo last_pwm sé nákvæmlega það sem fer á mótorinn
    if (pwm > 255.0)
    {
        pwm = 255.0;
    }
    else if (pwm < -255.0)
    {
        pwm = -255.0;
    }

    motor.set(pwm);

    last_actual_rpm = actual;
    last_pwm = pwm;

    return true;
}


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


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

void MotorController::reset_controller()
{
    controller->reset();
}

void MotorController::reset_parameters()
{
    p_controller.restore_defaults();
    pi_controller.restore_defaults();

    select_controller(ControllerType::PI);
    set_target_rpm(DEFAULT_TARGET_RPM);

    set_profile_acceleration(0);
    set_profile_deceleration(0);

}

void MotorController::set_Kp(double value)
{
    controller->set_Kp(value);
}

bool MotorController::set_Ti(double value)
{
    return controller->set_Ti(value);
}

void MotorController::select_controller(ControllerType type)
{
    if (type == ControllerType::PI) 
    {
        controller = &pi_controller;
    }
    else
    {
        controller = &p_controller;
    }

    controller->reset();
}

ControllerType MotorController::get_controller_type() const
{
    if (controller == &pi_controller)
    {
        return ControllerType::PI; 
    }
    else
    {
        return ControllerType::P;
    }
}

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

NodeState MotorController::get_state() const
{
    if (current_state == nullptr)
    {
        return NodeState::INITIALIZATION;
    }
    else{
        return current_state->id();
    }
}

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

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


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