/**
 * This class defines some basic states which apply to the robots movement.
 */

#ifndef MOTION_STATES_H
#define MOTION_STATES_H

#include "state.hh"
#include "behavior/linktypes.hh"

using namespace EyeMind;

/**
 * A state for making the robot go to a position (x,y,q)
 */
class PositionState : public State
{
friend SpeedLink& operator>>(PositionState& n, SpeedLink& o);
friend PositionLink& operator<<(PositionState& n, PositionLink& i);

 private:
	SpeedLink *speed;
	PositionLink *position;

	PositionType goal;  // desired position
	float dband; // distance deadband
	float qband; // angle deadband
	
	SpeedType m;	// output speeds

 public:
  PositionState()
    {
		dband = 0.001;  // meters^2
		qband = 0.1;  // radians^2
		
		m.v = 0.2;
		m.w = 0.1;
    }

	tristate Evaluate()
	{
		double d,dx,dy,dq;
		PositionType pos;

		if(!position)
			return TRISTATE_UNKNOWN;

		pos = position->Position();

		dx = goal.x - pos.x;
		dy = goal.y - pos.y;
		dq = goal.phi - pos.phi;

		d = dx*dx + dy*dy; // calculate norm-squared error

		if(d>dband)
		{
			//DEBUG_PRINT("Too far\n");
			value = TRISTATE_FALSE;
		}

		else if(dq>qband || dq<-dband)
		{
			//DEBUG_PRINT("Not oriented\n");
			value = TRISTATE_FALSE;
		}

		else
		{
			if(speed!=NULL)
			{
				//DEBUG_PRINT("At goal\n");
				SpeedType temp = {0,0};
				speed->Speed(temp);
				speed->Signal(EXCITE_HI_MIN);
				value = TRISTATE_TRUE;
			}
		}

		return value;
	}

	tristate Resolve()
	{
		float d,dx,dy,dq,p;
		PositionType pos;

		// Check the distance
		if(position==NULL || speed==NULL) return 0;

		pos = position->Position();

		dx = goal.x - pos.x;
		dy = goal.y - pos.y;
		dq = goal.phi - pos.phi;

		d = dx*dx + dy*dy;

		// Distance is too great
		if(d>dband)
		{
			// Calculate heading to point
			p = atan2(dy,dx) - pos.phi;

			DEBUG_PRINTF("%6.5f ",d);
			DEBUG_PRINTF("%4.3f\n",p);

			if(p>qband || p<-qband)
			{
				// Turn towards point
				if(p>0)
				{
					speed->Speed(m);
					speed->Signal(EXCITE_HI_MIN);
					speed->Update();
				}
				else
				{
					SpeedType temp = m;
					m.w *= -1.0;
					speed->Speed(m);
					speed->Signal(EXCITE_HI_MIN);
					speed->Update();
				}
			}

			else
			{
				// Drive straight
				//DEBUG_PRINT("driving\n");
				SpeedType temp = {2 * m.v, 0};
				speed->Speed(m);
				speed->Signal(EXCITE_HI_MIN);
				speed->Update();
			}
		}

		return 0;
	}

	inline float X(){return goal.x;}
	inline void X(float v){goal.x=v;}

	inline float Y(){return goal.y;}
	inline void Y(float v){goal.y=v;}

	inline float Q(){return goal.phi;}
	inline void Q(float v){goal.phi=v;}
	
	inline void Speed(const SpeedType& s){m=s;}

	void Speed(SpeedLink* l){speed = l;}
	void Position(PositionLink* l){position = l;}
};


inline SpeedLink& operator>>(PositionState& n, SpeedLink& o)
{
	n.speed = &o;
	StateOutputToSignalLink(n,o);
	return o;
}

inline PositionLink& operator<<(PositionState& n, PositionLink& i)
{
	n.position = &i;
	StateInputToSignalLink(n,i);
	return i;
}






#endif //MOTION_STATES_H