#include "PID.h"
#include "main.h"
#include "M3508.h"
#include <math.h>  // 用于isnan、isinf函数
#include "Remote.h"
#include "CToC.h"
/**
 * @brief PID控制器初始化
 */
void PID_Init(PID_Controller_t* pid, float kp, float ki, float kd, float integral_limit, float output_limit)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->output = 0.0f;
    pid->integral_limit = integral_limit;
    pid->output_limit = output_limit;
}

/**
 * @brief PID计算函数（带保护）
 */
float PID_Calculate(PID_Controller_t* pid, float error, float dt)
{
    // 检查输入参数有效性
    if (dt <= 0.0f || dt > 1.0f) {
        dt = 0.01f;  // 使用默认时间间隔
    }
    
    // 检查error是否为NaN或无穷大
    if (isnan(error) || isinf(error)) {
        error = 0.0f;
    }
    
    // 比例项
    float proportional = pid->kp * error;
    
    // 检查proportional是否为NaN
    if (isnan(proportional) || isinf(proportional)) {
        proportional = 0.0f;
    }
    
    // 积分项（带限幅）
    pid->integral += error * dt;
    if (pid->integral > pid->integral_limit) {
        pid->integral = pid->integral_limit;
    } else if (pid->integral < -pid->integral_limit) {
        pid->integral = -pid->integral_limit;
    }
    float integral = pid->ki * pid->integral;
    
    // 检查integral是否为NaN
    if (isnan(integral) || isinf(integral)) {
        integral = 0.0f;
        pid->integral = 0.0f;
    }
    
    // 微分项（添加dt保护）
    float derivative = 0.0f;
    if (dt > 0.001f) {  // 确保dt足够大，避免除以很小的数
        derivative = pid->kd * (error - pid->prev_error) / dt;
    }
    
    // 检查derivative是否为NaN
    if (isnan(derivative) || isinf(derivative)) {
        derivative = 0.0f;
    }
    
    pid->prev_error = error;
    
    // 计算总输出
    pid->output = proportional + integral + derivative;
    
    // 检查总输出是否为NaN
    if (isnan(pid->output) || isinf(pid->output)) {
        pid->output = 0.0f;
        // 重置PID状态
        pid->integral = 0.0f;
        pid->prev_error = 0.0f;
    }
    
    // 输出限幅
    if (pid->output > pid->output_limit) {
        pid->output = pid->output_limit;
    } else if (pid->output < -pid->output_limit) {
        pid->output = -pid->output_limit;
    }
    
    return pid->output;
}
/**
 * @brief 初始化所有电机的PID控制器
 */
void Motor_PID_Init(void)
{
    // PID参数（需要根据实际电机调试）
    float speed_kp = 400.0f;      // 速度环比例系数
    float speed_ki = 0.05f;      // 速度环积分系数
    float speed_kd = 300.0f;      // 速度环微分系数
    
    for (int i = 0; i < 4; i++) {
        PID_Init(&motor_controller[i].speed_pid, speed_kp, speed_ki, speed_kd, 3000.0f, 3000.0f);
    }
}

/*
 *函数简介:位置式PID清理
 *参数说明:位置式PID参数结构体
 *返回类型:无
 *备注:重置PID内部状态，用于模式切换或异常情况
 */
void PID_PositionClean(PID_Controller_t* pid)
{
    pid->integral = 0.0f;      // 清理积分项，防止积分饱和
    pid->prev_error = 0.0f;    // 清理上一次误差，防止微分冲击
    pid->output = 0.0f;        // 清理输出值
}