#include "AttitudeEstimation.h"

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

// 四元数归一化
void quaternion_normalize(Quaternion *q) {
    float norm = sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;
}


// 初始化 Mahony AHRS 状态
void MahonyAHRSInit(MahonyAHRSState *state) {
    state->quaternion.w = 1.0f;
    state->quaternion.x = 0.0f;
    state->quaternion.y = 0.0f;
    state->quaternion.z = 0.0f;
    state->integralError.exInt = 0.0f;
    state->integralError.eyInt = 0.0f;
    state->integralError.ezInt = 0.0f;
}

/**
 * @brief 对积分误差进行限幅处理，防止积分饱和
 * @param value 待限幅的积分误差值
 * @return 限幅后的积分误差值
 */
float limitIntegral(float value) {
    if (value > INTTT_MAX) return INT_MAX;
    if (value < INTTT_MIN) return INT_MIN;
    return 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) {
    dq->w = 0.5f * (-q->x * gx - q->y * gy - q->z * gz);
    dq->x = 0.5f * (q->w * gx + q->y * gz - q->z * gy);
    dq->y = 0.5f * (q->w * gy - q->x * gz + q->z * gx);
    dq->z = 0.5f * (q->w * gz + q->x * gy - q->y * gx);
}

/**
 * @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) {
    Quaternion k1, k2, k3, k4;
    Quaternion temp_q;

    // 计算 k1，即四元数在当前状态下的导数
    computeQuaternionDerivative(gx, gy, gz, q, &k1);
    temp_q.w = q->w + 0.5f * dt * k1.w;
    temp_q.x = q->x + 0.5f * dt * k1.x;
    temp_q.y = q->y + 0.5f * dt * k1.y;
    temp_q.z = q->z + 0.5f * dt * k1.z;

    // 计算 k2，基于 k1 预测的中间状态下的四元数导数
    computeQuaternionDerivative(gx, gy, gz, &temp_q, &k2);
    temp_q.w = q->w + 0.5f * dt * k2.w;
    temp_q.x = q->x + 0.5f * dt * k2.x;
    temp_q.y = q->y + 0.5f * dt * k2.y;
    temp_q.z = q->z + 0.5f * dt * k2.z;

    // 计算 k3，基于 k2 预测的中间状态下的四元数导数
    computeQuaternionDerivative(gx, gy, gz, &temp_q, &k3);
    temp_q.w = q->w + dt * k3.w;
    temp_q.x = q->x + dt * k3.x;
    temp_q.y = q->y + dt * k3.y;
    temp_q.z = q->z + dt * k3.z;

    // 计算 k4，基于 k3 预测的状态下的四元数导数
    computeQuaternionDerivative(gx, gy, gz, &temp_q, &k4);

    // 使用四阶龙格 - 库塔公式更新四元数
    q->w += (dt / 6.0f) * (k1.w + 2.0f * k2.w + 2.0f * k3.w + k4.w);
    q->x += (dt / 6.0f) * (k1.x + 2.0f * k2.x + 2.0f * k3.x + k4.x);
    q->y += (dt / 6.0f) * (k1.y + 2.0f * k2.y + 2.0f * k3.y + k4.y);
    q->z += (dt / 6.0f) * (k1.z + 2.0f * k2.z + 2.0f * k3.z + k4.z);
}

/**
 * @brief 根据加速度计数据自适应调整比例和积分参数，动态调整范围
 * @param ax 加速度计在 x 轴上的测量值，单位为 m/s²
 * @param ay 加速度计在 y 轴上的测量值，单位为 m/s²
 * @param az 加速度计在 z 轴上的测量值，单位为 m/s²
 * @param Kp 输出的比例参数，用于姿态融合的比例控制
 * @param Ki 输出的积分参数，用于姿态融合的积分控制
 */
// 全局变量，用于记录上一次的加速度幅值
float prev_accel_mag = 0.0f;
// 全局标志，用于判断是否为第一次调用函数
int is_first_call = 1;

void adaptiveParameterAdjustment(float ax, float ay, float az, float *Kp, float *Ki) {
    float accel_mag = sqrt(ax * ax + ay * ay + az * az);
    float accel_rate = 0;
    float dynamic_factor_kp = 1.0f;  // 用于调整 Kp 的动态因子
    float dynamic_factor_ki = 1.0f;  // 用于调整 Ki 的动态因子

    if (is_first_call) {
        // 第一次调用函数，不计算加速度变化率，使用默认动态因子
        dynamic_factor_kp = 1.0f;
        dynamic_factor_ki = 1.0f;
        is_first_call = 0;
    } else {
        // 计算加速度变化率，使用绝对值
        accel_rate = fabs(accel_mag - prev_accel_mag);

        // 根据加速度变化率判断运动状态
        if (accel_rate > 0.2f) {  // 震动非常剧烈，加速度变化很快
            dynamic_factor_kp = 0.1f;  // Kp 取初始化值的 0.1 倍
            dynamic_factor_ki = 2.0f;  // Ki 在同一数量级内适当取大一点
        } else if (accel_rate > 0.1f) {  // 震动剧烈，加速度有明显变化
            dynamic_factor_kp = 0.2f;  // Kp 适当减小
            dynamic_factor_ki = 1.5f;  // Ki 适当增大
        } else if (accel_rate < 0.03f) {  // 震动非常轻微，加速度变化极慢
            dynamic_factor_kp = 1.5f;  // Kp 增大
            dynamic_factor_ki = 0.5f;  // Ki 减小
        } else if (accel_rate < 0.06f) {  // 震动轻微，加速度变化较缓
            dynamic_factor_kp = 1.2f;  // Kp 适当增大
            dynamic_factor_ki = 0.7f;  // Ki 适当减小
        } else {  // 正常运动
            dynamic_factor_kp = 1.0f;
            dynamic_factor_ki = 1.0f;
        }
    }
    // 更新上一次的加速度幅值
    prev_accel_mag = accel_mag;
    *Kp = KP_INITIAL * dynamic_factor_kp;
    *Ki = KI_INITIAL * dynamic_factor_ki;
}

/**
 * @brief Mahony 姿态更新函数，融合加速度计和陀螺仪数据更新姿态
 * @param state Mahony AHRS 状态结构体
 * @param data 传感器数据结构体
 */
void MahonyAHRSupdate(MahonyAHRSState *state, SensorData *data) {
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float Kp, Ki;
    float dt = 1.0f / SAMPLE_FREQ;

    // 根据加速度计数据自适应调整比例和积分参数
    adaptiveParameterAdjustment(data->ax, data->ay, data->az, &Kp, &Ki);

    // 如果加速度计数据无效，直接使用陀螺仪数据更新四元数
    if (!((data->ax == 0.0f) && (data->ay == 0.0f) && (data->az == 0.0f))) {
        // 归一化加速度计数据，使其模长为 1
        recipNorm = 1.0f / sqrt(data->ax * data->ax + data->ay * data->ay + data->az * data->az);
        data->ax *= recipNorm;
        data->ay *= recipNorm;
        data->az *= recipNorm;

        // 根据当前四元数推算出的重力方向在机体坐标系下的分量
        halfvx = state->quaternion.x * state->quaternion.z - state->quaternion.w * state->quaternion.y;
        halfvy = state->quaternion.w * state->quaternion.x + state->quaternion.y * state->quaternion.z;
        halfvz = state->quaternion.w * state->quaternion.w - 0.5f + state->quaternion.z * state->quaternion.z;

        // 计算加速度计测量的重力方向与四元数推算的重力方向之间的叉积误差
        halfex = (data->ay * halfvz - data->az * halfvy);
        halfey = (data->az * halfvx - data->ax * halfvz);
        halfez = (data->ax * halfvy - data->ay * halfvx);

        // 条件积分法抗积分饱和
        if ((state->integralError.exInt < INT_MAX && halfex > 0) || (state->integralError.exInt > INT_MIN && halfex < 0)) {
            state->integralError.exInt += halfex * Ki * dt;
        }
        if ((state->integralError.eyInt < INT_MAX && halfey > 0) || (state->integralError.eyInt > INT_MIN && halfey < 0)) {
            state->integralError.eyInt += halfey * Ki * dt;
        }
        if ((state->integralError.ezInt < INT_MAX && halfez > 0) || (state->integralError.ezInt > INT_MIN && halfez < 0)) {
            state->integralError.ezInt += halfez * Ki * dt;
        }

        // 对积分误差进行限幅处理
        state->integralError.exInt = limitIntegral(state->integralError.exInt);
        state->integralError.eyInt = limitIntegral(state->integralError.eyInt);
        state->integralError.ezInt = limitIntegral(state->integralError.ezInt);

        // 使用比例 - 积分控制对陀螺仪的角速度进行补偿
        data->gx += Kp * halfex + state->integralError.exInt;
        data->gy += Kp * halfey + state->integralError.eyInt;
        data->gz += Kp * halfez + state->integralError.ezInt;
    }

    // 使用四阶龙格 - 库塔法更新四元数以提高积分精度
    rk4QuaternionUpdate(data->gx, data->gy, data->gz, dt, &state->quaternion);

    // 归一化更新后的四元数，保证其模长为 1
    recipNorm = 1.0f / sqrt(state->quaternion.w * state->quaternion.w + 
                            state->quaternion.x * state->quaternion.x + 
                            state->quaternion.y * state->quaternion.y + 
                            state->quaternion.z * state->quaternion.z);
    state->quaternion.w *= recipNorm;
    state->quaternion.x *= recipNorm;
    state->quaternion.y *= recipNorm;
    state->quaternion.z *= recipNorm;
}




// 从四元数中提取欧拉角
void quaternion_to_euler(Quaternion q, float *roll, float *pitch, float *yaw) {
    *roll = atan2(2 * (q.w * q.x + q.y * q.z), 1 - 2 * (q.x * q.x + q.y * q.y));
    *pitch = asin(2 * (q.w * q.y - q.z * q.x));
    *yaw = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z));

    // 将弧度转换为度
    *roll *= 180.0 / PI;
    *pitch *= -180.0 / PI;
    *yaw *= 180.0 / PI;
}


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

        float OutData[3];

        for(int i=0;i<3;i++)
        {
            OutData[i]=GyrCompensate[i+3];
        }

        // 对数据进行归一化
        float Guideline_norm = calculateAccelerationMagnitude(OutData);
        OutData[0] /= Guideline_norm;
        OutData[1] /= Guideline_norm;
        OutData[2] /= Guideline_norm;

        // 根据平均加速度计算欧拉角
        float roll = atan2(OutData[1], OutData[2]);
        float pitch = atan2(-OutData[0], sqrt(OutData[1] * OutData[1] + OutData[2] * OutData[2]));
    
        float yaw = 0.0f;  // 加速度计无法确定偏航角，初始化为 0

        // 将欧拉角转换为四元数
        float cr = cos(roll * 0.5f);
        float sr = sin(roll * 0.5f);
        float cp = cos(pitch * 0.5f);
        float sp = sin(pitch * 0.5f);
        float cy = cos(yaw * 0.5f);
        float sy = sin(yaw * 0.5f);

        q->w = cr * cp * cy + sr * sp * sy;
        q->x = sr * cp * cy - cr * sp * sy;
        q->y = cr * sp * cy + sr * cp * sy;
        q->z = cr * cp * sy - sr * sp * cy;

            quaternion_normalize(q);
            ESP_LOGE(TAG, "四元数初始化成功！");
    // 返回 1 表示成功完成校准
    return 1;
}


// 消息队列句柄
// QueueHandle_t sensor_data_queue;  // 传感器数据队列句柄
QueueHandle_t quaternion_queue;   // 四元数队列句柄
// 互斥锁句柄
// SemaphoreHandle_t sensor_queue_mutex;  // 传感器数据队列互斥锁句柄
SemaphoreHandle_t quaternion_queue_mutex;  // 四元数队列互斥锁句柄
// 全局的 MahonyAHRSState 变量
MahonyAHRSState state;

// 合并后的任务：获取传感器数据并更新四元数
void sensor_and_quaternion_task(void *pvParameters) {
    float data[6];
    SensorData sensorData;
    Quaternion q;


    while (1) {

        // 获取传感器数据:三轴加速度计单位为G，三轴角速度，单位为dps
        QMI8658A_Get_G_DPS(data);
        sensorData.ax = data[0] * 9.8;
        sensorData.ay = data[1] * 9.8;
        sensorData.az = data[2] * 9.8;
        sensorData.gx = data[3] * PI / 180.0;
        sensorData.gy = data[4] * PI / 180.0;
        sensorData.gz = data[5] * PI / 180.0;

        // 根据陀螺仪数据更新四元数
        MahonyAHRSupdate(&state, &sensorData);

        q.w = state.quaternion.w;
        q.x = state.quaternion.x;
        q.y = state.quaternion.y;
        q.z = state.quaternion.z;

        // 获取四元数队列互斥锁
        if (xSemaphoreTake(quaternion_queue_mutex, pdMS_TO_TICKS(4)) != pdTRUE) {
            ESP_LOGE(TAG, "在合并任务中获取四元数队列互斥锁失败");
            continue;
        }
        // 将四元数发送到消息队列，队列满时覆盖旧数据
        if (xQueueOverwrite(quaternion_queue, &q) != pdPASS) {
            ESP_LOGE(TAG, "在合并任务中覆盖四元数队列失败");
        }
        // 释放四元数队列互斥锁
        if (xSemaphoreGive(quaternion_queue_mutex) != pdTRUE) {
            ESP_LOGE(TAG, "在合并任务中释放四元数队列互斥锁失败");
        }


    }
}

// 任务3：将四元数转换为欧拉角并打印
void euler_angle_print_task(void *pvParameters) {
    Quaternion q;
    float roll, pitch, yaw;

    TickType_t xLastWakeTime;
    vTaskDelay(pdMS_TO_TICKS(1000));
    const TickType_t xFrequency = pdMS_TO_TICKS(100);  // 100ms 的时间间隔
    // 初始化上一次唤醒时间
    xLastWakeTime = xTaskGetTickCount();


    while (1) {
        // 获取四元数队列互斥锁
        if (xSemaphoreTake(quaternion_queue_mutex, pdMS_TO_TICKS(40)) != pdTRUE) {
            ESP_LOGE(TAG, "在欧拉角打印任务中获取四元数队列互斥锁失败");
            continue;
        }
        // 从消息队列接收四元数
        if (xQueueReceive(quaternion_queue, &q, pdMS_TO_TICKS(40)) != pdPASS) {
            ESP_LOGE(TAG, "在欧拉角打印任务中从四元数队列接收数据失败");
            // 释放四元数队列互斥锁
            if (xSemaphoreGive(quaternion_queue_mutex) != pdTRUE) {
                ESP_LOGE(TAG, "在欧拉角打印任务中释放四元数队列互斥锁失败");
            }
            continue;
        }
        // 释放四元数队列互斥锁
        if (xSemaphoreGive(quaternion_queue_mutex) != pdTRUE) {
            ESP_LOGE(TAG, "在欧拉角打印任务中释放四元数队列互斥锁失败");
        }

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

        // // 输出融合后的姿态角
        // printf("%.2f,%.2f,%.2f\n", roll, pitch, yaw);
        printf("%.6f,%.6f,%.6f,%.6f\n", q.w,q.x,q.y,q.z);
        // 等待直到下一个周期
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}

void app_QMI8658A() {
    LED_Init();

    // 创建消息队列  
    quaternion_queue = xQueueCreate(1, sizeof(Quaternion));

    // 创建互斥锁
    quaternion_queue_mutex = xSemaphoreCreateMutex();

    if (quaternion_queue == NULL || quaternion_queue_mutex == NULL) {
        ESP_LOGE(TAG, "创建队列或互斥锁失败");
        return;
    }
    Quaternion q;

    if (!Initialize_Attitude_With_Accelerometer(&q)) {
        ESP_LOGE(TAG, "使用加速度计初始化姿态失败");
        return;
    }
    MahonyAHRSInit(&state);
    state.quaternion.w = q.w;
    state.quaternion.x = q.x;
    state.quaternion.y = q.y;
    state.quaternion.z = q.z;

    // 创建任务并固定到 CPU0
    if (xTaskCreatePinnedToCore(sensor_and_quaternion_task, "传感器和四元数任务", 2*2048, NULL, 5, NULL, 0) != pdPASS) {
        ESP_LOGE(TAG, "创建传感器和四元数任务失败");
    }
    if (xTaskCreatePinnedToCore(euler_angle_print_task, "欧拉角打印任务", 2*2048, NULL, 3, NULL, 0) != pdPASS) {
        ESP_LOGE(TAG, "创建欧拉角打印任务失败");
    }
}