#include "quad_pos_control.h"
#include "quad_pos_control_math.h"
#include <math.h>

#define VEL_MAX_XY 5.0f // 最大XY速度
#define VEL_MAX_Z 2.0f  // 最大Z速度
#define CONSTANTS_ONE_G 9.80665f // 重力加速度

// 位置PID控制器
static KPID_t pos_control_xy;
static KPID_t pos_control_z;

// 速度PID控制器
static KPID_t vel_control_xy;
static KPID_t vel_control_z;

// 位置PID参数
static const KPID_Params_t pos_params_xy = {
    .k = 1.0f,
    .p = 0.8f,    // 调整后的位置环P增益
    .i = 0.05f,   // 位置环I增益
    .d = 0.0f,
    .i_limit = 0.5f,
    .out_limit = VEL_MAX_XY,
    .d_cutoff_freq = 0.0f,
    .range = 0.0f
};

static const KPID_Params_t pos_params_z = {
    .k = 1.0f,
    .p = 1.2f,    // Z轴更强的P增益
    .i = 0.1f,    // Z轴I增益
    .d = 0.0f,
    .i_limit = 0.5f,
    .out_limit = VEL_MAX_Z,
    .d_cutoff_freq = 0.0f,
    .range = 0.0f
};

// 速度控制参数
static const VelControlParams_t vel_params = {
    .xy = {
        .k = 1.0f,
        .p = 1.5f,    // 速度环XY P增益
        .i = 0.2f,     // XY I增益
        .d = 0.02f,    // XY D增益
        .i_limit = 1.0f,
        .out_limit = 5.0f, // 加速度限制
        .d_cutoff_freq = 20.0f,
        .range = 0.0f
    },
    .z = {
        .k = 1.0f,
        .p = 2.0f,     // Z轴更强的P增益
        .i = 0.3f,     // Z轴I增益
        .d = 0.04f,    // Z轴D增益
        .i_limit = 1.0f,
        .out_limit = 3.0f, // 加速度限制
        .d_cutoff_freq = 20.0f,
        .range = 0.0f
    },
    .hover_thrust = 0.6f,  // 悬停推力
    .thr_min = 0.1f,       // 最小推力
    .thr_max = 0.9f,       // 最大推力
    .thr_xy_margin = 0.1f, // 水平裕度
    .g = CONSTANTS_ONE_G
};

// 辅助函数
float constrain(float val, float min_val, float max_val) {
    return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
}

float vector3f_norm_sq(float x, float y, float z) {
    return x*x + y*y + z*z;
}

void QuadPosControl_Init(void) {
    // 初始化位置PID控制器
    PID_Init(&pos_control_xy, KPID_MODE_NO_D, 100.0f, &pos_params_xy);
    PID_Init(&pos_control_z, KPID_MODE_NO_D, 100.0f, &pos_params_z);
    
    // 初始化速度PID控制器
    PID_Init(&vel_control_xy, KPID_MODE_SET_D, 100.0f, &vel_params.xy);
    PID_Init(&vel_control_z, KPID_MODE_SET_D, 100.0f, &vel_params.z);
}

void PositionControl(QuadPosition_t *quad_pos, float dt) {
    // 计算位置误差并输出速度指令
    quad_pos->vel_desire.x = quad_pos->vel_forward.x + 
                             PID_Calc(&pos_control_xy, quad_pos->pos_desire.x, 
                                      quad_pos->pos_now.x, 0.0f, dt);
    
    quad_pos->vel_desire.y = quad_pos->vel_forward.y + 
                             PID_Calc(&pos_control_xy, quad_pos->pos_desire.y, 
                                      quad_pos->pos_now.y, 0.0f, dt);
    
    quad_pos->vel_desire.z = quad_pos->vel_forward.z + 
                             PID_Calc(&pos_control_z, quad_pos->pos_desire.z, 
                                      quad_pos->pos_now.z, 0.0f, dt);

    // 限制水平速度
    Vector2f vel_xy = {quad_pos->vel_desire.x, quad_pos->vel_desire.y};
    Vector2f constrained_xy = constrainXY(&vel_xy, &(Vector2f){0,0}, VEL_MAX_XY);
    
    quad_pos->vel_desire.x = constrained_xy.x;
    quad_pos->vel_desire.y = constrained_xy.y;
    
    // 限制垂直速度
    quad_pos->vel_desire.z = constrain(quad_pos->vel_desire.z, -VEL_MAX_Z, VEL_MAX_Z);
}

void VelocityControl(QuadPosition_t *quad_pos, float dt) {
    const VelControlParams_t *p = &vel_params;
    const float g = p->g;
    const float hover_thrust = p->hover_thrust;
    
    // 1. 计算速度环PID输出 (加速度指令)
    float acc_sp_x = PID_Calc(&vel_control_xy, quad_pos->vel_desire.x, 
                             quad_pos->vel_now.x, quad_pos->vel_dot.x, dt);
    
    float acc_sp_y = PID_Calc(&vel_control_xy, quad_pos->vel_desire.y, 
                             quad_pos->vel_now.y, quad_pos->vel_dot.y, dt);
    
    float acc_sp_z = PID_Calc(&vel_control_z, quad_pos->vel_desire.z, 
                             quad_pos->vel_now.z, quad_pos->vel_dot.z, dt);
    
    // 2. 转换为推力设定值
    quad_pos->thr_sp.x = acc_sp_x * (hover_thrust / g);
    quad_pos->thr_sp.y = acc_sp_y * (hover_thrust / g);
    quad_pos->thr_sp.z = (acc_sp_z + g) * (hover_thrust / g);
    
    // 3. 垂直推力限制
    quad_pos->thr_sp.z = constrain(quad_pos->thr_sp.z, p->thr_min, p->thr_max);
    
    // 4. 水平推力限制
    float thr_sp_xy_norm = sqrtf(quad_pos->thr_sp.x*quad_pos->thr_sp.x + 
                                 quad_pos->thr_sp.y*quad_pos->thr_sp.y);
    
    float thr_max_xy_sq = p->thr_max*p->thr_max - quad_pos->thr_sp.z*quad_pos->thr_sp.z;
    
    if (thr_sp_xy_norm > 0 && thr_max_xy_sq > 0) {
        float thr_max_xy = sqrtf(thr_max_xy_sq) - p->thr_xy_margin;
        
        if (thr_sp_xy_norm > thr_max_xy) {
            float scale = thr_max_xy / thr_sp_xy_norm;
            quad_pos->thr_sp.x *= scale;
            quad_pos->thr_sp.y *= scale;
        }
    }
    
    // 5. 抗饱和处理 (使用现有PID的内部机制)
    // 通过PID参数中的i_limit和out_limit实现
   
}

// 主控制循环
void ControlLoop(QuadPosition_t *quad_pos, float dt) {
    PositionControl(quad_pos, dt);  // 位置环
    VelocityControl(quad_pos, dt);  // 速度环
}