#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <stdint.h>
#include <math.h>
#include <sys/time.h>

/************************ 宏定义 ************************/
#define I2C_DEVICE_PATH  "/dev/i2c-2"  // RK3588 I2C2设备节点
#define MPU6050_ADDR     0x68          // MPU6050从机地址（AD0=GND）
// MPU6050寄存器地址
#define PWR_MGMT_1       0x6B          // 电源管理寄存器1
#define SMPLRT_DIV       0x19          // 采样率分频寄存器
#define CONFIG           0x1A          // 配置寄存器
#define GYRO_CONFIG      0x1B          // 陀螺仪配置寄存器
#define ACCEL_CONFIG     0x1C
#define ACCEL_XOUT_H     0x3B          // 加速度X轴高位数据寄存器
#define GYRO_XOUT_H      0x43          // 陀螺仪X轴高位数据寄存器
// 互补滤波系数（0.98偏向陀螺仪动态性能，0.02偏向加速度计静态稳定性）
#define COMPLEMENTARY_COEFF 0.98f
// 弧度转角度系数
#define RAD_TO_DEG       (180.0f / M_PI)

/************************ 全局变量 ************************/
int i2c_fd;  // I2C设备文件描述符
// 角度保存变量（静态变量，保持上次值用于互补滤波）
static float roll_angle = 0.0f;   // 横滚角（Roll）
static float pitch_angle = 0.0f;  // 俯仰角（Pitch）

float roll_angle1 = 0.0f;   // 横滚角（Roll）
float pitch_angle1 = 0.0f;  // 俯仰角（Pitch）

/************************ 函数声明 ************************/
// I2C设备初始化
int i2c_init(const char *i2c_path, uint8_t slave_addr);
// 向MPU6050单个寄存器写数据
int mpu6050_write_reg(uint8_t reg_addr, uint8_t data);
// 从MPU6050读取多个寄存器数据
int mpu6050_read_regs(uint8_t reg_addr, uint8_t *buf, uint8_t len);
// 初始化MPU6050
int mpu6050_init(void);
// 读取加速度和陀螺仪数据
void mpu6050_read_data(int16_t *accel_data, int16_t *gyro_data);
// 计算实际物理值
void mpu6050_calc_phy(float *accel_phy, float *gyro_phy, int16_t *accel_data, int16_t *gyro_data);
// 计算横滚角和俯仰角（互补滤波）
void mpu6050_calc_angle(float *accel_phy, float *gyro_phy, double dt);

/************************ 函数实现 ************************/
/**
 * @brief  I2C设备初始化（打开设备+设置从机地址）
 * @param  i2c_path: I2C设备路径（如/dev/i2c-2）
 * @param  slave_addr: 从机地址（如0x68）
 * @retval 0:成功, -1:失败
 */
int i2c_init(const char *i2c_path, uint8_t slave_addr)
{
    // 打开I2C设备
    i2c_fd = open(i2c_path, O_RDWR);
    if (i2c_fd < 0) {
        perror("open i2c device failed");
        return -1;
    }

    // 设置I2C从机地址
    if (ioctl(i2c_fd, I2C_SLAVE, slave_addr) < 0) {
        perror("set i2c slave address failed");
        close(i2c_fd);
        return -1;
    }

    printf("I2C init success (path: %s, slave addr: 0x%02X)\n", i2c_path, slave_addr);
    return 0;
}

/**
 * @brief  向MPU6050单个寄存器写数据
 * @param  reg_addr: 寄存器地址
 * @param  data: 要写入的数据
 * @retval 0:成功, -1:失败
 */
int mpu6050_write_reg(uint8_t reg_addr, uint8_t data)
{
    uint8_t buf[2];
    buf[0] = reg_addr;  // 寄存器地址
    buf[1] = data;      // 要写入的数据

    // 写入2字节（寄存器地址+数据）
    if (write(i2c_fd, buf, 2) != 2) {
        perror("write mpu6050 reg failed");
        return -1;
    }
    return 0;
}

/**
 * @brief  从MPU6050读取多个寄存器数据
 * @param  reg_addr: 起始寄存器地址
 * @param  buf: 数据接收缓冲区
 * @param  len: 要读取的字节数
 * @retval 0:成功, -1:失败
 */
int mpu6050_read_regs(uint8_t reg_addr, uint8_t *buf, uint8_t len)
{
    // 先写入要读取的起始寄存器地址
    if (write(i2c_fd, &reg_addr, 1) != 1) {
        perror("write reg addr failed");
        return -1;
    }

    // 读取指定长度的数据
    if (read(i2c_fd, buf, len) != len) {
        perror("read mpu6050 regs failed");
        return -1;
    }
    return 0;
}

/**
 * @brief  初始化MPU6050（配置采样率、量程等）
 * @retval 0:成功, -1:失败
 */
int mpu6050_init(void)
{
    // 1. 唤醒MPU6050（PWR_MGMT_1=0x00，解除睡眠模式）
    if (mpu6050_write_reg(PWR_MGMT_1, 0x00) < 0) {
        return -1;
    }

    // 2. 设置采样率分频（SMPLRT_DIV=0x07，采样率=1000/(1+7)=125Hz）
    if (mpu6050_write_reg(SMPLRT_DIV, 0x07) < 0) {
        return -1;
    }

    // 3. 配置低通滤波（CONFIG=0x06，滤波频率=5Hz）
    if (mpu6050_write_reg(CONFIG, 0x06) < 0) {
        return -1;
    }

    // 4. 配置陀螺仪量程（GYRO_CONFIG=0x18，±2000°/s）
    if (mpu6050_write_reg(GYRO_CONFIG, 0x18) < 0) {
        return -1;
    }

    // 5. 配置加速度计量程（ACCEL_CONFIG=0x18，±16g）
    if (mpu6050_write_reg(ACCEL_CONFIG, 0x18) < 0) {
        return -1;
    }

    printf("MPU6050 init success\n");
    return 0;
}

/**
 * @brief  读取加速度（3轴）和陀螺仪（3轴）原始数据
 * @param  accel_data: 加速度原始数据缓冲区（[0]=X, [1]=Y, [2]=Z）
 * @param  gyro_data: 陀螺仪原始数据缓冲区（[0]=X, [1]=Y, [2]=Z）
 */
void mpu6050_read_data(int16_t *accel_data, int16_t *gyro_data)
{
    uint8_t buf[14];  // 加速度(6字节)+温度(2字节)+陀螺仪(6字节)=14字节

    // 从ACCEL_XOUT_H开始读取14字节数据
    mpu6050_read_regs(ACCEL_XOUT_H, buf, 14);

    // 拼接16位原始数据（高位在前，低位在后）
    accel_data[0] = (int16_t)(buf[0] << 8 | buf[1]);  // 加速度X
    accel_data[1] = (int16_t)(buf[2] << 8 | buf[3]);  // 加速度Y
    accel_data[2] = (int16_t)(buf[4] << 8 | buf[5]);  // 加速度Z

    gyro_data[0] = (int16_t)(buf[8] << 8 | buf[9]);   // 陀螺仪X
    gyro_data[1] = (int16_t)(buf[10] << 8 | buf[11]); // 陀螺仪Y
    gyro_data[2] = (int16_t)(buf[12] << 8 | buf[13]); // 陀螺仪Z
}

/**
 * @brief  将原始数据转换为实际物理值
 * @param  accel_phy: 加速度物理值（单位：g）
 * @param  gyro_phy: 陀螺仪物理值（单位：°/s）
 * @param  accel_data: 加速度原始数据
 * @param  gyro_data: 陀螺仪原始数据
 */
void mpu6050_calc_phy(float *accel_phy, float *gyro_phy, int16_t *accel_data, int16_t *gyro_data)
{
    // 加速度计量程±16g，对应原始数据范围±32768，灵敏度=16g/32768=0.00048828125 g/LSB
    accel_phy[0] = accel_data[0] * 0.00048828125f;
    accel_phy[1] = accel_data[1] * 0.00048828125f;
    accel_phy[2] = accel_data[2] * 0.00048828125f;

    // 陀螺仪量程±2000°/s，对应原始数据范围±32768，灵敏度=2000°/s/32768≈0.06103515625 °/s/LSB
    gyro_phy[0] = gyro_data[0] * 0.06103515625f;
    gyro_phy[1] = gyro_data[1] * 0.06103515625f;
    gyro_phy[2] = gyro_data[2] * 0.06103515625f;
}

/**
 * @brief  用互补滤波计算横滚角（Roll）和俯仰角（Pitch）
 * @param  accel_phy: 加速度物理值（g）
 * @param  gyro_phy: 陀螺仪物理值（°/s）
 * @param  dt: 两次采样的时间差（s）
 */
void mpu6050_calc_angle(float *accel_phy, float *gyro_phy, double dt)
{
    // 1. 由加速度计计算静态角度（利用重力分量，仅静态有效）
    float accel_roll = atan2(accel_phy[1], accel_phy[2]) * RAD_TO_DEG;
    float accel_pitch = atan2(-accel_phy[0], sqrt(accel_phy[1]*accel_phy[1] + accel_phy[2]*accel_phy[2])) * RAD_TO_DEG;

    // 2. 由陀螺仪计算动态角度（积分得到，存在漂移）
    float gyro_roll = roll_angle + gyro_phy[0] * dt;
    float gyro_pitch = pitch_angle + gyro_phy[1] * dt;

    // 3. 互补滤波融合：兼顾动态响应和静态稳定性
    roll_angle = COMPLEMENTARY_COEFF * gyro_roll + (1 - COMPLEMENTARY_COEFF) * accel_roll;
    pitch_angle = COMPLEMENTARY_COEFF * gyro_pitch + (1 - COMPLEMENTARY_COEFF) * accel_pitch;
}

/************************ 主函数 ************************/
int main(int argc, char *argv[])
{
    int16_t accel_raw[3] = {0};  // 加速度原始数据
    int16_t gyro_raw[3] = {0};   // 陀螺仪原始数据
    float accel_phy[3] = {0.0f}; // 加速度物理值（g）
    float gyro_phy[3] = {0.0f};  // 陀螺仪物理值（°/s）
    struct timeval last_time, curr_time; // 时间戳（用于计算dt）
    double dt = 0.0; // 两次采样时间差（s）

    // 1. 初始化I2C
    if (i2c_init(I2C_DEVICE_PATH, MPU6050_ADDR) < 0) {
        exit(EXIT_FAILURE);
    }

    // 2. 初始化MPU6050
    if (mpu6050_init() < 0) {
        close(i2c_fd);
        exit(EXIT_FAILURE);
    }

    // 3. 获取初始时间戳
    gettimeofday(&last_time, NULL);

    // 4. 循环读取并打印数据
    printf("=========================================================================\n");
    printf("Accel(X/Y/Z) (g) | Gyro(X/Y/Z) (°/s) | Roll (°) | Pitch (°)\n");
    printf("=========================================================================\n");
    while (1) {
        // 获取当前时间戳
        gettimeofday(&curr_time, NULL);

        // 计算时间差dt（微秒转换为秒）
        dt = (curr_time.tv_sec - last_time.tv_sec) + (curr_time.tv_usec - last_time.tv_usec) / 1000000.0;
        last_time = curr_time; // 更新上一次时间戳

        // 读取原始数据
        mpu6050_read_data(accel_raw, gyro_raw);

        // 转换为物理值
        mpu6050_calc_phy(accel_phy, gyro_phy, accel_raw, gyro_raw);

        // 计算横滚角和俯仰角
        mpu6050_calc_angle(accel_phy, gyro_phy, dt);
   
	roll_angle1=roll_angle + 11.79;
	pitch_angle1=pitch_angle-2.39;
        // 打印数据（\r实现同一行刷新）
        printf("%.4f / %.4f / %.4f | %.4f / %.4f / %.4f | %.2f | %.2f\r",
               accel_phy[0], accel_phy[1], accel_phy[2],
               gyro_phy[0], gyro_phy[1], gyro_phy[2],
               roll_angle1, pitch_angle1);
        fflush(stdout);  // 强制刷新输出缓冲区

        // 延时100ms（读取频率约10Hz，可调整）
        usleep(100000);
    }

    // 关闭I2C设备（实际循环中不会执行到）
    close(i2c_fd);
    return 0;
}