using System;
using Microsoft.SPOT;

namespace CPF
{
    public class PID
    {
        private double _desired;
        private double _samplePeriod;  // Sample period (seconds)
        private double _pid_P;         // Constant for P term of PID
        private double _pid_I;         // Time constant for I term of PID (in denominator, bigger values less I term in output
        private double _pid_D;         // Time constant for D term of PID
        private double _dfiltk;        // Derivative filter time constant
        private double _antiWindUp;    // Anti-windup time constant
        private double _weight;        // Setpoint weight
        private double _minimum;       // minimum possible output
        private double _maximum;       // maximum possible output
        private double _y1;             // first state variable
        private double _y2;             // second state variable

        private double _p1;            // first part of state variable x2 equation
        private double _p2;            // second part of state variable x1 equation
        private double _p3;            // integral gain
        private double _p4;            // derivative gain
        private double _p5;            // anti-windup gain

        public PID(double uLow, double uHigh, double ysp, double hz = 10, double k = 1.0, double ti = 1.0, 
                   double td = 1.0, double tf = 1.0, double tt = 1.0, double b = 1.0)
        {
            _samplePeriod = (1.0 / hz);
            _desired = ysp;
            _pid_P  = k;         
            _pid_I  = ti;         
            _pid_D  = td;        
            _dfiltk = tf;
            _antiWindUp = tt;  
            _weight = b;
            _minimum = uLow;       
            _maximum = uHigh;       
            _y1 = 0.0;
            _y2 = 0.0;         
            Compute_PID_Constants();
        }

        private void Compute_PID_Constants()
        {
            double samplePeriodSquared = _samplePeriod * _samplePeriod;
            double twiceSamplePeriodSquared = 2.0 * samplePeriodSquared;
            double dfiltkSquared = _dfiltk * _dfiltk;

            // denominator of measured signal filter terms
            double den = dfiltkSquared + (2.0 * _samplePeriod * _dfiltk) + twiceSamplePeriodSquared;  
            _p1 = dfiltkSquared / den;                          // first part of state variable x2 equation
            _p2 = twiceSamplePeriodSquared / den;               // second part of state variable x1 equation
            _p3  = (_pid_P * _samplePeriod) / _pid_I;           // integral gain
            _p4  = (_pid_P * _pid_D) / _samplePeriod;           // derivative gain
            _p5 = _samplePeriod / _antiWindUp;                  // anti-windup gain
        }

        public double P
        {
            get { return _pid_P; }
            set { _pid_P = value; Compute_PID_Constants(); }
        }

        public double I
        {
            get { return _pid_I; }
            set { _pid_I = value; Compute_PID_Constants(); }
        }

        public double D
        {
            get { return _pid_D; }
            set { _pid_D = value; Compute_PID_Constants(); }
        }

        public double DerivativeFilterTimeConstant
        {
            get { return _dfiltk; }
            set { _dfiltk = value; Compute_PID_Constants(); }
        }

        public double AntiWindUp
        {
            get { return _antiWindUp; }
            set { _antiWindUp = value; Compute_PID_Constants(); }
        }

        public double SetPoint
        {
            get { return _desired; }
            set { _desired = value; }
        }

        public double SetPointWeight
        {
            get { return _weight; }
            set { _weight = value; Compute_PID_Constants(); }
        }

        public double SampleRateInHz
        {
            get { return (1.0 / _samplePeriod); }
            set { _samplePeriod = (1.0 / value); Compute_PID_Constants(); }
        }

        double Update(double measured)
        {
            double u;
            double v;

            // update filter state y2
            _y2 = _p1 * _y2 + _p2 * (measured - _y1);     
              
            // update filter state y1
            _y1 = _y1 + _y2;    
                                        
            /// Compute the PID output
            v = _pid_P * (_weight * _desired - _y1) - _p4 * _y2 + _pid_I;

            // Limit the output to desired range
            if (v < _minimum)                                                  
                u = _minimum;
            else if (v > _maximum)
                u = _maximum;
            else
                u = v;

            // Update the integral
            _pid_I = _pid_I + _p3 * (_desired - _y1) + _p5 * (u - v);    
            return u;
        }
    }
}