#ifndef ATTITUDE_ESTIMATION_H
#define ATTITUDE_ESTIMATION_H
#include <stdio.h>
#include <math.h>
#include "QMI8658A.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "LED.h"

#define PI 3.14159265358979323846

// 初始比例参数，用于姿态融合时的比例控制
#define KP_INITIAL 1.5f
// 初始积分参数，用于姿态融合时的积分控制
#define KI_INITIAL 0.005f
// 传感器数据的采样频率，单位为 Hz
#define SAMPLE_FREQ SAMPLERATE
// 积分误差的上限值，防止积分饱和
#define INTTT_MAX 1.0f
// 积分误差的下限值，防止积分饱和
#define INTTT_MIN -1.0f


// 四元数结构体
typedef struct {
    float w;
    float x;
    float y;
    float z;
} Quaternion;



// 定义积分误差结构体
typedef struct {
    float exInt;
    float eyInt;
    float ezInt;
} IntegralError;

// 定义传感器数据结构体
typedef struct {
    float gx;  // 陀螺仪 x 轴角速度
    float gy;  // 陀螺仪 y 轴角速度
    float gz;  // 陀螺仪 z 轴角速度
    float ax;  // 加速度计 x 轴测量值
    float ay;  // 加速度计 y 轴测量值
    float az;  // 加速度计 z 轴测量值
} SensorData;

// 定义 Mahony AHRS 状态结构体
typedef struct {
    Quaternion quaternion;
    IntegralError integralError;
} MahonyAHRSState;




// 初始化 Mahony AHRS 状态
void MahonyAHRSInit(MahonyAHRSState *state);

/**
 * @brief 对积分误差进行限幅处理，防止积分饱和
 * @param value 待限幅的积分误差值
 * @return 限幅后的积分误差值
 */
float limitIntegral(float value);

/**
 * @brief 计算四元数在当前角速度下的导数
 * @param gx 陀螺仪在 x 轴上的角速度，单位为弧度/秒
 * @param gy 陀螺仪在 y 轴上的角速度，单位为弧度/秒
 * @param gz 陀螺仪在 z 轴上的角速度，单位为弧度/秒
 * @param q 输入的四元数
 * @param dq 输出的四元数导数
 */
void computeQuaternionDerivative(float gx, float gy, float gz, Quaternion *q, Quaternion *dq);

/**
 * @brief 使用四阶龙格 - 库塔法更新四元数
 * @param gx 陀螺仪在 x 轴上的角速度，单位为弧度/秒
 * @param gy 陀螺仪在 y 轴上的角速度，单位为弧度/秒
 * @param gz 陀螺仪在 z 轴上的角速度，单位为弧度/秒
 * @param dt 时间步长，等于 1 / 采样频率
 * @param q 输入并输出的四元数
 */
void rk4QuaternionUpdate(float gx, float gy, float gz, float dt, Quaternion *q);

/**
 * @brief 根据加速度计数据自适应调整比例和积分参数，动态调整范围
 * @param ax 加速度计在 x 轴上的测量值，单位为 m/s²
 * @param ay 加速度计在 y 轴上的测量值，单位为 m/s²
 * @param az 加速度计在 z 轴上的测量值，单位为 m/s²
 * @param Kp 输出的比例参数，用于姿态融合的比例控制
 * @param Ki 输出的积分参数，用于姿态融合的积分控制
 */
void adaptiveParameterAdjustment(float ax, float ay, float az, float *Kp, float *Ki);

/**
 * @brief Mahony 姿态更新函数，融合加速度计和陀螺仪数据更新姿态
 * @param state Mahony AHRS 状态结构体
 * @param data 传感器数据结构体
 */
void MahonyAHRSupdate(MahonyAHRSState *state, SensorData *data);


// 从四元数中提取欧拉角
void quaternion_to_euler(Quaternion q, float *roll, float *pitch, float *yaw);

// 使用加速度计初始化初始姿态
int Initialize_Attitude_With_Accelerometer(Quaternion *q);

void app_QMI8658A() ;

#endif