#ifndef __CONTROLLER_THREAD_H__
#define __CONTROLLER_THREAD_H__

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

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

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

	private:
		Timer						_periodicTimer;
		PID							_depthControl;
		Out <PF_7>					_LED;
		RingBuffer<PID_State, 32>	_log;
		PID_State					_temp;
//      PressureSensor  _Omega;
//		MotorController	_Elmo;

};

bool ControllerThread::Run(bool unUsed)
{
    BEGIN();

	// Run the PID at 10 Hertz
	_periodicTimer.Start(1000);
	_depthControl.Initialize();
    while(1) {
        WAIT_UNTIL(_periodicTimer.Expired());
		_periodicTimer.Reset();
		_LED = _LED ^ 1;
#if 0
		double pressure = _Omega.Get()
		double speed	= _depthControl.Update(pressure);
		_Elmo.Speed(speed);
#endif
		_log.Push(_temp);
	}
    END();
}

#endif // __CONTROLLER_THREAD_H__