#ifndef COMPLIB_SERVER_MOTOR_HPP
#define COMPLIB_SERVER_MOTOR_HPP

#include <cstdint>
#include <vector>
#include <chrono>
#include <thread>

#include "include/robot.hpp"

class Motor {
public:
    enum Mode : uint8_t {
        COAST = 0,
        FORWARD = 1,
        BACKWARD = 2,
        BREAK = 3,
        SERVO = 4,
        NONE = 5
    };

    enum Control : uint8_t {
        POWER = 0,
        SPEED = 1
    };

    static Motor& get_instance()
    {
        static Motor instance;
        return instance;
    }

    Motor(Motor const&) = delete;
    void operator=(Motor const&) = delete;

    void set_speed(uint8_t port, double rpm);
    void set_power(uint8_t port, double percent);
    static void set_pwm(uint8_t port, uint16_t pwm, Mode mode);

    std::vector<double> get_speed();

private:
    Motor();

    // Speed calculation and speed filter
    int32_t last_encoder_values[MOTOR_COUNT] = {0};
    double filtered_speeds[MOTOR_COUNT] = {0};
    std::chrono::system_clock::time_point last_time_encoders_read;

    // Speed control
    double speed_targets[MOTOR_COUNT] = {0};
    double motor_control_modes[MOTOR_COUNT] = {0};
    std::thread speed_control_thread;

    void reset_speed();
    void speed_control_loop();
};

#endif // COMPLIB_SERVER_MOTOR_HPP