#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include <MPU6050.h>
#include "imu.h"

MPU6050 mpu;
bool dmpReady = false;
uint8_t fifoBuffer[64];

Quaternion q;
VectorFloat gravity;
GravityVector lastGravity = {0, 0, 0};

// ===== 初始化 =====
void imu_setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.setClock(400000);
  Wire.setTimeOut(5);

  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 连接失败");
    while (1);
  }

  uint8_t devStatus = mpu.dmpInitialize();
  if (devStatus == 0) {
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.setDMPEnabled(true);
    dmpReady = true;
    Serial.println("DMP 就绪");
  } else {
    Serial.print("DMP 初始化失败，错误码: ");
    Serial.println(devStatus);
  }
}

// ===== DMP 读取，返回单位重力向量在 XYZ 轴上的投影 =====

GravityVector dmp() {
  if (!dmpReady) return lastGravity;

  const uint16_t packetSize = mpu.dmpGetFIFOPacketSize();
  if (packetSize == 0 || packetSize > sizeof(fifoBuffer)) {
    return lastGravity;
  }

  uint16_t fifoCount = mpu.getFIFOCount();
  if (fifoCount >= 1024) {
    mpu.resetFIFO();
    return lastGravity;
  }

  if (fifoCount >= packetSize) {
    if (fifoCount > packetSize * 5U) {
      mpu.resetFIFO();
      return lastGravity;
    }

    while (fifoCount >= packetSize) {
      mpu.getFIFOBytes(fifoBuffer, static_cast<uint8_t>(packetSize));
      fifoCount -= packetSize;
    }

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    lastGravity.x = gravity.x;
    lastGravity.y = gravity.y;
    lastGravity.z = gravity.z;
  }
  return lastGravity;
}

// ===== 原始数据读取，返回 RawData =====
RawData raw() {
  RawData r;
  mpu.getMotion6(&r.ax, &r.ay, &r.az, &r.gx, &r.gy, &r.gz);

  // ±2g 量程下 1g = 16384 LSB
  // r.ax_g = r.ax / 16384.0f;
  // r.ay_g = r.ay / 16384.0f;
  // r.az_g = r.az / 16384.0f;

  // // ±250°/s 量程下 1°/s = 131 LSB
  // r.gx_dps = r.gx / 131.0f;
  // r.gy_dps = r.gy / 131.0f;
  // r.gz_dps = r.gz / 131.0f;
  // Serial.print("Accel(g): X="); Serial.print(r.ax_g, 3);
  // Serial.print(" Y=");          Serial.print(r.ay_g, 3);
  // Serial.print(" Z=");          Serial.print(r.az_g, 3);
  // Serial.print(" | Gyro(°/s): X="); Serial.print(r.gx_dps, 2);
  // Serial.print(" Y=");              Serial.print(r.gy_dps, 2);
  // Serial.print(" Z=");              Serial.println(r.gz_dps, 2);

  return r;
}