#ifndef COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
#define COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP


#include <thread>
#include <mutex>
#include <chrono>

#include "include/Odometry.hpp"

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

  OdometryController(OdometryController const &) = delete;

  void operator=(OdometryController const &) = delete;

  void enable();

  void disable();

  void reset();

  Odometry get();

  [[nodiscard]] bool is_enabled() const;

private:
  typedef std::chrono::steady_clock clock;

  OdometryController();

  std::thread odometry_thread;
  std::recursive_mutex odometry_mutex;

  [[noreturn]] void odometry_loop();

  Odometry current_odometry{};
  bool enabled = false;
  double last_position_left{0};
  double last_position_right{0};
  std::chrono::time_point<clock> last_run;

};


#endif //COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP