#ifndef COMPLIB_SERVER_ENCODERS_HPP
#define COMPLIB_SERVER_ENCODERS_HPP

#include <array>
#include <cstdint>
#include "include/Cache.hpp"
#include "include/Robot.hpp"

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

  Encoders(Encoders const &) = delete;

  void operator=(Encoders const &) = delete;

  std::array<int32_t, ROBOT_MOTOR_COUNT> get_positions();

  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rad_s();

  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_ticks_s();

private:
  Encoders() = default;

  Cache position_cache{ROBOT_ENCODER_RATE_HZ};
  Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
  Cache ticks_cache{ROBOT_ENCODER_RATE_HZ};

  std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rad_s = {0};
  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_ticks_s = {0};
};


#endif //COMPLIB_SERVER_ENCODERS_HPP