#include "easy_control.h"
#include "component/pid.h"
#include "bsp/pwm.h"


// 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
};

// 初始化卡尔曼滤波器
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;
}

// 全局变量
KalmanFilter roll_kf, pitch_kf;

// 控制器初始化函数
void FlightController_Init(float sample_freq) {
    // 初始化卡尔曼滤波器
    Kalman_Init(&roll_kf);
    Kalman_Init(&pitch_kf);
    
    
    // 初始化PID控制器
    PID_Init(&roll_angle_pid, KPID_MODE_NO_D, sample_freq, &angle_pid_params);
    PID_Init(&pitch_angle_pid, KPID_MODE_NO_D, sample_freq, &angle_pid_params);
    PID_Init(&roll_rate_pid, KPID_MODE_CALC_D, sample_freq, &rate_pid_params);
    PID_Init(&pitch_rate_pid, KPID_MODE_CALC_D, sample_freq, &rate_pid_params);
    PID_Init(&yaw_rate_pid, KPID_MODE_SET_D, sample_freq, &rate_pid_params);
    PID_Init(&altitude_pos_pid, KPID_MODE_NO_D, sample_freq, &alt_pos_pid_params);
	PID_Init(&altitude_vel_pid, KPID_MODE_CALC_D, sample_freq, &alt_vel_pid_params);
    
    // 重置所有PID状态
    PID_Reset(&roll_angle_pid);
    PID_Reset(&pitch_angle_pid);
    PID_Reset(&roll_rate_pid);
    PID_Reset(&pitch_rate_pid);
    PID_Reset(&yaw_rate_pid);
    PID_Reset(&altitude_pos_pid);
	PID_Reset(&altitude_vel_pid);
}

float target_yaw_rate;
float expected_speed;
float expect_thrust;
float target_roll_rate;
float target_pitch_rate;
float base_throttle = 0.4f;    // 基础油门
float target_altitude = 0.0f;  // 目标高度1米
float roll_angle;
float pitch_angle;
// 主控制循环（在固定时间间隔调用	
void FlightController_Update(SensorData sensors, Control_t* output, float dt, PS2_TypeDef rc) {
    // 1. 姿态估计（使用卡尔曼滤波）
     roll_angle = Kalman_Update(&roll_kf, 
                                    atan2f(sensors.accel[1], sensors.accel[2]), 
                                    sensors.gyro[0], 
                                    dt)+0.0302813556;
    
     pitch_angle = Kalman_Update(&pitch_kf, 
                                     atan2f(-sensors.accel[0], sensors.accel[2]), 
                                     sensors.gyro[1], 
                                     dt)+0.0181136467;
    
    // 2. 高度估计（融合加速度计和气压计）
    //AltKalman_Update(&alt_kf, sensors.accel[2], sensors.baro_alt, dt);
    
    // 3. 遥控器输入
//    float target_yaw_rate, target_throttle;
	
    target_altitude += (rc.Rocker_LY-0.5)*0.03;
	if(target_altitude<=-13){target_altitude=-13;}else if(target_altitude>=11){target_altitude=11;}
	
//	expected_speed = 
	
	output->Controltarget.pitch = (rc.Rocker_RY-0.5)*0.349065f;
	output->Controltarget.roll = (rc.Rocker_RX-0.5)*0.1745329f;
	target_yaw_rate = (rc.Rocker_LX-0.5)*0.349065f;
	
    // 4. 高度控制（单环PID）
	expected_speed = PID_Calc(&altitude_pos_pid, target_altitude, -sensors.baro_alt, 0, dt);
    expect_thrust = PID_Calc(&altitude_vel_pid, expected_speed, sensors.speed_z, 0, dt);
    if(expect_thrust<-1.0f){expect_thrust=-1.0f;}
	output->ControlOutput.throttle = base_throttle+expect_thrust;
    // 5. 双环姿态控制
    // 外环：角度控制（输出目标角速度）
//    float target_roll_rate = PID_Calc(&roll_angle_pid, output->Controltarget.roll, roll_angle, 0, dt);
//    float target_pitch_rate = PID_Calc(&pitch_angle_pid, output->Controltarget.pitch, pitch_angle, 0, dt);
	target_roll_rate = PID_Calc(&roll_angle_pid, output->Controltarget.roll, roll_angle, 0, dt);
    target_pitch_rate = PID_Calc(&pitch_angle_pid, output->Controltarget.pitch, pitch_angle, 0, dt);
    
    // 内环：角速度控制（输出控制量）
    output->ControlOutput.roll = 2*PID_Calc(&roll_rate_pid, target_roll_rate, sensors.gyro[0], 0, dt);
    output->ControlOutput.pitch = 2*PID_Calc(&pitch_rate_pid, target_pitch_rate, sensors.gyro[1], 0, dt);
    output->ControlOutput.yaw = 1.5*PID_Calc(&yaw_rate_pid, target_yaw_rate, sensors.gyro[2], 0, dt);
    
    // 6. 输出限幅保护
    output->ControlOutput.throttle = fmaxf(0.0f, fminf(1.0f, output->ControlOutput.throttle));
    output->ControlOutput.roll = fmaxf(-1.0f, fminf(1.0f, output->ControlOutput.roll));
    output->ControlOutput.pitch = fmaxf(-1.0f, fminf(1.0f, output->ControlOutput.pitch));
    output->ControlOutput.yaw = fmaxf(-1.0f, fminf(1.0f, output->ControlOutput.yaw));
	
}

// 重置所有控制器（在模式切换或紧急情况下调用）
void FlightController_Reset() {
    PID_Reset(&roll_angle_pid);
    PID_Reset(&pitch_angle_pid);
    PID_Reset(&roll_rate_pid);
    PID_Reset(&pitch_rate_pid);
    PID_Reset(&yaw_rate_pid);
    PID_Reset(&altitude_pos_pid);
    PID_Reset(&altitude_vel_pid);
    // 重置卡尔曼滤波器
    Kalman_Init(&roll_kf);
    Kalman_Init(&pitch_kf);
    //AltKalman_Init(&alt_kf);
}

/*********************************************************** */
/*                           混控器                           */
/*********************************************************** */

// 全局混控器实例
//static Mixer mixer;
Mixer mixer;
// 初始化混控器
void MotorMixer_Init() {
    // 电机1配置 (前右)
    mixer.motors[0].min_output = 0.1f;  
    mixer.motors[0].max_output = 1.0f;  
    
    // 电机2配置 (前左)
    mixer.motors[1].min_output = 0.1f;
    mixer.motors[1].max_output = 1.0f;
    
    // 电机3配置 (后左)
    mixer.motors[2].min_output = 0.1f;
    mixer.motors[2].max_output = 1.0f;
    
    // 电机4配置 (后右)
    mixer.motors[3].min_output = 0.1f;
    mixer.motors[3].max_output = 1.0f;
    
    // 初始输出为0
    for (int i = 0; i < 4; i++) {
        mixer.motor_output[i] = 0.0f;
    }
}
float mix[4];
// 应用电机输出
void Apply_Motor_Outputs(Control_t ctrl) {
    // 1. 基本混控公式 (X型四轴布局)
    // 电机布局:
    //   [0] 前右 (CW)
    //   [1] 前左 (CCW)
    //   [2] 后左 (CW)
    //   [3] 后右 (CCW)
    
    // 混控公式:
//    float mix[4];
    mix[0] = ctrl.ControlOutput.throttle + ctrl.ControlOutput.roll - ctrl.ControlOutput.pitch + ctrl.ControlOutput.yaw;  // 前右
    mix[1] = ctrl.ControlOutput.throttle - ctrl.ControlOutput.roll - ctrl.ControlOutput.pitch - ctrl.ControlOutput.yaw;  // 前左
    mix[2] = ctrl.ControlOutput.throttle - ctrl.ControlOutput.roll + ctrl.ControlOutput.pitch + ctrl.ControlOutput.yaw;  // 后左
    mix[3] = ctrl.ControlOutput.throttle + ctrl.ControlOutput.roll + ctrl.ControlOutput.pitch - ctrl.ControlOutput.yaw;  // 后右
    
    // 2. 输出限幅和约束
    for (int i = 0; i < 4; i++) {
        // 应用电机特定限幅
        if (mix[i] < mixer.motors[i].min_output) {
            mix[i] = mixer.motors[i].min_output;
        } else if (mix[i] > mixer.motors[i].max_output) {
            mix[i] = mixer.motors[i].max_output;
        }
        
        // 更新当前输出值
        mixer.motor_output[i] = 0.275+0.8*mix[i]*0.21; // 转换为PWM计数值
    }
    
    // 3. 输出到PWM
    BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, mixer.motor_output[0]);
    BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, mixer.motor_output[1]); 
    BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, mixer.motor_output[2]);
    BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, mixer.motor_output[3]);
}

// 安全停止所有电机
void MotorMixer_OutStop() {
	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);
}