#include "soccer_behaviors.hh"

namespace Soccer03 {

/** Behaviour to turn the head towards the ball AND to correct the
 *  the raw ball position. The raw ball position is the (r,phi) relative
 *  to the camera. The corrected (r,phi) is the (expected) position
 *  of the ball after taking into account the angle of the head at 
 *  the time the frame was taken
 */
int TurnHead::Execute()
{
	float dQ;
	float servo_angle;
	BallType raw;

	// find the maximum input signal
	BallLink *bl = reinterpret_cast<BallLink*>(FindMaxNonGenericInputSignal());
	if (bl==NULL) return EXCITE_NONE;
	raw = bl->Ball();

	if(raw.visible)
	{
		ticks = 0;

		dQ = Kp*raw.phi;
		if(dQ > dband || dQ < -dband)
		{
			servo_angle = dQ + head.AngleRad();  // use proportional error
			corrected.phi = head.AngleRad() + raw.phi;
		}
		else
		{
			corrected.phi = head.AngleRad();
		}

		corrected.r = raw.r;
		corrected.visible = true;
		DEBUG_PRINTF("TH: phi %4.2f\n",corrected.phi);

		if ((corrected.r < threshold_r)&&(corrected.phi < threshold_phi)&&(corrected.phi > -threshold_phi))
		{
			// Ball is very close
			corrected.close = true;
		}
		else {
			corrected.close = false;
		}
	}

	else if(ticks<=grace_period)
	{
		ticks++;
	}

	else
	{
		servo_angle = 0.0;
		corrected.visible = false;
		corrected.close = false;
	}


	// set the head angle
	head.AngleRad(servo_angle);
	head.Update();

	return EXCITE_IDLE;
}

TurnHead::TurnHead(): Behavior(), head(HeadServo::I())
{
	head.CalibrateRange(-50.0,50.0);
	memset(&corrected, 0 , sizeof corrected);
	turnhead_bt *th = (turnhead_bt*) HDTFindEntry(B_TURNHEAD,0);
	if(th==NULL) OSPanic("Cannot find\n trackheading data\n");
	Kp 		= th->Kp;
	dband 		= th->dband;
	threshold_r	= th->threshold_close_range;
	threshold_phi	= th->threshold_close_phi * M_PI / 180;
	DEBUG_PRINTF("TH: phi %4.2f\n",threshold_phi);
	grace_period	= th->grace_period;
}

/** Feed the signal
 */
void TurnHead::Feed(SignalLink &l)
{
	if (!l.Generic())
	{
		reinterpret_cast<BallLink&>(l).Ball(corrected);
	}

	Behavior::Feed(l);
}

/**Connect PolarLink output with target TurnHead.
 */
TurnHead& operator>>(BallLink& l, TurnHead& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}

/**Connect PolarLink input with source TurnHead.
 */
TurnHead& operator<<(BallLink& l, TurnHead& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}

/**Add output PolarLink to TurnHead.
 */
BallLink& operator>>(TurnHead& n, BallLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}

/**Add input PolarLink to TurnHead.
 */
BallLink& operator<<(TurnHead& n, BallLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}

Reacquire::Reacquire() : Behavior(), head(HeadServo::I())
{
	reacquire_bt *re = (reacquire_bt*) HDTFindEntry(B_REACQUIRE,0);
	if(re==NULL) OSPanic("Cannot find\n reacquire data\n");

	exhaust_period = re->exhaust_period;
	reacquire_v = re->reacquire_v;
	reacquire_w = re->reacquire_w;
	start_period = re->start_period;
	ticks = exhaust_period;
	averaged_phi = 0.0;

	memset(&command, 0, sizeof(SpeedType));
}


int Reacquire::Execute()
{
	int max_signal = 0;
	float phi = 0.0;

	// reset the speed command
	memset(&command, 0, sizeof(SpeedType));

	// find the maximum input signal
	BallLink* bl = reinterpret_cast<BallLink*>(FindMaxNonGenericInputSignal());
	if (bl==NULL) return EXCITE_NONE;

	if (bl->Ball().visible)
	{
		phi = bl->Ball().phi;
		averaged_phi = phi;
		ticks=0;
		return EXCITE_NONE;
	}

	// the ball is not visible, but it has been seen recently (delay < max_delay)
	// so we start searching for the ball.

	if (ticks < start_period)
	{
		command.v = reacquire_v;
	}

	else if (ticks < exhaust_period)
	{
		averaged_phi > 0 ? command.w = -reacquire_w : command.w = reacquire_w;
	}

	else
	{
		return EXCITE_NONE;
	}

	if(bl->Ball().close)
	{
		command.v*=-1;
		command.w*=-1;
	}

	ticks++;
	return EXCITE_IDLE;
}

void Reacquire::Feed(SignalLink &l)
{
	if (!l.Generic())
	{
		reinterpret_cast<SpeedLink&>(l).Speed(command);
	}
	Behavior::Feed(l);
}

/**Connect PolarLink output with target Reacquire.
 */
Reacquire& operator>>(BallLink& l, Reacquire& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}

/**Connect SpeedLink input with source Reacquire.
 */
Reacquire& operator<<(SpeedLink& l, Reacquire& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}

/**Add output SpeedLink to Reacquire.
 */
SpeedLink& operator>>(Reacquire& n, SpeedLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}

/**Add input PolarLink to Reacquire.
 */
BallLink& operator<<(Reacquire& n, BallLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}


/** Proportional feedback control
 */
int TrackHeading::Execute()
{
	int max_signal = 0;
	int Q = 0;
	int Qd;
	int e_compass;
	int e_psd;
	int dist_left;
	int dist_right;

	memset(&speed,0,sizeof speed);

	// By default, set Qd as the default angle, UNLESS there is an incoming float-link
	Qd = desired_angle;

	compass.Update();
	Q = compass.Angle();

	// Find the biggest signal
	SignalLink* i = FindMaxInputSignal();
	if(i==NULL) return EXCITE_NONE;

	BallLink* bl = reinterpret_cast<BallLink*>(FindMaxNonGenericInputSignal());
	if(bl!=NULL)
	{
		// If the user passes a BallLink, we must check that the ball is close (and visible).
		if ( (!bl->Ball().visible) || (!bl->Ball().close) ) {
			// The ball is not visible or not close. Do not propagate a signal
			return EXCITE_NONE;
		}
	}

	e_compass = Qd - Q;
	while (e_compass > 180) { e_compass -= 360; }
	while (e_compass < -180) { e_compass += 360; }

	if ( (e_compass < dband_compass) && (e_compass>-dband_compass) )
	{
		//Check PSD to see if we are centred on the field
		psdl.Update();
		dist_left = psdl.X();
		psdr.Update();
		dist_right = psdr.X();
		e_psd = (int)(((float)dist_right*right_bias) - ((float)dist_left*left_bias));

		if (e_psd < -dband_psd)
		{
			//DEBUG_PRINT("Left\n");
			// Turn to the left (+ve w)
			speed.v = v_slow;
			speed.w = w_slow;
		}
		else if (e_psd > dband_psd)
		{
			//DEBUG_PRINT("Right\n");
			//turn to the right (-ve w)
			speed.v = v_slow;
			speed.w = -w_slow;
		}
		else
		{
			//DEBUG_PRINT("LINED UP\n");
			// Robot is lined-up
			speed.v = v_fast;
			speed.w = 0.0;
		}
	}
	else
	{
		DEBUG_PRINTF("ECOMP %5d\n",e_compass);
		// We are not lined-up. Use the error to correct our orientation.

		speed.w = Kw*e_compass;
		speed.v = v_turn;
	}

	// Rate limiter
	if (speed.w > max_turn_rate) { speed.w = max_turn_rate; }
	if (speed.w < -max_turn_rate) { speed.w = -max_turn_rate; }

//	DEBUG_PRINTF("v:%4.2f ",speed.v);
//	DEBUG_PRINTF("w:%4.2f\n",speed.w);

	return EXCITE_IDLE;
}

TrackHeading::TrackHeading(): Behavior() , compass(Compass::I()), psdl(PsdLeft::I()), psdr(PsdRight::I()), psdf(PsdFront::I())
{
	memset(&speed,0,sizeof(SpeedType));
}

TrackHeading::TrackHeading(int device_semantics) : Behavior(), compass(Compass::I()), psdl(PsdLeft::I()), psdr(PsdRight::I()), psdf(PsdFront::I())
{
	trackheading_bt *th = (trackheading_bt*) HDTFindEntry(B_TRACKHEADING,device_semantics);
	if(th==NULL) OSPanic("Cannot find\n trackheading data\n");
	dband_compass			= th->dband_compass;
	Kw				= th->Kw*(M_PI/180.0);
	max_turn_rate			= th->max_turn_rate*(M_PI/180.0);
	left_bias			= th->left_bias;
	right_bias			= th->right_bias;
	v_fast				= th->v_fast;
	v_slow				= th->v_slow;
	v_turn				= th->v_turn;
	w_slow				= th->w_slow;
	compass_offset			= th->compass_offset;
	internal_threshold = th->internal_threshold;

	// update the compass to get a initial reading
	compass.Update();
	DEBUG_WAIT(50);
	compass.Update();

	int Q = compass.Angle() + compass_offset;
	while (Q > 180) { Q -= 360; }
	while (Q < -180) { Q += 360; }
	//DEBUG_PRINTF("HEADING = %d\n",Q);

	DesiredHeadingDeg(Q);
}

void TrackHeading::Feed(SignalLink &l)
{
	if (!l.Generic())
	{
		reinterpret_cast<SpeedLink&>(l).Speed(speed);
	}

	Behavior::Feed(l);
}

/**Connect TrackHeading input with source Centre.
 */
TrackHeading& operator<<(SpeedLink& l, TrackHeading& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}

/**Add output PolarLink to TrackHeading
 */
SpeedLink& operator>>(TrackHeading& n, SpeedLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}

TrackHeading& operator>>(BallLink& l,TrackHeading& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}

BallLink& operator<<(TrackHeading& n, BallLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}


void TurnCommand::Feed(SignalLink &l)
{
	if (!l.Generic()) {
		reinterpret_cast<SpeedLink&>(l).Speed(speed);
	}

	Behavior::Feed(l);
}


TurnCommand& operator>>(BallLink& l,TurnCommand& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}


TurnCommand& operator<<(SpeedLink& l, TurnCommand& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


SpeedLink& operator>>(TurnCommand& n, SpeedLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}


BallLink& operator<<(TurnCommand& n, BallLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}



TurnCommand::TurnCommand(): Behavior()
{
	memset(&speed, 0, sizeof speed);

	turncommand_bt *tc = (turncommand_bt*) HDTFindEntry(B_TURNCOMMAND,0);
	if(tc==NULL) OSPanic("Cannot find\n turncommand data\n");
	Kr	= tc->Kr;
	Kq	= tc->Kq;

	dband_phi = tc->dband_phi;
	speed_close = tc->speed_close;
	Kq_close = tc->Kq_close;
	Kq_close_hi = tc->Kq_close_hi;
	w_slow = tc->w_slow;
	close_threshold = tc->close_threshold;
}

int TurnCommand::Execute()
{
	int max_signal = 0;
	int Qd;
	int Q;
	int e_compass;
	memset(&speed, 0, sizeof speed);

	BallLink *bl = reinterpret_cast<BallLink*>(FindMaxNonGenericInputSignal());
	if (bl==NULL) return EXCITE_NONE;

	BallType ball = bl->Ball();

	//if ( (!ball.close)&&(ball.visible) ) {
	if ( ball.visible ) {
		if (ball.r < close_threshold) {
			if ( (ball.phi < dband_phi)&&(ball.phi > -dband_phi)) {
				// Close and within the phi deadband
			   //DEBUG_PRINT("TC: CLOSE+DB\n");
				speed.v = speed_close;
				speed.w = Kq_close*ball.phi;
			}
			else {
				DEBUG_PRINT("TC: DB\n");
				// Close, but outside the phi deadband
				speed.v = w_slow;
				speed.w = Kq_close_hi*ball.phi;
			}
		}
		else {
		 	//DEBUG_PRINT("TC: FAR\n");
			// Ball far...
			speed.v = Kr*ball.r;
			speed.w = Kq*ball.phi;
		}
		return EXCITE_IDLE;
	}

	return EXCITE_NONE;
}

}; // namespace Soccer03