#include "unused_soccer_behaviors.hh"

namespace Soccer03 {
#if 0
/**
 * Function to map image co-ordinates (x,y) to polar co-ordinates
 * (R, theta). Polar co-ordinates are relative to the centre of the
 * kicker mechanism of the robot.
 *
 * Inputs: x,y INT
 * Outputs: R,theta *INT
 *
 * Initial Implementation:
 * Equations to map to polar have been derived from experiments
 * which yeilded relationships between image co-ords and
 * polar co-ords.
 *
 * @author: Jarrod Bassan
 * @date: 11/04/03
 *
 */

int FindBall::image_to_polar(int x, int y)
{
	// theta = 0.613*x - 25.6
	//ball.phi = (float)(  ((20*x)>>5) - 26  );  // this line cause a problem when x = 43
	ball.phi = ((20.0*x)/(1<<5)) - 26.0;
	ball.r = (float)(  (y<20) ? lookup_range[y] : div(512,y)  );
	ball.phi = -1 * ball.phi * M_PI / 180.0;
	//DEBUG_PRINTF("r=%3.1f,",ball.r);
	//DEBUG_PRINTF("phi=%3.1f\n",ball.phi);
	return 0;
}

FindBall::FindBall()
{
	ball.r = 0;
	ball.phi = 0;
	ball_found = 0;

	findball_bt *fd = (findball_bt*) HDTFindEntry(B_FINDBALL,0);
	if(fd==NULL) OSPanic("Cannot find\n findball data\n");
/*
	threshold_green        = fd->threshold_green;
	threshold_blue         = fd->threshold_blue;
	threshold_intensity_lo = fd->threshold_intensity_lo;
	threshold_intensity_hi = fd->threshold_intensity_hi;
*/
//	threshold_r            = fd->threshold_close_range;
//	threshold_phi          = fd->threshold_close_phi * M_PI / 180;
}

int FindBall::Execute()
{
	int i, j, k;
	//int r_norm;
	int g_norm, b_norm, intensity[5];
	int ball_x;
	int ball_y;
	int north, south, east, west;
	int rows_to_scan;
	int num_matches;
	BYTE* I;

	num_matches = 0;
	ball_found = 1;  //nominal signal to say we have run, but didn't find the ball.
	ball.r = -1; // set the distance to the ball <0 so know if didn't find the ball.

	I = (BYTE*) buffer;
	ImageLink *l = (ImageLink*) inlinks.Begin();


	//// Save one level of indentation by returning here or using goto.
	//// It's ok in that case.
	if(l!=NULL)
	{
		LCDPutColorGraphic(l->Image());

		rows_to_scan = 2;

		/* Scan image */
		for (i=imagerows-2;i>rows_to_scan;(i>30) ? (i-=2) : (i-=1))
		//for (i=imagerows-2;i>rows_to_scan;i-=1)
		{
			k=i*imagecolumns*3;
			for (j=k+3;j<(k+(3*(imagecolumns-1)));(i>40) ? (j+=6) : (j+=3))
			//for (j=k+3;j<(k+(3*(imagecolumns-1)));j+=6)
			{
				g_norm=I[j+GREEN];
				b_norm=I[j+BLUE];
				intensity[0]=I[j+RED]+I[j+GREEN]+I[j+BLUE];

				if((intensity[0] > threshold_intensity_lo)&&(intensity[0] < threshold_intensity_hi))
				{
					g_norm = div(I[j+GREEN]*100,intensity[0]);
					b_norm = div(I[j+BLUE]*100,intensity[0]);
				}
				if ((ball_found==1)&&(g_norm > threshold_green)&&(b_norm < threshold_blue))
				{
					// Now check the 4 neighbouring pixels //
					north = NORTH(j);
					south = SOUTH(j);
					east = EAST(j);
					west = WEST(j);

						intensity[1]=I[east+RED]+I[east+GREEN]+I[east+BLUE];
						((div(I[east+GREEN]*100,intensity[1]) > threshold_green)&&(intensity[1]>threshold_intensity_lo)&&(intensity[1]<threshold_intensity_hi)) ? num_matches++ : 1;

						intensity[2]=I[west+RED]+I[west+GREEN]+I[west+BLUE];
						((div(I[west+GREEN]*100,intensity[2]) > threshold_green)&&(intensity[2]>threshold_intensity_lo)&&(intensity[2]<threshold_intensity_hi)) ? num_matches++ : 1;

						intensity[3]=I[north+RED]+I[north+GREEN]+I[north+BLUE];
						((div(I[north+GREEN]*100,intensity[3]) > threshold_green)&&(intensity[3]>threshold_intensity_lo)&&(intensity[3]<threshold_intensity_hi)) ? num_matches++ : 1;

						intensity[4]=I[south+RED]+I[south+GREEN]+I[south+BLUE];
						((div(I[south+GREEN]*100,intensity[4]) > threshold_green)&&(intensity[4]>threshold_intensity_lo)&&(intensity[4]<threshold_intensity_hi)) ? num_matches++ : 1;

						if (num_matches > 3)
						{
							num_matches = 1;
							(div(I[east+BLUE]*100,intensity[1]) < threshold_blue) ? num_matches++ : 1;
							(div(I[west+BLUE]*100,intensity[2]) < threshold_blue) ? num_matches++ : 1;
							(div(I[north+BLUE]*100,intensity[3]) < threshold_blue) ? num_matches++ : 1;
							(div(I[south+BLUE]*100,intensity[4]) < threshold_blue) ? num_matches++ : 1;

						if (num_matches > 3)
						{
							ball_y = (int)(j/(imagecolumns*3));
							ball_x = (int)((j - (ball_y * imagecolumns * 3))/3.0);

							//DEBUG_PRINTF("%4d ",ball_x);
							//DEBUG_PRINTF("%4d\n",ball_y);

							image_to_polar(ball_x, ball_y);
							if ((ball.r < threshold_r)&&(ball.phi < threshold_phi)&&(ball.phi > -threshold_phi))
							{
								//DEBUG_PRINTF("close %5.4f\n", ball.phi);
								return EXCITE_HI_AVG; // ball is close, return a high excitation
							}
							else
							{
								//DEBUG_PRINTF("far   %5.4f\n", ball.phi);
								return EXCITE_MED_AVG; // ball is far, return a medium excitation
							}
						}
					}
				}
			}
		}
	}
	//.DEBUG_PRINTF("b:%d\n",ball_found);

	return EXCITE_LOW_MIN;
}

void FindBall::Feed(SignalLink &l)
{
	if (!l.Generic())
		reinterpret_cast<PolarLink&>(l).Polar(ball);

	Behavior::Feed(l);
}


/** Behaviour to turn the head
 */
int TurnHeadTo::Execute()
{
	PolarType dQ;

	PolarLink* pl = reinterpret_cast<PolarLink*>(FindMaxNonGenericInputSignal());

	if(pl==NULL) return 0;

	dQ = pl->Polar();	// get the coordinates

	if(dQ.r < 0)
	{
		angle.r = 0.0;
		angle.phi = 0.0; // reset back to zero
	}
	else
	{
		angle.r = dQ.r;
		angle.phi += dQ.phi;
	}

	//DEBUG_PRINTF("Q: %f\n",angle.phi);

	// set the head angle
	head.AngleRad( angle.phi );
	angle.phi = head.AngleRad();
	head.Update();

	return pl->Signal();
}

/** Feed the signal
 */
void TurnHeadTo::Feed(SignalLink &l)
{
	if (!l.Generic())
	{
		reinterpret_cast<PolarLink&>(l).Polar(angle);
	}

	Behavior::Feed(l);
}

TurnHeadTo& operator>>(PolarLink& l, TurnHeadTo& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}

TurnHeadTo& operator<<(PolarLink& l, TurnHeadTo& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}

PolarLink& operator>>(TurnHeadTo& n, PolarLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}

PolarLink& operator<<(TurnHeadTo& n, PolarLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}


int Predict::Execute()
{
	int max_signal;
	float temp;
	//int return_val;

	SignalLink* i = inlinks.Begin();
	max_signal = 0;

	//phi = 0.0;
	//command.v = 0.0;
	//command.w = 0.0;

	// find the maximum input signal
	while (i!=NULL)
	{
		if (!i->Generic())
		{
			PolarLink* pl = reinterpret_cast<PolarLink*>(i);
			if (i->Signal()>max_signal)
			{
				actual = pl->Polar();
				max_signal = i->Signal();
			}
		}
		i = ++inlinks;
	}

	if (max_signal>ball_not_found_signal) {
		frames_held++;
		if (frames_held == 1)
		{
			predicted.r = actual.r; predicted.phi = actual.phi;
			Ep.phi = 0.0; Ei.phi = 0.0; Ed.phi = 0.0;
			Ep.r = 0.0; Ei.r = 0.0; Ed.r = 0.0;
		}

		temp = Ep.phi;
		Ep.phi = actual.phi - predicted.phi;
		Ed.phi = Ep.phi - temp;
		Ei.phi += Ep.phi;
		predicted.phi = Kp*Ep.phi + Ki*Ei.phi + Kd*Ed.phi;

		temp = Ep.r;
		Ep.r = actual.r - predicted.r;
		Ed.r = Ep.r - temp;
		Ei.r += Ep.r;
		//predicted.r = Kp*Ep.r + Ki*Ei.r + Kd*Ed.r;
		predicted.r = actual.r;
	}
	else
	{
		frames_held = 0;
		predicted.r = 0.0;
		predicted.phi = 0.0;
	}

	//DEBUG_PRINTF("rp= %3.1f,", predicted.r);
	//DEBUG_PRINTF("%3.1f\n", predicted.phi);
	//DEBUG_PRINTF("max_sig= %d\n", max_signal);

	//avg_ticks_per_frame=(avg_ticks_per_frame>>1) + (ticks_since_frame>>1);
	//ticks_since_frame=0;

	return max_signal;
}

void Predict::Feed(SignalLink &l)
{
	if (!l.Generic())
	{
		reinterpret_cast<PolarLink&>(l).Polar(predicted);
	}
	Behavior::Feed(l);
}

void Predict::Params(float Kproportional, float Kintegral, float Kdifferential)
{
	Kp = Kproportional;
	Ki = Kintegral;
	Kd = Kdifferential;
}

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

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

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

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


int TrackCloseObjects::Execute()
{
	int i=0;
	int min=0;
	int min_i=0;
	float min_Q;
	float Q;

	if(anglelink==NULL) return 0;

	// store current servo angle
	Q = anglelink->Polar().phi;

	// update the psd
	psd->Update();
	//DEBUG_PRINTF("d=%4d\n",psd->X());

	// store
	i = (int)((Q+(50.0*M_PI/180.0))/dQ);
	dist[i] = psd->X();
	//DEBUG_PRINTF("i=",i);
	//DEBUG_PRINTF("%d\n",dist[i]);
	//DEBUG_WAIT(100);

	// turn head to point with minimum distance
	// and decrement each distance value
	min = dist[0];
	dist[0]-=decrement;
	for(i=1;i<SCAN_POINTS;i++)
	{
		if(dist[i]<min)
		{
			min = dist[i];
			min_i = i;
		}
		dist[i]-=decrement;
	}

	min_Q = (min_i)*dQ - (50.0*M_PI/180.0);

	if( min_Q > Q + dQ)
	{
		angle.phi = dQ;
	}

	else if ( min_Q < Q - dQ)
	{
		angle.phi = -dQ;
	}

	//DEBUG_PRINTF("%4.3f ",anglelink->Polar().phi);
	//DEBUG_PRINTF("%d\n",min_i);

	return 1;
}

void TrackCloseObjects::Feed(SignalLink &l)
{
	if (!l.Generic())
	{
		reinterpret_cast<PolarLink&>(l).Polar(angle);
	}

	Behavior::Feed(l);
}


TrackCloseObjects& operator>>(PolarLink& l, TrackCloseObjects& o)
{
	o.anglelink = &l;
	SignalLinkOutputToBehavior(l,o);
	return o;
}

TrackCloseObjects& operator<<(PolarLink& l, TrackCloseObjects& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}

PolarLink& operator>>(TrackCloseObjects& n, PolarLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}

PolarLink& operator<<(TrackCloseObjects& n, PolarLink& i)
{
	n.anglelink = &i;
	BehaviorInputToSignalLink(n,i);
	return i;
}
#endif

}; // namespace Soccer03