/////////////////////////////////////////////////////////
//
//
//	Team Prometheus Artificial Intelligence
//	2001 Steven Olson, Chad Dawson
//
//  This is the code which we used in conjunction with our vision
//	system to control the robot.  The vision system fills the loc
//	structure with the x and y coordinates of the robot, ball and
//	the opponent as well as the angle of your robot.
//
//  The basic setup of the AI is this: when this code is called
//	it runs through the AI code at the section called "AI".
//	This code decides on a course of action based on where it is
//	in the game, who it is playing, where everything is on the
//	field, and what it all is doing.
//	
//	Then, various subcommands are called, such as SteveGoaliePosition,
//	or Ricochet, and these functions decide what to do on a more basic level.
//	Finally, these subcommands call basic command primitives, such as
//	ChadAngle or ChadPosition, which send the	correct velocities to the
//	robot by storing values in LeftVelocity and RightVelocity, two global
//	variables.  These variables are used in jcontrol.c where they are serially
//	sent out to the motors.
//
//
/////////////////////////////////////////////////////////


#include "teststrategy.h"
#include "vision_simplest.h"
#include <math.h>

//	Many of these #defines are used.  Many are not.

#define ANGLEDEADZONE 8
#define DAMPFACTOR 125.0
#define MAXSPEED 60
#define MINSPEED 20
#define FORWARD 1
#define BACKWARDS 2

#define POSITIONRADIUS 12 //this can be decreased to fit a tighter spot
#define ANGLEACCURACY ANGLEDEADZONE
#define DEFENDX 35        //This is where the goalie initially wants to be
#define DEFENDY 148
#define MIDCOURT 312
#define BOTTOMGOAL 204
#define TOPGOAL 102
#define DISTANCETOBALLSPIN 30
#define GOALIEYDEFENDACCURACY 10
#define GOALIESPEED 100	//This is how fast he defends the goal
#define GOALIEBOX 50
#define RADIUS 4
#define SCALE .8
#define ANGLEFILTERVAL 45
//#define TEST1
#define BOTTOMOFFIELD 305
#define TOPOFFIELD 0
#define	FREEBALL 0
#define MYBALL 1
#define	ENEMYBALL 2
#define JOINTBALL 3
#define WIDTHOFROBOT 20 //25
#define OPPONENTGOALX  614
#define OPPONENTGOALY  DEFENDY
#define MIDDLEOFCOURTX 312
#define MIDDLEOFCOURTY 150
#define BALLBUFSIZE 20
#define BALLVECTORLENGTH 3 //This helps ball prediction; Must be less than BallBufSize
#define BUFSIZE 20
#define BALLBEHAVIORSIZE 5
#define ROBOTSTUCKDIST 50
#define WALLSTUCKDIST 25
#define STUCKDIST 10
#define STUCKTIME 10
#define BALLSITWAITTIME 100
#define BALL 1
#define OPPONENT 2
#define THETAEBUFLENGTH 3
#define POSBUFLENGTH  3
#define LOSTBALLX 426
#define LOSTBALLSHORTX   150
#define PLAYDEADWAITTIME 180

#define ballmovingtowardsourgoal 0
#define ballmovingtowardsenemygoal 1
#define ballsitting 2
#define ballslowmove 3

#define guarding 0
#define guiding 1
#define kicking 2
#define sitting 3
#define malfunctioning 4

#define _goalie 0
#define _position 1
#define _kick 2
#define _opengoal 3
#define _fake 4
#define _behindball 5
#define _ricochet 6
#define _getbehindball 7

#define SWERVECYCLES  50


//	Uncomment PRINTAI in order to have information printed on the screen
//	as you control the robot.

//#define PRINTAI

//#define DEBUG


extern Loco loc;
extern int LeftVelocity;
extern int RightVelocity;
extern int firstrun;

int Ballx[BALLBUFSIZE];
int Bally[BALLBUFSIZE];
int PosBufx[BUFSIZE];
int PosBufy[BUFSIZE];
int BufCount;
int stuck;
int stuckvl;
int stuckvr;
int ballypredict;
int prevVR;
int prevVL;
int prevDZ;
int vr_offset;
int vl_offset;
int singletask;
int g_direction;
int movecountg;
int movecountg_opp;
int movecount2_s;
int movecount2;
int movecount_s;
int democount;
int testangle;
int testangle2;
int testangle3;
int testspeed;
int singletask_opp;
int tmp_angle;
int nKick;
int counter;
int lastangle;
int lastbx;
int lastby;
int counter2;
int comm[2];
int whohasball;
int TEST1;
int lostball;
int lostcount;
int out_of_box;
int attackanglehigh;
int attackanglelow;
int first;
int theta_eBUF[4];
float testfloat;
int avoidobject;
int steveanglecount;
int ballindex;
int ballindexlow;
int kickcount;
int sskickcount;
int positionstop;
int bbspotflag;
int theta_e_buf[THETAEBUFLENGTH];
int last_de;
int acount,bcount;
int integral_count;
int vel_call_count;
int goaliemoving;
int cornerstuckvl,cornerstuckvr;
int robotstuckcheck;
int StuckOnRobot;
int StuckOnRobotCount;
int VelocitySent;
int goalisopen;
int maxposspeed;
int trapballflag;
int waitend;
int Swerve_dir;

int enemybehavior;
int enemysitcount;
int attacktype;
int bbdirection;
int offenseflag;
int opening;
int openingshot;
int runcount;

int ballsittingcount;
int BALLSITTINGTHERE;
int ballbehavior;
int ballbehaviorindex;
int lastballbehavior[BALLBEHAVIORSIZE];

int firstcount;
int firstfill;

int lastmove[20];

int rickc;
int ricok;

	
  		
int pos_e_y_buf[POSBUFLENGTH];
  	

void My_Strategy()
{	
int angle1,i,j,tmp,ballmove;
int circx,circy,circxo,circyo;
double ballzonedist,ballzonedisto;

/////////////////////////////////////////////////////////////////////////////////////////

//     IIIIIIIIII  NN     NN  TTTTTTTT  RRRRRRR    OOOOOO
//         II      NNNN   NN     TT     RR    RR  OO    OO
//         II      NN NN  NN     TT     RR   RR   OO    OO
//         II      NN  NN NN     TT     RRRRRR    OO    OO
//         II      NN   NNNN     TT     RR   RR   OO    OO
//     IIIIIIIIII  NN     NN     TT     RR   RR    OOOOOO

/////////////////////////////////////////////////////////////////////////////////////////


//printf("US (%d,%d,<%d), THEM (%d,%d), BALL (%d,%d)\n",loc.rx,loc.ry,loc.ra,loc.ox,loc.oy,loc.bx,loc.by);


//	If you need to use any initialized global variables, create them in the above
//	section and then give them their initial values in the below section.
//	This allows for the program to be run again from the Start up interface without
//	having any problems.

 	if (firstrun)
 	{
 		ricok = 0;
 		Swerve_dir = 0;
 		BufCount=0;
		stuck = 0;
		stuckvl = 0;
		stuckvr = 0;
		ballypredict=0;
		prevDZ=0;			
		vl_offset=0;
		singletask=0;
		g_direction=0;
		movecountg=0;
		movecountg_opp=0;
		movecount2_s=0;
		movecount2=0;
		movecount_s=0;
		democount=0;
		testangle=0;
		testangle2=0;
		testangle3=0;
		testspeed=0;
		singletask_opp=0;
		tmp_angle=0;
		nKick = 0;
		counter = 0;
		lastangle=0;
		lastbx=0;
		lastby=0;
		counter2 = 0;
		TEST1 = 0;
		lostball=0;
		lostcount = 0;
		first=0;
		testfloat=0.0;
		avoidobject=0;
		steveanglecount=0;
		ballindex=BALLVECTORLENGTH;
		ballindexlow=0;
		kickcount = 0;
		sskickcount = 0;
		positionstop = 1;
		bbspotflag = 0;
		goaliemoving=0;
		robotstuckcheck=1;
		StuckOnRobot = 0;
		StuckOnRobotCount = SWERVECYCLES;
		enemysitcount=0;
		offenseflag=0;
		opening = 1;
		runcount = 1;
		ballsittingcount = 0;
		BALLSITTINGTHERE = 0;
		ballbehaviorindex=0;
		firstcount = 1;	
		firstfill = 0;
		LeftVelocity = 0;
		RightVelocity = 0;
		firstrun = 0;
		goalisopen = 0;
		waitend = 0;
		trapballflag = 0;
		rickc = 0;
		openingshot = 0;
		printf("INITIALIZING\n");
	}
  VelocitySent=0;
 	
 	loc.obd = sqrt((double)((loc.ox - loc.bx)*(loc.ox - loc.bx)) + (double)((loc.oy - loc.by)*(loc.oy - loc.by)));
	loc.rbd = sqrt((double)((loc.rx - loc.bx)*(loc.rx - loc.bx)) + (double)((loc.ry - loc.by)*(loc.ry - loc.by)));
  loc.rod = sqrt((double)((loc.rx - loc.ox)*(loc.rx - loc.ox)) + (double)((loc.ry - loc.oy)*(loc.ry - loc.oy)));
 	
 	if (!firstfill)
	{
		for (i=0;i<BALLBUFSIZE;i++)
			{
			Bally[i] = 0;
			Ballx[i] = 0;
			}
		for (i=0;i<BALLBEHAVIORSIZE;i++)
			{
			lastballbehavior[i] = 0;
			}
	}
	firstfill = 1;
			
  if (1)
  {
	PosBufx[BufCount] = loc.rx;
	PosBufy[BufCount] = loc.ry;
	}
	BufCount++;
	
	if (BufCount >= BUFSIZE)
	{
		BufCount = 0;
	}
	
	if (firstcount > 0)
		firstcount++;
	if (firstcount > BUFSIZE)
	{
		first = 1;
		firstcount = 0;
		if (!RFCHECK)
		{
			printf("Press a key to begin!\n");
			getchar();
		}
	}
		
	
  ////////////////////////This part filters bad angle values.////////////////////////
	if (loc.ra == -1) {
			loc.ra = lastangle;
			//printf("GOT BAD ANGLE!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
		}
	if (loc.lostball) {
			loc.bx = lastbx;
			loc.by = lastby;
			lostball = 1;
			//printf("----------------------------------GOT NO BALL!\n");
		}
	else
		lostball = 0;

	lastangle = loc.ra;
	lastbx = loc.bx;
	lastby = loc.by;
	
	/////////////////////////////////////////////////////////////////////////////////////
		
	counter++;
	if (counter==10)
		counter = 0;

		
	///////////////////////////Ball Prediction and Behavior///////////////////////////		
	
	
	ballindex++;
	ballindexlow++;
	if (ballindex >= BALLBUFSIZE)
		ballindex = 0;
	if (ballindexlow >= BALLBUFSIZE)
		ballindexlow = 0;
	
  Bally[ballindex] = loc.by;
	Ballx[ballindex] = loc.bx;
	
		

	lastballbehavior[ballbehaviorindex] = Ballx[ballindex] - Ballx[ballindexlow];
	ballbehaviorindex++;
	if (ballbehaviorindex > 4)
		ballbehaviorindex = 0;
	
	ballmove = 0;
	for (i=0;i<BALLBEHAVIORSIZE;i++)
		{
		ballmove = ballmove + lastballbehavior[i];
		}
	ballmove = ballmove / 5;
	
	
	if (ballmove <= -5)
		{
		ballbehavior = ballmovingtowardsourgoal;
		ballsittingcount = 0;
		BALLSITTINGTHERE = 0;
		}
	else if (abs(ballmove) <= 1)
		{
		ballbehavior = ballsitting;
		ballsittingcount++;
		if (ballsittingcount > BALLSITWAITTIME)
			{
			ballsittingcount = 0;
			BALLSITTINGTHERE = 1;
			}
		}
	else if (ballmove >= 2)
		{
		ballbehavior = ballmovingtowardsenemygoal;
		ballsittingcount = 0;
		BALLSITTINGTHERE = 0;
		}
	else
		{
		ballbehavior = ballslowmove;
		ballsittingcount = 0;
		BALLSITTINGTHERE = 0;		
		}
	
	if (abs(ballmove)>2)
		ballypredict = Bally[ballindex] + (Bally[ballindexlow]-Bally[ballindex])*(Ballx[ballindex] - loc.rx)/(ballmove);
	if(ballypredict < TOPGOAL) //The ball will ricochet off the wall
		ballypredict = TOPGOAL;
	else if (ballypredict > BOTTOMGOAL)
		ballypredict = BOTTOMGOAL;


	/////////////////////////////////////////////////////////////////////
	

	
	/////////////////////////Ball Possesion//////////////////////////////
	circx = loc.rx + WIDTHOFROBOT * cos(2*M_PI*((double)loc.ra/360));
	circy = loc.ry - WIDTHOFROBOT * sin(2*M_PI*((double)loc.ra/360));
	ballzonedist = sqrt((double)((loc.bx-circx)*(loc.bx-circx))+(double)((loc.by-circy)*(loc.by-circy)));
  ballzonedisto = sqrt((double)((loc.bx-loc.ox)*(loc.bx-loc.ox))+(double)((loc.by-loc.oy)*(loc.by-loc.oy)));
		

  if (loc.ox < LOSTBALLX)
  	goalisopen = 1;
  else
  	goalisopen = 0;

 //added this for the lost ball case
	if ((whohasball == MYBALL)&&(lostball))
		{
		whohasball = MYBALL;
	
		loc.by = loc.ry - WIDTHOFROBOT * sin(2*M_PI*(double)loc.ra/180.);
		loc.bx = loc.rx + WIDTHOFROBOT * cos(2*M_PI*(double)loc.ra/180.);
		lostcount = 0;
		enemysitcount = 0;
		}
	else if ((whohasball == ENEMYBALL)&&(lostball))
		{
		whohasball = ENEMYBALL;
		loc.by = loc.oy;
		loc.bx = loc.ox;
		lostcount = 0;
		enemysitcount = 0;
		}
	else if ((whohasball == JOINTBALL)&&(lostball))
		{
		whohasball = JOINTBALL;
		lostcount = 0;
		enemysitcount = 0;
		}
		
	else
	{
		if ((ballzonedist < (double)(WIDTHOFROBOT))&&(ballzonedisto < (double)(2*WIDTHOFROBOT)))
			{
			whohasball = JOINTBALL;
			lostcount = 0;
			enemysitcount = 0;
			}
		else if (ballzonedist < (double)(WIDTHOFROBOT))
			{
			whohasball = MYBALL;
			lostcount = 0;
			enemysitcount = 0;
			}
		else if (ballzonedisto < (double)(2*WIDTHOFROBOT))
			{
			whohasball = ENEMYBALL;
			lostcount = 0;
			if ((abs(Ballx[ballindex] - Ballx[ballindexlow]) < 2)&&(abs(Bally[ballindex] - Bally[ballindexlow]) < 2))
			{
				enemysitcount++;
				if (enemysitcount > 10)
				{
					enemysitcount = 0;
					enemybehavior = sitting;
				}
			}
			else if (Ballx[ballindex] - Ballx[ballindexlow] > 7)
				{
				enemysitcount = 0;
				enemybehavior = malfunctioning;
				}
			else
				{
				enemybehavior = guiding;
				enemysitcount = 0;
				}
			}
		else
			{
			enemysitcount = 0;
			lostcount++;
				if (lostcount == 6)
					{
					whohasball = FREEBALL;
					lostcount = 0;
					}
			}
	
	}

	/////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////////////////////////
//
//   CCCC  OOOO  MM   MM MM   MM  AAA  NN   N DDDD
//  C     O    O M M M M M M M M A   A N N  N D   D
//  C     O    O M  M  M M  M  M AAAAA N  N N D   D
//   CCCC  OOOO  M     M M     M A   A N   NN DDDD
//
///////////////////////////////////////////////////////////////////////////////////////////////////


/*
if (!first)
  {
  	switch(lastmove[0])
  	{
  		case _goalie:
  		break;
  		case _position:
  		break;
  		case _kick:
  		break;
  		case _opengoal:
  		break;
  		case _fake:
  		break;
  		case _behindball:
  		break;
  		case _ricochet:
  		break;
  		case _getbehindball:
  		break;
  	}
	}
*/

if (!RFCHECK)
{
	if (first)
 	{
 	
		if (openingoffense)
			{
			OpeningOffense();
			}
		else if (openingdefense)
			OpeningDefense();
		else
		{	
			if (!offenseflag)
			{
				#ifdef PRINTAI
				printf("D");
				#endif
				Defense();
			}
			else
			{
				#ifdef PRINTAI				
				printf("O");
				#endif				
				Offense();
			}
		}
	}
}

if (RFCHECK)
{
	printf("Running RF Check\n");
	if (runcount<=20)
		Velocity(103,103);
	//else if (runcount == 100 || runcount == 101 || runcount == 102 || runcount == 103 )         //these lines send bad characters.
		//Velocity(50,50);
	else if (runcount < 120)
		{
		//printf("Sent 100  ");
		Velocity(0,0);
		}
	else if (runcount>=120)
		Velocity(102,102);
	else;
  printf("R %d  ",runcount);

	runcount++;
}
	//PrepareForBounceKick();

	
if (!RFCHECK)
{
	if (whohasball == MYBALL)
	{
		if ((goalisopen)&&(loc.ox < loc.rx)&&(loc.rx > LOSTBALLX)&&(loc.ry < BOTTOMGOAL + 5)&&(loc.ry > TOPGOAL - 5))
			{
			angle1 = (int) -180./ 3.1415 * atan2((double)(OPPONENTGOALY-loc.ry), (double)(OPPONENTGOALX-loc.rx));
			#ifdef PRINTAI
				printf(" override to goal :%d ",angle1);
			#endif
			ChadAngle(angle1);
			}
		ShootBall();
	}
}
	
//	To test single commands, comment out the three previous sections that involve
//	RFCHECK, and issue commands in the format that is commented out below
	/*
	if (first)
		ChadPosition(MIDDLEOFCOURTX,MIDDLEOFCOURTY);
  */
	
////////////////////////////////////////////////////////////////////////////////////////////////////

}

//////////////////////////////////////////////////////////////////////////////
//
//						AAAAAAAAA        IIIIIIIIIIIIII
//            AAAAAAAAA        IIIIIIIIIIIIII
//           AAAA   AAAA            IIII
//          AAAA     AAAA           IIII
//          AAAAAAAAAAAAA           IIII
//         AAAAAAAAAAAAAAA          IIII
//         AAAA       AAAA          IIII
//        AAAA         AAAA    IIIIIIIIIIIIII
//        AAAA         AAAA    IIIIIIIIIIIIII
//
//
//////////////////////////////////////////////////////////////////////////////

//  The AI section is composed of opening offense, opening defense, offense,
//	and defense.  This is the branch structure.  One thing to keep in mind
//	when using this is that if you set conditions for switching from offense
//	to defense and then you have entirely different conditions to switch back
//	you could oscillate between offense and defense.  I don't think this is
//	the best way in the world to set up the AI, it took quite a bit of debugging,
//	but it worked well once it was completed.
//
//	The opening offense and defense are only called when the teststrategy is
//	first called.  The variables startingattackstrategy and startingdefensivestrategy
//	are set in the GUI.

void OpeningOffense(void)
{

	#define WSANGLE 325
	
	offenseflag = 1;
	switch(startingattackstrategy)
	{
		case fakeattack:
			#ifdef PRINTAI							
			printf(" Opening Fake ");
			#endif			
			Fake();
			if ((ballbehavior == ballmovingtowardsourgoal)||(whohasball == ENEMYBALL))
				{
				openingoffense = 0;
				offenseflag = 1;
				}
		break;
		case fake2attack:
			#ifdef PRINTAI							
			printf(" Opening Fake2 ");
			#endif			
			Fake2();
			if ((ballbehavior == ballmovingtowardsourgoal)||(whohasball == ENEMYBALL))
				{
				openingoffense = 0;
				offenseflag = 1;
				}
		break;
		case opengoalattack:
			#ifdef PRINTAI				
			printf(" Opening Opengoal ");
			#endif	
			StuckOnRobotCount = 0;									
			Opengoal();
			StuckOnRobotCount = SWERVECYCLES;									
			if ((ballbehavior == ballmovingtowardsourgoal)||(whohasball == ENEMYBALL))
				{
				openingoffense = 0;
				offenseflag = 1;
				}
		break;
		case ricochetattack:
			#ifdef PRINTAI							
			printf(" Opening Ricochet ");
			#endif			
			Ricochet();
			if ((ballbehavior == ballmovingtowardsourgoal)||(whohasball == ENEMYBALL))
				{
				openingoffense = 0;
				offenseflag = 1;
				}
		break;
		case behindbackattack:
			#ifdef PRINTAI							
			printf(" Opening Behindback ");
			#endif			
			Behindback(0);
			if ((ballbehavior == ballmovingtowardsourgoal)||(whohasball == ENEMYBALL))
				{
				openingoffense = 0;
				offenseflag = 1;
				#ifdef PRINTAI								
				printf("\n");
				#endif				
				}
		break;
		case trapballattack:
			#ifdef PRINTAI
			printf(" TRAPPING THE BALL!:(%d,%d):%f ",loc.bx,loc.by,loc.rbd);
			#endif
			trapballflag = 1;
			TrapBall();
			trapballflag = 0;
			
			if ((whohasball == ENEMYBALL)||((whohasball==FREEBALL)&&(loc.rbd > 100.)))
				{
				openingoffense = 0;
				offenseflag = 1;
				#ifdef PRINTAI
				printf("\n");
				#endif
				}
			break;
		case playdeadattack:
			#ifdef PRINTAI
			printf("(%d,%d)",loc.ox,OPPONENTGOALX - 100);
			#endif
			Velocity(0,0);
			waitend++;
			if ((whohasball == ENEMYBALL)||(loc.ox < OPPONENTGOALX - 80))
				{
				openingshot = 1;
				/*openingoffense = 0;
				offenseflag = 1;
				#ifdef PRINTAI
				printf("\n");
				#endif
				*/
				}
			else if(waitend > PLAYDEADWAITTIME)
			 {
			 	attacktype = fakeattack;
				openingoffense = 0;
				offenseflag = 1;
				#ifdef PRINTAI
				printf("\n");
				#endif
				}
			else if (openingshot)
			{
				ChadPosition(LOSTBALLSHORTX,TOPOFFIELD);
				if ((loc.ra < WSANGLE+10)&&(loc.ra > WSANGLE-10))
				{
					ricok++;
					if (ricok > 5)	
					{
						ricok = 0;		
						#ifdef PRINTAI
						printf(" FIRE!!! ");
						#endif
						Velocity(101,101);
					}
				}
			}
			
			if ((ballbehavior == ballmovingtowardsourgoal)||(whohasball == ENEMYBALL)||(loc.ry < TOPOFFIELD + 80))
			{
				openingoffense = 0;
				offenseflag = 1;
			}
			break;
		break;
	}
	//printf("%d/%d, %d<%d<%d, %d>%d, b:%d<%d||%d>%d||%d<%d\n",whohasball,MYBALL,TOPGOAL-5,loc.ry,BOTTOMGOAL+5,loc.rx,OPPONENTGOALX-20,loc.ox,loc.rx,loc.oy,WIDTHOFROBOT+loc.ry,loc.oy,loc.ry-WIDTHOFROBOT);
  if ((whohasball == MYBALL)&&(loc.ry > TOPGOAL - 15)&&(loc.ry < BOTTOMGOAL + 15)&&(loc.rx > OPPONENTGOALX-20)&&((loc.ox < loc.rx)||(loc.oy > WIDTHOFROBOT+loc.ry)||(loc.oy < loc.ry - WIDTHOFROBOT)))
	{
		#ifdef PRINTAI
		printf(" !!IN FRONT OF GOAL!!->TTS ");
		#endif
		ChadAngle(0);
	}

}

void OpeningDefense(void)
{
	if ((ballbehavior == ballmovingtowardsenemygoal)&&(startingdefensivestrategy!=chargedefense))
		{
		openingdefense = 0;
		offenseflag = 1;
		#ifdef PRINTAI						
		printf("\n");
		#endif		
		}
	else
		{
		if (startingdefensivestrategy == chargedefense)
			{
			#ifdef PRINTAI							
			printf(" Charging!!! ");
			#endif	
			StuckOnRobotCount = 0;
			Opengoal();
			StuckOnRobotCount = SWERVECYCLES;
			if ((ballbehavior == ballmovingtowardsourgoal)||(loc.ox < LOSTBALLSHORTX))
				{
				openingdefense = 0;
				offenseflag = 0;
				#ifdef PRINTAI
				printf("\n");
				#endif
				}
			}
		else if ((BALLSITTINGTHERE)&&(startingdefensivestrategy == goaldefense))
			{
			openingdefense = 0;
			offenseflag = 1;			
			#ifdef PRINTAI							
			printf("\n");
			#endif			
			}
		else 			
			{
			#ifdef PRINTAI							
			printf(" Opening Defense ");
			#endif			
			SteveGoaliePosition();
			}
		}
}
		
void Defense(void)
{
	offenseflag = 0;
	attacktype = noattackchosen;
	switch(whohasball)
	{
		case MYBALL:
			offenseflag = 1;
			break;
		case FREEBALL:    //Add different cases depending on position in court
			/*if (ballbehavior == ballmovingtowardsourgoal)
				SteveGoaliePosition();
			else*/ if ((loc.bx > DEFENDX) && (loc.bx - loc.ox > 0))
				{
				offenseflag = 1;
				#ifdef PRINTAI								
				printf("2\n");
				#endif				
				}
			else if (ballbehavior == ballsitting)
				{
				offenseflag = 1;
				#ifdef PRINTAI								
				printf("3\n");
				#endif				
				}
			else if (loc.rbd < loc.obd)
				{
				offenseflag = 1;
				#ifdef PRINTAI								
				printf("1\n");
				#endif				
				}			
			else if (loc.bx > LOSTBALLSHORTX)//MIDCOURT)
				{
				offenseflag = 1;
				#ifdef PRINTAI								
				printf("5\n");
				#endif				
				}			
			else
				{
				SteveGoaliePosition();
				}
			break;
		case ENEMYBALL:
			switch (enemybehavior)
			{
				case guarding:
					SteveGoaliePosition();
					break;
				case guiding:
					SteveGoaliePosition();
					break;
				case kicking:
					SteveGoaliePosition();
					break;
				case sitting:
					//Offense(); //Stealball
					#ifdef PRINTAI									
					printf("4\n");
					#endif					
					offenseflag = 1;
					break;
				case malfunctioning:
					#ifdef PRINTAI
					printf("5\n");
					#endif
					offenseflag = 1;
					//SteveGoaliePosition();
					break;
				break;
			}
			
		case JOINTBALL:
			SteveGoaliePosition();
			break;
		}
}
					
void Offense(void)
{
	offenseflag = 1;

	switch(whohasball)
	{
		case MYBALL:
			if (attacktype == noattackchosen)
				if (loc.ox < LOSTBALLX)
					attacktype = opengoalattack;
				else if ((loc.oy > BOTTOMGOAL)||(loc.oy < TOPGOAL))
					attacktype = opengoalattack;
				else if (loc.oy < TOPGOAL)
					{
					attacktype = opengoalattack;
					//attacktype = behindbackattack;
					bbdirection = 1;
					}
				else if (loc.oy > BOTTOMGOAL)
					{
					attacktype = opengoalattack;
					//attacktype = behindbackattack;
					bbdirection = 0;
					}
				else
					attacktype = fakeattack;
			
			switch(attacktype)
			{
				case opengoalattack:
					#ifdef PRINTAI									
					printf(" OG ");
					#endif					
					StuckOnRobotCount = 0;
					Opengoal();
					StuckOnRobotCount = SWERVECYCLES;					
					break;
				case fakeattack:
					#ifdef PRINTAI									
					printf(" F ");
					#endif					
					Fake();
					break;
				case behindbackattack:
					#ifdef PRINTAI									
					printf(" BB ");
					#endif					
					Behindback(bbdirection);
					break;
				case ricochetattack:
					#ifdef PRINTAI									
					printf(" R ");
					#endif					
					Ricochet();
					break;
			}
			if ((loc.ry > TOPGOAL - 15)&&(loc.ry < BOTTOMGOAL + 15)&&(loc.rx > OPPONENTGOALX-20)&&((loc.ox < loc.rx)||(loc.oy > WIDTHOFROBOT+loc.ry)||(loc.oy < loc.ry - WIDTHOFROBOT)))
			{
				#ifdef PRINTAI
				printf(" !!IN FRONT OF GOAL!!->TTS ");
				#endif
				ChadAngle(0);
			}
			break;
		case FREEBALL:
			//printf(" Freeball! ");
/*			if (ballbehavior == ballmovingtowardsourgoal)
				{
				offenseflag = 0;
				printf("1\n");				
				}
			else */if((loc.bx > DEFENDX) && (loc.bx - loc.ox > 0))
				{
				TEST1 = 1;
				#ifdef PRINTAI
				printf(" K1 ");
				#endif
				Kick();
				TEST1 = 0;
				}
			else if(ballbehavior == ballsitting)
				{
				TEST1 = 1;
				#ifdef PRINTAI
				printf(" K2 ");
				#endif
				Kick();
				TEST1 = 0;
				}
			else if (loc.rbd < loc.obd)
				{
				TEST1 = 1;
				#ifdef PRINTAI
				printf(" K3 ");
				#endif
				Kick();
				TEST1 = 0;
				}
			else if (loc.bx > LOSTBALLSHORTX)//MIDCOURT)
				{
				TEST1 = 1;
				#ifdef PRINTAI
				printf(" K6 ");
				#endif
				Kick();
				TEST1 = 0;
				}
			else
				{
				attacktype = 0;
				offenseflag = 0;
				#ifdef PRINTAI								
				printf("2\n");
				#endif				
				}				
			break;
		case ENEMYBALL:
			//printf(" EnemyBall! ");
			if (enemybehavior == sitting)
				//Steal The Ball
				//SteveSteal();
				{
				#ifdef PRINTAI
				printf(" K4 ");
				#endif
				Kick();
				}
			else if (loc.bx > MIDCOURT)
				{
				#ifdef PRINTAI
				printf(" GET BALL AWAY ");
				#endif
				Kick();
				}
			else
				{
				offenseflag = 0;
				#ifdef PRINTAI								
				attacktype = 0;
				printf("3\n");
				#endif				
				}				
			break;
		case JOINTBALL:
			//Yeah, right.  Good luck.
			//SteveSteal();
			#ifdef PRINTAI
			printf(" K5 ");
			#endif
			Kick();
			break;
		default:
			printf(" !>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>!>\n");
			break;
	}

			
}



		
		
		
		
		

//////////////////////////////////////////////////////////////
//                                                  PPPPPP
//     PRIMITIVES            PRIMITIVES             PP   PP
// 					PRIMITIVES            PRIMITIVES        PPPPPP
//               PRIMITIVES            PRIMITIVES   PP
//                                                  PP
//////////////////////////////////////////////////////////////


//	The primitives that are called in the code are:
//	ChadPosition and ChadAngle -- our bread and butter,
//	Velocity -- called everytime you want to set voltages for the motors,
//	AmIStuck and DontCrash
//	Don't use SteveAngle, StevePosition, or Position, their methodology leads
//	to less accuracy.
//	You can use HSChadAngle, the HS for HighSpeed, but it doesn't differ
//	much from ChadAngle.


//	Velocity
//
//	This function's main purpose is to linearize, then dump the chosen
//	velocities into two variables that are then used by the code in
//	jcontrol.  Since this was called everytime it is also a dumping ground
//	for other things, such as calling functions to avoid the ball and the
//	other robot, as	well as getting unstuck from the walls.
//	If the velocity sent to this function is greater than 100, it will pass
//	through the function untouched and not become linearized in any way.
//	This is for the kicker which activated when a velocity of (101,101) was
//	received and for RF checking.
//	Not the best setup in the world, this is a function that needs a rewrite.

void Velocity(int vl, int vr)
{
#define MAXVELOCITY 100
	  //We need to send a value from 0 to 254 for the
		//correct reception by the robot
	  char right;
	  int vr_sign,vl_sign,stuckspeed,offset;
	

	
	stuckspeed = 0;	
	printf(" initial (%d,%d) ",vl,vr);

if (!RFCHECK)
{

 		if(AmIStuck())
 		{
 			#ifdef PRINTAI
 			printf(" STUCK ");
 			#endif
			stuckspeed = 1;
			vl = stuckvl;
			vr = stuckvr;
		}
    else if (StuckOnRobot)
    	{
    	StuckOnRobotCount=0;
    	StuckOnRobot = 0;
    	}

    if (StuckOnRobotCount < SWERVECYCLES)
    	{
    	StuckOnRobotCount++;
    	#ifdef PRINTAI
    	printf("Avoiding Opponent");
    	#endif
    	offset = 3*DontCrash(OPPONENT);
    	if (offset>50)
    		offset=50;
    	if (offset<-50)
    		offset = -50;
    		
    	vl = vl-offset;
    	vr = vr+offset;
    	if (vr>100)
    		vr=100;
    	if (vr<-100)
    		vr=-100;
    	if (vl>100)
    		vl=100;
    	if (vl<-100)
    		vl=-100;
    	
    	}
}
  //printf("Final Vel:(%d,%d)  ",vl,vr);
	if (abs(vl)<= 100 )
		{
		if ((!goaliemoving)||(stuckspeed))
			{
			while((abs(vr)>MAXVELOCITY)||(abs(vl)>MAXVELOCITY))
				{
				vr=0.9*vr;
				vl=0.9*vl;
				}
			}
		else
			{
			//printf("fast vr:%d,vl:%d\n",vr,vl);
			}
	
	
		prevVR = vr;
		prevVL = vl;
	
		vr = vr + vr_offset*vr;                      //offsets are used by avoid wall function.  They are normally zero.
		vl = vl + vl_offset*vl;

		vl_offset = 0;
		vr_offset = 0;
	  //printf("sent (%d,%d)\n",vl,vr);
//	vr = vr*(1-v_scale);
//	vl = vl*(1+v_scale);
	  if (vr<0)
	  	{
	  	vr = -vr;
	  	vr_sign = -1;
	  	}
	  else
	  	vr_sign = 1;
	 	vr = vr_sign*(int)(-31*(float)vr/(float)(131-vr));
		if (vl<0)
	  	{
	  	vl = -vl;
	  	vl_sign = -1;
	  	}
	  else
	  	vl_sign = 1;
	  vl = vl_sign*(int)(-31*(float)vl/(float)(131-vl));
    if ( vr > 100 ) vr = 100;
		if ( vl > 100 ) vl = 100;
		if ( vr < -100) vr = -100;
		if ( vl < -100) vl = -100;
  }
  else
  	VelocitySent = 0;
  printf(" Linearized to (%d,%d)\n",vl,vr);
  if (!VelocitySent)
  	{
  	LeftVelocity = vl;
		RightVelocity = vr;
		VelocitySent = 1;
		}
	//if (!counter)
	//printf("sent Vr: %d, Vl: %d\n",vr,vl);
	
}


void SteveAngle(int desired_angle)
{
  #define MINANGLESPEED 15
	#define MAXANGLESPEED 35

	int turn_angle, pos, speed;

	//printf("Angle: %d AVel: %d\n",loc.ra,loc.rav);
	turn_angle = desired_angle - loc.ra;
	if (turn_angle > 180)
		turn_angle = turn_angle - 360;
	else if (turn_angle < -180)
		turn_angle = turn_angle +360;


	testangle = turn_angle;
	
	speed = (MAXANGLESPEED * (pow(((double)turn_angle/150),2))); //(double)(tanh((1/DAMPFACTOR)*turn_angle)));

 /*	if (speed>0)
		pos = -1;
	else
		pos = 1;
	*/
	if (turn_angle < 0)
		pos = 1;
	else
		pos = -1;
	speed = abs(speed);
//	if (speed>MAXANGLESPEED)
//		speed = MAXANGLESPEED;
	if (speed < MINANGLESPEED)
		speed = MINANGLESPEED;
	speed = speed*pos;

	tmp_angle = turn_angle;       //Deletable

	if (abs(turn_angle) > ANGLEDEADZONE)
	{
		Velocity(-speed, speed);
		//printf("Speed:%d Angle:%d TA:%d\n",speed,loc.ra,turn_angle);
	}
	else
	{
		Velocity(0, 0);
		//printf("Speed:%d Angle:%d TA:%d\n",0,loc.ra,turn_angle);
	}


}

void ChadAngle(int desired_angle)
{
  #define MINCHADANGLESPEED 30
	#define MAXCHADANGLESPEED 80
  #define INT_CTL 0.2
  #define INTEGRALCUTTOFF 20
  #define PROP_CTL 1.2
  #define DERIV_CTL 0.03
  #define K 50
  #define PD
  //#define DEMOANGLE

	int theta_e, Delta_theta_e, pos, speed, speed2, i, count;
	double prop_term, int_term, deriv_term;
#ifdef DEMOANGLE
	//printf("DA: %d\n",desired_angle);
	if (loc.ra>170  && loc.ra<190)
		{
		if (acount == 30)
			{
			desired_angle = 90;
			//printf("DA1: %d\n",desired_angle);
			bcount = 0;
			}
		else
			{
			acount++;
			//printf("DA2: %d\n",desired_angle);
			desired_angle = 180;
			}
		last_de = desired_angle;
		}
	else if (loc.ra<100  &&  loc.ra>80)
		{
		if (bcount == 30)
			{
			acount = 0;
			//printf("DA3: %d\n",desired_angle);
			desired_angle = 180;
			}
		else
			{
			bcount++;
			//printf("DA4: %d\n",desired_angle);
			desired_angle = 90;
			}
		last_de = desired_angle;
		}
	else desired_angle = last_de;	
	
#endif	
	//printf("DA5: %d\n",desired_angle);
	//printf("Angle: %d AVel: %d\n",loc.ra,loc.rav);
	theta_e = desired_angle - loc.ra;
	if ((theta_e<INTEGRALCUTTOFF  &&  theta_e>ANGLEDEADZONE)  ||  (theta_e >-INTEGRALCUTTOFF  &&  theta_e <-ANGLEDEADZONE))
	  {
	  if (theta_e>0)
	  	integral_count++;
	  else
	  	integral_count--;
	  }
	else
		integral_count = 0;
	
	
	
	if (theta_e > 180)
		theta_e = theta_e - 360;
	else if (theta_e < -180)
		theta_e = theta_e +360;

  for (i=0;i<THETAEBUFLENGTH-1;i++)
  	{
  	theta_e_buf[i] = theta_e_buf[i+1];
  	}
  theta_e_buf[THETAEBUFLENGTH-1]=theta_e;
  Delta_theta_e = theta_e_buf[THETAEBUFLENGTH-1]-theta_e_buf[0];

	prop_term = PROP_CTL*(float)theta_e/90;
	int_term = INT_CTL*integral_count;
	deriv_term = DERIV_CTL*(float)Delta_theta_e ;
	speed = (int)(K*(prop_term));// + deriv_term));
	speed2 = (int)(K*(prop_term + int_term + deriv_term));
	//if (int_term)
		//printf("                                      INT TERM %f\n",int_term);
	//printf("T: %d  P: %2.2f  De: %d  D: %2.2f  S: %d   S2: %d \n",theta_e,prop_term,Delta_theta_e,deriv_term,speed,speed2);
	//printf("Dt: %d ",Delta_theta_e);
	
#ifdef PD
	if (speed2 < 0)
		pos = 1;
	else
		pos = -1;
	speed2 = abs(speed2);
	if (speed2>MAXCHADANGLESPEED)
		speed2 = MAXCHADANGLESPEED;
	if (speed2 < MINCHADANGLESPEED)
		speed2 = MINCHADANGLESPEED;
	speed2 = speed2*pos;

	if (abs(theta_e) > ANGLEDEADZONE)
	{
		Velocity(-speed2, speed2);
		//printf("Speed:%d Angle:%d TA:%d\n",speed,loc.ra,turn_angle);
	}
	else
	{
		Velocity(0, 0);
		//printf("Speed:%d Angle:%d TA:%d\n",0,loc.ra,turn_angle);
	}
#endif

#ifdef P
  if (speed < 0)
		pos = 1;
	else
		pos = -1;
	speed = abs(speed);
	if (speed>MAXCHADANGLESPEED)
		speed = MAXCHADANGLESPEED;
	if (speed < MINCHADANGLESPEED)
		speed = MINCHADANGLESPEED;
	speed = speed*pos;

	if (abs(theta_e) > ANGLEDEADZONE)
	{
		Velocity(-speed, speed);
		//printf("Speed:%d Angle:%d TA:%d\n",speed,loc.ra,turn_angle);
	}
	else
	{
		Velocity(0, 0);
		//printf("Speed:%d Angle:%d TA:%d\n",0,loc.ra,turn_angle);
	}
#endif


}

void HSChadAngle(int desired_angle)
{
  #define MINHSCHADANGLESPEED 50
	#define MAXHSCHADANGLESPEED 100
  #define INT_CTL 0.2
  #define INTEGRALCUTTOFF 20
  #define PROP_CTL 1.2
  #define DERIV_CTL 0.03
  #define K 50

	int theta_e, Delta_theta_e, pos, speed, speed2, i, count;
	double prop_term, int_term, deriv_term;
	theta_e = desired_angle - loc.ra;
	if ((theta_e<INTEGRALCUTTOFF  &&  theta_e>ANGLEDEADZONE)  ||  (theta_e >-INTEGRALCUTTOFF  &&  theta_e <-ANGLEDEADZONE))
	  {
	  if (theta_e>0)
	  	integral_count++;
	  else
	  	integral_count--;
	  }
	else
		integral_count = 0;
	
	
	
	if (theta_e > 180)
		theta_e = theta_e - 360;
	else if (theta_e < -180)
		theta_e = theta_e +360;

  for (i=0;i<THETAEBUFLENGTH-1;i++)
  	{
  	theta_e_buf[i] = theta_e_buf[i+1];
  	}
  theta_e_buf[THETAEBUFLENGTH-1]=theta_e;
  Delta_theta_e = theta_e_buf[THETAEBUFLENGTH-1]-theta_e_buf[0];

	prop_term = PROP_CTL*(float)theta_e/90;
	int_term = INT_CTL*integral_count;
	deriv_term = DERIV_CTL*(float)Delta_theta_e ;
	speed = (int)(K*(prop_term));// + deriv_term));
	speed2 = (int)(K*(prop_term + int_term + deriv_term));

	if (speed2 < 0)
		pos = 1;
	else
		pos = -1;
	speed2 = abs(speed2);
	if (speed2>MAXCHADANGLESPEED)
		speed2 = MAXCHADANGLESPEED;
	if (speed2 < MINCHADANGLESPEED)
		speed2 = MINCHADANGLESPEED;
	speed2 = speed2*pos;

	if (abs(theta_e) > ANGLEDEADZONE)
	{
		cornerstuckvl = -speed2;
		cornerstuckvr =  speed2;
		//printf("Speed:%d Angle:%d TA:%d\n",speed,loc.ra,turn_angle);
	}
	else
	{
		cornerstuckvl = 0;
		cornerstuckvr = 0;
		//printf("Speed:%d Angle:%d TA:%d\n",0,loc.ra,turn_angle);
	}

}

void ChadPosition(int x, int y)
{	
	
	/*********************************************************************************************************
	**	This bit of code uses a PID scheme to control the angular velocity of the robot and                 **
	**	basically                                                                                           **
	**	a Proportional scheme to control the linear velocity.                                               **
  **	                                                                                                    **
	**	The desired position is passed in.  That position can be modified by the                            **
	**	get_behind_ball function which will be explained later.                                             **
  **	                                                                                                    **
	**	The next segment of code makes sure the target is not outside of the bounds of the field.           **
  **	                                                                                                    **
	**	d_e contains the distance to the target.                                                            **
	**	desired_angle is the angle tho robot should be pointing to be facing the target.                    **
	**	theta_e is how far the robot needs to turn.                                                         **
  **	                                                                                                    **
	**	If the robot is within a specified distance to the target, stop.                                    **
  **	                                                                                                    **
	**	The integral control only becomes a factor when the robot is close to the right                     **
	**	angle.  This keeps the robot from having a large amount of overshoot and                            **
	**	subsequent oscillations.                                                                            **
  **	                                                                                                    **
	**	I used theta_e_buf to average the last 3 angles for  the robot.  This is because                    **
	**	the angle tends to be a little noisy.  Especially for derivative control                            **
  **	                                                                                                    **
	**	The PID terms are calculated, then wieghted and added.  Dv is the differential velocity             **
	**	It will be added to one wheel and subtracted from the other.                                        **
	**	                                                                                                    **
	**	The forward velocity is calculated by subtracting a percentage of Dv from a                         **
	**	maximum speed.  This way, the robot will not follow a large curved path to the target               **
	**	but will mostly turn to face the target and then go.                                                **
  **	                                                                                                    **
	**	If there is an object we are trying to avoid, another offset is calculated which can be added in.   **
  **		                                                                                                  **
  **********************************************************************************************************/
	
	
	#define CHADPOSITIONRADIUS   	1
	#define POSINTEGRALCUTTOFF    20
	#define POSANGLEDEADZONE      5
	#define THETAEBUFLENGTH      	3
	#define P_PROP_CTL             	1.2
	#define P_INT_CTL              	0.1
	#define P_DERIV_CTL            	0.02
	#define MINCHADANGLEPOSSPEED 	5
	#define MAXCHADANGLEPOSSPEED 	65
	#define MAXCHADSTRAIGHTSPEED 	80
	#define K											50
	#define CURVERADIUSSCALINGFACTOR 1
	
	int desired_angle=0, theta_e = 0, Delta_theta_e, vl, vr;
	int dx, dy, dv;
	int d_e,offset;
 	int Dv,Df,i,Mag_Dv,sign;
 	double prop_term,int_term,deriv_term;
 	
 	maxposspeed = 100;
 	
 	if (TEST1)
 	{
 		#ifdef PRINTAI
 		printf(" G ");
 		#endif
 		get_behind_ball();
		//printf("original target (%d,%d)",x,y);
		//printf("old(%d,%d) ",x,y);
		x = x + comm[0];
		y = y - comm[1];
	 // printf("new(%d,%d)\n",x,y);
	}
 	if (x>OPPONENTGOALX)
 		x=OPPONENTGOALX;
 	else if (x<15)
 		x=15;
 	if (y<TOPOFFIELD+5)
 		y=TOPOFFIELD+5;
 	else if (y>BOTTOMOFFIELD-5)
 		y=BOTTOMOFFIELD-5;
 	
	//printf("dloc.r:(%d,%d) loc.b:(%d,%d) dx%d,tof%d",x,y,loc.bx,loc.by,DEFENDX,TOPOFFIELD);
	if (y<TOPOFFIELD+15)
 		y=TOPOFFIELD+15;
 	else if (y>BOTTOMOFFIELD-15)
 		y=BOTTOMOFFIELD-15;

 	//printf("RA: %d OA:%d ",loc.ra,loc.oa);
 	//printf("target:(%d,%d) location(%d,%d) ball:(%d,%d)\n",x,y,loc.rx,loc.ry,loc.bx,loc.by);
 	
 	dx=x-loc.rx;
	dy=y-loc.ry;
	d_e=sqrt((double)(dx*dx)+(double)(dy*dy));

	if(dx==0 && dy==0)	
		desired_angle = 90;
	desired_angle = (int)(180./M_PI*atan2((double)(dy),(double)(dx)));

	if (desired_angle < 0)
		desired_angle = -desired_angle;
	else
		desired_angle = 360 - desired_angle;
	theta_e = loc.ra - desired_angle;

	while(theta_e > 180)	theta_e -= 360;
	while(theta_e < -180)	theta_e += 360;
	
	//printf("Theta: %d \n",theta_e);
	if (d_e<CHADPOSITIONRADIUS)
		{
		//vr = 0;
		//vl = 0;
		Velocity(0,0);
		}
	
	else
		{
		if (abs(theta_e)<POSANGLEDEADZONE)
			Dv = 0;
		else
			{
			if ((theta_e<POSINTEGRALCUTTOFF  &&  theta_e>POSANGLEDEADZONE)  ||  (theta_e >-POSINTEGRALCUTTOFF  &&  theta_e <-POSANGLEDEADZONE))
	  		{
	  		if (theta_e>0)
	  			integral_count++;
	  		else
	  			integral_count--;
	  		}
			else
				integral_count = 0;
		
	 		for (i=0;i<THETAEBUFLENGTH-1;i++)
  			{
  			theta_e_buf[i] = theta_e_buf[i+1];
  			}
  		theta_e_buf[THETAEBUFLENGTH-1]=theta_e;
  		Delta_theta_e = theta_e_buf[THETAEBUFLENGTH-1]-theta_e_buf[0];

			prop_term = P_PROP_CTL*(float)theta_e/90;
			int_term = P_INT_CTL*integral_count;
			deriv_term = P_DERIV_CTL*(float)Delta_theta_e ;
			Dv = (int)(K*(prop_term + int_term + deriv_term));
			//if (int_term)
				//printf("                                      INT TERM %f\n",int_term);
			printf("Loc: (%d,%d,%d) P: %2.2f  I: %2.2f  D: %2.2f Dv: %d theta_e: %d\n",loc.rx,loc.ry,loc.ra,prop_term,int_term,deriv_term,Dv,theta_e);

			if (Dv > 0)
				sign = 1;
			else
				sign = -1;
			Mag_Dv = abs(Dv);
			if (Mag_Dv>MAXCHADANGLEPOSSPEED)
				Mag_Dv = MAXCHADANGLEPOSSPEED;
			if (Mag_Dv < MINCHADANGLEPOSSPEED)
				Mag_Dv = MINCHADANGLEPOSSPEED;
			Dv = Mag_Dv*sign;
		  }
		Df = maxposspeed-abs(Dv)/CURVERADIUSSCALINGFACTOR;

   if (avoidobject)
	   offset = 10*DontCrash(BALL);
	 else
	 	 offset = 0;
	 	
		vr = -Df - Dv - offset;
		vl = -Df + Dv + offset;

		//printf("Offset:%d ",offset);
	printf("Diff: %d, Straight: %d V(%d,%d) \n",Dv,Df,vl,vr);
		Velocity(vr,vl);
		}

}


void StevePosition(int x, int y, int speed, int final_angle)
{
	#define OFFCOURSEANGLE 35
  #define DSPEEDFACTOR 8
	int dx,dy,dz;
	int angle, angle2,angle3, dampspeed;

	dx = loc.rx - x;
	dy = loc.ry - y;
	dz = sqrt(pow(dx,2)+pow(dy,2)); //dz is the straight-line distance to the desired location

	angle = 180 + (int)(180.0/M_PI*(atan2((double)(dy),(double)(dx))));
	angle = 360 - angle;
	angle2 = abs(angle - loc.ra);
	if (angle2 > 180)
		angle2 = abs(angle2 - 360);
	angle3 = angle2; //angle2 is calculated as the degrees needed to turn to get to the correct starting angle
								// This can be either positive or negative depending on the direction to turn

	//INDEX:
	//movecount_s = 0
	//	You aren't facing the correct travel direction
	//	Run SteveAngle on angle
	//movecount_s = 1
	//	You are ready to move forward
	//	If you move forward and the angle you need to face changes,
	//	you can offset the wheel velocities to correct.
	//	If the angle difference is too great, you go back to movecount_s = 0
	//movecount_s = 2
	//	If you are in the right spot.
	//	Either you will stay where you are, or turn to the correct
	//	final angle.
	//**movecount_s = 1 && angle2 > 40
	//	This is a failsafe.  If you pass the location,
	//	Turn around and go back.

	// I don't like the way this is done
	if (((dx>POSITIONRADIUS)||(dx<-POSITIONRADIUS)||(dy>POSITIONRADIUS)||(dy<-POSITIONRADIUS))&&(movecount_s == 0))
		{
		SteveAngle(angle);				//IF I'm not close enough to the correct spot and IF I'm not at the right angle, turn.
		if (angle2<ANGLEACCURACY)
			steveanglecount++;
		else
			steveanglecount = 0;
		if (steveanglecount > 5)
			{
			movecount_s=1;
			steveanglecount = 0;
			}
		}
	else if (movecount_s != 2)		//This moves the robot forward
		{
		movecount_s = 1;
		angle2 = angle - loc.ra;
		if (angle2 > 180)
			angle2 = angle2 - 360;
		else if (angle2 < -180)
			angle2 = angle2 + 360;
		//dampspeed = ((int)(speed/10)*dz - (int)((prevVL + prevVR)/4))/3;
		dampspeed = (int)(MAXSPEED*(double)dz/400);
		if (dampspeed > MAXSPEED)
			dampspeed = MAXSPEED;
		if (dampspeed < MINSPEED)
			dampspeed = MINSPEED;
		//printf("SPEED:%d, DAMPSPEED:%d, dz:%d angle2:%d angle3:%d\n",dampspeed,dampspeed/5,dz,angle2,angle3);
		if (angle2 > 11)             //If one side is moving faster than the other, slow it down a bit.
			Velocity(-(dampspeed-(dampspeed/(DSPEEDFACTOR))),-dampspeed);
		else if (angle2 < -11)
			Velocity(-dampspeed,-(dampspeed-(dampspeed/(2*DSPEEDFACTOR))));
		else
			Velocity(-dampspeed,-dampspeed);
		//printf("dampspeed:%d\n",dampspeed);
		if (angle3 > OFFCOURSEANGLE)
			movecount_s = 0;
		}

	if((dx<=POSITIONRADIUS)&&(dx>-POSITIONRADIUS)&&(dy<=POSITIONRADIUS)&&(dy>-POSITIONRADIUS)||(movecount_s==2)) {
		movecount_s = 2;					//If I'm at the correct spot, but not at the right angle
		angle2 = abs(final_angle - loc.ra);

		if (angle2 > 180)
			angle2 = abs(angle2 - 360);
		if (angle2<ANGLEDEADZONE)    //This means you are at the right final angle
			{                          //Anything that needs to be incremented is added here
			Velocity(0,0);
			movecount_s = 0;

			if (singletask == 1)    //Singletask and movecountg are for the goalie algorithms
					singletask = 0;
			else
					{
					movecountg++;
					}
			}
		else
			{
			SteveAngle(final_angle);
			}
	}

	if ((movecount_s == 1) && (angle2 > 40))
		movecount_s = 0;
	prevDZ = dz;
}


void Position(int x, int y)
{	
	
	int desired_angle=0, theta_e = 0, vl, vr, vc, offset;
	int dx, dy, dv;
	double fda;
	double d_e,Ka;
 	double DFACT;
	//double DFACT = 60;

//	if (whohasball == MYBALL)
//		DFACT = 40;
	
	//vc = MAXSPEED;
//		if (whohasball==MYBALL)//&&(loc.ra<15)&&(loc.ra>345))
//			vc = 2*MAXSPEED;
//		else
	vc = 2*MAXSPEED/3;
	//vc = MAXSPEED/3;
//	printf("ballpos (%d, %d)", x,y);
//	TEST1 = 0;
	if (TEST1){
  	get_behind_ball();
		//printf("original target (%d,%d)",x,y);
		x = x + comm[0];
		y = y + comm[1];
	//printf(" new target (%d, %d)", x,y);
	}
	//printf("\n");


	dx=x-loc.rx;
	dy=y-loc.ry;
	//printf("x:%d,loc.rx:%d,y:%d,loc.ry:%d\n",x,loc.rx,y,loc.ry);
	//printf("dx2:%f, dy2:%f\n",(double)(dx*dx),(double)(dy*dy));
	d_e=sqrt((double)(dx*dx)+(double)(dy*dy));

	if(dx==0 && dy==0)	desired_angle = 90;
	else
		fda = (180./M_PI*atan2((double)(dy),(double)(dx)));
	desired_angle = (int)(180./M_PI*atan2((double)(dy),(double)(dx)));

	if (desired_angle < 0)
		desired_angle = -desired_angle;
	else
		desired_angle = 360 - desired_angle;
	theta_e = loc.ra - desired_angle;

//	if (theta_e > 180)
//		theta_e = -(360 - theta_e);
	while(theta_e > 180)	theta_e -= 360;
	while(theta_e < -180)	theta_e += 360;
	
	DFACT = (-1./200.)*d_e + 2.2;
	
	if (DFACT < 1)
		DFACT = 1;
	if (DFACT > 2)
		DFACT = 2;
	if (theta_e >= 0)
		dv = (pow(((double)theta_e/90.),2))*(double)DFACT*(double)vc;
		//dv = ((double)theta_e/180.)*DFACT*(double)vc + 20;
	else if (theta_e < 0)
		dv = -(pow(((double)theta_e/90.),2))*(double)DFACT*(double)vc;
		//dv = ((double)theta_e/180.)*DFACT*(double)vc - 20;		
	//printf("1:%f dv:%d ",(pow(((double)theta_e/180.),2))*(double)DFACT*(double)vc,dv);
	if (dv>80)
		dv = 80;
	if (dv<-80)
		dv = -80;
		
	vr = 75 - dv;
	vl = 75 + dv;
	//printf("x:%d,loc.rx:%d,y:%d,loc.ry:%d ",x,loc.rx,y,loc.ry);
	//printf("desired angle:%d,loc.ra:%d ",desired_angle,loc.ra);
	//printf("theta_e:%d, dv:%d,vr:%d,vl:%d, DFACT:%f, d_e:%f\n",theta_e, dv,vr,vl,DFACT,d_e);
 if ((d_e<POSITIONRADIUS)&&(positionstop)) {
			vr = 0;
			vl = 0;
		}
 if (avoidobject)
	   offset = 2*DontCrash(BALL);
	 else
	 	 offset = 0;
   //offset = 0;
   //printf("Offset: %d\n",offset);
//	vl = vl/1.5;
//	vr = vr/1.5;
//	vl = vl * (float)(MAXSPEED)/127.;
//	vr = vr * (float)(MAXSPEED)/127.;
	vl = -vl + offset;
	vr = -vr - offset;
//	printf("THESE ARE THE SENT VELOCITIES: vl=%d, vr=:%d\n",vl,vr);

	Velocity(vl, vr);

}

void SteveAvoidWall(int vl, int vr)
{
	#define WALLOFFSET 30
	
	if (loc.ry < TOPOFFIELD + WALLOFFSET)
		{
		if (loc.ra > 180)
			return;
		else if (loc.ra > 90)
			{
			vl_offset = (-loc.ra/90 + 2);
			vr_offset = -vl_offset;
			}
		else
			{
			vr_offset = (loc.ra/90);
			vl_offset = -vr_offset;
			}
		}
	else if (loc.ry > BOTTOMOFFIELD - WALLOFFSET)
		{
		}
	else if (loc.rx < DEFENDX + WALLOFFSET) //Left
		{
		}
	else if (loc.rx > OPPONENTGOALX - WALLOFFSET) //Right
		{
		}
	else
		return;
	
}


//	This wonderful function was called everytime velocity was called.  It
//	checks to see if Prometheus managed to get himself stuck against a wall
//	or another robot.
//	Essentially, this calculates the distance between Prometheus and his
//	opponent, and if they were too close, (a.k.a. stuck), this function counted
//	the number of frames in which they hadn't moved.  If this count got too
//	high, then the velocities being sent to the robot were overridden by new
//	velocities, which should resolve the situation.
//	The principle was the same for the walls, the only tricky part being the
//	corners which usually required the robot to spin to get out.

int AmIStuck(void)
{
	int i,distminx,distminy,distmaxy,distmaxx,angle1,anglelimhigh,anglelimlow;
	double robotdist;
	
	robotdist = sqrt(pow((double)(loc.rx-loc.ox),2)+pow((double)(loc.ry-loc.oy),2));
	
	if (stuck)
	{
		stuck--;
		if (stuck < 1)
			stuck = 0;
		return 1;
	}
	else if(((int)robotdist < ROBOTSTUCKDIST)&&(robotstuckcheck))
	{
		distminx = PosBufx[0];
		distmaxx = PosBufx[0];
		distminy = PosBufy[0];
		distmaxy = PosBufy[0];
		for(i=1;i<BUFSIZE;i++)
		{
			if (PosBufx[i] < distminx)
				distminx = PosBufx[i];
			else if (PosBufx[i] > distmaxx)
				distmaxx = PosBufx[i];
			if (PosBufy[i] < distminy)
				distminy = PosBufy[i];
			else if (PosBufy[i] > distmaxy)
				distmaxy = PosBufy[i];
			
		}
		if ((distmaxx - distminx + distmaxy - distminy) < STUCKDIST)
		{
			#ifdef PRINTAI
			printf(" >>Stuck Robot<< ",loc.rx,loc.ry,loc.ox,loc.oy);
			#endif
			StuckOnRobot = 1;
			stuck = STUCKTIME;
			angle1 = -180./ 3.1415 * atan2((double)(loc.ry-loc.oy), (double)(loc.rx-loc.ox));

  		if(angle1 < 0) angle1 += 360;
  		anglelimhigh = angle1 + 450;
  		anglelimlow = angle1 + 270;
  		if ((loc.ra + 360 > anglelimhigh) || (loc.ra + 360 < anglelimlow))
  		{
  			stuckvl = MAXSPEED;
  			stuckvr = MAXSPEED;
  		}
  		else
  		{
  			stuckvl = -MAXSPEED;
  			stuckvr = -MAXSPEED;
  		}
  	}
  }
  else if((loc.ry < WALLSTUCKDIST) || (loc.rx < WALLSTUCKDIST) || (loc.ry > BOTTOMOFFIELD - WALLSTUCKDIST) || (loc.rx >  OPPONENTGOALX - WALLSTUCKDIST))
	{
		distminx = PosBufx[0];
		distmaxx = PosBufx[0];
		distminy = PosBufy[0];
		distmaxy = PosBufy[0];
			for(i=1;i<BUFSIZE;i++)
		{
			if (PosBufx[i] < distminx)
				distminx = PosBufx[i];
			else if (PosBufx[i] > distmaxx)
				distmaxx = PosBufx[i];
			if (PosBufy[i] < distminy)
				distminy = PosBufy[i];
			else if (PosBufy[i] > distmaxy)
				distmaxy = PosBufy[i];
			
		}
		if (((distmaxx - distminx + distmaxy - distminy) < STUCKDIST)&&(!trapballflag))
		{
			stuck = STUCKTIME;
			
			if ((loc.ry < WALLSTUCKDIST)&&(loc.rx > OPPONENTGOALX - WALLSTUCKDIST))
			{
				#ifdef PRINTAI
				printf(" >>Top Right Corner<< ");
				#endif
				stuck = STUCKTIME/3;				
				stuckvl = -GOALIESPEED;
				stuckvr = GOALIESPEED;						
				if ((loc.ra > 45) && (loc.ra < 90))
				{
					stuckvl = GOALIESPEED;
					stuckvr = GOALIESPEED;
				}
				else if ((loc.ra > 235) && (loc.ra < 270))
				{
					stuckvl = -GOALIESPEED;
					stuckvr = -GOALIESPEED;
				}
				
			}
			else if ((loc.ry < WALLSTUCKDIST)&&(loc.rx < WALLSTUCKDIST))
			{
				#ifdef PRINTAI
				printf(" >>Top Left Corner<< ");				
				#endif				
				stuck = STUCKTIME/3;
				stuckvl = -GOALIESPEED;
				stuckvr = GOALIESPEED;						
				if ((loc.ra > 135) && (loc.ra < 180))
				{
					stuckvl = GOALIESPEED;
					stuckvr = GOALIESPEED;
				}
				else if ((loc.ra > 315) && (loc.ra < 360))
				{
					stuckvl = -GOALIESPEED;
					stuckvr = -GOALIESPEED;
				}
			
			}
			else if ((loc.ry > BOTTOMOFFIELD - WALLSTUCKDIST)&&(loc.rx > OPPONENTGOALX - WALLSTUCKDIST))
			{
				#ifdef PRINTAI
				printf(" >>Bottom Right Corner<< ");				
				#endif				
				stuck = STUCKTIME/3;
				stuckvl = GOALIESPEED;
				stuckvr = -GOALIESPEED;						
				if ((loc.ra > 90) && (loc.ra < 135))
				{
					stuckvl = -GOALIESPEED;
					stuckvr = -GOALIESPEED;
				}
				else if ((loc.ra > 270) && (loc.ra < 305))
				{
					stuckvl = GOALIESPEED;
					stuckvr = GOALIESPEED;
				}

				
			}
			else if ((loc.ry > BOTTOMOFFIELD - WALLSTUCKDIST)&&(loc.rx < WALLSTUCKDIST))
			{
				#ifdef PRINTAI
				printf(" >>Bottom Left Corner<< ");				
				#endif				
				stuck = STUCKTIME/3;
				stuckvl = GOALIESPEED;
				stuckvr = -GOALIESPEED;						
				if ((loc.ra > 0) && (loc.ra < 45))
				{
					stuckvl = -GOALIESPEED;
					stuckvr = -GOALIESPEED;
				}
				else if ((loc.ra > 180) && (loc.ra < 235))
				{
					stuckvl = GOALIESPEED;
					stuckvr = GOALIESPEED;
				}

			}
			else if (loc.ry < WALLSTUCKDIST)
			{
				#ifdef PRINTAI
				printf(" >>Top<< ");				
				#endif				
				if (loc.ra < 180)
				{
					stuckvl = MAXSPEED;
					stuckvr = MAXSPEED;
				}
				else
				{
					stuckvl = -MAXSPEED;
					stuckvr = -MAXSPEED;
				}
			}
			else if(loc.rx < WALLSTUCKDIST)
			{
				#ifdef PRINTAI
				printf(" >>Left<< ");				
				#endif
				if ((whohasball == MYBALL)&&(loc.ry > DEFENDY))
				{
					stuckvl = -MAXSPEED;
					stuckvr = MAXSPEED;
				}
				else if ((whohasball == MYBALL) && (loc.ry < DEFENDY))
				{
					stuckvl = MAXSPEED;
					stuckvr = -MAXSPEED;
				}
				else if ((loc.ra < 270) && (loc.ra > 90))
				{
					stuckvl = MAXSPEED;
					stuckvr = MAXSPEED;
				}
				else
				{
					stuckvl = -MAXSPEED;
					stuckvr = -MAXSPEED;
				}
			}
			else if(loc.ry > BOTTOMOFFIELD - WALLSTUCKDIST)
			{
				#ifdef PRINTAI
				printf(" >>Bottom<< ");				
				#endif				
				if (loc.ra > 180)
				{
					stuckvl = MAXSPEED;
					stuckvr = MAXSPEED;
				}
				else
				{
					stuckvl = -MAXSPEED;
					stuckvr = -MAXSPEED;
				}
			}
			else if(loc.rx >  OPPONENTGOALX - WALLSTUCKDIST)
			{
				#ifdef PRINTAI
				printf(" >>Right<< ");				
				#endif				
				if ((whohasball == MYBALL)&&(loc.ry > DEFENDY))
				{
					stuckvl = MAXSPEED;
					stuckvr = -MAXSPEED;
				}
				else if ((whohasball == MYBALL) && (loc.ry < DEFENDY))
				{
					stuckvl = -MAXSPEED;
					stuckvr = MAXSPEED;
				}
				else if ((loc.ra < 270) && (loc.ra > 90))
				{
					stuckvl = -MAXSPEED;
					stuckvr = -MAXSPEED;
				}
				else
				{
					stuckvl = MAXSPEED;
					stuckvr = MAXSPEED;
				}
			}
		}
	}		
		
		
  return 0;
}
  			
int DontCrash(int object_to_avoid)
{

 /*************************************************************************************************
  **  The idea here is to create sort of a magnetic repulsion between the robot and an            **
  **  object (the opponent or the ball)                                                           **
  **                                                                                              **
  **  The function calculates the robots distance to the object and how closely the robot         **
  **  is pointed to the object.  based on that data an offset is calculated which is              **
  **  added to one wheel and subtracted from the other.  This makes the robot swerve around       **
  **  the obstacle.  The offset has two terms which are multiplied together.  One is a function   **
  **  of the distance and the other is a function of the angle.  The offset will be fery          **
  **  high if the robot is close to and pointed at the obstacle, and will be zero if              **
  **  it is far away from or not pointed at it.                                                   **
  **                                                                                              **
  **  This function was problematic to write and I think I would do it differently now.           **
  **  My suggestion would be to not bother with the offsets, rather to do something more          **
  **  like the first phase of get_behind_ball where the target is shifted slightly in             **
  **  order to avoid the object.                                                                  **
  *************************************************************************************************/



//#define DEBUG_CRASH
#define DISTERROR 200

	double dy,dx,d_e,offset,k1,k2;
	int theta_e;
	int factor;
	int max_offset = 15;
	int v_offset = 0;

	//printf("DC!\n");	
	k1 = 20;
	k2 = 50;
	if (object_to_avoid == BALL)
		{
		dx=loc.rx-loc.bx;
		dy=loc.ry-loc.by;
		}
	else
		{
		dx=loc.rx-loc.ox;
		dy=loc.ry-loc.oy;
		}
	
	d_e=sqrt(dx*dx+dy*dy);
	

	if (d_e>DISTERROR)
		{
		Swerve_dir = 0;
		return(0);
		}
			
	
	
	theta_e = (int)(-180./ 3.1415 * atan2(dy,dx));
 	//printf("First Theta_e:%d ",theta_e);
 	//if (theta_e<0)
 		//theta_e=180+theta_e;
 	//else
 		//theta_e=theta_e-180;
 		
 	//if(theta_e < 0) theta_e += 360;
	theta_e = theta_e - loc.ra;
 	if (theta_e > 180) theta_e=theta_e - 360;
 	if (theta_e < -180) theta_e=theta_e + 360;
	                                //theta_e >0 if robot is pointing clockwise of ball  (left wheel should go faster)
	
	if (theta_e<0)
 		theta_e=180+theta_e;
 	else
 		theta_e=theta_e-180;
 	//theta_e = -1*theta_e;
 	
 	#ifdef DEBUG_CRASH
 	printf("Angle to Ball:%d ",theta_e);
	#endif
		
 	
	
	
 if (Swerve_dir == 0);
		/*{
	  if (loc.rx < TOPGOAL )
	  	{
	  	if (loc.ra < 90 || loc.ra > 270)
	  		Swerve_dir = 1;                    //Swerve_dir = 1 : left wheel goes faster
	  	else
	  		Swerve_dir = -1;
	  	}
	  else if (loc.rx > BOTTOMGOAL)
	  	{
	  	if (loc.ra < 90 || loc.ra > 270)
	  		Swerve_dir = -1;                    //Swerve_dir = 1 : left wheel goes faster
	  	else
	  		Swerve_dir = 1;
	  	}
	  else*/
/*	  	{
	  	if (theta_e>0)
	  		Swerve_dir = -1;
	  	else
	  		Swerve_dir = 1;
	  	}
	  //}
	
	if (Swerve_dir == 1 && theta_e<1)
		theta_e = 1;
	else if (Swerve_dir == -1 && theta_e>-1)
		theta_e = -1;
	*/	
	
#ifdef DEBUG_CRASH
  printf("Robot Angle:%d Angle Error:%d ",loc.ra, theta_e);
#endif

/*	 if (theta_e<theta_eBUF[2]-10 && theta_e<theta_eBUF[2]+10)			//the buffer is old
	 	{
	 	theta_eBUF[0] = 0;
	  theta_eBUF[1] = 0;
		theta_eBUF[2] = 0;
	  theta_eBUF[3] = 2;
	  }                                                             //write new data to the buffer
	if (theta_eBUF[3])
		{
		theta_eBUF[theta_eBUF[3]]=theta_e;
		theta_eBUF[3] = theta_eBUF[3] - 1;
		return 0;
		}
	theta_eBUF[0] = theta_eBUF[1];
	theta_eBUF[1] = theta_eBUF[2];
	theta_eBUF[2] = theta_e;
	
	theta_e = (theta_eBUF[0] + theta_eBUF[1] + theta_eBUF[2])/3;
*/		
	if (theta_e < 0) factor = -1;
	else factor = 1;
	
	//offset = (1-tanh((abs((int)theta_e) - 60)/k1))/2*(1-tanh((d_e - 50)/k2))/2*factor*max_offset;
	if (object_to_avoid == BALL)
		offset = ((1-tanh((abs((int)theta_e) - 60)/k1))/2)*((1-tanh((d_e - 50)/k2))/2)*factor*max_offset;
	else
		offset = (1-tanh((abs((int)(theta_e*2)) - 60)/k1))*1.5*(1-tanh((0.5*d_e - 50)/k2))*factor*max_offset;
#ifdef DEBUG_CRASH
	//printf("Swerve_dir %d  ",Swerve_dir);
	printf("1st:%f, 2nd:%f, 3rd:%d ",(1-tanh((abs((int)(theta_e)) - 60)/k1)) ,2*(1-tanh((d_e - 50)/k2)),factor*max_offset);
	printf("Offset:%d  ",(int)offset);
#endif
	if (offset==0)
		{
		Swerve_dir = 0;
		}
	return((int)offset);
}  			
	
		
			


///////////////////////////////////////////////////////////////////////////////////////////////////
//
//		GGG      OOO      AAA    LL     IIIIIII  EEEEE
//  GG   GG  OO   OO  AA   AA  LL       III    EE
//  GG       OO		OO  AA   AA  LL       III    EEEE
//	GG  GGG  OO   OO  AAAAAAA  LL       III    EE
//  GG   GG  OO   OO  AA   AA  LL       III    EE
//    GGG      OOO    AA   AA  LLLLL  IIIIIII  EEEEE
//
///////////////////////////////////////////////////////////////////////////////////////////////////

//	In the goalie section the functions that were used were:
//	SteveGoaliePosition, SteveGoalie, and SteveGoalieMove.
//	Don't use ChadGoaliePosition, but the idea behind it is good.
//	The three goalie functions simply tried to keep the robot defending
//	in one vertical line.  The ball prediction earlier in the code determined
//	the y-coordinate to defend.

void ChadGoaliePosition(int x, int y, int dir)
{	
	#define GOLCHADPOSITIONRADIUS   	15
	#define GOLINTEGRALCUTTOFF    15
	#define GOLANGLEDEADZONE      5
	#define THETAEBUFLENGTH      	3
	#define GOLP_PROP_CTL             	1.2
	#define GOLP_INT_CTL              	0.05
	#define GOLP_DERIV_CTL            	0.02
	#define GOLMINCHADANGLESPEED 	5
	#define GOLMAXCHADANGLESPEED 	15
	#define GOLMAXCHADSTRAIGHTSPEED 	100
	#define GOLK											50
	#define GOLCURVERADIUSSCALINGFACTOR 1
	
	int desired_angle=0, theta_e = 0, Delta_theta_e, vl, vr;
	int dx, dy, dv;
	int d_e,offset;
 	int Dv,Df,i,Mag_Dv,sign;
 	double prop_term,int_term,deriv_term;
 	
 	if (TEST1)
 	{
 		//printf("GBB\n");
 		get_behind_ball();
		//printf("original target (%d,%d)",x,y);
		//printf("old(%d,%d) ",x,y);
		x = x + comm[0];
		y = y - comm[1];
	 // printf("new(%d,%d)\n",x,y);
	}
 	if (x>OPPONENTGOALX)
 		x=OPPONENTGOALX;
 	else if (x<15)
 		x=15;
 	if (y<TOPOFFIELD+5)
 		y=TOPOFFIELD+5;
 	else if (y>BOTTOMOFFIELD-5)
 		y=BOTTOMOFFIELD-5;
 	
	//printf("dloc.r:(%d,%d) loc.b:(%d,%d) dx%d,tof%d",x,y,loc.bx,loc.by,DEFENDX,TOPOFFIELD);
	if (y<TOPOFFIELD+15)
 		y=TOPOFFIELD+15;
 	else if (y>BOTTOMOFFIELD-15)
 		y=BOTTOMOFFIELD-15;

 	//printf("RA: %d OA:%d ",loc.ra,loc.oa);
 	//printf("target:(%d,%d) location(%d,%d) ball:(%d,%d)\n",x,y,loc.rx,loc.ry,loc.bx,loc.by);
 	
 	dx=x-loc.rx;
	dy=y-loc.ry;
	d_e=sqrt((double)(dx*dx)+(double)(dy*dy));

	if(dx==0 && dy==0)	
		desired_angle = 90;
	desired_angle = (int)(180./M_PI*atan2((double)(dy),(double)(dx)));

	if (desired_angle < 0)
		desired_angle = -desired_angle;
	else
		desired_angle = 360 - desired_angle;
	theta_e = loc.ra - desired_angle;
  if (dir == BACKWARDS)
  	theta_e = theta_e + 180;

	while(theta_e > 180)	theta_e -= 360;
	while(theta_e < -180)	theta_e += 360;
	
	//printf("Theta: %d \n",theta_e);
	if (d_e<GOLCHADPOSITIONRADIUS)
		{
		//vr = 0;
		//vl = 0;
		Velocity(0,0);
		}
	
	else
		{
		if (abs(theta_e)<POSANGLEDEADZONE)
			Dv = 0;
		else
			{
			if ((theta_e<GOLINTEGRALCUTTOFF  &&  theta_e>GOLANGLEDEADZONE)  ||  (theta_e >-GOLINTEGRALCUTTOFF  &&  theta_e <-GOLANGLEDEADZONE))
	  		{
	  		if (theta_e>0)
	  			integral_count++;
	  		else
	  			integral_count--;
	  		}
			else
				integral_count = 0;
		
	 		for (i=0;i<THETAEBUFLENGTH-1;i++)
  			{
  			theta_e_buf[i] = theta_e_buf[i+1];
  			}
  		theta_e_buf[THETAEBUFLENGTH-1]=theta_e;
  		Delta_theta_e = theta_e_buf[THETAEBUFLENGTH-1]-theta_e_buf[0];

			prop_term = GOLP_PROP_CTL*(float)theta_e/90;
			int_term = GOLP_INT_CTL*integral_count;
			deriv_term = GOLP_DERIV_CTL*(float)Delta_theta_e ;
			Dv = (int)(GOLK*(prop_term + int_term + deriv_term));
			//if (int_term)
				//printf("                                      INT TERM %f\n",int_term);
			//printf("P: %2.2f  I: %2.2f  D: %2.2f Dv: %d RA: %d\n",prop_term,int_term,deriv_term,Dv,loc.ra);

			if (Dv > 0)
				sign = 1;
			else
				sign = -1;
			Mag_Dv = abs(Dv);
			if (Mag_Dv>GOLMAXCHADANGLESPEED)
				Mag_Dv = GOLMAXCHADANGLESPEED;
			if (Mag_Dv < GOLMINCHADANGLESPEED)
				Mag_Dv = GOLMINCHADANGLESPEED;
			Dv = Mag_Dv*sign;
		  }
		Df = GOLMAXCHADSTRAIGHTSPEED-abs(Dv)/GOLCURVERADIUSSCALINGFACTOR;

  if (dir == FORWARD)
  	{
		vr = -Df - Dv;
		vl = -Df + Dv;
    }
  else
  	{
  	vr = Df - Dv;
  	vl = Df + Dv;
  	}
		//printf("Offset:%d ",offset);
	printf("Diff: %d, Straight: %d V(%d,%d) \n",Dv,Df,vl,vr);
		Velocity(vr,vl);
		}

}

void SteveGoaliePosition()
{

	int dx,dy,dz, xloc,yloc,Dist_from_goal,x;

	xloc = DEFENDX;
	yloc = DEFENDY;
	dx = abs(loc.bx - loc.rx);
	dy = abs(loc.by - loc.ry);
	dz = sqrt(pow(dx,2)+pow(dy,2));
	
	robotstuckcheck = 0;
	
	//INDEX:
	//movecountg = 0
	//	You aren't in the initial goalie position
	//	Run StevePosition until you are there
	//movecountg = 1
	//	Normal goalie operation
	//	Move back and forth along the goal directed by
	//	where SteveGoalie tells you to go
	//	If you step out of the goalie box, move back to the
	//	goalie x-location and the predicted ball y-location
	//	Singletask stays high until you are back

	//	g_direction never changes now.  Originally, it allowed our robot to face
	//  either north or south while defending, but it wasn't needed.
	
	if (loc.rx>150)
		movecountg=0;
	if (movecountg == 0)    //Setting the initial position
	{
		avoidobject = 1;


		Dist_from_goal = loc.rx - xloc;
		yloc = ballypredict;
		ChadPosition(xloc,DEFENDY);
		avoidobject = 0;


		if (abs(loc.rx - xloc)<10)
			{
			ChadAngle(90+g_direction);
			}
			if (abs(90+g_direction - loc.ra) < ANGLEACCURACY)
				movecountg = 1;
	}
	if (movecountg == 1) {
		yloc = SteveGoalie();
		if ((loc.bx < LOSTBALLSHORTX-85)&&(loc.by < TOPGOAL ))
		{
			 #ifdef PRINTAI
			 printf("Blocking Top**********************************");
			 #endif
			 ChadPosition(0,TOPGOAL);
			 if (loc.rx < 18)
				{
				#ifdef PRINTAI
				printf("braking");
				#endif
				Velocity(0,0);
				}
		}
		else if ((loc.bx < DEFENDX)&&(loc.by < TOPGOAL)&&(loc.by > BOTTOMGOAL))
		{
			ChadPosition(loc.ox,loc.oy);
		}
		else if ((loc.rx < xloc-GOALIEBOX) || (loc.rx > xloc+GOALIEBOX) || (singletask == 1))
		{
			//printf("Out of Goalbox (%d < %d or %d > %d)\n",loc.rx,DEFENDX-GOALIEBOX,loc.rx,DEFENDX+GOALIEBOX);
			//singletask = 1;
			//printf("o");
			goaliemoving = 0;
			ChadPosition(xloc, yloc);
		}
		else
		{	
			SteveGoalieMove(xloc, yloc, GOALIESPEED);
		}
		
			
    goaliemoving = 0;
		dx = abs(loc.rx - xloc);
		dy = abs(loc.ry - yloc);
	}

	if ((movecountg == 2) && (dz < DISTANCETOBALLSPIN)){
		if (loc.by > loc.ry)
			Velocity(50, -50);
		else
			Velocity(-50, 50);
		if (loc.ra > 180)
			g_direction = 180;
		else
			g_direction = 0;
	}
	else if (movecountg == 2)
		movecountg = 1;
	
	robotstuckcheck = 1;
	
}

void SteveGoalieMove(int x, int y, int speed)
{
	
	int dy,dx,desired_angle,theta_e;
	int dampspeed,slowleft=0,slowright=0;
	int sign;

  dx = loc.rx - DEFENDX;
  if (!g_direction)
  	desired_angle = 90+dx;
  else
  	desired_angle = 270-dx;
  theta_e = loc.ra-desired_angle;
	dy = abs(loc.ry - y);
	
	if ((dy<POSITIONRADIUS) || (loc.ra>110+g_direction) || (loc.ra<70+g_direction))
	 	{
		ChadAngle(90+g_direction);
		}
	else if (loc.ry > y)
		{
		if (g_direction == 0)
			sign = -1;
		else
			sign = 1;
		
		Velocity(sign*speed,sign*speed);
		}
	else
		{
		if (g_direction == 0)
			sign = 1;
		else
			sign = -1;
		Velocity(sign*speed,sign*speed);		
		}
		
}

//This should be called SteveGoaliePredict or something
int SteveGoalie()
{

	int hlocy;
	
    goaliemoving = 1;
		if (Ballx[ballindexlow] < Ballx[ballindex]) {
				hlocy = loc.by;
			}
		else if (abs(Ballx[ballindex] - Ballx[ballindexlow]) <= 1) { //1 or whatever ball stopped reads as
			if (Ballx[ballindex] > MIDCOURT)
				hlocy = DEFENDY;
			else
				hlocy = loc.by;
		}
		else 
			hlocy = ballypredict;
			
	if (hlocy > BOTTOMGOAL + 25)
		hlocy = BOTTOMGOAL + 25;
	if (hlocy < TOPGOAL - 25)
			hlocy = TOPGOAL - 25;
	return hlocy;

}


/////////////////////////////////////////////////////////////////////
//
//    OOOO   FFFFF  FFFFF  EEEEE  NN   NN   SSSSS  EEEEE
//   OO  OO  FF     FF     EE     NNN  NN  SS      EE
//   OO  OO  FFFF   FFFF   EEEE   NN N NN   SSSS   EEEE
//   OO  OO  FF     FF     EE     NN  NNN      SS  EE
//    OOOO   FF     FF     EEEEE  NN   NN  SSSSS   EEEEE
//
/////////////////////////////////////////////////////////////////////

//	Almost all of the offensive functions work in the same way as Opengoal,
//	which is get behind the ball, then drive it various spots to try to get
//	a clean shot at the goal.  These functions include Opengoal, BehindBack,
//	Fake, Fake2, and Kick.  Opengoal just heads straight for the goal, BehindBack
//	runs it along the bottom wall, Fake and Fake2 both do some zigzagging and Kick
//	is the default thing to do if you are on offense and don't have the ball.
//	The more unique functions are Shootball, get_behind_ball, Ricochet, WallSpin,
//	TrapBall, SteveStealBall, and BackIntoBall.

void ShootBall(void)
{
   /***************************************************************************************************************
  **  The idea behind this is very simple.  The computer basically draws a rectangular                          **
  **  path in front of the robot.  If the opponent is not in that rectangle and                                 **
  **  the rectangle intersects the goal, we shoot.                                                              **
  **                                                                                                            **
  **  The problem with doing it this way is that to draw this rectangle we calculate 2 lines                    **
  **  in a typical cartesian fashion.  One corresponds with the top of the robot and the                        **
  **  other with the bottom.  We use the angle of the robot to determine where these lines                      **
  **  will intersect the wall, and hence, the goal.  Now, the problem.  The camera gives a slight               **
  **  amount of "fish-eye" distortion to the image which means that any lines you draw                          **
  **  would have to be curved lines to represent lines that would actually straight                             **
  **  on the field.  In other words, You might think the robot is aimed at the goal                             **
  **  and he's really not. (Or vise-versa)  The distortion was small when we were working in the lab upstairs   **
  **  but got pretty bad when we had the competition downstairs.  So that function is                           **
  **  not bulletproof.  We were not able to correct for this problem in time to make                            **
  **  it work well dowmstairs.  Anyway, something to think about.                                               **
  ***************************************************************************************************************/




#define INITROBOTWIDTH 30
#define OPPONENTWIDTH 25
#define MAXROBOTWIDTH 2*INITROBOTWIDTH

double m,m2;
int b_1,b_2;
int ROBOTWIDTH;
double goal_top_angle,goal_bot_angle,opp_top_angle,opp_bot_angle,robot_angle;

		ROBOTWIDTH = INITROBOTWIDTH;
		
		if (loc.rx > LOSTBALLX)
			ROBOTWIDTH = (int)ROBOTWIDTH * (1 - (double)(loc.rx - LOSTBALLX) / (double)LOSTBALLX);
		if (loc.rx < MIDCOURT)
			ROBOTWIDTH = ROBOTWIDTH + (int)ROBOTWIDTH * (double)(MIDCOURT - loc.rx) / (double)MIDCOURT;
		if (ROBOTWIDTH < 1)
			ROBOTWIDTH = 1;
		if (ROBOTWIDTH > MAXROBOTWIDTH)
			ROBOTWIDTH = MAXROBOTWIDTH;
		
		
		
		m = tan(loc.ra*3.1415/180);
		b_2 = loc.ry + m*loc.rx + ROBOTWIDTH/2;
		b_1 = b_2 - ROBOTWIDTH;
		
		
		if ((loc.ra<90 || loc.ra>270) && (TOPGOAL<(int)(-OPPONENTGOALX*m+b_1)) && (BOTTOMGOAL>(int)(-OPPONENTGOALX*m + b_2)) && ((loc.ox < loc.rx) || ((loc.oy+OPPONENTWIDTH)<(int)(-loc.ox*m + b_1)) || ((loc.oy-OPPONENTWIDTH)>(int)(-loc.ox*m + b_2))) )
			{
			  printf("AIMED AT GOAL:%d \7",sskickcount);
				if (sskickcount > 2)
				{
				VelocitySent = 0;
				Velocity(101,101);
				sskickcount++;
					if (sskickcount > 10)
						sskickcount = 0;
				}
				else
					sskickcount++;
		}
		else
			sskickcount = 0;
		
}
			
//	All of the functions that operate on the Opengoal scheme use a variable
//	called nKick to decide which action to take.	If nKick == 0, then Prometheus
//	is presumed to have the ball and he tries to put the ball in the correct spot.
//	If nKick is 1, then we don't have the ball and get_behind_ball is called,
//	(because TEST1 = 1), and we try to get the ball back.  This simple method
//	worked well because of our accurate Position and Angle functions.
//	Knowing when to shoot is always taken care of by ShootBall.

void Opengoal(void)
{
	#define K_BALLSTUCKONWALL OPPONENTGOALX - 30
	
	int angle3;
	double angle1,angle2,tanfactor;
	double yvalue;
	////////////////////Get behind the ball and push it in the goal/////////////
	switch(nKick){
		case 0:

			TEST1 = 0;
			avoidobject = 0;
			if (WallSpin());
			else if (loc.bx>K_BALLSTUCKONWALL)
	 			{
	 			if (loc.RBdist>30)
	 				ChadPosition(loc.bx-loc.RBdist/2,loc.by);
	 			else if (loc.by<OPPONENTGOALY)
	 				{
	 				Velocity(-25,75);
	 				}
	 			else
	 				{
	 				Velocity(75,-25);
	 				}
	 			}
	 		else
	 			{		
	 			if ((loc.ox > LOSTBALLX) && (loc.oy > DEFENDY))
		 			{
		 			ChadPosition(OPPONENTGOALX,TOPGOAL+25);
		 			}
		 		else if ((loc.ox > LOSTBALLX) && (loc.oy < DEFENDY))
		 			{
		 			ChadPosition(OPPONENTGOALX,BOTTOMGOAL-25);
		 			}
		 		else
		 			{
		 			ChadPosition(OPPONENTGOALX,OPPONENTGOALY);
		 			}
				if(whohasball == FREEBALL)
					{
					TEST1 = 1;
					nKick = 1;
					}
				else if (whohasball == MYBALL)
					{
					angle1 = -180./ 3.1415 * atan2((double)(TOPGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
					angle2 = -180./ 3.1415 * atan2((double)(BOTTOMGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
					if (loc.ra > 180)
						angle3 = loc.ra - 360;
					else
						angle3 = loc.ra;
					}
				else
					{
					nKick = 1;
					TEST1 = 1;
					}
				}
			break;
		case 1:
			if (!WallSpin())
				{
				ChadPosition(loc.bx,loc.by);
				}
	 		if(whohasball == MYBALL)
    			nKick = 0;
			break;		
	}

}


//Use shootball instead of simpleshoot ball.

void SimpleShootBall(void)
{
	int angle1, angle2,angle3;
	angle1 = -180./ 3.1415 * atan2((double)(TOPGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
		angle2 = -180./ 3.1415 * atan2((double)(BOTTOMGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
		if (loc.ra > 180)
			angle3 = loc.ra - 360;
		else
		angle3 = loc.ra;
		
		printf("SS (%d,%d,%d) %d",angle2,angle3,angle1,kickcount);
			if ((angle3 > angle2/* + (abs(angle2 - angle1))/3*/) && (angle3 < angle1 /*- (abs(angle2 - angle1))/3)*/))
			{
				if (sskickcount > 5)
				{
				printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>SSHOOT!!\n");
				VelocitySent = 0;
				Velocity(101,101);
				sskickcount++;
					if (sskickcount > 10)
						sskickcount = 0;
				}
				else
					sskickcount++;
			}
			else
				sskickcount = 0;
}	

//	Ricochet is a function called only through OpeningOffense.
//	This idea is simple, grab the ball, move to a preset location in front of
//	the opponent's goal, near the top to lead him out of defending the middle
//	of the goal, then turn to an angle and fire the ball into the wall.
//	Very effective.  Very nasty.
//	Without our plow design, however, this would have been much more difficult
//	to execute.

void Ricochet(void)
{
#define SANGLE 300

	if (whohasball != MYBALL)
	{
		#ifdef PRINTAI
		printf(" Getting Ball ");
		#endif
		TEST1 = 1;
		ChadPosition(loc.bx,loc.by);
		TEST1 = 0;
	}
	else if (loc.rx < LOSTBALLX-100)
	{
		#ifdef PRINTAI
		printf(" GT1P ");
		#endif
		ChadPosition(LOSTBALLX-100,TOPGOAL);
	}
	else if ((loc.ry > BOTTOMGOAL)&&(whohasball == MYBALL))
	{
		#ifdef PRINTAI
		printf(" GU ");
		#endif
		ChadPosition(OPPONENTGOALX,OPPONENTGOALY);
	}
	else
	{
		#ifdef PRINTAI
		printf(" PPTF:%d ",loc.ra);
		#endif
		ChadPosition(LOSTBALLX+10,BOTTOMOFFIELD);
		
		if ((loc.ra < SANGLE+10)&&(loc.ra > SANGLE-10))
			{
			rickc++;
				if (rickc > 5)	
				{
				rickc = 0;		
				#ifdef PRINTAI
				printf(" FIRE!!! ");
				#endif
				Velocity(101,101);
				}
			}
		
	}

}

void Behindback(int direction)
{
	int angle3;
	double angle1,angle2,tanfactor;
	double yvalue;
	
	direction = 0;
	
	switch(nKick){
		case 0:
			TEST1 = 0;
			avoidobject = 0;
			if (WallSpin());
			else if (loc.rx < LOSTBALLX)
			{
				#ifdef PRINTAI
				printf(" GT1P ");
				#endif
				if (!direction)
					ChadPosition(loc.rx + 50,BOTTOMOFFIELD - 80);
				else
					ChadPosition(loc.rx + 50,TOPOFFIELD + 80);
			}
			else if (loc.rx < OPPONENTGOALX - 50)
	 		{
	 			#ifdef PRINTAI
	 			printf(" GT2P ");
	 			#endif
	 			if (!direction)
			 		ChadPosition(OPPONENTGOALX+15,BOTTOMOFFIELD - 80);
			 	else
			 		ChadPosition(OPPONENTGOALX+15,TOPOFFIELD + 80);
		 	}
		 	else
		 	{
		 		#ifdef PRINTAI
		 		printf(" GTFP ");
		 		#endif
		 		ChadPosition(OPPONENTGOALX+5,OPPONENTGOALY);
		 	}
		 	
			if(whohasball == FREEBALL)//||((loc.ra>90)&&(loc.ra<270)))
			{
				TEST1 = 1;
				nKick = 1;
			}
			else if ((whohasball == MYBALL)&&(loc.ry > TOPGOAL - 5)&&(loc.ry < BOTTOMGOAL + 5))
			{
				#ifdef PRINTAI
				printf(" TTS ");
				#endif
				ChadAngle(0);
			}
			else
			{
			//printf("Lost Ball\7\n");
				nKick = 1;
				TEST1 = 1;
			}

			break;
		case 1:
			if (!WallSpin())
				ChadPosition(loc.bx,loc.by);
	 		if(whohasball == MYBALL)
    			nKick = 0;
			break;		
	}

}

//	This function was never used in competition.
						
void BackIntoBall()
{
	#define BACKPOSANGLEDEADZONE      5
	#define BACKTHETAEBUFLENGTH      	3
	#define BACKP_PROP_CTL             	1.2
	#define BACKP_INT_CTL              	0.05
	#define BACKP_DERIV_CTL            	0.02
	#define BACKMINCHADANGLEPOSSPEED 	5
	#define BACKMAXCHADANGLEPOSSPEED 	15
	#define BACKMAXCHADSTRAIGHTSPEED 	60
	#define BACKK											50
	#define BACKCURVERADIUSSCALINGFACTOR 1
	
	int desired_angle=0, theta_e = 0, Delta_theta_e, vl, vr;
	int dx, dy, dv;
	int d_e;
 	int Dv,Df,i,Mag_Dv,sign;
 	double prop_term,int_term,deriv_term;
 	
 	
 	dx=loc.bx-loc.rx;
	dy=loc.by-loc.ry;
	d_e=sqrt((double)(dx*dx)+(double)(dy*dy));

	if(dx==0 && dy==0)	
		desired_angle = 90;
	desired_angle = (int)(180./M_PI*atan2((double)(dy),(double)(dx)));

	if (desired_angle < 0)
		desired_angle = -desired_angle;
	else
		desired_angle = 360 - desired_angle;
	theta_e = loc.ra - desired_angle +180;
  	
	while(theta_e > 180)	theta_e -= 360;
	while(theta_e < -180)	theta_e += 360;
	
	//printf("Theta: %d \n",theta_e);
		
	if (abs(theta_e)<BACKPOSANGLEDEADZONE)
		Dv = 0;
	else
		{
		for (i=0;i<THETAEBUFLENGTH-1;i++)
  		{
  		theta_e_buf[i] = theta_e_buf[i+1];
  		}
  	theta_e_buf[THETAEBUFLENGTH-1]=theta_e;
  	Delta_theta_e = theta_e_buf[THETAEBUFLENGTH-1]-theta_e_buf[0];
  	prop_term = BACKP_PROP_CTL*(float)theta_e/90;
		int_term = BACKP_INT_CTL*integral_count;
		deriv_term = BACKP_DERIV_CTL*(float)Delta_theta_e ;
		Dv = (int)(BACKK*(prop_term + int_term + deriv_term));
		//if (int_term)
			//printf("                                      INT TERM %f\n",int_term);
		//printf("P: %2.2f  I: %2.2f  D: %2.2f Dv: %d RA: %d\n",prop_term,int_term,deriv_term,Dv,loc.ra);

		if (Dv > 0)
			sign = 1;
		else
			sign = -1;
		Mag_Dv = abs(Dv);
		if (Mag_Dv>BACKMAXCHADANGLEPOSSPEED)
			Mag_Dv = BACKMAXCHADANGLEPOSSPEED;
		if (Mag_Dv < BACKMINCHADANGLEPOSSPEED)
			Mag_Dv = BACKMINCHADANGLEPOSSPEED;
		Dv = Mag_Dv*sign;
	  }
	Df = BACKMAXCHADSTRAIGHTSPEED-abs(Dv)/BACKCURVERADIUSSCALINGFACTOR;

	vr = Df - Dv;
	vl = Df + Dv;
	//printf("Diff: %d, Straight: %d V(%d,%d) \n",Dv,Df,vl,vr);
	Velocity(vr,vl);
	

}



void Fake()
{
#define KICKDIST  500
#define OUTOFBOX  1
#define TOP       2
#define MIDDLE    3
#define BOTTOM    4


double angle1,angle2,angle3,angle4,AngularGoalSize,AngularGoaliePosition,AngularAim;
int GoaliePos,MyAim;

		
		angle1 = -180./ 3.1415 * atan2((double)(TOPGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
		angle2 = -180./ 3.1415 * atan2((double)(BOTTOMGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
		angle4 = -180./ 3.1415 * atan2((double)(loc.oy-loc.ry), (double)(loc.ox-loc.rx));
		
		AngularGoalSize = angle1 - angle2;
		if (AngularGoalSize<0)
			AngularGoalSize = AngularGoalSize + 360;
		AngularGoaliePosition = angle1 - angle4;
		if (AngularGoaliePosition<0)
			AngularGoaliePosition = AngularGoaliePosition + 360;
		AngularAim = angle1-loc.ra;
		if (AngularAim<0)
			AngularAim = AngularAim + 360;
		
		if (AngularGoaliePosition<-5)
			GoaliePos = OUTOFBOX;
		else if (AngularGoaliePosition<AngularGoalSize/4)
			GoaliePos = TOP;
		else if (AngularGoaliePosition<3*AngularGoalSize/4)
			GoaliePos = MIDDLE;
		else if (AngularGoaliePosition<AngularGoalSize+5)
			GoaliePos = BOTTOM;
		else
			GoaliePos = OUTOFBOX;
		
		if (AngularAim<5)
			MyAim = OUTOFBOX;
		else if (AngularAim<AngularGoalSize/3)
			MyAim = TOP;
		else if (AngularAim<2*AngularGoalSize/3)
			MyAim = MIDDLE;
		else if (AngularAim<AngularGoalSize-5)
			MyAim = BOTTOM;
		else
			MyAim = OUTOFBOX;
				
		//printf("GoaliePos: %d, MyAim: %d\n",GoaliePos,MyAim);
		
		
		if (loc.ra > 180)
			angle3 = loc.ra - 360;
		else
			angle3 = loc.ra;
	if (loc.rx < MIDCOURT)
	{
		TEST1 = 1;		
		ChadPosition(loc.bx,loc.by);
		TEST1 = 0;
	}
	
	if (loc.rx > /*OPPONENTGOALX - 70*/OPPONENTGOALX - 110)
		{
			#ifdef PRINTAI
			printf("3");
			#endif
			ChadPosition(OPPONENTGOALX,OPPONENTGOALY);
		}
		else if (loc.rx > LOSTBALLX)//&&(whohasball == MYBALL))
		{
			#ifdef PRINTAI
			printf("2");
			#endif
			ChadPosition(/*OPPONENTGOALX-45*/OPPONENTGOALX - 100 ,TOPGOAL);
		}
		else if ((loc.rx > MIDCOURT)&&(whohasball == MYBALL))
		{
			#ifdef PRINTAI
			printf("1");
			#endif
			ChadPosition(LOSTBALLX+10,BOTTOMGOAL+20);
		}	
		else
		{
			TEST1 = 1;		
			ChadPosition(loc.bx,loc.by);
			TEST1 = 0;
		}

}

void Fake2()
{
#define KICKDIST  500
#define OUTOFBOX  1
#define TOP       2
#define MIDDLE    3
#define BOTTOM    4


double angle1,angle2,angle3,angle4,AngularGoalSize,AngularGoaliePosition,AngularAim;
int GoaliePos,MyAim;

		
		angle1 = -180./ 3.1415 * atan2((double)(TOPGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
		angle2 = -180./ 3.1415 * atan2((double)(BOTTOMGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
		angle4 = -180./ 3.1415 * atan2((double)(loc.oy-loc.ry), (double)(loc.ox-loc.rx));
		
		AngularGoalSize = angle1 - angle2;
		if (AngularGoalSize<0)
			AngularGoalSize = AngularGoalSize + 360;
		AngularGoaliePosition = angle1 - angle4;
		if (AngularGoaliePosition<0)
			AngularGoaliePosition = AngularGoaliePosition + 360;
		AngularAim = angle1-loc.ra;
		if (AngularAim<0)
			AngularAim = AngularAim + 360;
		
		if (AngularGoaliePosition<-5)
			GoaliePos = OUTOFBOX;
		else if (AngularGoaliePosition<AngularGoalSize/4)
			GoaliePos = TOP;
		else if (AngularGoaliePosition<3*AngularGoalSize/4)
			GoaliePos = MIDDLE;
		else if (AngularGoaliePosition<AngularGoalSize+5)
			GoaliePos = BOTTOM;
		else
			GoaliePos = OUTOFBOX;
		
		if (AngularAim<5)
			MyAim = OUTOFBOX;
		else if (AngularAim<AngularGoalSize/3)
			MyAim = TOP;
		else if (AngularAim<2*AngularGoalSize/3)
			MyAim = MIDDLE;
		else if (AngularAim<AngularGoalSize-5)
			MyAim = BOTTOM;
		else
			MyAim = OUTOFBOX;
				
		//printf("GoaliePos: %d, MyAim: %d\n",GoaliePos,MyAim);
		
		
		if (loc.ra > 180)
			angle3 = loc.ra - 360;
		else
			angle3 = loc.ra;
	if (loc.rx < MIDCOURT)
	{
		TEST1 = 1;		
		ChadPosition(loc.bx,loc.by);
		TEST1 = 0;
	}
	
	if (loc.rx > OPPONENTGOALX - 70)
		{
			#ifdef PRINTAI
			printf("3");
			#endif
			ChadPosition(OPPONENTGOALX,OPPONENTGOALY);
		}
		else if (loc.rx > LOSTBALLX)//&&(whohasball == MYBALL))
		{
			#ifdef PRINTAI
			printf("2");
			#endif
			ChadPosition(OPPONENTGOALX-45,TOPGOAL);
		}
		else if ((loc.rx > MIDCOURT)&&(whohasball == MYBALL))
		{
			#ifdef PRINTAI
			printf("1");
			#endif
			ChadPosition(LOSTBALLX+10,BOTTOMGOAL+20);
		}	
		else
		{
			TEST1 = 1;		
			ChadPosition(loc.bx,loc.by);
			TEST1 = 0;
		}

}

//	Hypothetically, this function tries to spin the ball into the opponent
//	goal.  Very difficult to measure its effects.	It returns a 1 if it is
//	planning on sending its own velocities to the robot.  Any function that
//	calls this has it in a if(WallSpin()); format so the function that calls
//	WallSpin doesn't set new velocities of its own.

int WallSpin()
{
#define BALLSTUCKONRIGHTWALL OPPONENTGOALX - 30
#define BALLSTUCKONLEFTWALL 10
#define SPINBALLDIST 20

		
	TEST1 = 0;
	if (loc.bx>BALLSTUCKONRIGHTWALL)
	 	{
	 	if (loc.rbd > SPINBALLDIST)
	 			{
	 			ChadPosition(loc.bx-loc.rbd/2,loc.by);
	 			#ifdef PRINTAI
	 			printf(" Move left of ball ");
	 			#endif
	 			}
	 	else if (loc.by<OPPONENTGOALY)
	 		{
	 		Velocity(-MAXSPEED,MAXSPEED);
	 		#ifdef PRINTAI
	 		printf(" *********SPIN** ");
	 		#endif
	 		}
	 	else
	 		{
	 		Velocity(MAXSPEED,-MAXSPEED);
	 		#ifdef PRINTAI
	 		printf(" *********SPIN** ");
	 		#endif
	 		}
	 	
	 	return 1;
	 	}	
	else if (loc.bx < BALLSTUCKONLEFTWALL)
		{
	 	if ((loc.rbd > SPINBALLDIST)&&(loc.by < TOPGOAL-20))
 			{
 			ChadPosition(loc.bx+(int)loc.rbd/2,loc.by+(int)loc.rbd/2);
 			#ifdef PRINTAI
 			printf(" Move right upper of ball ");
 			#endif
 			}
	 	else if ((loc.rbd > SPINBALLDIST)&&(loc.by > BOTTOMGOAL+20))
 			{
 			#ifdef PRINTAI 			
 			ChadPosition(loc.bx+(int)loc.rbd/2,loc.by-(int)loc.rbd/2);
 			printf(" Move right lower of ball ");
 			#endif
 			}
 		else if ((loc.by > TOPGOAL-5) && (loc.by < BOTTOMGOAL + 5))
 			{
 			//TEST1 = 1;
 			#ifdef PRINTAI 			
 			printf(" BALL BEHIND NEAR GOAL!!! ");
 			#endif
 			if ((loc.rx > loc.ox)&&(loc.ox < LOSTBALLSHORTX))
 				Velocity(0,0);
 			else
	 			{
	 			robotstuckcheck = 0;
 				ChadPosition(loc.ox,loc.oy);
 				robotstuckcheck = 1;
 				}
 			
 		//TEST1 = 0;
 			}
	 	else if (loc.by<OPPONENTGOALY)
	 		{
	 		Velocity(-MAXSPEED,MAXSPEED);
 			#ifdef PRINTAI	 		
	 		printf(" *********SPIN** ");
			#endif
	 		}
	 	else
	 		{
	 		Velocity(MAXSPEED,-MAXSPEED);
 			#ifdef PRINTAI	 		
	 		printf(" *********SPIN** ");
	 		#endif
	 		}
	 	//printf("*********SPIN**");
	 	return 1;
	 	}	
	return 0;	
}	 	

void PrepareForBounceKick()
{
	ChadPosition(loc.bx-loc.RBdist/5,loc.by-loc.RBdist/1.5);
}


void Kick()
{	
#define K_BALLSTUCKONWALL OPPONENTGOALX - 30
	
	int angle3;
	double angle1,angle2,tanfactor;
	double yvalue;
	////////////////////Get behind the ball and push it in the goal/////////////
	//printf(" k ");
	switch(nKick){
		case 0:
			TEST1 = 0;
			avoidobject = 0;
			if (!WallSpin())
	 			{		
	 			ChadPosition(OPPONENTGOALX,OPPONENTGOALY);
				if(whohasball == FREEBALL)//||((loc.ra>90)&&(loc.ra<270)))
					{
					TEST1 = 1;
					nKick = 1;
					//printf("\7");
					}
				else if (whohasball == MYBALL)
					{
					angle1 = -180./ 3.1415 * atan2((double)(TOPGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
					angle2 = -180./ 3.1415 * atan2((double)(BOTTOMGOAL-loc.ry), (double)(OPPONENTGOALX-loc.rx));
					if (loc.ra > 180)
						angle3 = loc.ra - 360;
					else
						angle3 = loc.ra;
					}
				else
					{
					//printf("Lost Ball\7\n");
					nKick = 1;
					TEST1 = 1;
					}
				}
			break;
		case 1:
			if(!WallSpin())
			{
				TEST1 = 1;
				ChadPosition(loc.bx,loc.by);
			}
	 		if(whohasball == MYBALL)
    			nKick = 0;
			break;		
	}

}

void get_behind_ball()
{
	
	/*************************************************************************************************
	**  One major control problem is being able to contact the ball moving in the right direction   **
	**  so that you can make a reasonable drive for the opponents goal.  In other words, if you     **
	**  don't have the ball you want to approach the ball from a direction such that when           **
	**  you contact it you are facing the oponents goal.                                            **
	**                                                                                              **
	**  The way this function works is that you call it when the position of your target            **
	**  is the balls position.  This function calculates an x and y offset which are used in        **
	**  ChadPosition to add to that position.                                                       **
	**  This "tricks" the position function into going somewhere else first.  That way the robot    **
	**  makes a more intelligent approach.                                                          **
	**                                                                                              **
	**  there are three regions this function uses.                                                 **
	**                                                                                              **
	**                                                                                              **
	**  First, if you aren't behind the ball (you are on the wrong side of it) it makes the         **
	**  target either below or above the ball so you can start your approach without                **
	**  accidentally hitting the ball into your own goal.                                           **
	**                                                                                              **
	**  Once the robot is on the other side of the ball, you need to make the approach.             **
	**  We basically draw a line that goes through the center of the goal and the ball.  We         **
	**  would like the robot to travel on this line.  To do that we say that if the                 **
	**  robot is far away from the ball, its target should be on the line we drew, but              **
	**  a far distance from the ball.  The robot will start going to that point, but then           **
	**  be a little closer to the ball, so the target shifts closer.  The target keeps              **
	**  sliding closre to the ball until the robot finally contacts it, presumably pointing         **
	**  right at the goal.                                                                          **
	**                                                                                              **
	**  However, we found that it didn't work very well a lot of times, so we added a third         **
	**  step to the sequence.  Once the robot is within a certain region with respect to the ball   **
	**   we say it's close enough and to just go for the ball.                                      **
	**************************************************************************************************/
	
	
	
	#define DIST 25
	#define VECTORSCALINGFACTOR 1.75
	#define MAXVECTORLENGTH 150
	#define BEHINDDISTANCE 50
	#define SCALEDIST 5
	#define XIMIN 50
	#define BBANGLE 35
	//#define YMIN 10
	
	double dx, dy, slope, angle1;
	int xi, yi, r_to_b_dist, add_on_vector;
	
	dx = OPPONENTGOALX - loc.bx;
	dy = OPPONENTGOALY - loc.by;
	if ((int)dy == 0)
		slope = 0;
	else if ((int)dx == 0)
		slope = 100;
	else
		slope = dy/dx;
	
	positionstop = 0;
	
	if (loc.rbd<25 && loc.ra>100 && loc.ra<260)
		{
		Velocity(100,100);
		#ifdef PRINTAI
		printf("LOOK OUT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
		#endif
		}
	
	
	else if (loc.rx > loc.bx)
		{
		bbspotflag = 0;
	  if (loc.by < OPPONENTGOALY)
		 	yi = -75;
		else
		 	yi = 75;
		xi = -10;
		}
	else
		{
			maxposspeed = 80;
			r_to_b_dist =  sqrt((double)((loc.rx - loc.bx)*(loc.rx - loc.bx)) + (double)((loc.ry - loc.by)*(loc.ry - loc.by)));
			add_on_vector = r_to_b_dist/VECTORSCALINGFACTOR;	
			if (add_on_vector > MAXVECTORLENGTH)
				add_on_vector = MAXVECTORLENGTH;
	  	if (r_to_b_dist < POSITIONRADIUS+15)
	  		{
	  		//printf("!!!!!!!!!!!!!!!!!!!!!!!!CAUGHT BALL\n");
	  		xi = OPPONENTGOALX - loc.bx;
	  		yi = DEFENDY - loc.by;
	  		}
	  	else if (abs(dy)<50)
	  		{
	  		xi=0;
	  		yi=0;
	  		}	  	
	  	else
	  		{
			  	
				xi = add_on_vector/sqrt(1 + slope*slope);
				yi = (xi * slope)/2;
				xi = -add_on_vector/sqrt(1 + slope*slope);
				yi = (-xi * slope);
				//printf("AOV %d slope^2:%f,slope:%f, yi:%d,xi:%d",add_on_vector,sqrt(1 + slope*slope),slope,yi,xi);
		
				}
		}
	

	comm[0] = xi;
	comm[1] = yi;
}


//	An evil way to win a match.  This function works perfectly.

void TrapBall(void)
{
	if ((whohasball == MYBALL)&&(loc.ry < BOTTOMOFFIELD - 15))
	{
		ChadPosition(MIDCOURT+40,BOTTOMOFFIELD);
	}
	else if ((whohasball == MYBALL)&&(loc.ry > BOTTOMOFFIELD - 15))
	{
		#ifdef PRINTAI
		printf("braking");
		#endif
		Velocity(0,0);
	}	
	else
	{
		TEST1 = 1;
		ChadPosition(loc.bx,loc.by);
		TEST1 = 0;
	}
}
	

//	This function was never used in competition.

void SteveSteal(void)
{
	if (whohasball == ENEMYBALL)  //If the enemy has the ball, take it
	 	{
	 	TEST1 = 1;
	 	ChadPosition(loc.bx,loc.by);
	 	TEST1 = 0;
	 	}
	else if (whohasball == JOINTBALL) //If we both have it...
	 	{
	 	//printf("\7");
	 	if (loc.by > DEFENDY) //veer left...
	 		//Position(loc.bx-10,loc.by - DEFENDY/2);
	 		Velocity(0,MAXSPEED);
	 	else 									//veer right
			//Position(loc.bx-10,loc.by + DEFENDY/2);
			Velocity(MAXSPEED,0);
		}
	else if (whohasball == MYBALL)
		{
		StuckOnRobotCount = 0;
		Kick();
		StuckOnRobotCount = SWERVECYCLES;
		}
	else if (whohasball == FREEBALL)
		{
		TEST1 = 1;
		ChadPosition(loc.bx,loc.by);
		TEST1 = 0;
		}
		
}


void Demo()
{
	int i;
	double edist,rdist;
	if (democount == 0)
	{
		printf("D");
		avoidobject = 0;
		SteveGoaliePosition();
		//ChadGoalie();
	}
	else if (democount == 1)
	{
		printf("A");
		avoidobject = 0;
		Kick();
		
	}
	if (democount == 0)
	{
		edist = sqrt((double)((loc.ox - loc.bx)*(loc.ox - loc.bx)) + (double)((loc.oy - loc.by)*(loc.oy - loc.by)));
		rdist = sqrt((double)((loc.rx - loc.bx)*(loc.rx - loc.bx)) + (double)((loc.ry - loc.by)*(loc.ry - loc.by)));
		//printf("(");
		//printf("Ballx ballindex:%d, Ballx ballindexlod:%d, ball vector:%d\n",  Ballx[ballindex],Ballx[ballindexlow],Ballx[ballindex] - Ballx[ballindexlow]);
		if ((whohasball == MYBALL)||((whohasball == FREEBALL)&&(loc.bx > MIDCOURT)) || (Ballx[ballindex] - Ballx[ballindexlow] > 10))
		{
			//printf("\7");
			democount = 1;
		}
	}
	else if (democount == 1)
	{
		//printf("Ballown:%d Balldist:%d rdist-edist:%f\n",whohasball,Ballx[ballindexlow]-Ballx[ballindex],rdist-edist);
		if ((whohasball == ENEMYBALL)||((loc.bx < MIDCOURT) && (Ballx[ballindexlow] - Ballx[ballindex] > 5)))
		{
			//printf("\7");
			democount = 0;
		}
	}
	
	
}