//
// Created by Administrator on 2026/1/16.
//

#ifndef RTTHREAD_PID_CONTROLLER_H
#define RTTHREAD_PID_CONTROLLER_H





class PidController
{
public:
    PidController();

    // 设置目标（通用 target）
    void setTarget(double target);

    // PID 增益
    void setGains(double kp, double ki, double kd);

    /// 设置积分上下限： lower <= I <= upper
    /// 若 upper <= lower，则不启用积分限幅
    void setIntegralLimits(double lower, double upper);

    /// 设置误差死区： |error| < deadzone 时视为 0
    void setDeadzone(double deadzone);

    /// 设置 D 项滤波系数（简单一阶滤波）
    /// coeff <= 0 表示不启用滤波
    void setDerivativeFilterCoeff(double coeff);

    /// 重置内部状态（积分/上一拍误差/滤波器）
    void reset(double integral = 0.0, double prev_error = 0.0);

    /// 核心：根据当前值计算控制量
    double compute(double x, double dt);

private:
    // gains
    double kp_;
    double ki_;
    double kd_;

    // target
    double target_;

    // state
    double integralError_;
    double prevError_;

    // integral clamp
    double iLower_;
    double iUpper_;

    // deadzone
    double deadzone_;

    // derivative filter
    double dFilterCoeff_;
    double dErrorFiltered_;

    bool initialized_;
};


#endif //RTTHREAD_PID_CONTROLLER_H