using TestApp.Configuration;

namespace TestApp
{
    internal class PidController
    {
        private DateTime _PreviousUpdateTime;
        private double _PreviousError;
        private double _Integral;

        public PidSetting PositiveSettings { get; set; } = new PidSetting();
        public PidSetting NegativeSettings { get; set; } = new PidSetting();
        public double SetPoint { get; set; }

        public double Update(double measurement) {
            var error = measurement - SetPoint;
            var dt = (DateTime.UtcNow - _PreviousUpdateTime).TotalSeconds;

            var settings = error > 0 ? PositiveSettings : NegativeSettings;

            var proportional = error * settings.KP;
            _Integral += error * settings.KI * dt;
            var differential = (error - _PreviousError) * settings.KD / dt;

            _PreviousError = error;
            _PreviousUpdateTime = DateTime.UtcNow;

            return proportional + _Integral + differential;
        }
    }
}