#ifndef COMPLIB_SERVER_MOTOR_HPP
#define COMPLIB_SERVER_MOTOR_HPP

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

#include "include/robot.hpp"

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

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

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

    static void power(uint8_t port, double percent);
    static void pwm(uint8_t port, uint16_t pwm, Mode mode);

    std::vector<double> get_speed();
private:
    Motor();

    int32_t last_encoder_values[MOTOR_COUNT] = {0};
    double filtered_speeds[MOTOR_COUNT] = {0};
    std::chrono::system_clock::time_point last_time_read;
    
    void reset_speed();
};

#endif // COMPLIB_SERVER_MOTOR_HPP