/**
  ******************************************************************************
  * @file           : studio_pid_c.c
  * @author         : wangyingjie
  * @brief          : None
  * @attention      : None
  * @date           : 2025/5/23
  ******************************************************************************
  */

#include "studio_pid_c.h"


// 初始化PID结构体
void init_pid(studio_pid *pid, double init_v, double goal_v, double p, double i, double d,
              double integral_limit_val, double output_limit_val)
{
    pid->initval = init_v;
    pid->goal = goal_v;
    pid->Kp = p;
    pid->Ki = i;
    pid->Kd = d;
    pid->error = goal_v - init_v;
    pid->prev_error = pid->error;
    pid->inte_error = 0.0;
    pid->dif_error = 0.0;
    pid->integral_limit = integral_limit_val;
    pid->output_limit = output_limit_val;
}

// 计算PID输出
double compute_pid(studio_pid *pid, double current_value)
{
    // 计算当前误差 CTE
    double cte = pid->goal - current_value;

    // 更新积分和微分项
    pid->inte_error += cte; // 积分项
    pid->dif_error = cte - pid->prev_error; // 微分项
    pid->prev_error = cte;

    // 应用积分限幅
    if (pid->inte_error > pid->integral_limit)
    {
        pid->inte_error = pid->integral_limit;
    } else if (pid->inte_error < -pid->integral_limit)
    {
        pid->inte_error = -pid->integral_limit;
    }
    // 计算PID输出
    double output = pid->Kp * cte + pid->Ki * pid->inte_error + pid->Kd * pid->dif_error;
    // 应用输出限幅
    if (output > pid->output_limit)
    {
        output = pid->output_limit;
    } else if (output < -pid->output_limit)
    {
        output = -pid->output_limit;
    }
    return output;
}

// 设置PID参数
void set_pid_params(studio_pid *pid, double p, double i, double d)
{
    pid->Kp = p;
    pid->Ki = i;
    pid->Kd = d;
}

// 设置积分限幅
void set_integral_limit(studio_pid *pid, double limit)
{
    pid->integral_limit = limit;
}

// 设置输出限幅
void set_output_limit(studio_pid *pid, double limit)
{
    pid->output_limit = limit;
}

// 获取目标值
double get_goal(const studio_pid *pid)
{
    return pid->goal;
}

// 重置PID状态
void reset_pid(studio_pid *pid)
{
    pid->prev_error = pid->goal - pid->initval;
    pid->inte_error = 0.0;
    pid->dif_error = 0.0;
}

// 设置目标值并重置PID状态
void set_goal(studio_pid *pid, double goal_v)
{
    pid->goal = goal_v;
    reset_pid(pid);
}