#include "goalie_behaviors.hh"

namespace Soccer03 {
/** Proportional feedback control
 */

CentreGoalie::CentreGoalie(): Behavior(), compass(Compass::I()), psdl(PsdLeft::I()), psdr(PsdRight::I()), psdb(PsdBack::I())
{
	memset(&speed,0,sizeof(SpeedType));

	centregoalie_bt *cg = (centregoalie_bt*) HDTFindEntry(B_CENTREGOALIE,0);
	if(cg==NULL) OSPanic("Cannot find\n CentreGoalie data\n");
	dband_v = cg->dband_v;
	dband_w = cg->dband_w;
	Kv = cg->Kv;
	Kw = cg->Kw * M_PI / 180;
	left_bias = cg->left_bias;
	right_bias = cg->right_bias;
	max_speed = cg->max_speed;
	max_rate_of_turn = cg->max_rate_of_turn * M_PI / 180;
	min_rear_distance = cg->min_rear_distance;
	max_rear_distance = cg->max_rear_distance;
	reset_rear_distance = cg->reset_rear_distance;
	int compass_offset = cg->compass_offset;
	int second_offset = cg->second_offset;

	compass.Update();
	DEBUG_WAIT(50);
	compass.Update();
	int Q = compass.Angle() + compass_offset;
	while (Q > 180) { Q -= 360; }
	while (Q < -180) { Q += 360; }
	desired_angle = Q;
	second_angle = Q + second_offset;
	while (second_angle > 180) { second_angle -= 360; }
	while (second_angle < -180) { second_angle += 360; }
	reset_distance = false;
}

int CentreGoalie::Execute()
{
	int max_signal = 0;
	int Q = 0;
	int Qd;
	int e_w;
	int e_v;
	int dist_left;
	int dist_right;
	int dist_back;
	bool lined_up = false;
	bool centred = false;
	bool turning = false;

	// Are we resetting our distance from the back of the net?
	// If so, we need to lineup along the second angle.
	if (reset_distance) { Qd = second_angle; }
	else { Qd = desired_angle; }
	//DEBUG_PRINTF("Qd: %d",Qd);

	compass.Update();
	Q = compass.Angle();
	//DEBUG_PRINTF("Q:%d\n",Q);

	psdl.Update();
	dist_left = psdl.X();
	psdb.Update();
	dist_back = psdb.X();
	psdr.Update();
	dist_right = psdr.X();

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

	// don't turn
	if ( (e_w < dband_w) && (e_w > -dband_w) )
	{
		speed.w = 0;
		lined_up = true;
		// We are in the deadband (ie lined-up) send a HI_MAX signal
		//DEBUG_PRINTF("LINED UP!!\n",e_w);
	}
	else
	{
		//DEBUG_PRINTF("e_w:%d\n",e_w);
		speed.w = Kw*e_w;
		//DEBUG_PRINTF("sp w %4.2f\n",speed.w);
		//DEBUG_WAIT(50);
		turning = true;
	}

	if (!reset_distance)
	{
		// Not resetting distance. We are centring on the field (take left/right PSD)
		//e_v = (int)(((float)dist_right*right_bias) - ((float)dist_left*left_bias));
		e_v = (int)(((float)dist_left*left_bias) - ((float)dist_right*right_bias));
	}
	else
	{
		// Take reading to the (back) of goals
		e_v = reset_rear_distance - dist_right;
	}

	if ((e_v < dband_v) && (e_v > -dband_v)) {
		speed.v = 0.0;
		centred = true;
	}
	else {
		speed.v = -Kv * e_v;
	}

	//DEBUG_PRINTF("L,R= %d ",dist_left);
	//DEBUG_PRINTF("%d ",dist_back);
	//DEBUG_PRINTF("%d\n",dist_right);


	// Rate limiter
	if (speed.w > max_rate_of_turn) { speed.w = max_rate_of_turn; }
	if (speed.w < -max_rate_of_turn) { speed.w = -max_rate_of_turn; }
	if (!turning) {
		if (speed.v > max_speed) { speed.v = max_speed; }
		if (speed.v < -max_speed) { speed.v = -max_speed; }
	}
	else {
		// If we are turning, then limit the max velocity to 0.5 * max_speed.
		// We need to do this to prevent the turning-circle becoming too wide.
		if (speed.v > (max_speed*0.5)) { speed.v = (max_speed*0.5); }
		if (speed.v < -(max_speed*0.5)) { speed.v = -(max_speed*0.5); }
	}

	if (centred && lined_up)
	{
		//DEBUG_PRINT("CENT + LINEUP");
		if (!reset_distance)
		{
			//DEBUG_PRINT("!RESET");
			// Check if we are to far / close to the back of the net.
			if ((dist_back < min_rear_distance) || (dist_back > max_rear_distance)) {
				//DEBUG_PRINT("DIST BK NOK");
				reset_distance = true;
			}
			else {
				reset_distance = false;
				//DEBUG_PRINT("DISK BK OK!");
			}
		}
		else
		{
			//DEBUG_PRINT("");
			// We are now at the correct disatnce to the back of the net.
			//DEBUG_PRINT("BK + CENT OK!");
			reset_distance = false;
		}
	}
	return EXCITE_IDLE;
}

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


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

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


/** Behaviour to turn & tilt 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 orientation of the head at
 *  the time the frame was taken
 *
 *  Assumptions:
 *     1)  All incoming links are polar
 *     2)  The exciatation signal of the 'raw ball position' is the greatest.
 */
MoveHead2DOF::MoveHead2DOF(): Behavior(), compass(Compass::I()), headturn(HeadServo::I()), headtilt(TiltServo::I())
{
	head2dof_bt *hd = (head2dof_bt*) HDTFindEntry(B_HEAD2DOF,0);
	if(hd==NULL) OSPanic("Cannot find\n Head2DOF data\n");
	Kp_turn = hd->Kp_turn;
	Kp_tilt = hd->Kp_tilt;
	Kp_drive = hd->Kp_drive;
	dband_turn = hd->dband_turn;
	dband_tilt = hd->dband_tilt;
	max_speed = hd->max_speed;
	range_modifier = hd->range_modifier;
	compass_dband = hd->compass_dband;
	int compass_offset = hd->compass_offset;
	Kw = hd->Kw * M_PI / 180;
	max_rate_of_turn = hd->max_rate_of_turn  * M_PI / 180;

	compass.Update();
	DEBUG_WAIT(50);
	compass.Update();
	int Q = compass.Angle() + compass_offset;
	while (Q > 180) { Q -= 360; }
	while (Q < -180) { Q += 360; }
	desired_angle = Q;

	memset(&raw,0,sizeof(BallType));
	memset(&speed,0,sizeof(SpeedType));
}

int MoveHead2DOF::Execute()
{
	float dQ, e, Q;
	float e_compass;
	int max_signal;
	float turn_angle, tilt_angle;
	SignalLink* i = inlinks.Begin();
	max_signal = 0;

	speed.v = 0.0; speed.w = 0.0;

	// find the maximum input signal
	BallLink *bl = reinterpret_cast<BallLink*>(FindMaxNonGenericInputSignal());
	if (bl==NULL) return EXCITE_NONE;
	raw = bl->Ball();
//	DEBUG_PRINTF("r,phi=%3.1f,",raw.r);
//	DEBUG_PRINTF("%3.1f\n",raw.phi);

	// check if compass is roughly lined up so we don't move too far from the goals
	compass.Update();
	Q = compass.Angle();
	//DEBUG_PRINTF("Q:%d\n",Q);

	e_compass = desired_angle - Q;
	while (e_compass > 180) { e_compass -= 360; }
	while (e_compass < -180) { e_compass += 360; }
	// don't turn
	if ( (e_compass < compass_dband) && (e_compass > -compass_dband) ) {
		speed.w = 0;
	}
	else {
		speed.w = Kw*e_compass;
	}


	// check if the distance to the object is greater than 0
	// if not, turn the servo back to middle position
	if(!raw.visible)
	{
		tilt_angle = 0.0;
		turn_angle = 0.0;
		return EXCITE_NONE;
	}
	else
	{
		dQ = Kp_turn*raw.phi;
		turn_angle = headturn.AngleRad();
		if(dQ > dband_turn || dQ < -dband_turn)
		{
			turn_angle += dQ;
		}
		speed.v = turn_angle * Kp_drive;

		e = range_modifier - raw.r;
		dQ = e * Kp_tilt;
		tilt_angle = headtilt.AngleRad();
		if(dQ > dband_tilt || dQ < -dband_tilt)
		{
			tilt_angle += dQ;
		}
		// set the head angle
		headturn.AngleRad( turn_angle );
		headturn.Update();
		headtilt.AngleRad( tilt_angle );
		headtilt.Update();
	}

	if (speed.v > max_speed) { speed.v = max_speed; }
	if (speed.v < -max_speed) { speed.v = -max_speed; }
	if (speed.w > max_rate_of_turn) { speed.w = max_rate_of_turn; }
	if (speed.w < -max_rate_of_turn) { speed.w = -max_rate_of_turn; }

	return EXCITE_IDLE;
}

/** Feed the signal
 */
void MoveHead2DOF::Feed(SignalLink &l)
{
	if (!l.Generic())
	{
		reinterpret_cast<SpeedLink&>(l).Speed(speed);
	}

	Behavior::Feed(l);
}

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

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

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

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

KickBall::KickBall(): Behavior(), boot(KickerServo::I())
{
	boot.Angle( 255 );
	boot.Update();

	kickball_bt *kb = (kickball_bt*) HDTFindEntry(B_KICKBALL,0);
	if(kb==NULL) OSPanic("Cannot find\n KickBall data\n");
	fire_range = kb->fire_range;
}

/** Proportional feedback control
 */
int KickBall::Execute()
{
	BallType ball;
	DEBUG_PRINT("KICK\n");
	// find the maximum input signal
	BallLink *bl = reinterpret_cast<BallLink*>(FindMaxNonGenericInputSignal());
	if (bl==NULL) {
		if (OSGetCount() > (last_kick_time  + (8*100))) {
				// return to rest position (ready for another kick !!)
				boot.Angle( 200 );
				boot.Update();
				DEBUG_WAIT(30);
				DEBUG_PRINT("KICK\n");
				boot.Angle( 255 );
				boot.Update();
				last_kick_time = OSGetCount();
		}
		return EXCITE_NONE;
	}

	ball = bl->Ball();

	if (ball.visible && (ball.r < fire_range)) {
		// return to rest position (ready for another kick !!)
		boot.Angle( 200 );
		boot.Update();
		DEBUG_WAIT(30);
		DEBUG_PRINT("KICK\n");
		boot.Angle( 200 );
		boot.Update();
		last_kick_time = OSGetCount();
	}
	else {
		// return to rest position
		boot.Angle( 255 );
	}

	boot.Update();
	if (ball.visible && ball.close) {
		// return a signal (is passed to the 'sayattack' behaviour)
		return EXCITE_IDLE;
	}
	// else... say nothing
	return EXCITE_NONE;
}

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

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


int CheckBallFar::Execute(){
	BallType ball;

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

	if (ball.visible && !ball.close) return EXCITE_IDLE;
	return EXCITE_NONE;
}

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

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


HeadSearch2DOF::HeadSearch2DOF(): Behavior(), headturn(HeadServo::I()), headtilt(TiltServo::I())
{
	headsearch_bt *hs = (headsearch_bt*) HDTFindEntry(B_HEADSEARCH,0);
	if(hs==NULL) OSPanic("Cannot find\n HeadSearch data\n");
	speed_x = hs->speed_x;
	speed_y = hs->speed_y;
	look_far = hs->look_far;
	look_close = hs->look_far;

	x_min = headturn.SafeMin()+speed_y;
	x_max = headturn.SafeMax()-speed_y;
	y_min =  headtilt.SafeMin();
	y_max = headtilt.SafeMax();

	orig_speed_x = speed_x;
	orig_speed_y = speed_y;
}

/** Move head in a search pattern
 */
int HeadSearch2DOF::Execute()
{
	x_pos = headturn.Angle() + speed_x;

	if ( x_pos >= x_max ) {
		speed_x = -orig_speed_x;
		if (y_pos == look_close) {
			y_pos = look_far;
		}
		else {
			y_pos = look_close;
		}
	}
	if ( x_pos <= x_min ) {
		speed_x = orig_speed_x;
		if (y_pos == look_close) {
			y_pos = look_far;
		}
		else {
			y_pos = look_close;
		}
	}

//	if ( y_pos >= y_max )  {
//		speed_y = -orig_speed_y;
//	}
//	if ( y_pos  <= y_min ) {
//		speed_y = orig_speed_y;
//	}

	headturn.Angle( x_pos );
	//headtilt->Angle( y_pos + speed_y );
	headtilt.Angle(y_pos);

	headturn.Update();
	headtilt.Update();

	return EXCITE_IDLE;
}

}; // namespace Soccer03