#include "ugv_io_mpu.hh"

#include <driver/uart.h>
#include <esp_log.h>

#include "MPU.hpp"
#include "mpu/math.hpp"

namespace ugv {
namespace io {

static constexpr mpud::accel_fs_t MPU_ACCEL_FS    = mpud::ACCEL_FS_2G;
static constexpr mpud::gyro_fs_t  MPU_GYRO_FS     = mpud::GYRO_FS_500DPS;
static constexpr float            MPU_MAG_TO_FLUX = (4912.f) / (32760.f);

static const char *TAG = "ugv_io_mpu";

MPU::MPU() { mpu_ = new mpud::MPU(); }

MPU::~MPU() { delete mpu_; }

void MPU::Init() {
  esp_err_t ret;
  ret = mpu_->testConnection();
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "MPU not connected");
    return;
  }
  ret = mpu_->initialize();
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "MPU initialization error");
    return;
  }
  mpu_->setAccelFullScale(MPU_ACCEL_FS);
  mpu_->setGyroFullScale(MPU_GYRO_FS);
  ESP_LOGI(TAG, "MPU initialized");
}

void MPU::GetData(MpuData &data) {
  esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_);
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "error reading MPU");
  }
  data.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
  data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
  data.mag.x     = ((float)mag_.x) * MPU_MAG_TO_FLUX;
  data.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
  data.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
}

};  // namespace io
};  // namespace ugv