/**********************************************************************************************************************/
/*E.Davids Bot 1.2*/
/*Lim Chin Teong (0147905)*/
/*Robot scans around for ball then quickly centers itself to the ball position*/
/*robot knows where it last saw the ball n starts scanning again in that direction*/
/*robot will now gauge the distance and move to the ball depending on distance gauged*/
/*the robot checks the goal infront of it and 'shoots' the ball accordingly*/
/***********************************************************************************************************************/


/*standard*/
#include "eyebot.h"
#include "math.h"

/*simple ways to find the bigger and smaller of 2 integers*/
#define MIN(a,b) (a<b?a:b)
#define MAX(a,b) (a>b?a:b)



#define L 1
#define R 3
#define C 2


/*Declare Functions*/
void drive(float dist,float angle, float speed);
void turn(float angle, float anglespeed);
int RGBtoHue(BYTE r, BYTE g, BYTE b);
int AnalyseBlueGoalImage(colimage im);
int AnalyseYellowGoalImage(colimage im);
void decide_and_execute_drive(float dist);
int checkgoal(int goal);
float gaugedistance(image bwimage);
void simplescan(int scandir);
void quickcenter();
void setmenu();


/*binary image to analyze*/
image bwimage;

/*color image that we capture from the camera*/
colimage colourimage;

/*define the vw driving driver*/
VWHandle vw;

/*histogram that we analyze*/
int histogram[imagecolumns];

/*boolean to see if the ball is in view to be centered*/
int ballinview, ballcentered;

/*variable to pass to the scanning function so it knows if to look left or right first*/
int scandirection;

/*to see which stream is running (eg. program end..finding ball..setting..etc)*/
int stream;

/*to identify whether it is scoring in the blue goal (1) or yellow goal (0)*/
int goal;

/*key for identifying which key is pressed in the menu*/
int key;

/*step size to loop through pixels in the image*/
int LOOPSTEP = 1;

/*this means the number of correct color pixels in the image that is enough to say that there is a ball in the image*/
int QUICKCENTERTHRESHOLD;
int SIMPLESCANTHRESHOLD;

/*define speeds and angles for quickcenter and turnspeed*/
float QUICKCENTERTURNSPEED;
float QUICKCENTERTURNANGLE;
float SIMPLESCANTURNSPEED;
float SIMPLESCANTURNANGLE;

float APPROACHSPEED; /*speed the the robot will approach the ball (not shooting)*/
float SHOOTANGLE;	/*angle that the robot drive turns when shooting the ball to goal*/
float SHOOTSPEED;	/*speed at which robot drives to ball when shooting*/
float SHOOTPOWER;	/*the extra distance that is added to the distance to the ball so that it 'shoots' the ball with some follow through (ie power)*/

int GOALTHRESHOLD;

/*some variables to assist in counting the fps*/
int time1,time2;
int framecounter;
float fps;

int stall;
int obst;



PSDHandle front;
PSDHandle left;
PSDHandle right;



/* own drive turn function
*/
void turn(float angle, float anglespeed)	{
	PositionType turnpos;
	float angleturned, angleleft;
	int finishedturn;
	int leftdist, rightdist;
	int turnstall;
	int c;
	float c1,c2;

	c = 0; c1=0.0; c2=0.0;
	finishedturn = 0;
	turnstall = 0;
	angleleft = angle;
	angleturned = 0;

	VWSetPosition(vw,0,0,0);
	VWGetPosition(vw, &turnpos);
	
	while(finishedturn == 0)	{
		LCDSetPos(6,10);
		LCDPutIntS(c,5);
		c++;
		if(c == 2001)c =0;

		

		VWDriveTurn(vw,angleleft,anglespeed);
		
		
		leftdist = PSDGet(left);
		rightdist = PSDGet(right);

		VWGetPosition(vw,&turnpos);

		//angleturned = turnpos.phi;
		//angleleft = angleleft - angleturned;

		LCDSetPos(1,10);
		LCDPutFloatS(turnpos.phi,4,3);

		
			
		

		if( fabs(turnpos.phi) >= fabs(angleleft))	{
			//AUBeep();
			VWSetSpeed(vw,0,0);
			finishedturn= 1;
			
		}

		if(		
				(leftdist <= 70)	||
				(rightdist <=70)
			)	{
			
			VWSetSpeed(vw,0,0);
			
			angleturned = angleturned + turnpos.phi;
			angleleft = angle - angleturned;
			
			
			LCDSetPos(0,10);
			LCDPutFloatS(angle-angleleft,4,3);
			LCDSetPos(5,10);
			LCDPutFloatS(angleleft,4,3);
			
			
			VWSetPosition(vw,0,0,0);
			//finishedturn = 1;
		}

		if(VWStalled(vw) == 1)	{
			turnstall++;
		}
		if( turnstall>=1000 )	{
			turnstall = 0;
			VWSetSpeed(vw,0,0);
			AUBeep();
			VWDriveCurve(vw,-0.1,0,0.5);
			finishedturn = 1;
		}

		if(c == 1) c1 = angleleft;
		if(c == 2000)	{
			c2 = angleleft;
			if(fabs(c1)-fabs(c2) <= 0.4)	{
				AUBeep();
				LCDSetPos(4,10); LCDPutString("stall");
				VWSetSpeed(vw,0,0);
				//VWDriveWait();
				VWDriveStraight(vw,-0.1,0.5);
				finishedturn = 1;
			}
		}

	}
	

}





//	go foward trying to avoid obstacles
//
void drive(float dist,float angle, float speed)	{
	
	float distleft,disttravelled,ang;
	
	PositionType pos;
	
	int frontdist,leftdist, rightdist;
	int finisheddrive;
	int obst;
	//int i,j,pixelcount;
	
	
	finisheddrive = 0;
	distleft = dist;
	
	ang = angle;
	

	VWSetPosition(vw,0,0,0);
	VWGetPosition(vw, &pos);

	

	while(finisheddrive == 0)	{
		
		VWDriveCurve(vw,distleft,ang,speed);
		
		if(angle==0) ang = 0;
		
		/*
		pixelcount = 0;

		for (i=0; i<imagerows; i+=2)	{
			for(j=0; j<imagecolumns; j+=2)	{		
				if (colourimage[i][j][0] - colourimage[i][j][1] > 30)	{	
							bwimage[i][j] = 0;
							pixelcount++;
				}
				else	{ 
					bwimage[i][j] = 255;	//set to white
				}
			}
		}
		LCDPutGraphic(&bwimage);
		*/

		frontdist = PSDGet(front);
		leftdist = PSDGet(left);
		rightdist = PSDGet(right);

		VWGetPosition(vw,&pos);
		
		
		
		if( pos.x >= distleft)	{
			finisheddrive= 1;
		}
		
		if(		(frontdist <= 80)
			)	{
			
			VWSetSpeed(vw,0,0);
			disttravelled = pos.x;
			distleft = dist - disttravelled;
			/*
			LCDSetPos(0,10);
			LCDPutFloatS(disttravelled,4,3);
			LCDSetPos(5,10);
			LCDPutFloatS(distleft,4,3);
			*/
			dist = distleft;
			VWSetPosition(vw,0,0,0);
			obst++;
			finisheddrive = 1;
		}

		

		if( (rightdist <= 70) && (angle==0))	{
			ang+=0.01;
			//AUBeep();
		}
		
		if( leftdist <= 70 && (angle==0))	{
			ang-=0.01;
			//AUBeep();
		}
		
		if(VWStalled(vw) == 1)	{
			stall++;
		}
		if( (stall==1000)||(obst==100) )	{
			stall = 0;
			obst = 0;
			VWSetSpeed(vw,0,0);
			AUBeep();
			VWDriveCurve(vw,-0.1,-angle,speed);
			finisheddrive = 1;
		}
		
	}
	
	

	
	
	
}








/******************************************************************************************************************************/
/*function to convert RGB values to hue values (copied from findball.c)*/
int RGBtoHue(BYTE r, BYTE g, BYTE b)
{
    int hue /*,sat, val*/, delta, max, min;

    max   = MAX(r, MAX(g,b));
    min   = MIN(r, MIN(g,b));
    delta = max - min;
    hue =0;
    /* initialise hue*/
  
    if (2 * delta <= max)	{
	    hue = -1;
    }
    else	{
	  if (max==r)	{
		  hue = (32*(1+g-b))/(r-min);
	  }
	  else if (max==g)	{
	   	  hue = (32*(3+r-g))/(g-min);
	  }
	  else if (max==b)	{
		  hue = (32*(5+r-g))/(b-min);
	  }
    }
	return hue;
}
/******************************************************************************************************************************/




/********************************************************************************************************************************/
/* int AnalyseBlueGoalImage(colimage goalimage)	
*/

int AnalyseBlueGoalImage(colimage im)	{
	int i, j, Bgoalpixelcount, Bgoalleftpixels, Bgoalrightpixels, Bgoalcenterpixels, Bgoalmaxregion;
	
	image bwgoalimage;
	Bgoalpixelcount = 0;
	Bgoalleftpixels = 0;
	Bgoalrightpixels = 0;
	Bgoalcenterpixels = 0;
	Bgoalmaxregion = 0;

	/*CHECK FOR BLUE*/

	for (j=0; j<imagecolumns; j+=2)	{
		for(i=0; i<15; i+=2)	{		
			//in most circumstances the B value will be higher than the R value and the G value will not exceed 90
			if (	(im[i][j][2] > im[i][j][0] ) && 
					(im[i][j][1] < 90)
				)	{	
						/*set to black and build up histogram*/
						bwgoalimage[i][j] = 0;
						Bgoalpixelcount++;
						
						if (j < 28)	{
							Bgoalleftpixels++;
						}
						else if (j > 53)	{
							Bgoalrightpixels++;
						}
						else 	{
							Bgoalcenterpixels++;
						}
			}
			else	{ 
				/*set to white*/
				bwgoalimage[i][j] = 255;
			}
		}
	}
	
	LCDPutGraphic(&bwgoalimage);

	if(Bgoalpixelcount > GOALTHRESHOLD)	{
		Bgoalmaxregion   = MAX(Bgoalcenterpixels, MAX(Bgoalrightpixels,Bgoalleftpixels));
		LCDSetPos(1,12);		
		if(Bgoalmaxregion == Bgoalcenterpixels)	{
			LCDPrintf("B C");
			return 1;
		}
		if(Bgoalmaxregion == Bgoalleftpixels)	{
			LCDPrintf("B R");
			return 0;
		}
		if(Bgoalmaxregion == Bgoalrightpixels)	{
			LCDPrintf("B L");
			return 2;
		}
	}
	else	{
		LCDSetPos(1,12);
		LCDPrintf("NBG");
		return -1;
	}		
	return -1;
}
/********************************************************************************************************************************/



/********************************************************************************************************************************/
/* int AnalyseYellowGoalImage(colimage goalimage)	
*/

int AnalyseYellowGoalImage(colimage im)	{
	int i, j, Ygoalpixelcount, Ygoalleftpixels, Ygoalrightpixels, Ygoalcenterpixels, Ygoalmaxregion;
	
	image bwgoalimage;
	Ygoalpixelcount = 0;
	Ygoalleftpixels = 0;
	Ygoalrightpixels = 0;
	Ygoalcenterpixels = 0;
	Ygoalmaxregion = 0;

	/*CHECK FOR YELLOW*/

	for (j=0; j<imagecolumns; j+=2)	{
		for(i=0; i<15; i+=2)	{		
			//in most circumstances the G value greater than B value by more than 50 and R and G wont differ much
				if (	(im[i][j][1]-im[i][j][2]>50) &&
						(fabs(im[i][j][0]-im[i][j][1]) <20)
					)	{
						/*set to black and build up histogram*/
						bwgoalimage[i][j] = 0;
						Ygoalpixelcount++;
						
						if (j < 28)	{
							Ygoalleftpixels++;
						}
						else if (j > 53)	{
							Ygoalrightpixels++;
						}
						else 	{
							Ygoalcenterpixels++;
						}
			}
			else	{ 
				/*set to white*/
				bwgoalimage[i][j] = 255;
			}
		}
	}
	
	LCDPutGraphic(&bwgoalimage);

	if(Ygoalpixelcount > GOALTHRESHOLD)	{
		Ygoalmaxregion   = MAX(Ygoalcenterpixels, MAX(Ygoalrightpixels,Ygoalleftpixels));
		LCDSetPos(1,12);		
		if(Ygoalmaxregion == Ygoalcenterpixels)	{
			LCDPrintf("Y C");
			return 4;
		}
		if(Ygoalmaxregion == Ygoalleftpixels)	{
			LCDPrintf("Y R");
			return 3;
		}
		if(Ygoalmaxregion == Ygoalrightpixels)	{
			LCDPrintf("Y L");
			return 5;
		}
	}
	else	{
		LCDSetPos(1,12);
		LCDPrintf("NYG");
		return -1;
	}	
	return -1;
}
/********************************************************************************************************************************/



/********************************************************************************************************************************/
/*function to take a glance and know whether the robot is facing the right goal and also roughly where the goal					*/
/*is relative to the robot's orientation.																						*/
/*If it sees no goal it will return -1																							*/
/*if it sees blue goal.. returns 0 if it sees left part of goal, 1 if it sees center of goal and 2 if it sees right side of goal*/																			
/*if it sees yell goal.. returns 3 if it sees left part of goal, 4 if it sees center of goal and 5 if it sees right side of goal*/
int checkgoal(int goal)
{	
	int resultofimageanalysis;
	colimage goalimage;

    CAMGetColFrame(&goalimage, FALSE);
	//if attacking blue goal check blue first then yellow
	if(goal == 1)	{
		resultofimageanalysis = AnalyseBlueGoalImage(goalimage);
		if(resultofimageanalysis != -1)	{
			return resultofimageanalysis;
		}
		else	{
			return AnalyseYellowGoalImage(goalimage);
		}
	}
	//if attacking yellow goal check yellow first then blue
	else if(goal == 0)	{
		resultofimageanalysis = AnalyseYellowGoalImage(goalimage);
		if(resultofimageanalysis != -1)	{
			return resultofimageanalysis;
		}
		else	{
			return AnalyseBlueGoalImage(goalimage);
		}
	}
	return -1;
	
}
/******************************************************************************************************************************/







/******************************************************************************************************************************/
/*function that drives the robot depending on how much of the goal it can see*/
void decide_and_execute_drive(float dist)	{
	colimage goalimage;
	CAMGetColFrame(&goalimage, FALSE);
    
	
		
	//shooting
	if(	(checkgoal(goal) == 0) && (goal == 1) )	{
		drive(dist+SHOOTPOWER, SHOOTANGLE, SHOOTSPEED);
	}
	else if(	(checkgoal(goal) == 1) && (goal == 1) )	{
		drive(dist+SHOOTPOWER, 0, SHOOTSPEED);
	}
	else if(	(checkgoal(goal) == 2) && (goal == 1) )	{
		drive(dist+SHOOTPOWER, -SHOOTANGLE, SHOOTSPEED);
	}
	else if(	(checkgoal(goal) == 3) && (goal == 0) )	{
		drive(dist+SHOOTPOWER, SHOOTANGLE, SHOOTSPEED);
	}
	else if(	(checkgoal(goal) == 4) && (goal == 0) )	{
		drive(dist+SHOOTPOWER,0, SHOOTSPEED);
	}
	else if(	(checkgoal(goal) == 5) && (goal == 0) )	{
		drive(dist+SHOOTPOWER, -SHOOTANGLE, SHOOTSPEED);
	}
		
	
	
	else	{
		//AUBeep();
		drive(dist+0.03,0,APPROACHSPEED);

		
	}
	
}
/******************************************************************************************************************************/










/*************************************************************************************************************************/
/*method to give an estimate on how far the ball is from the robot based on uppermost or lowermost line in image*/
/*a lookup table method is used*/
float gaugedistance(image bwimage)	{
	

	int i,j,bottomrowpixels,lowestcorrectpixel;
	
	bottomrowpixels = 0;
	lowestcorrectpixel = 0;
	
	//lookup table based on the lowest pixel of the image on the ball
	static float DIST_TABLE[60] = {1.5,	1.3,	1.1,	1,		0.92,	0.82,	0.77,	0.7,	0.66,	0.61,	0.56,	0.52,	0.49,	0.46,	0.44,	0.42,	0.41,	0.39,	0.37,	0.35,	0.34,	0.33,	0.31,	0.3,	0.29,	0.28,	0.27,	0.26,	0.26,	0.25,	0.24,	0.23,	0.22,	0.21,	0.2,	0.2,	0.19,	0.19,	0.18,	0.18,	0.17,	0.16,	0.16,	0.15,	0.15,	0.14,	0.14,	0.14,	0.13,	0.13,	0.13,	0.12,	0.12,	0.12,	0.11,	0.11,	0.11,	0.1,	0.05,	0.05	};

	//image analysis... we want estimate the distance that must be driven to the ball by looking at the picture
	
	
	

	//scan bottom row to check if the ball is near the robot (the bottom of the ball cannot be seen)
	for (j=0; j<imagecolumns; j++)	{
		if(bwimage[60][j] == 0)	{
			bottomrowpixels++;
		}
	}

	//else use the last row of the ball to check how far away the ball is (according to lookup table)
	if (bottomrowpixels==0)	{
		
		for (i=0; i<imagerows; i++)	{
			for(j=0; j<imagecolumns; j++)	{
				if(bwimage[i][j] == 0 && bwimage[i][j+1] == 0)	{
					lowestcorrectpixel = i;
				}
			}
		}
		
		LCDSetPos(3,12);	
		
		//LCDPutIntS(lowestcorrectpixel,5);
		LCDPutFloatS(DIST_TABLE[lowestcorrectpixel-1],3,2);
		return DIST_TABLE[lowestcorrectpixel-1];
	}
	else	{
		return 0.1;
	}
}
/******************************************************************************************************************************/











/******************************************************************************************************************************/
/*robot scans by making small turns around the pitch to see spot the ball*/
/*scan dir says which way to look arounf first.. left or right*/
void simplescan(int scandir)	{
	
	int i,j,ballpixelcount,loop;


	
	ballpixelcount=0;
	loop=0;
	
	while(ballinview == 0)	{
		
		//to check if program ends
		key = KEYRead();
		if(key == KEY4)	{
			VWStopControl(vw);
			stream = 0;
			break;
		}



		if(scandir == L)	{
			VWDriveTurn(vw, 4, SIMPLESCANTURNSPEED);
		}
		else if(scandir == R)	{
			VWDriveTurn(vw, -4, SIMPLESCANTURNSPEED);
		}



		/*@@@@@@@@@@@@@@ S T A R T   T I M I N G @@@@@@@@@@@@@@@@*/
		if(framecounter == 0)	{
			time1 = OSGetCount();
		}
		if(framecounter == 9)	{
			time2 = OSGetCount();
			fps = 1000/ ((float)time2 - (float)time1);
			LCDSetPos(6,10);
			LCDPutFloatS(fps, 5,1);
		}
		/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/

		//get the color image
		CAMGetColFrame(&colourimage, FALSE);
		//build up the binary image
		for (j=0; j<imagecolumns; j=j+2)	{
			for(i=0; i<imagerows; i=i+2)	{
				if (	(colourimage[i][j][0] - colourimage[i][j][1] > 30)
					)	{	
							//set appropriate pixels to black in binary image
							bwimage[i][j] = 0;
							ballpixelcount++;
					}
				else	{ 
					//set to white the unwanted pixels
					bwimage[i][j] = 255;
				}
			}
		}
		if(ballpixelcount > SIMPLESCANTHRESHOLD)	{
			VWSetSpeed(vw,0,0);
			ballinview = 1;
		}
		//ballpixelcount = 0;

		LCDPutGraphic(&bwimage);
		
		/*@@@@@@@ S T O P  T I M I N G @@@@@@@@@@@@@*/
		framecounter++;
		if(framecounter == 10)	{
			framecounter = 0;
		}
		/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/

		if(VWStalled(vw) == 1)	{
			stall++;
		}
		if(stall==1000)	{
			stall = 0;
			VWSetSpeed(vw,0,0);
			AUBeep();
			VWDriveStraight(vw,-0.2,1);
					
		}
		

	}


		//counter to count the cycles
		loop++;	
		if(loop == 50)	{
			loop = 0;
		}

}
/******************************************************************************************************************************/









/******************************************************************************************************************************/
/*quickcenter() is a simple function that helps the robot track the ball and keep it relatively in the center*/
/*the ball is identified to either in the left, right or center of the robot's view and the robots orientation is quickly shifted accordingly*/
void quickcenter()	{
	
	

	/*pixel counters to count number of correct pixels in region (left, right or center) and also a counter to store the largest of these*/
	int leftcount, centercount, rightcount, maxcount;

	/*event counters to store the number of consecutive loops that the ball is spotted in the area*/
	int leftvote, rightvote, centervote, noballvote;;

	/*counter to count the number of correct pixels there are, ie the no of pixels that make up the ball in the image*/
	int ballpixelcount;

	/*counters for looping through pixels*/
	int i,j;
	
	
	

	float dist;

	centervote = 0; leftvote = 0; rightvote = 0, noballvote = 0;

	/*main loop that ends the ball is not in sight for a certain period of time... or when KEY4 is pressed*/
	/*note that events in the loop are sequential*/
	
	while(ballinview == 1)	{

		//to check if program ends
		key = KEYRead();
		if(key == KEY4)	{
			VWStopControl(vw);
			//VWRelease(vw);
			stream = 0;
			break;
		}


		/*reset pixel counters*/
		leftcount = 0;
		centercount = 0;
		rightcount = 0;
		maxcount = 0;
		ballpixelcount =0;
		

		/*@@@@@@@@@@@@@@ S T A R T   T I M I N G @@@@@@@@@@@@@@@@*/
		if(framecounter == 0)	{
			time1 = OSGetCount();
		}
		if(framecounter == 9)	{
			time2 = OSGetCount();
			fps = 1000/ ((float)time2 - (float)time1);
			LCDSetPos(6,10);
			LCDPutFloatS(fps, 5,1);
		}
		/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/



		/*get the color image*/
		CAMGetColFrame(&colourimage, FALSE);
	

		/*build up the binary image*/		
		for (i=0; i<imagerows; i=i+LOOPSTEP)	{
			for(j=0; j<imagecolumns; j=j+LOOPSTEP)	{
				
				if (	//(colourimage[i][j][0] - colourimage[i][j][2] > 50) && 
						(colourimage[i][j][0] - colourimage[i][j][1] > 30)
					)	{	
							/*set appropriate pixels to black in binary image*/
							bwimage[i][j] = 0;

							/**/
							if (j < 28)	{
								leftcount++;
							}
							else if (j > 53)	{
								rightcount++;
							}
							else 	{
								centercount++;
							}
							ballpixelcount++;
				}
				else	{ 
					/*set to white the unwanted pixels*/
					bwimage[i][j] = 255;
				}
			}
		}



		/*find out in which region had the most correct pixels*/
		maxcount   = MAX(leftcount, MAX(rightcount,centercount));


		if ( maxcount > QUICKCENTERTHRESHOLD)	{
			//note that a voting system is employed and the ball has to be at a certain place for a certain number 
			// of cycles before an event is called.. this is to discourage unneccessary function calls
			
			if (maxcount == centercount)	{
				LCDSetPos(0,15);	
				LCDPrintf("C");
				leftvote = 0; rightvote = 0; noballvote = 0;	//reset vote
				centervote++;
				LCDSetPos(0,0);	
				LCDPutIntS(centervote,2);	//display the count	
				if(centervote == 6)	{
					LCDSetPos(0,15);	
					LCDPrintf("c");	
					centervote =0;
					//ballcentered = 1;
					dist = gaugedistance(bwimage)-0.05;
					decide_and_execute_drive(dist);
					//OSWait(30);

					break;
					//centerdone = 1;
				}
				
			}
			if (maxcount == leftcount)	{
				leftvote++;
				//so the robot knows that it last saw the ball at left side
				scandirection = L;
				LCDSetPos(0,0);	
				LCDPutIntS(leftvote,2);		//display the count
				if (leftvote ==2)	{

					VWDriveTurn(vw, QUICKCENTERTURNANGLE, QUICKCENTERTURNSPEED);
					leftvote =0;
				}
				noballvote = 0;
				rightvote = 0;
				centervote = 0;

				LCDSetPos(0,15);	
				LCDPrintf("L");

			}
			if (maxcount == rightcount)	{
				rightvote++;
				scandirection = R;
				LCDSetPos(0,0);
				LCDPutIntS(rightvote,2);		//display the count
				if (rightvote ==2)	{
					VWSetSpeed(vw,0,0);
					VWDriveTurn(vw, -QUICKCENTERTURNANGLE, QUICKCENTERTURNSPEED);
					rightvote=0;
				}
				leftvote = 0;
				noballvote = 0;
				centervote = 0;
				LCDSetPos(0,15);	
				LCDPrintf("R");
				
			}
		}
		else{
			LCDSetPos(0,15);
			LCDPrintf("N");
			noballvote++;
			LCDSetPos(0,0);
			LCDPutIntS(noballvote,1);		//display the count
			leftvote = 0; rightvote = 0; centervote = 0;
			
			
			if(noballvote > 5)	{
				LCDSetPos(0,15);
				LCDPrintf("n");
				//AUBeep();
				noballvote = 0;
				ballinview =0;
			}
			
		}
		
		LCDPutGraphic(&bwimage);

		/*@@@@@@@ S T O P  T I M I N G @@@@@@@@@@@@@*/
		framecounter++;
		if(framecounter == 10)	{
			framecounter = 0;
		}
		/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/





		
		
	}
	
}
/******************************************************************************************************************************/















/******************************************************************************************************************************/
/*Code for the SET menu to set and adjust all the variables*/
void setmenu()	{
	
	int setmenurunning, numvars, var_to_vary, lineforpointer, page;

	setmenurunning = 1;
	numvars = 13;
	var_to_vary = 0;
	lineforpointer = 0;
	page = 0;

	LCDClear();
	LCDMenu("nxt", "+", "-", "Bk");
	

	do{	
		/*DISPLAY*/
		LCDSetPos(lineforpointer, 0);
		LCDPutChar('*');
		if(page == 0)	{
			LCDSetPos(0,1);	LCDPutString("Goal     ");	
				LCDSetPos(0,11);	
				if(goal == 1)	LCDPutString("blue");
				else			LCDPutString(" yel");
			LCDSetPos(1,1);	LCDPutString("ScanDir  ");	
				LCDSetPos(1,11); 
				if(scandirection == L)	LCDPutString("   L");
				else LCDPutString("   R");
			LCDSetPos(2,1);	LCDPutString("LoopStep ");	LCDSetPos(2,11);	LCDPutIntS(LOOPSTEP,4);
			LCDSetPos(3,1);	LCDPutString("ScanSpd  ");	LCDSetPos(3,11); LCDPutFloatS(SIMPLESCANTURNSPEED,4,2);
			LCDSetPos(4,1);	LCDPutString("ScanAng  ");	LCDSetPos(4,11); LCDPutFloatS(SIMPLESCANTURNANGLE,4,2);
			LCDSetPos(5,1);	LCDPutString("ScanThres");	LCDSetPos(5,11); LCDPutIntS(SIMPLESCANTHRESHOLD,4);
			LCDSetPos(6,1);	LCDPutString("CentSpd  ");	LCDSetPos(6,11); LCDPutFloatS(QUICKCENTERTURNSPEED,4,2);
			
		}
		else if(page == 1)	{
			LCDSetPos(0,1);	LCDPutString("CentAng  ");	LCDSetPos(0,11); LCDPutFloatS(QUICKCENTERTURNANGLE,4,2);
			LCDSetPos(1,1);	LCDPutString("CentThres");	LCDSetPos(1,11); LCDPutIntS(QUICKCENTERTHRESHOLD,4);
			LCDSetPos(2,1);	LCDPutString("ShootAng ");	LCDSetPos(2,11); LCDPutFloatS(SHOOTANGLE,4,2);
			LCDSetPos(3,1);	LCDPutString("ShootSpd ");	LCDSetPos(3,11); LCDPutFloatS(SHOOTSPEED,4,2);
			LCDSetPos(4,1);	LCDPutString("ShootPwr ");	LCDSetPos(4,11); LCDPutFloatS(SHOOTPOWER,4,2);
			LCDSetPos(5,1);	LCDPutString("GoalThres");	LCDSetPos(5,11); LCDPutIntS(GOALTHRESHOLD, 4);
			LCDSetPos(6,1);	LCDPutString("         ");	LCDSetPos(6,11); LCDPutString("    ");
		}
		

		switch(KEYGet())	{
			case KEY1:

				/*Pointer adjustment*/

				/*clear old pointer*/
				LCDSetPos(lineforpointer, 0);
				LCDPutChar(' ');

				/*nxt variable to adjust and next line to displat pointer*/
				var_to_vary++;
				lineforpointer++;

				/*if reach end of screen or the max number of variables then adjust pointer back*/
				/*note that we count the number of variables starting from zero*/
				if( (lineforpointer == 7) || (var_to_vary == numvars) )	{	
					lineforpointer = 0;
				}
				
				/*2nd page*/
				if(var_to_vary == 7)	{
					page = 1;
				}

				// add in here if need more pages

				/*back to 1st page*/
				if(var_to_vary == numvars)	{
					var_to_vary = 0;
					page = 0;
				}
				break;

			case KEY2:
				switch(var_to_vary)	{
					
					case 0:
						if(goal == 1) goal = 0;
						else goal = 1;
						break;

					case 1:	
						if(scandirection == L)	{
							scandirection = R;
						}
						else if(scandirection == R)	{
							scandirection = L;
						}
						break;

					case 2:
						LOOPSTEP = LOOPSTEP + 1;
						break;
					
					case 3:
						SIMPLESCANTURNSPEED = SIMPLESCANTURNSPEED + 0.1;
						break;
					
					case 4:
						SIMPLESCANTURNANGLE = SIMPLESCANTURNANGLE + 0.05;
						break;
					
					case 5:
						SIMPLESCANTHRESHOLD = SIMPLESCANTHRESHOLD + 1;
						break;
					
					case 6:
						QUICKCENTERTURNSPEED = QUICKCENTERTURNSPEED + 0.1;
						break;
					
					case 7:
						QUICKCENTERTURNANGLE = QUICKCENTERTURNANGLE + 0.05;
						break;

					case 8:
						QUICKCENTERTHRESHOLD = QUICKCENTERTHRESHOLD + 1;
						break;
						
					case 9:
						SHOOTANGLE = SHOOTANGLE + 0.05;
						break;

					case 10:
						SHOOTSPEED = SHOOTSPEED + 0.05;
						break;

					case 11:
						SHOOTPOWER = SHOOTPOWER + 0.05;
						break;
					
					case 12:
						GOALTHRESHOLD = GOALTHRESHOLD + 1;
						break;
				} 
				break;

			case KEY3:
				switch(var_to_vary)	{
					
					case 0:
						if(goal == 1) goal = 0;
						else goal = 1;
						break;

					case 1:	
						if(scandirection == L)	{
							scandirection = R;
						}
						else if(scandirection == R)	{
							scandirection = L;
						}
						break;

					case 2:
						LOOPSTEP = LOOPSTEP - 1;
						break;
					
					case 3:
						SIMPLESCANTURNSPEED = SIMPLESCANTURNSPEED - 0.1;
						break;
					
					case 4:
						SIMPLESCANTURNANGLE = SIMPLESCANTURNANGLE - 0.05;
						break;
					
					case 5:
						SIMPLESCANTHRESHOLD = SIMPLESCANTHRESHOLD - 1;
						break;
					
					case 6:
						QUICKCENTERTURNSPEED = QUICKCENTERTURNSPEED - 0.1;
						break;
					
					case 7:
						QUICKCENTERTURNANGLE = QUICKCENTERTURNANGLE - 0.05;
						break;

					case 8:
						QUICKCENTERTHRESHOLD = QUICKCENTERTHRESHOLD - 1;
						break;

					case 9:
						SHOOTANGLE = SHOOTANGLE - 0.05;
						break;

					case 10:
						SHOOTSPEED = SHOOTSPEED - 0.05;
						break;

					case 11:
						SHOOTPOWER = SHOOTPOWER - 0.05;
						break;

					case 12:
						GOALTHRESHOLD = GOALTHRESHOLD - 1;
						break;
						
				} 
				break;

			case KEY4:
				setmenurunning = 0;
				break;
		}


	}while(setmenurunning == 1);
}
/******************************************************************************************************************************/











/******************************************************************************************************************************/
int main()	{	
	LCDClear();
	/*initialize camera*/
	CAMInit(NORMAL);
	CAMSet (FPS1_875, 0, 0);
    // CAMMode(AUTOBRIGHTNESS);

	/*initialise driving*/
	vw = VWInit(VW_DRIVE,1);
	//VWStartControl(vw,7,0.3,7,0.1);
	ballinview = 0;
	//start off with scanning left when can't see the ball...
	scandirection = L;
	//start off scoring in blue goal
	goal = 0;

	LOOPSTEP = 1;
	APPROACHSPEED = 0.2;
	QUICKCENTERTHRESHOLD = 5;
	SIMPLESCANTHRESHOLD = 13;

	/*define speeds and angles for quickcenter and turnspeed*/
	QUICKCENTERTURNSPEED = 2;
	QUICKCENTERTURNANGLE = 0.15;
	SIMPLESCANTURNSPEED = 1;
	SIMPLESCANTURNANGLE = 0.75;

	SHOOTANGLE = 0.4;	/*angle that the robot drive turns when shooting the ball to goal*/
	SHOOTSPEED = 1.6;	/*speed at which robot drives to ball when shooting*/
	SHOOTPOWER = 0.2;	/*the extra distance that is added to the distance to the ball so that it 'shoots' the ball with some follow through (ie power)*/

	GOALTHRESHOLD = 20;

	framecounter = 0;

	stall = 0;
	obst = 0;
	

	
		do{

		LCDClear();

		LCDSetPos(0,2);
		LCDPutString("E.DaviDs Bot");
		LCDSetPos(1,2);
		LCDPutString("ball-tracker");
		
		
		LCDMenu("GO!", "SET", "", "END");	
		

		switch(KEYGet())	{
			case KEY1:
				LCDClear();
				LCDSetPos(5,10);
				LCDPrintf("fps:");
				LCDSetPos(3,10);
				LCDPrintf("d:");
				LCDSetPos(0,10);
				LCDPrintf("pos:");
				LCDSetPos(1,10);
				LCDPrintf("g:");
				LCDMenu("", "", "", "Stp");
				//vw = VWInit(VW_DRIVE,1);
				VWStartControl(vw,7,0.3,7,0.3);
				front = PSDInit(PSD_FRONT);
				left = PSDInit(PSD_LEFT);
				right = PSDInit(PSD_RIGHT);
				PSDStart(front,TRUE);
				PSDStart(left,TRUE);
				PSDStart(right,TRUE);
				
				stream = 1;
				while(stream == 1)	{
					//turn(-1.57,0.5);
					//stream = 0;
					simplescan(scandirection);
					quickcenter();
				}
				break;
	
			case KEY2:
				
				setmenu();
				stream = 0;
				break;

			case KEY3:

				break;

			case KEY4:
				VWSetSpeed(vw,0,0);
				VWStopControl(vw);
				LCDClear();
				LCDPutString("Game Over!");
				
				
				stream = -1;
				break;

		}
	}while(stream != -1);

	VWSetSpeed(vw,0,0); 
	VWStopControl(vw);
	VWRelease(vw);
	PSDStop();
	PSDRelease();
	CAMRelease();
	return 0;
	
}
/******************************************************************************************************************************/