namespace RobotNet10.RobotApp.Services.Navigation.CSharp;

public class PIDConfig
{
    public double Kp { get; set; }
    public double Ki { get; set; }
    public double Kd { get; set; }
    /// <summary>
    /// Integral chỉ tích lũy khi |error| &lt;= IntegralZone.
    /// Giá trị 0 = không giới hạn (integral luôn tích lũy).
    /// </summary>
    public double IntegralZone { get; set; }
}

public class PID(PIDConfig config)
{
    private double Kp = config.Kp;
    private double Ki = config.Ki;
    private double Kd = config.Kd;
    private double IntegralZone = config.IntegralZone;

    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 PID WithIntegralZone(double integralZone)
    {
        IntegralZone = integralZone;
        return this;
    }

    public double PID_step(double error, double max, double min, double timeSample)
    {
        double integralStep = 0.5 * (error + _prevError) * timeSample;

        // Integral Zone: chỉ tích lũy khi |error| nằm trong vùng cho phép
        bool inIntegralZone = IntegralZone <= 0 || Math.Abs(error) <= IntegralZone;
        if (inIntegralZone)
            _integral += integralStep;
        else
            _integral = 0;

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

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

        // Anti-windup: hoàn tác integralStep khi output bị bão hòa
        double clamped = Math.Clamp(Out, min, max);
        if (clamped != Out && inIntegralZone)
            _integral -= integralStep;

        return clamped;
    }

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