#include "include/Encoders.hpp"
#include "include/spi.hpp"
#include "include/mathUtils.hpp"
#include <spdlog/spdlog.h>

std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
  if (position_cache.is_expired()) {
    uint8_t data[ROBOT_MOTOR_COUNT * 4] = {0};
    Spi::getInstance().read_array(Spi::MOTOR_1_POS_B3, 4 * ROBOT_MOTOR_COUNT, data);
    for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
      cached_positions.at(i) = mathUtils::from_bytes<int32_t>(data + i * 4, 4);
    }
    position_cache.update();
  }
  return cached_positions;
}

std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rad_s() {
  if (velocity_cache.is_expired()) {
    uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
    Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
    for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
      auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
      auto velocity_rot_s = velocity_ticks_s / ROBOT_TICKS_PER_TURN;
      auto velocity_rad_s = velocity_rot_s * 2 * M_PI;

      auto change = velocity_rad_s - cached_velocities_rad_s.at(i);
      change = mathUtils::limit_max(change, -ROBOT_ENCODER_MAX_CHANGE_RAD_S, ROBOT_ENCODER_MAX_CHANGE_RAD_S);
      cached_velocities_rad_s.at(i) = cached_velocities_rad_s.at(i) + change;
    }
    velocity_cache.update();
  }
  return cached_velocities_rad_s;
}

std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ticks_s() {
  if (ticks_cache.is_expired()) {
    uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
    Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
    for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
      auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
      cached_velocities_ticks_s.at(i) = velocity_ticks_s;
    }
    ticks_cache.update();
  }
  return cached_velocities_ticks_s;
}
