#ifndef __CONTROLLER_THREAD_H__
#define __CONTROLLER_THREAD_H__

#include "continuation.h"
#include "timer.h"
#include "pin.h"
#include "pid.h"
#include "ringbuffer.h"
#include "motor_controller.h"
#include "pressure_sensor.h"

typedef struct {
	double	Pressure;
	double	ProfileRate;
	double	CalculatedOutput;
	double  SaturatedOutput;
} PID_State;

class ControllerThread : public Continuation
{
	public:
		ControllerThread(void) {}
		virtual bool Run(bool unUsed);

	private:
		Timer						_sampleRate;
		Out <PF_6>					_LED;
		PID							_depthControl;
		MotorController				_elmo;
		PressureSensor  			_Omega;
		char						_buffer[100];
//		RingBuffer<PID_State, 32>	_log;
//		PID_State					_temp;
};

bool ControllerThread::Run(bool unUsed)
{
    BEGIN();
		_depthControl.Initialize();
		_depthControl.SetPoint(7.5);
		_LED = 0;
		_elmo.Enable();
    	_sampleRate.Start(100);
		while(1) {
			WAIT_UNTIL(_sampleRate.Expired());
			_sampleRate.Restart();
			double pressureInPSI = _Omega.Pressure(_buffer);
			double speed	= _depthControl.Update(pressureInPSI);
			_elmo.UpdateVelocity(speed);
			_LED = _LED ^ 1;
		}
		_elmo.Disable();
    END();
}

#endif // __CONTROLLER_THREAD_H__