#ifndef PID_C_H
#define PID_C_H

#include <stdint.h>

// PID控制器结构体
typedef struct
{
    double initval;     // 初始值
    double goal;        // 目标值
    double error;       // 当前误差
    uint32_t pid_step;  // 步数计数器
    double Kp;          // 比例常数
    double Ki;          // 积分常数
    double Kd;          // 微分常数
    double prev_error;  // 前一个误差
    double inte_error;  // 积分误差
    double dif_error;   // 微分误差
} studio_pid;

// 初始化PID控制器
void studio_pid_init(studio_pid *pid, double init_v, double goal_v, double p, double i, double d)
{
    pid->initval = init_v;
    pid->goal = goal_v;
    pid->Kp = p;
    pid->Ki = i;
    pid->Kd = d;
    pid->error = goal_v - init_v;
    pid->pid_step = 1;
    pid->prev_error = 0;
    pid->inte_error = 0;
    pid->dif_error = 0;
}

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

    // 更新误差项
    if (pid->pid_step == 1)
    {
        pid->prev_error = cte;
    }

    pid->inte_error += cte;
    pid->dif_error = cte - pid->prev_error;
    pid->prev_error = cte;

    ++pid->pid_step;

    // 计算并返回PID输出
    return pid->Kp * cte + pid->Ki * pid->inte_error + pid->Kd * pid->dif_error;
}

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

// 设置目标值
void studio_pid_set_goal(studio_pid *pid, double goal_v)
{
    pid->goal = goal_v;
}

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

#endif