#ifndef MOTOR_CONTROLLER_H
#define MOTOR_CONTROLLER_H

#include <stdint.h>

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

class State;

class MotorController : public NodeApplication
{
private:
    static constexpr double DEFAULT_TARGET_RPM = 0.0;
    static constexpr double PWM_PER_PERCENT = 255.0 / 100.0;
    static constexpr uint8_t LED_PIN = Digital_out::STATUS_LED;

    State* current_state;

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

    RotaryEncoderDriver &encoder;
    MotorDriver &motor;

    P_controller &p_controller;
    P_controller &pi_controller;
    P_controller *controller;   // Bendir á p_controller eða pi_controller

    Digital_out led;

    double target_rpm;  // Það sem beðið er um (RPDO1)
    double reference_rpm;   // Það sem stýringin fær

    uint32_t profile_acceleration; // RPM/s
    uint32_t profile_deceleration;

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




public:
    MotorController(
        RotaryEncoderDriver &encoder,
        MotorDriver &motor,
        P_controller &p_controller,
        P_controller &pi_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() override;

    void to_pre_operational() override;

    void to_operational() override;

    void to_stopped() override;

    NodeState get_state() const override;

    bool fault_active();


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

    void set_target_rpm(double rpm) override { target_rpm = rpm; }
    double get_target_rpm() const override { return target_rpm; }
    double get_reference_rpm() const { return reference_rpm; }
    double get_actual_rpm() const override { return last_actual_rpm; }

    double get_pwm() const { return last_pwm; }
    double get_duty_percent() const override { return last_pwm / PWM_PER_PERCENT; }

    void set_profile_acceleration(uint32_t value) override {profile_acceleration = value; }
    uint32_t get_profile_acceleration() const override { return profile_acceleration; }
    void set_profile_deceleration(uint32_t value) override {profile_deceleration = value; }
    uint32_t get_profile_deceleration() const override { return profile_deceleration; }

    void stop_motor();

    void reset_speed_measurement();

    void reset_controller();
    void reset_parameters() override;

    void set_Kp(double value) override;
    double get_Kp() const override { return controller->get_Kp(); }

    bool set_Ti(double value) override;
    double get_Ti() const override { return controller->get_Ti(); }

    void select_controller(ControllerType type) override;
    ControllerType get_controller_type() const override;

    void led_on();

    void led_off();
};

#endif