namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;

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

    private double _prevError;
    private double _integral;

    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 max, double min, double timeSample)
    {
        double integralStep = 0.5 * (error + _prevError) * timeSample;
        _integral += integralStep;

        double derivative = (error - _prevError) / timeSample;
        _prevError = error;

        double Out = Kp * error
                   + Ki * _integral
                   + Kd * derivative;

        // Anti-windup
        double clamped = Math.Clamp(Out, min, max);
        if (clamped != Out)
            _integral -= integralStep;

        return clamped;
    }

    public void Reset()
    {
        _prevError = 0;
        _integral = 0;
    }
}