using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace AutomaticController
{
    public class PIDController
    {
        public double Kp { get; set; }
        public double Ki { get; set; }
        public double Kd { get; set; }

        private double _prevError;
        private double _integralError;

        public PIDController(double kp, double ki, double kd)
        {
            Kp = kp;
            Ki = ki;
            Kd = kd;

            _prevError = 0;
            _integralError = 0;
        }

        public double Compute(double setpoint, double actualValue, double deltaTime = 0.01)
        {
            double error = setpoint - actualValue;
            _integralError += error * deltaTime;
            double derivative = (error - _prevError) / deltaTime;

            double output = Kp * error + Ki * _integralError + Kd * derivative;

            _prevError = error;

            return output;
        }
    }
}