/* Includes ----------------------------------------------------------------- */
#include "xm_quadctrl.h"
#include "component/pid.h"
#include "bsp/pwm.h"
#include "bsp/time.h"
#include "module/HeightEstimation.h"
#include "component/filter.h"
// 电机布局:
//   [0] 前右 (CW)
//   [1] 前左 (CCW)
//   [2] 后左 (CW)
//   [3] 后右 (CCW)



//低通滤波初始化和计算











//// PID控制器实例
//KPID_t roll_angle_pid, pitch_angle_pid;  // 角度环PID (外环)
//KPID_t roll_rate_pid, pitch_rate_pid;    // 角速度环PID (内环)
//KPID_t yaw_rate_pid;                     // 偏航角速度环PID
//KPID_t altitude_pos_pid;                     // 高度环PID
//KPID_t altitude_vel_pid;                     // 高度环PID
//// PID参数配置
//KPID_Params_t angle_pid_params = {
//    .k = 1.0f,          // 控制器增益
//    .p = 0.8f,          // 比例增益
//    .i = 0.05f,         // 积分增益
//    .d = 0.0f,          // 微分增益 (角度环通常不需要D)
//    .i_limit = 0.3f,     // 积分限幅
//    .out_limit = 1.0f,   // 输出限幅
//    .d_cutoff_freq = 0.0f, // 截止频率(不使用)
//    .range = 0.0f        // 误差范围(不使用)
//};
//KPID_Params_t rate_pid_params = {
//    .k = 1.0f,
//    .p = 0.25f,          // 比例增益
//    .i = 0.05f,         // 积分增益
//    .d = 0.01f,         // 微分增益
//    .i_limit = 0.05f,
//    .out_limit = 0.2f,
//    .d_cutoff_freq = 30.0f, // 微分项低通截止频率(Hz)
//    .range = 0.0f
//};
//KPID_Params_t alt_pos_pid_params = {
//    .k = 0.25f,
//    .p = 1.0f,          // 比例增益
//    .i = 0.05f,          // 积分增益
//    .d = 0.00f,          // 微分增益
//    .i_limit = 0.5f,
//    .out_limit = 3.0f,   // 输出限幅(油门变化量)
//    .d_cutoff_freq = 0.0f, // 截止频率(Hz)
//    .range = 0.0f
//};
//KPID_Params_t alt_vel_pid_params = {
//    .k = 0.08f,
//    .p = 2.2f,          // 比例增益
//    .i = 0.05f,          // 积分增益
//    .d = 0.01f,          // 微分增益
//    .i_limit = 0.05f,
//    .out_limit = 0.2f,   // 输出限幅(油门变化量)
//    .d_cutoff_freq = -1.0f, // 截止频率(Hz)
//    .range = 0.0f
//};

/* Private typedef ---------------------------------------------------------- */

/* Private define ----------------------------------------------------------- */

/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
static inline void Quad_Math_LimitFloat(float* value, float min, float max) {
    if (*value < min) {
        *value = min;
    } else if (*value > max) {
        *value = max;
    }
}

static inline void Quad_Math_ScaleSumTo1(float *a, float *b, float *c, float *d) {
    float sum = *a + *b + *c + *d;
    if (sum > 1.0f) {
        float scale = 1.0f / sum;
        *a *= scale;
        *b *= scale;
        *c *= scale;
        *d *= scale;
    }
}

// 初始化卡尔曼滤波器
void Kalman_Init(KalmanFilter* kf) {
    kf->Q_angle = 0.001f;
    kf->Q_bias = 0.003f;
    kf->R_measure = 0.03f;
    kf->angle = 0.0f;
    kf->bias = 0.0f;
    kf->P[0][0] = 0.0f;
    kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f;
    kf->P[1][1] = 0.0f;
}
// 卡尔曼滤波器更新
float Kalman_Update(KalmanFilter* kf, float newAngle, float newRate, float dt) {
    // 预测步骤
    kf->angle += dt * (newRate - kf->bias);
    kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
    kf->P[0][1] -= dt * kf->P[1][1];
    kf->P[1][0] -= dt * kf->P[1][1];
    kf->P[1][1] += kf->Q_bias * dt;
    // 更新步骤
    float y = newAngle - kf->angle;
    float S = kf->P[0][0] + kf->R_measure;
    float K[2];
    K[0] = kf->P[0][0] / S;
    K[1] = kf->P[1][0] / S;
    // 更新状态和协方差
    kf->angle += K[0] * y;
    kf->bias += K[1] * y;
    float P00_temp = kf->P[0][0];
    float P01_temp = kf->P[0][1];
    kf->P[0][0] -= K[0] * P00_temp;
    kf->P[0][1] -= K[0] * P01_temp;
    kf->P[1][0] -= K[1] * P00_temp;
    kf->P[1][1] -= K[1] * P01_temp;
    return kf->angle;
}

static bool Quad_UnlockDetection(Quad_t *q, Quad_CMD_t *cmd){
    static bool arm_flag=false;
    float thr = cmd->throttle;
    float yaw = cmd->yaw;
    float pit = cmd->pit;
    float rol = cmd->rol;
    /* 2. 内八判定 */
    bool inside_8 = (thr < 0.1f) &&
                    (yaw > 0.9f) &&
                    (pit < -0.9f) &&
                    (rol < -0.9f);

    uint32_t now_ms = BSP_TIME_Get_us() / 1000.0f;

    if (!inside_8) {                 // 一旦出界，计时清零
        q->timer_ms = 0;
        return arm_flag;
    }

    if (q->timer_ms == 0)        // 第一次进入
        q->timer_ms = now_ms;

    if ((now_ms - q->timer_ms) >= 1000 && !arm_flag) {
        arm_flag = true;         
        q->timer_ms = 0;
    }
    return arm_flag;
}

static bool Quad_OutZeroDetection(Quad_CMD_t *cmd)
{
    return (cmd->throttle < 0.05f) &&
           (fabsf(cmd->yaw)   < 0.05f) &&
           (fabsf(cmd->pit)   < 0.05f) &&
           (fabsf(cmd->rol)   < 0.05f);
}

int8_t Quad_UpdateStatus(Quad_t *q) {
    // 1. 姿态估计（使用卡尔曼滤波）精度不好
//    q->current_status.eulr.rol = Kalman_Update(&q->kalman.rol_kf, 
//                                    atan2f(q->current_status.accl.y, q->current_status.accl.z), 
//                                    q->current_status.gyro.x, 
//                                    q->dt);
//    
//    q->current_status.eulr.pit = Kalman_Update(&q->kalman.pit_kf, 
//                                    atan2f(-q->current_status.accl.x, q->current_status.accl.z), 
//                                    q->current_status.gyro.y, 
//                                    q->dt);
    LowPassFilter2p_Apply(&q->filter.gyro_x,q->current_status.gyro.x);
    LowPassFilter2p_Apply(&q->filter.gyro_y,q->current_status.gyro.y);
    LowPassFilter2p_Apply(&q->filter.gyro_z,q->current_status.gyro.z);
    LowPassFilter2p_Apply(&q->filter.eulr_rol,q->current_status.eulr.rol);
    LowPassFilter2p_Apply(&q->filter.eulr_pit,q->current_status.eulr.pit);
    LowPassFilter2p_Apply(&q->filter.alt,q->current_status.alt);
    LowPassFilter2p_Apply(&q->filter.vel_z,q->current_status.vel_z);
    return 0; 
}

int8_t Quad_SetTargetStates(Quad_t *q, Quad_CMD_t *cmd) {

    q->expect_status.vel     = cmd->throttle;
    q->expect_status.yaw_omg = cmd->yaw;
    q->expect_status.rol_agl = cmd->rol+q->calibration_status.eulr.rol;
    q->expect_status.pit_agl = cmd->pit+q->calibration_status.eulr.pit;

    Quad_Math_LimitFloat(&q->expect_status.yaw_omg,
                        -q->param->expect_status_limit.yaw_omg, 
                        q->param->expect_status_limit.yaw_omg);
    Quad_Math_LimitFloat(&q->expect_status.rol_agl,
                        q->expect_status.rol_agl-q->param->expect_status_limit.rol_agl, 
                        q->expect_status.rol_agl+q->param->expect_status_limit.rol_agl);
    Quad_Math_LimitFloat(&q->expect_status.pit_agl,
                        -q->param->expect_status_limit.pit_agl, 
                        q->param->expect_status_limit.pit_agl);
    return 0; 
}

//位置控制
int8_t Quad_PosCtrl(Quad_t *q,float dt) {

//    q->expect_status.vel=PID_Calc(&q->pid.alt_pos, 
//                                    q->expect_status.alt,   
//                                    q->current_status.alt, 
//                                    0, 
//                                    dt);
    q->expect_status.thrust=PID_Calc(&q->pid.alt_vel, 
                                    q->expect_status.vel,              
                                    q->current_status.vel_z, 
                                    0, 
                                    dt);
    Quad_Math_LimitFloat(&q->expect_status.thrust, -1.0f, 1.0f);
    q->output.throttle=q->param->baseThrottle
                        +q->expect_status.thrust;
    return 0; 
}
//姿态控制
int8_t Quad_AttCtrl_agl(Quad_t *q, float dt) {
    //角度控制
	q->expect_status.rol_omg = PID_Calc(&q->pid.rol_agl, 
                                        q->expect_status.rol_agl, 
                                        q->current_status.eulr.rol, 
                                        0, 
                                        q->dt);
    q->expect_status.pit_omg = PID_Calc(&q->pid.pit_agl, 
                                        q->expect_status.pit_agl, 
                                        q->current_status.eulr.pit, 
                                        0, 
                                        q->dt);
	return 0;
}

int8_t Quad_AttCtrl_omg(Quad_t *q, float dt) {
    //角速度控制
    q->output.pit = PID_Calc(&q->pid.pit_omg, 
                            q->expect_status.pit_omg, 
                            q->current_status.gyro.x, 
                            0, 
                            q->dt);
    q->output.yaw = PID_Calc(&q->pid.yaw_omg, 
                            q->expect_status.yaw_omg, 
                            -q->current_status.gyro.z, 
                            0, 
                            q->dt);
    q->output.rol = PID_Calc(&q->pid.rol_omg,
                            q->expect_status.rol_omg, 
                            -q->current_status.gyro.y,
                            0, 
                            q->dt);
    LowPassFilter2p_Apply(&q->filter.output_pit,q->output.pit);
    return 0; 
}

int8_t Quad_Mixer(Quad_t *q) {

    Quad_Math_ScaleSumTo1(&q->output.throttle, &q->output.rol, &q->output.pit, &q->output.yaw);

    q->output.mix[0] = q->output.throttle - q->output.rol - q->output.pit - q->output.yaw;// 前右
    q->output.mix[1] = q->output.throttle + q->output.rol - q->output.pit + q->output.yaw;// 前左
    q->output.mix[2] = q->output.throttle + q->output.rol + q->output.pit - q->output.yaw;// 后左
    q->output.mix[3] = q->output.throttle - q->output.rol + q->output.pit + q->output.yaw;// 后右
    
    for(int i=0;i<4;i++){
    Quad_Math_LimitFloat(&q->output.mix[i], q->param->motor_limit.min_output, q->param->motor_limit.max_output);
    q->output.motor[i] = 0.275+q->output.mix[i]*0.21;
    LowPassFilter2p_Apply(&q->filter.motor[i],q->output.motor[i]);
    }

    return 0; 
}

int8_t Quad_OutPut(Quad_Output_t *output){

    if(output == NULL){
        return -1;
    }
    BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, output->motor[0]);
    BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, output->motor[1]); 
    BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, output->motor[2]);
    BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, output->motor[3]);
	return 0;
}

int8_t Quad_ResetStatus(Quad_CurrentStatus_t *currentStatus) {

    return 0; // 返回0表示成功
}

int8_t Quad_ResetOutput(Quad_Output_t *output) {

    for (int i = 0; i < 4; i++) {
        output->motor[i] = 0.0f;
    }
    return 0; // 返回0表示成功
}
/* Exported functions ------------------------------------------------------- */

int8_t Quad_Init(Quad_t *q, Quad_Params_t *param, Quad_CMD_t *cmd, float freq) {
    
    if(param == NULL) {
        return -1; // 参数错误
    }
    q->param = param;
    // 初始化卡尔曼滤波器
    Kalman_Init(&q->kalman.rol_kf);
    Kalman_Init(&q->kalman.pit_kf);
    
    uint16_t pos_alt=param->ctrl_freq.pos_alt;
    uint16_t pos_vel=param->ctrl_freq.pos_vel;
    uint16_t att_agl=param->ctrl_freq.att_agl;
    uint16_t att_omg=param->ctrl_freq.att_omg;

    LowPassFilter2p_Init(&q->filter.accl_x, freq, 30.0f);
    LowPassFilter2p_Init(&q->filter.accl_y, freq, 30.0f);
    LowPassFilter2p_Init(&q->filter.accl_z, freq, 30.0f);
    LowPassFilter2p_Init(&q->filter.gyro_x, freq, 20.0f);
    LowPassFilter2p_Init(&q->filter.gyro_y, freq, 20.0f);
    LowPassFilter2p_Init(&q->filter.gyro_z, freq, 20.0f);
    LowPassFilter2p_Init(&q->filter.eulr_yaw, freq, 20.0f);
    LowPassFilter2p_Init(&q->filter.eulr_pit, freq, 10.0f);
    LowPassFilter2p_Init(&q->filter.eulr_rol, freq, 10.0f);
    LowPassFilter2p_Init(&q->filter.alt, freq, 10.0f);
    LowPassFilter2p_Init(&q->filter.vel_z, freq, 15.0f);
    LowPassFilter2p_Init(&q->filter.output_pit, freq, 5.0f);
    for(int i=0;i<4;i++)
    LowPassFilter2p_Init(&q->filter.motor[i],freq,30.0f);


    PID_Init(&q->pid.yaw_agl, KPID_MODE_CALC_D, att_agl, &param->pid.yaw_agl);
    PID_Init(&q->pid.pit_agl, KPID_MODE_CALC_D, att_agl, &param->pid.pit_agl);
    PID_Init(&q->pid.rol_agl, KPID_MODE_CALC_D, att_agl, &param->pid.rol_agl);
    PID_Init(&q->pid.yaw_omg, KPID_MODE_CALC_D, att_omg, &param->pid.yaw_omg);
    PID_Init(&q->pid.pit_omg, KPID_MODE_CALC_D, att_omg, &param->pid.pit_omg);
    PID_Init(&q->pid.rol_omg, KPID_MODE_CALC_D, att_omg, &param->pid.rol_omg);
    PID_Init(&q->pid.alt_pos, KPID_MODE_CALC_D, pos_alt, &param->pid.alt_pos);
	PID_Init(&q->pid.alt_vel, KPID_MODE_CALC_D, pos_vel, &param->pid.alt_vel);

    q->expect_status.alt = q->current_status.alt;
    q->expect_status.pit_agl = q->current_status.eulr.pit;
    q->expect_status.rol_agl = q->current_status.eulr.rol;
    q->expect_status.yaw_agl = q->current_status.eulr.yaw;
    q->calibration_status.eulr.rol = q->current_status.eulr.rol;
    q->calibration_status.eulr.pit = q->current_status.eulr.pit;
    q->calibration_status.eulr.yaw = q->current_status.eulr.yaw;

    BSP_PWM_Start(BSP_PWM_MOTOR_OUT1);
    BSP_PWM_Start(BSP_PWM_MOTOR_OUT2);
    BSP_PWM_Start(BSP_PWM_MOTOR_OUT3);
    BSP_PWM_Start(BSP_PWM_MOTOR_OUT4);
	
	
    return 0; 
}
uint16_t dt;
int8_t Quad_Ctrl(Quad_t *q, Quad_CMD_t *cmd) {

    if(q == NULL) {
        return -1; // 参数错误
    }

    q->now =BSP_TIME_Get_us() / 1000000.0f;
    q->dt  =(BSP_TIME_Get_us() - q->last) / 1000000.0f;
    q->last=BSP_TIME_Get_us();	 

    switch(q->fsm_states){
        case JOYSTICKDETECTION:
			BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, 0.275+0.21*cmd->throttle);
            BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, 0.275+0.21*cmd->throttle); 
            BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, 0.275+0.21*cmd->throttle);
            BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, 0.275+0.21*cmd->throttle);
            if(!Quad_UnlockDetection(q,cmd)){
                Quad_ResetOutput(&q->output);
                return -1;
            }
            q->fsm_states=UNLOCKMOTOR;
            q->timer_ms=q->now;
        case UNLOCKMOTOR:
            BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, 0.275);
            BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, 0.275); 
            BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, 0.275);
            BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, 0.275);
            dt=1000*q->now-q->timer_ms;
            if(dt>2000){
                q->fsm_states=RUNNING;
                q->timer_ms=1000*q->now;
            }
        break;
        case RUNNING:
            Quad_UpdateStatus(q);
            Quad_SetTargetStates(q, cmd);

            static uint8_t cnt_1ms = 0;
            cnt_1ms = (cnt_1ms + 1) % 10;       

            /* 位置环 100 Hz */
            if (cnt_1ms == 0) {
                Quad_PosCtrl(q,10*q->dt);
            }

            /* 角度环 250 Hz */
            if (cnt_1ms % 4 == 0) {
                Quad_AttCtrl_agl(q,4*q->dt);
            }

            /* 角速度环 1000 Hz */
            Quad_AttCtrl_omg(q,q->dt);

            /* 混控 & 输出（1000 Hz） */
            Quad_Mixer(q);

            if(cmd->lockoutput){
            	for(int i=0;i<4;i++){
            	q->output.motor[i]=0;
            	}
            }else if(cmd->stop){
            	Quad_stop();
            }

            Quad_OutPut(&q->output);
            return 0; 
                break;
    }
}

void Quad_stop() {
	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT1);
	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT2);
	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT3);
	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT4);
}
