#ifndef COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
#define COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP


#include <cstdint>
#include <array>
#include <thread>

#include "include/PID.hpp"
#include "include/Robot.hpp"

class ClosedLoopMotorController {
public:
  static ClosedLoopMotorController &getInstance() {
    static ClosedLoopMotorController instance;
    return instance;
  }

  ClosedLoopMotorController(ClosedLoopMotorController const &) = delete;

  void operator=(ClosedLoopMotorController const &) = delete;

  void set_power(uint8_t port, double power);

  void set_speed(uint8_t port, double speed_rad_s);

  void generate_step_response_data();

  void generate_tuned_step_response_data();

private:
  enum ControlMode : uint8_t {
    NONE = 0,
    POWER = 1,
    SPEED = 2
  };

  ClosedLoopMotorController();

  [[noreturn]] void speed_control_loop();

  std::array<PID, ROBOT_MOTOR_COUNT> pids;
  std::array<ControlMode, ROBOT_MOTOR_COUNT> control_modes{ControlMode::NONE};
  std::array<double, ROBOT_MOTOR_COUNT> speed_targets{0};

  std::thread speed_control_thread;
};


#endif //COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP