namespace RobotNet.RobotManager.Services.Simulation.Algorithm;

public class PID
{
    private double Kp = 0.3;
    private double Ki = 0.0001;
    private double Kd = 0.01;
    private double Pre_Error;
    private double Pre_Pre_Error;
    private double Pre_Out;

    public PID WithKp(double kp)
    {
        Kp = kp;
        return this;
    }

    public PID WithKi(double ki)
    {
        Ki = ki;
        return this;
    }

    public PID WithKd(double kd)
    {
        Kd = kd;
        return this;
    }

    public double PID_step(double Error, double LimitMax, double LimitMin, double TimeSample)
    {
        double P_part = Kp * (Error - Pre_Error);
        double I_part = 0.5 * Ki * TimeSample * (Error + Pre_Error);
        double D_part = Kd / TimeSample * (Error - 2 * Pre_Error + Pre_Pre_Error);
        double Out = Pre_Out + P_part + I_part + D_part;
        Pre_Pre_Error = Pre_Error;
        Pre_Error = Error;
        Pre_Out = Out;
        Out = MathExtension.CheckLimit(Out, LimitMax, LimitMin);
        return Out;
    }
}