/**
 * implements vision_behaviors.h
 */


#include "vision_behaviors.hh"
#define MIN(a,b) (a<b?a:b)
#define MAX(a,b) (a>b?a:b)
#define NO_HUE -1
#define THRESH 6

//#define LOOKUP_RANGE={164,108,85,71,62,56,51,47,44,41,39,37,35,33,32,31,30,29,28};

namespace Soccer03 {

/**
 * 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
 *
 */

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");

	hue		= fd->hue;
	threshold	= fd->threshold;
	intensity_lo	= fd->intensity_lo;
	intensity_hi	= fd->intensity_hi;
	K			= fd->K_far;
	intensity_thresh	= fd->intensity_thresh;
	req_matches	= fd->req_matches;
}

int FindBall::image_to_polar(int x, int y)
{

	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;
	ball.visible = true;
	return 0;
}


/**
 * return hue value for RGB color
 */
int FindBall::RGBtoHue(BYTE r, BYTE g, BYTE b)
{
	int hue, delta, max, min, intensity;

	max   = MAX(r, MAX(g,b));
	min   = MIN(r, MIN(g,b));
	delta = max - min;
	hue = 0; /* init hue*/

	if (K*delta <= (max*10) )
	{
		hue = NO_HUE; /* gray, no color */
	}
	else
	{
		if (r==max) hue = 42 + 42*(g-b)/delta; /* 1*42 */
		else if (g==max) hue = 126 +42*(b-r)/delta; /* 3*42 */
		else if (b==max) hue = 210 +42*(r-g)/delta; /* 5*42 */
	}

	return (BYTE) hue; /* now: hue is in range [0..252] */
}

int FindBall::CheckHue(BYTE r, BYTE g, BYTE b)
{
	int h, intensity, distance;

	h = RGBtoHue(r,g,b);
	intensity=r+g+b;
	if ((h != NO_HUE)&&(intensity < intensity_hi)&&(intensity > intensity_lo))
	{
		distance = ((int)h-hue);  /* hue distance */
		if (distance < 0) distance = -distance;
		if (distance > 126) distance = 253 - distance;
		if (distance < threshold) return 1;
	}
	return 0;
}

int FindBall::CheckIntensity(BYTE r, BYTE g, BYTE b)
{
	if ((r+g+b)<intensity_thresh) return 0;
	return 1;
}

int FindBall::Execute()
{
	int i, j, k, m;
	//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;

	int dist;
	int K_param;

	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();
	if(l==NULL) return EXCITE_NONE;

	LCDPutColorGraphic(l->Image());

	rows_to_scan = 2;
	ball.visible = false;

	/* Scan image */
	//K_param = K_close;
	//Check bottom row of image...
	for (i=imagerows-2;i>rows_to_scan;(i>30) ? (i-=2) : (i-=1))
	{
		k=i*3*imagecolumns + 3*imagecolumns/2;
		m=-1;

		//if (i>55) K_param = K_far;
		for (j=0; j<3*(imagecolumns-4); (i>40) ? (j+=6) : (j+=3))
		{
			m*=-1;
			k+=m*j;

			if (CheckHue(I[k+RED],I[k+GREEN],I[k+BLUE]) == 1)
			{
				// Now check the 4 neighbouring pixels //
				north = NORTH(k);
				south = SOUTH(k);
				east = EAST(k);
				west = WEST(k);

				num_matches =  CheckHue(I[east+RED],I[east+GREEN],I[east+BLUE]) + 1;
				num_matches += CheckHue(I[west+RED],I[west+GREEN],I[west+BLUE]);
				num_matches += CheckHue(I[north+RED],I[north+GREEN],I[north+BLUE]);
				num_matches += CheckHue(I[south+RED],I[south+GREEN],I[south+BLUE]);

				if (num_matches >= req_matches)
				{
					ball_y = i;
					ball_x = (int)(j/3.0);
					image_to_polar(ball_x, ball_y);

					return EXCITE_IDLE;
				}
			}
		}
	}

	// Check bottom row of image
	k = (imagerows-2)*imagecolumns*3;
	int current_pix=0;
	int end_pix=0;
	int best_run=0;
	int best_end=0;
	int run=0;
	int x = 0;
	for (j=k+3;j<(k+(3*(imagecolumns-1)));j+=3)
	{
		current_pix = x++;
		if (CheckIntensity(I[j+RED],I[j+GREEN],I[j+BLUE]) == 1) {
			end_pix = current_pix;
			run++;
			if (run > best_run) {
				best_run = run;
				best_end = end_pix;
			}
		}
	}

	if (best_run > req_matches) {
		ball.visible = true;
		ball.r = 3.0;
		ball.phi = 0.0; //((20.0* (best_end - (best_run/2)) )/(1<<5)) - 26.0;
		return EXCITE_IDLE;
	}

	return EXCITE_IDLE;
}


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

	Behavior::Feed(l);
}

}; // namespace Soccer03