#ifndef MOTOR_CONTROLLER_H
#define MOTOR_CONTROLLER_H

#include "rotary_encoder_driver.h"
#include "motor_driver.h"
#include "p_controller.h"
#include "digital_out.h"

class State;


class MotorController
{
private:
    static constexpr double DEFAULT_REFERENCE_RPM = 40.0;
    static constexpr double PWM_PER_PERCENT = 255.0 / 100.0;
    static const uint8_t LED_PIN = 5;   // PB5 = D13 = LED_BUILTIN

    State* current_state;

    State& initialization_state;
    State& pre_operational_state;
    State& operational_state;
    State& stopped_state;

    RotaryEncoderDriver& encoder;
    MotorDriver& motor;
    P_controller* controller;

    Digital_out led;

    double reference_rpm;

    // Síðasta sýni, notað fyrir CSV-útprentun
    double last_actual_rpm;
    double last_pwm;


public:
    MotorController(
        RotaryEncoderDriver& encoder,
        MotorDriver& motor,
        P_controller& controller,
        State& initialization,
        State& pre_operational,
        State& operational,
        State& stopped
    );


    void begin();

    void update();

    void command(char c);

    void transition_to(State& new_state);


    void to_initialization();

    void to_pre_operational();

    void to_operational();

    void to_stopped();

    bool fault_active();


    // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna
    bool run_speed_controller();

    double get_reference_rpm() const { return reference_rpm; }
    double get_actual_rpm() const { return last_actual_rpm; }
    double get_pwm() const { return last_pwm; }

    void stop_motor();

    void reset_speed_measurement();

    void reset_controller();

    void set_Kp(double value);

    void set_Ti(double value);

    void set_controller(P_controller& new_controller);

    void led_on();

    void led_off();
};

#endif