#include "app_config.h"

#include "app_pid.h"




// PID控制器初始化
void initializePid(PID_t *pid, pid_mode_t mode, float dtMin)
{
    pid->mode = mode;
    pid->dt_min = dtMin > SIGMA ? dtMin : SIGMA;
    pid->kp = 0.0f;
    pid->ki = 0.0f;
    pid->kd = 0.0f;
    pid->integral = 0.0f;
    pid->integral_limit = 0.0f;
    pid->output_limit = 0.0f;
    pid->error_previous = 0.0f;
    pid->last_output = 0.0f;
}

// 设置PID参数
int32_t setPidParameters(PID_t *pid, float kp, float ki, float kd, float integralLimit, float outputLimit)
{
    int32_t ret = 0;

    if (isfinite(kp)) {
        pid->kp = kp;
    } else {
        ret = -1;
    }

    if (isfinite(ki)) {
        pid->ki = ki;
    } else {
        ret = -2;
    }

    if (isfinite(kd)) {
        pid->kd = kd;
    } else {
        ret = -3;
    }

    if (isfinite(integralLimit)) {
        pid->integral_limit = integralLimit;
    } else {
        ret = -4;
    }

    if (isfinite(outputLimit)) {
        pid->output_limit = outputLimit;
    } else {
        ret = -5;
    }

    return ret;
}

/**
 * @brief 计算PID控制器的输出
 *
 * @param pid 指向PID_t结构体的指针，包含PID控制器的状态和参数
 * @param sp  设定值(Setpoint)，期望系统达到的目标值
 * @param val 当前值(Current Value)，系统的实际测量值
 * @param val_dot 当前值的导数(Derivative of Current Value)，即测量值的变化率（用于微分项计算）
 * @param dt  时间增量(Time Increment)，两次调用之间的时间间隔，用于计算积分和微分
 *
 * @return float 返回PID控制器计算出的输出值
 */
float calculatePidOutput(PID_t *pid, float sp, float val, float val_dot, float dt)
{
    // 检查输入参数的有效性
    if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt) || dt < pid->dt_min) {
        return pid->last_output;
    }

    // 计算误差
    float error = sp - val;

    // 根据模式计算微分项
    float derivative = 0.0f;
    switch (pid->mode) {
        case PID_MODE_DERIVATIVE_CALC:
            derivative = (error - pid->error_previous) / dt;
            pid->error_previous = error;
            break;
        case PID_MODE_DERIVATIVE_CALC_NO_SP:
            derivative = (-val - pid->error_previous) / dt;
            pid->error_previous = -val;
            break;
        case PID_MODE_DERIVATIVE_SET:
            derivative = -val_dot;
            break;
        default:
            derivative = 0.0f;
            break;
    }

    // 计算比例和微分项的输出
    float output = (pid->kp * error) + (pid->kd * derivative);

    // 计算积分项，并检查积分饱和
    if (pid->ki > SIGMA) {
        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;
        }
        output += pid->ki * pid->integral;
    }

    // 限制输出范围
    if (output > pid->output_limit) {
        output = pid->output_limit;
    } else if (output < -pid->output_limit) {
        output = -pid->output_limit;
    }

    pid->last_output = output;
    return output;
}

// 重置积分器
void resetPidIntegral(PID_t *pid)
{
    pid->integral = 0.0f;
}