#ifndef __PID_H__
#define __PID_H__
#include "types.h"

class PID {
	public:

		PID() {
			Initialize();
		}

		void Initialize() {
			double den;
			_tf 	= 2.0;
			_h  	= 0.1;
			_k  	= 100.0;
			_ti 	= 5000.0;
			_td 	= 0.1;
			_tt 	= 100.0;
			_b  	= 1.0;
			_ysp	= 0.0;
			_uLow	= -1000.00;
			_uHigh	= 1000.00;
			den		= (_tf * _tf) + (2 * _h * _tf) + (2 * _h * _h);
			_p1 	= (_tf * _tf) / den;
			_p2 	= (2 * _h * _h) / den;
			_p3 	= (_k * _h) / _ti;
			_p4 	= (_k * _td) / _h;
			_p5 	= _h / _tt;
			_y1		= 0.0;
			_y2		= 0.0;
		}

		void SetPoint(double psi) {
			_ysp = psi;
		}

		double Update(double pressure) {
			double y = _velocity(pressure);
			_y2 = (_p1 * _y2) + _p2 * (y - _y1);
			_y1 = _y1 + _y2;
			_v	= _k * (_b * _ysp - _y1) - _p4 * _y2 + _I;
			if (_v < _uLow) {
				_u = _uLow;
			} else if (_v > _uHigh) {
				_u = _uHigh;
			} else {
				_u = _v;
			}
			// Update the integral
			_I = _I + _p3 * (_ysp - _y1) + _p5 * (_u - _v);
			return _u;
		}

	private:
		double	_velocity(double pressure) { return pressure; }
		double	_tf;		// Derivative filter time constant
		double	_h;			// Sample period in seconds
		double	_k;			// Constant for "P" term of PID
		double	_ti;		// Time constant for "K" term
		double	_td;		// Time constant for "D" term
		double	_tt;		// Anti-windup time constant
		double	_b;			// Set point weight
		double	_ysp;		// Desired setpoint
		double	_uLow;		// Minimum Possible Output
		double	_uHigh;		// Maximum Possible Output
		double	_p1;
		double	_p2;
		double	_p3;
		double	_p4;
		double	_p5;
		double	_I;
		double	_y1;
		double	_y2;
		double	_kOld;
		double	_kNew;
		double	_bOld;
		double	_bNew;
		double	_v;
		double	_u;
};

#endif // __PID_H__