#pragma once

#ifdef __cplusplus
extern "C" {
#endif
#include "device/at9s_pro.h"
#include "component/pid.h"
#include "component/ahrs.h"
// 卡尔曼滤波器结构体
typedef struct {
    float Q_angle;     // 过程噪声协方差（角度）
    float Q_bias;      // 过程噪声协方差（陀螺仪偏置）
    float R_measure;   // 测量噪声协方差
    float angle;       // 估计角度
    float bias;        // 估计陀螺仪偏置
    float P[2][2];     // 误差协方差矩阵
} KalmanFilter;

typedef enum {
    JOYSTICKDETECTION,
    UNLOCKMOTOR,
    RUNNING
} Quad_CtrlFSM;

typedef struct {
    uint16_t pos_alt;
    uint16_t pos_vel;
    uint16_t att_agl;
    uint16_t att_omg;
}Quad_CtrlFreqParams_t;

typedef struct {
    LowPassFilter2p_t accl_x;
    LowPassFilter2p_t accl_y;
    LowPassFilter2p_t accl_z;
    LowPassFilter2p_t gyro_x;
    LowPassFilter2p_t gyro_y;
    LowPassFilter2p_t gyro_z;
    LowPassFilter2p_t eulr_yaw;
    LowPassFilter2p_t eulr_pit;
    LowPassFilter2p_t eulr_rol;
    LowPassFilter2p_t alt;
    LowPassFilter2p_t vel_z;
    LowPassFilter2p_t output_pit;

    LowPassFilter2p_t motor[4];
}Quad_LowPassFilter2p_t;

typedef struct {
    KPID_Params_t pit_agl;
    KPID_Params_t rol_agl;
    KPID_Params_t yaw_agl;
    KPID_Params_t pit_omg;
    KPID_Params_t rol_omg;
    KPID_Params_t yaw_omg;
    KPID_Params_t alt_pos;
    KPID_Params_t alt_vel;
}Quad_PIDParams_t;

typedef struct {
    KPID_t rol_omg, rol_agl;        // rol轴PID 
    KPID_t pit_omg, pit_agl;        // rol轴PID 
    KPID_t yaw_omg, yaw_agl;        // rol轴PID
    KPID_t alt_pos, alt_vel;        // 高度环PID
}Quad_PID_t;

typedef struct {
    KalmanFilter rol_kf, pit_kf;
}Quad_Kalman_t;

// 电机配置结构体
typedef struct {
    float min_output;          // 最小输出值 (0.0-1.0)
    float max_output;          // 最大输出值 (0.0-1.0)
}Quad_MotorConfig_t;
typedef struct {
	bool online;
	bool start;
	bool stop;
	bool lockoutput;
	
    float throttle;
    float yaw;
    float rol;
    float pit;
	
}Quad_CMD_t;

typedef struct{
    AHRS_Eulr_t eulr;
}Quad_CalibrationStatus_t;

typedef struct{
    AHRS_Accl_t accl;    // m/s²
    AHRS_Gyro_t gyro;
    AHRS_Eulr_t eulr;
    float alt;           // 气压计加速度计融合后相对高度 (m)
	float vel_z;	        //z轴速度
}Quad_CurrentStatus_t;

typedef struct {
    float yaw_agl;
    float rol_agl;
    float pit_agl;

    float yaw_omg;
    float rol_omg;
    float pit_omg;

    float vel; 
    float thrust;

    float alt;
}Quad_ExpectStatus_t;

typedef struct{
    float yaw_omg;
    float rol_agl;
    float pit_agl;
}Quad_ExpectStatusLimit_t;

typedef struct {

    float throttle;
    float rol;
    float pit;
    float yaw;

    float mix[4];     // 混控计算中间变量
    float motor[4];  // 电机输出值 (0.0-1.0)
}Quad_Output_t;
typedef struct {
    Quad_CtrlFreqParams_t ctrl_freq; // 控制频率参数

    float baseThrottle;        // 基础油门值

    Quad_PIDParams_t pid; // PID参数
     // 低通滤波器参数

    Quad_ExpectStatusLimit_t expect_status_limit;

    Quad_MotorConfig_t motor_limit;     // 四个电机的配置
}Quad_Params_t;

typedef struct{
    int64_t now;
    float dt;
    int64_t last;

    uint64_t timer_ms;
    Quad_CtrlFSM fsm_states;
    
	
    Quad_Params_t *param;

    Quad_PID_t pid;
    Quad_LowPassFilter2p_t filter;
    Quad_Kalman_t kalman;

    Quad_CalibrationStatus_t calibration_status;
    Quad_CurrentStatus_t current_status;
    Quad_ExpectStatus_t expect_status;

    Quad_Output_t output;
}Quad_t;

int8_t Quad_Init(Quad_t *q, Quad_Params_t *param, Quad_CMD_t *cmd, float freq);
int8_t Quad_Ctrl(Quad_t *q, Quad_CMD_t *cmd);
void   Quad_stop();
#ifdef __cplusplus
}
#endif