#include "ugv_io_mpu.hh"

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

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

namespace ugv {
namespace io {

static constexpr gpio_num_t       MPU_SDA         = GPIO_NUM_5;
static constexpr gpio_num_t       MPU_SCL         = GPIO_NUM_4;
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";

Vec3f::Vec3f() : x(0), y(0), z(0) {}
Vec3f::Vec3f(float x, float y, float z) : x(x), y(y), z(z) {}
Vec3f::Vec3f(const mpud::float_axes_t &axes)
    : x(axes.x), y(axes.y), z(axes.z) {}

MPU::MPU() : mpu_(nullptr), mpu_data_() {}

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

void MPU::Init() {
  mutex_    = xSemaphoreCreateMutex();
  mpu_data_ = MpuData{};

  mpu_bus_ = &i2c0;
  // This is shared with the oled, so just use those pins
  xSemaphoreTake(i2c_mutex, portMAX_DELAY);
  mpu_bus_->begin(MPU_SDA, MPU_SCL);
  mpu_ = new mpud::MPU(*mpu_bus_);

  esp_err_t ret;
  ret = mpu_->testConnection();
  if (ret != ESP_OK) {
    uint8_t wai = mpu_->whoAmI();
    ESP_LOGE(TAG, "MPU not connected (whoAmI: %d)", wai);
    return;
  }
  ret = mpu_->compassTestConnection();
  if (ret != ESP_OK) {
    uint8_t wai = mpu_->compassWhoAmI();
    ESP_LOGW(TAG, "MPU compass not connected (whoAmI: %d)", wai);
  }
  ret = mpu_->initialize();
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "MPU initialization error");
    return;
  }
  mpu_->setAccelFullScale(MPU_ACCEL_FS);
  mpu_->setGyroFullScale(MPU_GYRO_FS);
  // force magnetometer into continuous mode
  mpu_->compassWriteByte(0x0A, 0x12);
  xSemaphoreGive(i2c_mutex);

  BaseType_t xRet =
      xTaskCreate(MPU::MPU_Task, "ugv_io_mpu", 2 * 1024, this, 3, &this->task_);
  if (xRet != pdTRUE) {
    ESP_LOGE(TAG, "error creating MPU task");
    return;
  }
  ESP_LOGI(TAG, "MPU initialized and task started");
}

void MPU::GetData(MpuData &data) {
  xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
  data = MpuData(mpu_data_);
  xSemaphoreGive(mutex_);
}

void MPU::DoTask() {
  esp_err_t ret;
  while (true) {
    vTaskDelay(pdMS_TO_TICKS(50));
    uint8_t mxh, mxl, myh, myl, mzh, mzl, n;
    xSemaphoreTake(i2c_mutex, portMAX_DELAY);
    ret = mpu_->motion(&accel_, &gyro_);
    mpu_->compassReadByte(0x03, &mxl);
    mpu_->compassReadByte(0x04, &mxh);
    mpu_->compassReadByte(0x05, &myl);
    mpu_->compassReadByte(0x06, &myh);
    mpu_->compassReadByte(0x07, &mzl);
    mpu_->compassReadByte(0x08, &mzh);
    mpu_->compassReadByte(0x09, &n);
    xSemaphoreGive(i2c_mutex);
    if (ret != ESP_OK) {
      ESP_LOGE(TAG, "error reading MPU");
      continue;
    }
    int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
    int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
    int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl);
    ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz);
    xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
    mpu_data_.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
    mpu_data_.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
    mpu_data_.mag.x     = ((float)mx) * MPU_MAG_TO_FLUX;
    mpu_data_.mag.y     = ((float)my) * MPU_MAG_TO_FLUX;
    mpu_data_.mag.z     = ((float)mz) * MPU_MAG_TO_FLUX;
    xSemaphoreGive(mutex_);
  }
  vTaskDelete(NULL);
}

void MPU::MPU_Task(void *arg) { ((MPU *)arg)->DoTask(); }

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