/**
 * Defines states for sensors
 */
 
 
#ifndef SENSOR_STATES
#define SENSOR_STATES

#include <math.h> ///@note for M_PI
#include "state/state.hh"
#include "behavior/sensor_behaviors.hh"



/**
 * State is true if the compass and odometry angles
 * are approximately equal.
 */

// class Oriented : public State
// {
// private:
// 	FeelCompass* feelcompass;
// 	OdometryTwoWheeled* odometry;
// 
// 	int tolerance;
// 
// public:
// 	Oriented()
// 	{
// 		feelcompass = &FeelCompass::I();
// 		odometry = &OdometryTwoWheeled::I();
// 		tolerance = 5;
// 
// 		// Set the odometry reading to the compass reading
// 		feelcompass->Excite(1);
// 		odometry->SetPosition(0.0,0.0,
// 		                      (feelcompass->Angle()*M_PI)/180.0);
// 
// 		value = TRISTATE_TRUE;
// 	}
// 
// 	virtual ~Oriented(){}
// 
// 	int Resolve()
// 	{
// 		DEBUG_PRINT("Resolving\n");
// 		odometry->SetAngle((feelcompass->Angle()*M_PI)/180.0);
// 
// 		return 0;
// 	}
// 
// 	tristate Evaluate()
// 	{
// 		PositionType pos;
// 		int c,q;
// 
// 		feelcompass->Excite(1);
// 		pos = odometry->Position();
// 
// 		c = feelcompass->Angle();
// 		q = (int)((pos.phi*180)/M_PI);
// 
// 		// Convert radians to degrees and compare
// 		DEBUG_PRINTF("C:%d\n",c);
// 		DEBUG_PRINTF("Q:%d\n",q);
// 
// 		if((c - q) > tolerance || (q - c) > tolerance)
// 			value = TRISTATE_FALSE;
// 		else
// 			value = TRISTATE_TRUE;
// 
// 		return value;
// 	}
// 
// };
// 

#endif //SENSOR_STATES