#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;

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

  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 读取，返回 YPR（角度制）=====
YPR dmp() {
  YPR out = {0, 0, 0};
  if (!dmpReady) return out;

  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    float ypr[3];
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    out.yaw   = ypr[0] * 180.0f / M_PI;
    out.pitch = ypr[1] * 180.0f / M_PI;
    out.roll  = ypr[2] * 180.0f / M_PI;
  }
  // Serial.print("Yaw: ");   Serial.print(a.yaw);
  // Serial.print(" Pitch: "); Serial.print(a.pitch);
  // Serial.print(" Roll: ");  Serial.println(a.roll);
  return out;
}

// ===== 原始数据读取，返回 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;
}