#include "QMI8658A.h"

// 定义日志标签
static const char *TAG = "QMI8658A.c";


// 定义一个数组存储所有命令信息
CommandInfo commandInfos[] = {
    {"CTRL_CMD_ACK", 0x00, "Ctrl9", "确认。主机向QMI8658确认，以结束协议。"},
    {"CTRL_CMD_RST_FIFO", 0x04, "Ctrl9", "从主机重置FIFO。"},
    {"CTRL_CMD_REQ_FIFO", 0x05, "Ctrl9R", "从设备获取FIFO数据。"},
    {"CTRL_CMD_WRITE_WOM_SETTING", 0x08, "WCtrl9", "设置并启用运动唤醒（WoM）功能。"},
    {"CTRL_CMD_ACCEL_HOST_DELTA_OFFSET", 0x09, "WCtrl9", "更改加速度计偏移量。"},
    {"CTRL_CMD_GYRO_HOST_DELTA_OFFSET", 0x0A, "WCtrl9", "更改陀螺仪偏移量。"},
    {"CTRL_CMD_CONFIGURE_TAP", 0x0C, "WCtrl9", "配置敲击检测。"},
    {"CTRL_CMD_CONFIGURE_PEDOMETER", 0x0D, "WCtrl9", "配置计步器。"},
    {"CTRL_CMD_CONFIGURE_MOTION", 0x0E, "WCtrl9", "配置任意运动/静止/显著运动检测。"},
    {"CTRL_CMD_RESET_PEDOMETER", 0x0F, "WCtrl9", "重置计步器步数。"},
    {"CTRL_CMD_COPY_USID", 0x10, "Ctrl9R", "将USID和固件版本复制到UI寄存器。"},
    {"CTRL_CMD_SET_RPU", 0x11, "WCtrl9", "配置IO上拉电阻。"},
    {"CTRL_CMD_AHB_CLOCK_GATING", 0x12, "WCtrl9", "内部AHB时钟门控开关。"},
    {"CTRL_CMD_ON_DEMAND_CALIBRATION", 0xA2, "WCtrl9", "对陀螺仪进行按需校准。"},
    {"CTRL_CMD_APPLY_GYRO_GAINS", 0xAA, "WCtrl9", "恢复保存的陀螺仪增益。"}
};

// 实现函数，通过枚举获取对应的命令信息结构体
CommandInfo getCommandInfo(CommandEnum cmd) {
    return commandInfos[cmd];
}

/*使用示例：
    // 通过枚举获取命令信息并打印
    CommandInfo info = getCommandInfo(CTRL_CMD_REQ_FIFO_ENUM);
    printf("Command Name: %s\n", info.commandName);
    printf("CTRL9 Command Value: 0x%X\n", info.ctrl9CommandValue);
    printf("Protocol Type: %s\n", info.protocolType);
    printf("Description: %s\n", info.description);
    */


/**
 * @brief 对加速度计进行自检操作
 * 
 * 此函数通过一系列的I2C操作对加速度计进行自检，包括禁用传感器、设置输出数据速率、
 * 等待特定状态位变化、读取自检结果并判断是否正常。
 * 
 * @param 无
 * 
 * @return uint8_t 
 *         - 1: 加速度计自检正常
 *         - 0: 加速度计自检异常或在自检过程中出现I2C通信错误
 */
uint8_t Acc_Self_Test()
{
    uint8_t data = 0;

    // 1. 禁用传感器
    // 向寄存器CTRL7写入0x00以禁用传感器
    if (!i2cwrite(CTRL7, 0x00))
    {
        // 若写入失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "发送禁用传感器命令失败！");
        return 0;
    }

    // 2. 设置合适的加速度计输出数据速率4G,896.8Hz
    // 向寄存器CTRL2写入0x93以设置加速度计输出数据速率为4G,896.8Hz
    if (!i2cwrite(CTRL2, 0x93))
    {
        // 若写入失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "设置合适的加速度计输出数据速率失败！");
        return 0;
    }

    // 3. 等待STATUSINT第0位为1
    // 初始化data为0x00
    data = 0x00;
    // 循环读取STATUSINT寄存器，直到第0位为1
    while ((data & 0x01) == 0)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示自检失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 4. 将CTRL2.aST（第7位）设为0
    // 读取CTRL2寄存器的值到data
    if (!i2cread(CTRL2, &data))
    {
        // 若读取失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "读取CTRL2失败！");
        return 0;
    }
    // 将data的第7位清零
    data &= 0x7F;
    // 将修改后的值写回CTRL2寄存器
    if (!i2cwrite(CTRL2, data))
    {
        // 若写入失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "设置CTRL2.aST（第7位）设为0失败！");
        return 0;
    }

    // 5. 等待STATUSINT第0位为0
    // 初始化data为0xFF
    data = 0xFF;
    // 此处逻辑有误，应改为(data & 0x01) != 0，原逻辑(data | 0xFE) == 0不可能成立
    while ((data & 0x01) != 0)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示自检失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 6. 读取加速度计自检结果
    // 定义一个长度为6的数组datas用于存储自检结果
    unsigned char datas[6] = {};
    // 从地址0x51开始连续读取6个字节的数据到datas数组
    if (!i2creads(0x51, 6, datas))
    {
        // 若读取失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "读取加速度计自检结果失败！");
        return 0;
    }

    // 判断结果绝对值是否高于200mg
    int16_t dVxData = 0, dVyData = 0, dVzData = 0;
    // 组合低字节和高字节得到完整的x轴数据
    dVxData = (datas[1] << 8) | datas[0];
    // 组合低字节和高字节得到完整的y轴数据
    dVyData = (datas[3] << 8) | datas[2];
    // 组合低字节和高字节得到完整的z轴数据
    dVzData = (datas[5] << 8) | datas[4];

    // 判断x、y、z轴数据的绝对值是否低于200mg
    if (fabs(dVxData * 0.5) < 200 || fabs(dVyData * 0.5) < 200 || fabs(dVzData * 0.5) < 200)
    {
        // 若有任何一个轴的数据绝对值低于200mg，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "读取加速度计自检结果为：异常！");
        return 0;
    }
    // 若所有轴的数据绝对值都高于200mg，记录日志表示自检正常并返回1
    ESP_LOGE(TAG, "读取加速度计自检结果为：正常！");
    return 1;
}


/**
 * @brief 对陀螺仪进行自检操作
 * 
 * 该函数通过一系列I2C通信步骤对陀螺仪进行自检，包含禁用传感器、设置特定寄存器位、
 * 等待状态位变化、读取自检结果并判断是否符合正常范围。
 * 
 * @param 无
 * @return uint8_t 
 *         - 1: 陀螺仪自检正常
 *         - 0: 陀螺仪自检异常或在自检过程中出现I2C通信错误
 */
uint8_t Gyr_Self_Test()
{
    uint8_t data = 0;

    // 1. 禁用传感器
    // 向寄存器CTRL7写入0x00，以禁用陀螺仪传感器
    if (!i2cwrite(CTRL7, 0x00))
    {
        // 若写入失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "发送禁用传感器命令失败！");
        return 0;
    }

    // 2. 将gST位设为1（CTRL3第7位 = 1’b1）
    // 从寄存器CTRL3读取数据到变量data
    if (!i2cread(CTRL3, &data))
    {
        // 若读取失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "读取CTRL3失败！");
        return 0;
    }
    // 将data的第7位设置为1
    data |= 0x80;
    // 将修改后的数据写回寄存器CTRL3
    if (!i2cwrite(CTRL3, data))
    {
        // 若写入失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "设置CTRL3.gST（第7位）设为1失败！");
        return 0;
    }

    // 3. 等待STATUSINT第0位为1
    // 初始化data为0x00
    data = 0x00;
    // 循环读取STATUSINT寄存器，直到其第0位变为1
    while ((data & 0x01) == 0)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示自检失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 4. 将gST位设为0（CTRL3第7位 = 0’b1）
    // 再次从寄存器CTRL3读取数据到变量data
    if (!i2cread(CTRL3, &data))
    {
        // 若读取失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "读取CTRL3失败！");
        return 0;
    }
    // 将data的第7位清零
    data &= 0x7F;
    // 将修改后的数据写回寄存器CTRL3
    if (!i2cwrite(CTRL3, data))
    {
        // 若写入失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "设置CTRL3.gST（第7位）设为0失败！");
        return 0;
    }

    // 5. 等待STATUSINT第0位为0
    // 初始化data为0xFF
    data = 0xFF;
    // 此处原逻辑(data | 0xFE) == 0有误，应改为(data & 0x01) != 0，以等待STATUSINT第0位为0
    while ((data & 0x01) != 0)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示自检失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 6. 读取陀螺仪自检结果
    // 定义一个长度为6的数组datas用于存储自检结果
    unsigned char datas[6] = {};
    // 从地址0x51开始连续读取6个字节的数据到datas数组
    if (!i2creads(0x51, 6, datas))
    {
        // 此处注释有误，应是读取陀螺仪自检结果，若读取失败，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "读取陀螺仪自检结果失败！");
        return 0;
    }

    // 判断结果绝对值是否高于300dps
    int16_t dVxData = 0, dVyData = 0, dVzData = 0;
    // 组合低字节和高字节得到完整的x轴数据
    dVxData = (datas[1] << 8) | datas[0];
    // 组合低字节和高字节得到完整的y轴数据
    dVyData = (datas[3] << 8) | datas[2];
    // 组合低字节和高字节得到完整的z轴数据
    dVzData = (datas[5] << 8) | datas[4];

    // 判断x、y、z轴数据经转换后的绝对值是否低于300dps
    if (fabs(dVxData * 62.5 / 1000) < 300 || fabs(dVyData * 62.5 / 1000) < 300 || fabs(dVzData * 62.5 / 1000) < 300)
    {
        // 若有任何一个轴的数据绝对值低于300dps，记录错误日志并返回0表示自检失败
        ESP_LOGE(TAG, "读取陀螺仪自检结果为：异常！");
        return 0;
    }
    // 若所有轴的数据绝对值都高于300dps，记录日志表示自检正常并返回1
    ESP_LOGE(TAG, "读取陀螺仪自检结果为：正常！");
    return 1;
}

/**
 * @brief 对陀螺仪进行按需校准（COD, Calibration On Demand）操作
 * 
 * 此函数用于对陀螺仪进行按需校准，校准过程中建议将设备置于安静环境，
 * 否则校准可能失败并报错。函数通过一系列I2C通信操作完成校准流程，
 * 包括禁用传感器、发送校准指令、等待校准完成、确认校准结果、检查校准状态，
 * 最后开启加速度计和陀螺仪的同步采样模式。
 * 
 * @param 无
 * @return uint8_t 
 *         - 1: 陀螺仪校准成功
 *         - 0: 陀螺仪校准失败或在校准过程中出现I2C通信错误
 */
uint8_t Gyr_COD()
{
    uint8_t data = 0;

    // 1. 禁用传感器
    // 向寄存器CTRL7写入0x00，以禁用传感器，为后续校准操作做准备
    if (!i2cwrite(CTRL7, 0x00))
    {
        // 若写入失败，记录错误日志并返回0表示校准失败
        ESP_LOGE(TAG, "发送禁用传感器命令失败！");
        return 0;
    }

    // 2. 通过CTRL9命令发出CTRL_CMD_ON_DEMAND_CALIBRATION（0xA2）指令
    // 向寄存器CTRL9写入0xA2，启动陀螺仪的按需校准操作
    if (!i2cwrite(CTRL9, 0xA2))
    {
        // 若写入失败，记录错误日志并返回0表示校准失败
        ESP_LOGE(TAG, "发送CTRL_CMD_ON_DEMAND_CALIBRATION（0xA2）指令失败！");
        return 0;
    }

    // 3. 等待约1.5秒，让QMI8658A完成CTRL9命令
    // 延时1500毫秒，确保设备有足够时间执行校准命令
    vTaskDelay(pdMS_TO_TICKS(1500));

    // 4. 等待STATUSINT第7位为1
    // 初始化data为0x00
    data = 0x00;
    // 循环读取STATUSINT寄存器，直到其第7位变为1，表示校准操作开始
    while (((data >> 7) & 0x01) == 0)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示校准失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 5. 向CTRL9寄存器写入CTRL_CMD_ACK（0x00）来确认
    // 向寄存器CTRL9写入0x00，确认校准操作开始
    if (!i2cwrite(CTRL9, 0x00))
    {
        // 若写入失败，记录错误日志并返回0表示校准失败
        ESP_LOGE(TAG, "发送CTRL_CMD_ACK（0x00）指令失败！");
        return 0;
    }

    // 6. 等待STATUSINT第7位为0
    // 初始化data为0xFF
    data = 0xFF;
    // 循环读取STATUSINT寄存器，直到其第7位变为0，表示校准操作完成
    while (((data >> 7) & 0x01) == 1)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示校准失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 7. 读取COD_STATUS寄存器（0x46），检查COD实施的结果/状态
    // 从寄存器COD_STATUS读取校准结果
    if (!i2cread(COD_STATUS, &data))
    {
        // 若读取失败，记录错误日志并返回0表示校准失败
        ESP_LOGE(TAG, "读取COD_STATUS失败！");
        return 0;
    }
    // 若data不为0，表示校准失败
    if (data)
    {
        // 记录错误日志，显示错误类型
        ESP_LOGE(TAG, "陀螺仪校准失败，错误类型：%X", data);
        return 0;
    }

    // 8. 开启加速度计和陀螺仪，同步采样模式
    // 向寄存器CTRL7写入0x83，开启加速度计和陀螺仪的同步采样模式
    if (!i2cwrite(CTRL7, 0x83))
    {
        // 若写入失败，记录错误日志并返回0表示校准失败
        ESP_LOGE(TAG, "启加速度计和陀螺仪,同步采样模式失败！");
        return 0;
    }

    // 9. 校准成功
    // 记录日志表示陀螺仪校准成功，并返回1
    ESP_LOGE(TAG, "陀螺仪校准成功!");
    return 1;
}


// 存储陀螺仪校准值
float GyrCompensate[6];

/**
 * @brief 初始化QMI8658A传感器
 * 
 * 该函数用于对QMI8658A传感器进行初始化操作，包括复位传感器、配置通讯方式和中断引脚、
 * 进行加速度计和陀螺仪的自检、配置传感器参数、使用锁定机制、开启传感器同步采样模式、
 * 进行陀螺仪自带校准和手动校准等步骤。
 * 
 * @param 无
 * @return int 
 *         - 1: 传感器初始化成功
 *         - 0: 传感器初始化失败，可能是某个步骤的I2C通信出错或自检、校准失败
 */
int QMI8658A_Init(void)
{
    uint8_t data = 0;

    // 1. 复位QMI8658A传感器
    // 向复位寄存器RESET写入0xB0，触发传感器复位操作
    if (!i2cwrite(RESET, 0xB0))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "发送复位命令失败！");
        return 0;
    }
    // 延时100毫秒，等待复位操作完成
    vTaskDelay(pdMS_TO_TICKS(100));

    // 读取复位状态
    // 从dQY_L寄存器读取复位状态数据到变量data
    if (!i2cread(dQY_L, &data))
    {
        // 若读取失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "读取复位状态失败！");
        return 0;
    }

    // 检查复位状态
    // 判断读取到的复位状态数据是否为0x80，若不是则表示复位失败
    if (data != 0x80)
    {
        // 记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "复位失败！");
        return 0;
    }

    // 2. 配置通讯方式和中断引脚
    // 向寄存器CTRL1写入0x60，配置传感器的通讯方式和中断引脚
    if (!i2cwrite(CTRL1, 0x60))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "配置通讯方式和中断引脚失败！");
        return 0;
    }

    // 3. 加速度计自检
    // 调用Acc_Self_Test函数进行加速度计自检
    if (!Acc_Self_Test())
    {
        // 若自检失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "加速度计自检失败！");
        return 0;
    }

    // 4. 陀螺仪自检
    // 调用Gyr_Self_Test函数进行陀螺仪自检
    if (!Gyr_Self_Test())
    {
        // 若自检失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "陀螺仪自检失败！");
        return 0;
    }

    // 5. 配置加速度计
    // 向寄存器CTRL2写入0x33，禁用加速度自检，设置量程为16G，输出数据速率为896.8Hz
    if (!i2cwrite(CTRL2, 0x33))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "配置加速度计失败！");
        return 0;
    }

    // 6. 配置陀螺仪
    // 向寄存器CTRL3写入0x73，禁用陀螺仪自检，设置量程为2048dps，输出数据速率为896.8Hz
    if (!i2cwrite(CTRL3, 0x73))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "配置陀螺仪计失败！");
        return 0;
    }

    // 7. 配置低通滤波器
    // 向寄存器CTRL5写入0x35，配置低通滤波器为3.63%
    if (!i2cwrite(CTRL5, 0x35))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "配置低通滤波器失败！");
        return 0;
    }

    // 8. 使用锁定机制
    // 8.1 禁用内部AHB时钟
    // 向CAL1_L寄存器写入0x01
    if (!i2cwrite(CAL1_L, 0x01))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "CAL1_L寄存器写入失败！");
        return 0;
    }
    // 在CTRL9协议中写入0x12（CTRL_CMD_AHB_CLOCK_GATING）
    if (!i2cwrite(CTRL9, 0x12))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "发送CTRL_CMD_ON_DEMAND_CALIBRATION（0x12）指令失败！");
        return 0;
    }
    // 等待10毫秒，让QMI8658A完成CTRL9命令
    vTaskDelay(pdMS_TO_TICKS(10));

    // 8.2 等待STATUSINT第7位为1
    // 初始化data为0x00
    data = 0x00;
    // 循环读取STATUSINT寄存器，直到其第7位变为1
    while (((data >> 7) & 0x01) == 0)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示初始化失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 8.3 向CTRL9寄存器写入CTRL_CMD_ACK（0x00）来确认
    // 向寄存器CTRL9写入0x00，确认操作
    if (!i2cwrite(CTRL9, 0x00))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "发送CTRL_CMD_ACK（0x00）指令失败！");
        return 0;
    }

    // 8.4 等待STATUSINT第7位为0
    // 初始化data为0xFF
    data = 0xFF;
    // 循环读取STATUSINT寄存器，直到其第7位变为0
    while (((data >> 7) & 0x01) == 1)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示初始化失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 9. 开启加速度计和陀螺仪，同步采样模式
    // 向寄存器CTRL7写入0x83，开启加速度计和陀螺仪的同步采样模式
    if (!i2cwrite(CTRL7, 0x83))
    {
        // 若写入失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "启加速度计和陀螺仪,同步采样模式失败！");
        return 0;
    }
    // 延时10毫秒，等待模式开启完成
    vTaskDelay(pdMS_TO_TICKS(10));

    // 10. 陀螺仪自带校准
    // 调用Gyr_COD函数进行陀螺仪自带校准
    if (!Gyr_COD())
    {
        // 若校准失败，记录错误日志并返回0表示初始化失败
        ESP_LOGE(TAG, "陀螺仪校准失败！");
        return 0;
    }
    // 延时100毫秒，等待校准完成
    vTaskDelay(pdMS_TO_TICKS(100));

    // 11. 陀螺仪手动校准
    // 循环调用calibrationGYR函数进行陀螺仪手动校准，直到校准成功
    while (!calibration_ACC_GYR(GyrCompensate))
    {
    }

    // 12. 初始化成功
    // 记录日志表示传感器初始化成功，并返回1
    ESP_LOGE(TAG, "初始化成功！");
    return 1;
}


/**
 * @brief 读取QMI8658A六轴传感器数据
 * 
 * 该函数用于读取QMI8658A六轴传感器的数据，在读取前会先检查数据的可用性和锁定状态，
 * 确保数据有效后再进行读取操作，并将读取到的原始数据存储到传入的数组中。
 * 
 * @param DATA 指向一个长度为6的int16_t类型数组的指针，用于存储读取到的六轴传感器原始数据，
 *             数组元素依次为AX、AY、AZ、GX、GY、GZ
 * @return int 
 *         - 1: 数据读取成功
 *         - 0: 读取过程中出现错误，如读取状态寄存器失败或读取传感器数据寄存器失败
 */
int QMI8658A_ReadData(int16_t *DATA)
{
    uint8_t data = 0;

    // 1. 判断数据是否可用（Avail）
    // 循环读取STATUSINT寄存器，直到其第0位为1，表示数据可用
    while ((data & 0x01) != 1)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示读取失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 2. 判断数据是否锁定（Locked）
    // 重置data为0
    data = 0;
    // 循环读取STATUSINT寄存器，直到其第1位为1，表示数据已锁定
    while (((data >> 1) & 0x01) != 1)
    {
        // 读取STATUSINT寄存器的值到data
        if (!i2cread(STATUSINT, &data))
        {
            // 若读取失败，记录错误日志并返回0表示读取失败
            ESP_LOGE(TAG, "读取STATUSINT状态失败！");
            return 0;
        }
    }

    // 3. 读取传感器数据寄存器
    // 定义一个长度为12的数组datas，用于存储从传感器数据寄存器读取的数据
    uint8_t datas[12] = {};
    // 从地址A_XYZ开始连续读取12个字节的数据到datas数组
    if (!i2creads(A_XYZ, 12, datas))
    {
        // 此处注释有误，应是读取传感器数据失败，若读取失败，记录错误日志并返回0表示读取失败
        ESP_LOGE(TAG, "读取传感器数据失败！");
        return 0;
    }

    // 4. 组合数据
    // 将读取到的低字节和高字节数据组合成完整的16位数据，并存储到DATA数组中
    DATA[0] = (datas[1] << 8) | datas[0]; // AX
    DATA[1] = (datas[3] << 8) | datas[2]; // AY
    DATA[2] = (datas[5] << 8) | datas[4]; // AZ
    DATA[3] = (datas[7] << 8) | datas[6]; // GX
    DATA[4] = (datas[9] << 8) | datas[8]; // GY
    DATA[5] = (datas[11] << 8) | datas[10]; // GZ

    // 5. 返回读取成功标志
    return 1;
}

/**
 * @brief 将QMI8658A传感器的原始数据根据加速度计和陀螺仪的量程进行转换，并补偿陀螺仪数据
 * 
 * 该函数接收QMI8658A传感器的原始数据、加速度计和陀螺仪的量程作为输入，
 * 根据不同的量程将原始数据转换为实际的物理量值。加速度计数据转换为以g为单位，
 * 陀螺仪数据转换为以dps（度每秒）为单位，并对陀螺仪数据进行补偿。
 * 
 * @param InData 指向包含原始传感器数据的数组的指针，数组长度应为6，
 *               前3个元素为加速度计数据（AX, AY, AZ），后3个元素为陀螺仪数据（GX, GY, GZ）
 * @param OutData 指向用于存储转换后数据的数组的指针，数组长度应为6，
 *                前3个元素存储转换后的加速度计数据，后3个元素存储转换后的陀螺仪数据
 * @param accelRange 加速度计的量程，合法值为2, 4, 8, 16（单位：g）
 * @param gyroRange 陀螺仪的量程，合法值为16, 32, 64, 128, 256, 512, 1024, 2048（单位：dps）
 * @return 无
 */
void QMI8658A_ConvertData(int16_t *InData, float *OutData, int accelRange, int gyroRange)
{
    float accelFactor, compensate;

    // 1. 计算加速度计转换因子
    // 根据加速度计量程选择合适的转换因子
    switch (accelRange)
    {
        case 2: // ±2g
            accelFactor = 2.0f * 2 / 65536;
            break;
        case 4: // ±4g
            accelFactor = 4.0f * 2 / 65536;
            break;
        case 8: // ±8g
            accelFactor = 8.0f * 2 / 65536;
            break;
        case 16: // ±16g
            accelFactor = 16.0f * 2 / 65536;
            break;
        default:
            accelFactor = 0;
            break;
    }

    // 2. 转换加速度计数据
    // 将原始加速度计数据乘以转换因子，得到以g为单位的加速度计数据
    for (int i = 0; i < 3; i++)
    {
        OutData[i] = InData[i] * accelFactor;
    }

    float gyroFactor;

    // 3. 计算陀螺仪转换因子
    // 根据陀螺仪量程选择合适的转换因子
    switch (gyroRange)
    {
        case 16: // ±16dps
            gyroFactor = 16.0f * 2 / 65536;
            break;
        case 32: // ±32dps
            gyroFactor = 32.0f * 2 / 65536;
            break;
        case 64: // ±64dps
            gyroFactor = 64.0f * 2 / 65536;
            break;
        case 128: // ±128dps
            gyroFactor = 128.0f * 2 / 65536;
            break;
        case 256: // ±256dps
            gyroFactor = 256.0f * 2 / 65536;
            break;
        case 512: // ±512dps
            gyroFactor = 512.0f * 2 / 65536;
            break;
        case 1024: // ±1024dps
            gyroFactor = 1024.0f * 2 / 65536;
            break;
        case 2048: // ±2048dps
            gyroFactor = 2048.0f * 2 / 65536;
            break;
        default:
            gyroFactor = 0;
            break;
    }

    // 4. 转换并补偿陀螺仪数据
    // 将原始陀螺仪数据乘以转换因子，并减去对应的补偿值，得到以dps为单位的陀螺仪数据
    for (int i = 3; i < 6; i++)
    {
        switch (i)
        {
            case 3:
                compensate = GyrCompensate[3];
                break;
            case 4:
                compensate = GyrCompensate[4];
                break;
            case 5:
                compensate = GyrCompensate[5];
                break;
            default:
                // 如果 i 不是 3、4、5，可以在这里添加默认处理逻辑
                break;
        }
        OutData[i] = InData[i] * gyroFactor - compensate;
    }
}

/**
 * @brief 获取单位为g的三轴加速度计和单位为dps的三轴陀螺仪数据
 * 
 * 该函数通过调用QMI8658A_ReadData函数读取传感器的原始数据，
 * 再调用QMI8658A_ConvertData函数将原始数据转换为以g为单位的加速度计数据
 * 和以dps为单位的陀螺仪数据，并将转换后的数据存储到传入的数组中。
 * 
 * @param OutData 指向一个长度为6的float类型数组的指针，用于存储转换后的六轴传感器数据，
 *                数组元素依次为AX、AY、AZ、GX、GY、GZ，单位分别为g和dps
 * @return 无
 */
void QMI8658A_Get_G_DPS(float *OutData)
{
    // 1. 定义用于存储原始数据的数组
    int16_t data[6];

    // 2. 读取原始数据
    // 调用QMI8658A_ReadData函数读取传感器的原始数据
    QMI8658A_ReadData(data);

    // 3. 转换数据
    // 调用QMI8658A_ConvertData函数将原始数据转换为以g和dps为单位的数据
    QMI8658A_ConvertData(data, OutData, ACCRANGE, GYRRANGE);
}

/**
 * @brief 计算加速度的模长
 * 
 * 该函数接收一个包含三轴加速度数据的数组，通过计算三个轴加速度值的平方和的平方根，
 * 得到加速度的模长。
 * 
 * @param OutData 指向一个长度至少为 3 的 float 类型数组的指针，数组前三个元素依次为 X、Y、Z 轴的加速度值，单位为 g
 * @return float 加速度的模长，单位为 g
 */
float calculateAccelerationMagnitude(float *OutData)
{
    return (float)sqrt(OutData[0] * OutData[0] + OutData[1] * OutData[1] + OutData[2] * OutData[2]);
}

/**
 * @brief 比较函数，用于 qsort
 * 
 * 该函数是一个用于 qsort 函数的比较函数，用于对 float 类型的数据进行排序。
 * 它会比较两个 float 类型的值，并根据它们的大小关系返回相应的结果。
 * 
 * @param a 指向第一个 float 类型数据的指针
 * @param b 指向第二个 float 类型数据的指针
 * @return int 
 *         - 若 *a > *b，返回 1
 *         - 若 *a <= *b，返回 -1
 */
int compareFloat(const void *a, const void *b)
{
    return (*(float *)a - *(float *)b) > 0 ? 1 : -1;
}

/**
 * @brief 计算平均值的辅助函数
 * 
 * 该函数用于计算陀螺仪在一段时间内采集的多个数据的平均值。具体做法是，
 * 先将采集到的陀螺仪AX、AY、AZ、GX、GY、GZ 六个轴数据分别复制到临时数组中并排序，然后选取中间的
 * USED_DATA_COUNT 条数据计算平均值，最终将结果存储到 OutData 数组中。
 * 
 * @param DATA 一个二维 float 类型数组，大小为 [MIN_COLLECTION_COUNT][6]，
 *             存储了陀螺仪在 MIN_COLLECTION_COUNT 次采集过程中AX、AY、AZ、GX、GY、GZ 六个轴的数据，单位为 g和dps
 * @param OutData 指向一个长度至少为 6 的 float 类型数组的指针，用于存储计算得到的陀螺仪AX、AY、AZ、GX、GY、GZ轴数据的平均值，单位为 g和dps
 * @return 无
 */
void calculateAverages(float DATA[MIN_COLLECTION_COUNT][6], float *OutData) {
    // 动态分配内存用于存储排序后的数据
    float **sortedData = (float **)malloc(6 * sizeof(float *));
    if (sortedData == NULL) {
        ESP_LOGE(TAG, "内存分配失败");
        return;
    }

    for (int j = 0; j < 6; j++) {
        sortedData[j] = (float *)malloc(MIN_COLLECTION_COUNT * sizeof(float));
        if (sortedData[j] == NULL) {
            // 释放已分配的内存
            for (int k = 0; k < j; k++) {
                free(sortedData[k]);
            }
            free(sortedData);
            ESP_LOGE(TAG, "内存分配失败");
            return;
        }
    }

    // 复制数据到临时数组
    for (int i = 0; i < MIN_COLLECTION_COUNT; i++) {
        for (int j = 0; j < 6; j++) {
            sortedData[j][i] = DATA[i][j];
        }
    }

    // 对数据分别排序
    for (int j = 0; j < 6; j++) {
        qsort(sortedData[j], MIN_COLLECTION_COUNT, sizeof(float), compareFloat);
    }

    // 计算中间 USED_DATA_COUNT 条数据的起始索引
    int startIndex = (MIN_COLLECTION_COUNT - USED_DATA_COUNT) / 2;

    // 计算中间 USED_DATA_COUNT 条数据的总和并计算平均值
    for (int j = 0; j < 6; j++) {
        float sum = 0;
        for (int i = startIndex; i < startIndex + USED_DATA_COUNT; i++) {
            sum += sortedData[j][i];
        }
        OutData[j] = sum / USED_DATA_COUNT;
    }

    // 释放动态分配的内存
    for (int j = 0; j < 6; j++) {
        free(sortedData[j]);
    }
    free(sortedData);
}


/**
 * @brief 校准陀螺仪和加速度计，等待收集至少 500 次静止状态下的数据，对数据排序后取中间 200 条计算平均值。
 *
 * 该函数会不断读取六轴传感器的数据，计算加速度的模长，判断设备是否处于静止状态。
 * 当收集到至少 500 次静止状态下的数据后，对陀螺仪的 AX、AY、AZ、GX、GY、GZ 六个轴数据分别排序，
 * 取排序后中间的 200 条数据计算平均值，并将结果存储在 OutData 数组中。
 *
 * @param[out] OutData 用于存储校准后陀螺仪六个轴（ AX、AY、AZ、GX、GY、GZ）的平均值，数组长度至少为 6。
 * @return uint8_t 校准结果状态码：
 *         - 0：表示读取六轴数据失败、传入参数无效或者还未收集满 500 次静止数据。
 *         - 1：表示成功收集 500 次静止数据并完成陀螺仪校准。
 */
uint8_t calibration_ACC_GYR(float *OutData) {
    // 检查传入的 OutData 指针是否为 NULL
    if (OutData == NULL) {
        ESP_LOGE(TAG, "传入的 OutData 指针为 NULL，无法进行校准。");
        return 0;
    }

    // 用于存储从传感器读取的原始六轴数据，数组长度为 6，分别对应 AX、AY、AZ、GX、GY、GZ
    int16_t rawData[6];
    // 用于存储转换后的六轴数据，同样数组长度为 6
    float convertedData[6], AM = 0;
    // 用于存储上一次计算得到的加速度模长，初始值为 0
    float OldAM = 0;
    // 动态分配内存，用于存储至少 500 次静止状态下的陀螺仪数据（AX、AY、AZ、GX、GY、GZ）
    float (*DATA)[6] = (float (*)[6])malloc(MIN_COLLECTION_COUNT * sizeof(float[6]));
    if (DATA == NULL) {
        ESP_LOGE(TAG, "内存分配失败，无法进行校准。");
        return 0;
    }
    // 计数器，记录当前已经收集到的静止数据的次数，初始值为 0
    int i = 0;

    // 若还未收集满 500 次静止数据，则继续收集
    while (i < MIN_COLLECTION_COUNT) {
        // 调用 QMI8658A_ReadData 函数读取六轴数据
        if (!QMI8658A_ReadData(rawData)) {
            // 若读取失败，打印错误信息，释放内存并返回 0 表示失败
            ESP_LOGE(TAG, "读取六轴数据失败，无法继续处理。");
            free(DATA);
            return 0;
        }

        // 调用 QMI8658A_ConvertData 函数将原始数据进行转换，转换结果存储在 convertedData 数组中
        QMI8658A_ConvertData(rawData, convertedData, ACCRANGE, GYRRANGE);

        // 计算加速度的模长，存储在 AM 变量中
        AM = calculateAccelerationMagnitude(convertedData);

        // 判断当前加速度模长与上一次的差值是否小于阈值，若小于则认为处于静止状态
        if (fabs(AM - OldAM) < STATIONARY_THRESHOLD) {
            // 保存数据（AX、AY、AZ、GX、GY、GZ）
            DATA[i][0] = convertedData[0];
            DATA[i][1] = convertedData[1];
            DATA[i][2] = convertedData[2];
            DATA[i][3] = convertedData[3];
            DATA[i][4] = convertedData[4];
            DATA[i][5] = convertedData[5];
            // 计数器加 1，表示又收集到一次静止数据
            i++;
        } else {
            // 若不处于静止状态，更新 OldAM 为当前的加速度模长
            OldAM = AM;
        }
    }

    // 已经收集到至少 500 次静止数据，计算陀螺仪平均值
    calculateAverages(DATA, OutData);

    // 释放动态分配的内存
    free(DATA);

    // 打印校准结果，输出 GX、GY、GZ 轴的平均值
    ESP_LOGI(TAG, "陀螺仪校准完成，AX 平均值: %.2f, AY 平均值: %.2f, AZ 平均值: %.2f,GX 平均值: %.2f, GY 平均值: %.2f, GZ 平均值: %.2f", OutData[0], OutData[1], OutData[2],OutData[3], OutData[4], OutData[5]);

    // 返回 1 表示成功完成陀螺仪校准
    return 1;
}


/**
 * @brief 读取QMI8658A传感器的六轴数据，进行转换并打印转换后的数据
 *
 * 该函数会调用 QMI8658A_ReadData 函数从 QMI8658A 传感器读取原始的六轴数据，
 * 若读取成功，再调用 QMI8658A_ConvertData 函数根据传入的加速度计和陀螺仪量程
 * 对原始数据进行转换，最后使用 ESP_LOGE 函数将转换后的加速度计和陀螺仪数据打印输出。
 * 如果读取数据失败，会打印错误信息并提前返回。
 *
 * @param accelRange 加速度计的量程，合法值为 2, 4, 8, 16（单位：g）
 * @param gyroRange 陀螺仪的量程，合法值为 16, 32, 64, 128, 256, 512, 1024, 2048（单位：dps）
 *
 * @return 无
 */
void QMI8658A_ReadConvertAndPrint() {
    
    static int i=0;
    
    // 用于存储从传感器读取的原始六轴数据
    int16_t rawData[6];
    // 用于存储转换后的六轴数据
    float convertedData[6],AM=0;

    // 调用 QMI8658A_ReadData 函数读取六轴数据
    if (!QMI8658A_ReadData(rawData)) {
        // 若读取失败，打印错误信息并返回
        ESP_LOGE(TAG, "读取六轴数据失败，无法继续处理。");
        return;
    }

    // 调用 QMI8658A_ConvertData 函数将原始数据进行转换
    QMI8658A_ConvertData(rawData, convertedData, ACCRANGE, GYRRANGE);


    // 计算加速度的模长
    AM= calculateAccelerationMagnitude(convertedData);


    // 打印转换后的加速度计数据，保留小数点后6位
    // ESP_LOGE(TAG, " (g): AX=%d, AY=%d, AZ=%d (dps): GX=%d, GY=%d, GZ=%d", 
    //  rawData[0], rawData[1], rawData[2],rawData[3], rawData[4], rawData[5]);
    if(i==100)
    {
        i=0;
    ESP_LOGE(TAG, " (g): AX=%.3f, AY=%.3f, AZ=%.3f, AM=%.3f (dps): GX=%.3f, GY=%.3f, GZ=%.3f",
              (float)convertedData[0], (float)convertedData[1], (float)convertedData[2],AM,(float)convertedData[3], (float)convertedData[4], (float)convertedData[5]);
    }
               i++;
}