// 6.270 Final Code: Team 23


#include <joyos.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#define GYRO_PORT       23
#define LSB_US_PER_DEG  1406366.2
#define VPS_PER_FOOT 443.4
#define RADIANS_TO_DEGREES 57.2957795
#define ANGLE_CONVERSION 0.087890625 //180/2048  
#define ROBOT_HEIGHT 10
#define VPS_HEIGHT  129


#define MOTORS_LEFT_ONE 0
#define MOTORS_LEFT_TWO 1
#define MOTORS_RIGHT_ONE 2
#define MOTORS_RIGHT_TWO 3
#define MOTOR_FOR_GEARBOX 4

#define TURN_ANGLE_THRESHOLD 3

//our color
#define RED 1
#define BLUE 0

//for navigateTo function
#define STOP 1
#define DONT_STOP 0

//navigaion
#define FORWARD_VELOCITY 190
#define ROTATIONAL_GAIN 4

#define MAX_ROT_SPEED 200
#define MIN_ROT_SPEED 100 //has to be 40 for our robot

#define MAX_SPEED 255
#define MIN_SPEED 120

//drive directions
#define FORWARD 0
#define BACKWARD 1

#define TEST_OFFSET 500 //for testing movement around the field.


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

//defines that need to be figured out and set.

//for throwing balls the top release servo.
#define THROW_BALL_SERVO 2
#define THROW_BALL_SERVO_CLOSE 470
#define THROW_BALL_SERVO_OPEN 250

//for collecting balls on the top
#define COLLECT_BALL_SERVO 1
#define COLLECT_BALL_SERVO_CLOSE 470
#define COLLECT_BALL_SERVO_OPEN 250

//for pressing switch
#define SWITCH_PRESS_SERVO 0
#define SWITCH_PRESS_SERVO_DOWN 80
#define SWITCH_PRESS_SERVO_UP 240




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

typedef struct{
	int x;
	int y;
}Point;

Point T0D={
    .x = 616,
    .y = 61
};


Point T0={
    .x = 1024,
    .y = 0
};

Point T0B={
    .x = 1451,
    .y = 247
};

Point T0F={
    .x = 1419,
    .y = -227
};

Point T1={
    .x = 512,
    .y = 886
};

Point T1D={
    .x = 273,
    .y = 571
};

Point T1B={
    .x = 512,
    .y = 1450
};

Point T1F={
    .x = 867,
    .y = 1071
};

Point T2={
    .x = -512,
    .y = 886
};

Point T2B={
    .x = -1011,
    .y = 1175
};

Point T2D={
    .x = -290,
    .y = 525
};

Point T2F={
    .x = -512,
    .y = 1325
};

Point T3={
    .x = -1024,
    .y = 0
};

Point T3D={
    .x = -665,
    .y = -24
};

Point T3B={
    .x = -1513,
    .y = -283
};

Point T3F={
    .x = -1451,
    .y = 247
};

Point T4={
    .x = -512,
    .y = -886
};

Point T4B={
    .x = -512,
    .y = -1390
};

Point T4D={
    .x = -302,
    .y = -569
};


Point T4F={
    .x = -870,
    .y = -1178
};


Point T5={
    .x = 512,
    .y = -886
};

Point T5D={
	.x = 291,
	.y = -504
};

Point T5B={
    .x = 960,
    .y = -1145
};

Point T5F={
    .x = 512,
    .y = -1320
};


void stop();

float targetAngle(Point target);

float distanceFromTarget(Point target);

int limitVelocity(int);

int next_territory(int);

void moveSequence(int current_territory,int target_territory,int opponent,int);

float modifyAngle(float); //gives angle from -180 to 180

int limitRotationalVelocity(int);

Point offsetter(Point,Point,int); // just for testing, offsets a point by certain specified amount

float vpsAngle(); //gives the angle given by the VPS

void explore(int); // explore durring the start of the match

void navigateTo(Point,int,int,int,int);

int territoryNumber(Point); //takes a point and gives the territory number of hte point

void recaliberate(); // recaliberate the gyro

int point_correction(int); // correction of points got from the VPS

int robotCurrentTerritory();

void turnTo(float,int);

int opponentCurrentTerritory();

void gearbox();

void collectBalls();

void drop_balls();

void glideTowardsTarget(int direction);


//global variables

int robot_front = 1;

int last_territory = 0;

int current_territory = 0;

float offset = 0;

int color = 0;

extern volatile uint8_t robot_id;

int timeout = 0;

int speed_option = 0;

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


int usetup (void) {
    printf("\nPlace robot,    press go.");
    printf ("\nStabilizing...");
    pause (1000);
    printf ("\nCalibrating     offset...\n");
    gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
	robot_id = 23;
    return 0;

}

int umain(void){
	//when doing the actual code, set the servo positions.
	
	
	
	
	copy_objects();
	gyro_set_degrees((float)((game.coords[0].theta)*ANGLE_CONVERSION));
	explore(BACKWARD);
	servo_set_pos(SWITCH_PRESS_SERVO,SWITCH_PRESS_SERVO_UP);
	while(get_time() < 100000){ //change this to a meaningful loop once,you are able to do everything else
	moveSequence(robotCurrentTerritory(),next_territory(opponentCurrentTerritory()),opponentCurrentTerritory(),1);
	}
	drop_balls();
	while(1){ //change this to a meaningful loop once,you are able to do everything else
	moveSequence(robotCurrentTerritory(),next_territory(opponentCurrentTerritory()),opponentCurrentTerritory(),1);
	}
	
	/*
	while(1){
	copy_objects();
	printf("x is = %d\n",point_correction(game.coords[0].x));
	printf("y is = %d\n",point_correction(game.coords[0].y));
	pause (2500);
	}*/
	
	/*
	copy_objects();
	gyro_set_degrees((float)((game.coords[0].theta)*ANGLE_CONVERSION));
	//explore(FORWARD);
	//explore(BACKWARD);
	servo_set_pos(SWITCH_PRESS_SERVO,SWITCH_PRESS_SERVO_UP);
	servo_disable(SWITCH_PRESS_SERVO);
	navigateTo(T5,FORWARD,STOP);
	navigateTo(T5F,FORWARD,STOP);
	navigateTo(T5,FORWARD,STOP);
	navigateTo(T5B,BACKWARD,STOP);
	recaliberate();
	navigateTo(T4,FORWARD,STOP);
	navigateTo(T4F,FORWARD,STOP);
	navigateTo(T4,FORWARD,STOP);
	navigateTo(T4B,BACKWARD,STOP);
	recaliberate();
	navigateTo(T3,FORWARD,STOP);
	navigateTo(T3F,FORWARD,STOP);
	navigateTo(T3,FORWARD,STOP);
	navigateTo(T3B,BACKWARD,STOP);
	recaliberate();
	navigateTo(T2,FORWARD,STOP);
	navigateTo(T2F,FORWARD,STOP);
	navigateTo(T2,FORWARD,STOP);
	navigateTo(T2B,BACKWARD,STOP);
	recaliberate();
	navigateTo(T1,FORWARD,STOP);
	navigateTo(T1F,FORWARD,STOP);
	navigateTo(T1,FORWARD,STOP);
	navigateTo(T1B,BACKWARD,STOP);
	recaliberate();
	navigateTo(T0,FORWARD,STOP);
	navigateTo(T0F,FORWARD,STOP);
	navigateTo(T0,FORWARD,STOP);
	navigateTo(T0B,BACKWARD,STOP);
	*/
	
	//while(1){
	//printf("encoder = %d\n",encoder_read(26));
	//}
	
	/*
	while(1){
	servo_set_pos(0,frob_read_range (50,460));
	printf("frobvalue = %u\n" ,frob_read_range (50,460));
	}*/
	
	/*
	while(1){
	go_click ();
	copy_objects();
	printf("x is %d\n",point_correction(game.coords[0].x));
	printf("y is %d\n",point_correction(game.coords[0].y));
	}
	*/
	
	/*
	navigateTo(T1,BACKWARD);
	navigateTo(offsetter(T1,T1B,TEST_OFFSET),BACKWARD); 
	navigateTo(T1,BACKWARD);
	navigateTo(offsetter(T1,T1F,TEST_OFFSET),FORWARD);
	
	navigateTo(T2,BACKWARD);
	navigateTo(offsetter(T2,T2B,TEST_OFFSET),BACKWARD); 
	navigateTo(T2,BACKWARD);
	navigateTo(offsetter(T2,T2F,TEST_OFFSET),FORWARD);
	
	navigateTo(T3,BACKWARD);
	navigateTo(offsetter(T3,T3B,TEST_OFFSET),BACKWARD); 
	navigateTo(T3,BACKWARD);
	navigateTo(offsetter(T3,T3F,TEST_OFFSET),FORWARD);
	
	navigateTo(T4,BACKWARD);
	navigateTo(offsetter(T4,T4B,TEST_OFFSET),BACKWARD); 
	navigateTo(T4,BACKWARD);
	navigateTo(offsetter(T4,T4F,TEST_OFFSET),FORWARD);
	
	navigateTo(T5,BACKWARD);
	navigateTo(offsetter(T5,T5B,TEST_OFFSET),BACKWARD); 
	navigateTo(T5,BACKWARD);
	navigateTo(offsetter(T5,T5F,TEST_OFFSET),FORWARD);
	
	navigateTo(T0,BACKWARD);
	navigateTo(offsetter(T0,T0B,TEST_OFFSET),BACKWARD); 
	navigateTo(T0,BACKWARD);
	navigateTo(offsetter(T0,T0F,TEST_OFFSET),FORWARD);
	*/
	/*while(1){
	printf("gyro = %.2f\n",gyro_get_degrees());
	}*/
	return 0;
}

void glideTowardsTarget(int direction){
	if (direction = FORWARD){
		motor_set_vel(MOTORS_RIGHT_ONE,60);
		motor_set_vel(MOTORS_RIGHT_TWO,60);
		motor_set_vel(MOTORS_LEFT_ONE,60);
		motor_set_vel(MOTORS_LEFT_TWO,60);
	}
	else{
		motor_set_vel(MOTORS_RIGHT_ONE,-60);
		motor_set_vel(MOTORS_RIGHT_TWO,-60);
		motor_set_vel(MOTORS_LEFT_ONE,-60);
		motor_set_vel(MOTORS_LEFT_TWO,-60);
	}
}

int opponentCurrentTerritory(){
	copy_objects();
	Point current_position;
	current_position.x = point_correction(game.coords[1].x);
	current_position.y = point_correction(game.coords[1].y);
	return territoryNumber(current_position);
}

int robotCurrentTerritory(){
	copy_objects();
	Point current_position;
	current_position.x = point_correction(game.coords[0].x);
	current_position.y = point_correction(game.coords[0].y);
	return territoryNumber(current_position);
}


int point_correction(int value){
	float error = (value*ROBOT_HEIGHT)/VPS_HEIGHT;
	int corrected = value - (int)error;
	return corrected;
}


float vpsAngle(){
	copy_objects();
	float ret = (float)((game.coords[0].theta)*ANGLE_CONVERSION);
	return ret;
}


float modifyAngle(float angle){
	float result = fmod(angle,360);
	float return_value = (result >= 180) ? result -360 : (result < -180) ? result +360 :result;
	return return_value;
}


void stop() {
	motor_set_vel(MOTORS_RIGHT_ONE,0);
	motor_set_vel(MOTORS_RIGHT_TWO,0);
	motor_set_vel(MOTORS_LEFT_ONE,0);
	motor_set_vel(MOTORS_LEFT_TWO,0);
}


float targetAngle(Point target){
	copy_objects();
	return (atan2(((target.y)-point_correction(game.coords[0].y)), //[0] denotes Robot's current x and y.
							((target.x)-point_correction(game.coords[0].x)))) * RADIANS_TO_DEGREES;
}


int limitVelocity(int velocity){
	velocity = (velocity > MAX_SPEED) ? MAX_SPEED :
				(velocity < -(MAX_SPEED)) ? -(MAX_SPEED) :
				((velocity < MIN_SPEED) && (velocity > 0)) ? MIN_SPEED :
				((velocity > -(MIN_SPEED)) && (velocity < 0)) ? -(MIN_SPEED) : velocity;
	return velocity;
}

int limitRotationalVelocity(int velocity){
	velocity = (velocity > MAX_ROT_SPEED) ? MAX_ROT_SPEED :
				(velocity < -(MAX_ROT_SPEED)) ? -(MAX_ROT_SPEED) :
				((velocity < MIN_ROT_SPEED) && (velocity > 0)) ? MIN_ROT_SPEED :
				((velocity > -(MIN_ROT_SPEED)) && (velocity < 0)) ? -(MIN_ROT_SPEED) : velocity;
	return velocity;
}

float distanceFromTarget(Point target){
	copy_objects();
	float y_square = (float)(target.y-point_correction(game.coords[0].y))*(float)(target.y-point_correction(game.coords[0].y));
	float x_square = (float)(target.x-point_correction(game.coords[0].x))*(float)(target.x-point_correction(game.coords[0].x));
	return (sqrtf(x_square + y_square));
}

void recaliberate(){
	stop();
	pause(400);
	copy_objects();
	gyro_set_degrees((float)((game.coords[0].theta)*ANGLE_CONVERSION));
}


void explore(int k){  //to be completed for getting a continuous motion.
	int current_territory = robotCurrentTerritory(); //our robot territory.
	if(current_territory == 0){
		color = BLUE;
		navigateTo(T1,k,DONT_STOP,1,0);
		navigateTo(T2,k,DONT_STOP,1,0);
		navigateTo(T3,k,DONT_STOP,1,0);
		navigateTo(T4,k,DONT_STOP,1,0);
		navigateTo(T5,k,STOP,1,0);
		recaliberate();
		navigateTo(T5B,BACKWARD,STOP,1,100);
		gearbox();
		navigateTo(T5,BACKWARD,STOP,1,0);
		navigateTo(T5F,FORWARD,STOP,1,100);
		collectBalls();
		last_territory = 3;
		current_territory = 5;
		
	}
	else if(current_territory == 3){
		color = RED;
		navigateTo(T4,k,DONT_STOP,1,0);
		navigateTo(T5,k,DONT_STOP,1,0);
		navigateTo(T0,k,DONT_STOP,1,0);
		navigateTo(T1,k,DONT_STOP,1,0);
		navigateTo(T2,k,STOP,1,0);
		recaliberate();
		navigateTo(T2B,BACKWARD,STOP,1,100);
		gearbox();
		navigateTo(T2,BACKWARD,STOP,1,0);
		navigateTo(T2F,FORWARD,STOP,1,100);
		collectBalls();
		last_territory = 0;
		current_territory = 2;
	}
	else {
	stop();
	}
	
	//have to set current_territory and last_territory before this function ends.
}


int territoryNumber(Point point){

    if ((point.x >= 443) && (point.x<=2047) && (point.y >= -887) && (point.y <= 887)) {return 0;}
    else if ((point.x >= 0) && (point.x<=1536) && (point.y >= 256) && (point.y <= 1773)) {return 1;}
    else if ((point.x >= -1536) && (point.x<= 0) && (point.y >= 256) && (point.y <= 1773)) {return 2;}
    else if ((point.x >= -2047) && (point.x<=-443) && (point.y >= -887) && (point.y <= 887)) {return 3;}
    else if ((point.x >= -1536) && (point.x<= 0) && (point.y >= -1773) && (point.y <= -256)) {return 4;}
    else if ((point.x >= 0) && (point.x<=1536) && (point.y >= -1773) && (point.y <= -256)) {return 5;}

}

//creates an offset of a point in accordance to its slope
Point offsetter(Point start,Point End,int offset){

	Point return_point;
	float slope = (float)(End.y - start.y)/(float)(End.x - start.x);

    if ((slope > -1) && (slope < 1)){
        return_point.x = (End.x > start.x) ? End.x - (int)offset : End.x + (int)offset;
        return_point.y = (End.y > start.y) ? End.y - abs((int)(offset*slope)) : End.y + abs((int)(offset*slope));
        return return_point;
    }
    else{
        return_point.y = (End.y > start.y) ? End.y - (int)offset : End.y + (int)offset;
        return_point.x = (End.x > start.x) ? End.x - abs((int)(offset*(float)(1/slope))) : End.x + abs((int)(offset*(float)(1/slope)));
        return return_point;
    }

}


void turnTo(float angle,int direction){
if (direction == BACKWARD) {
		offset = 180.0;
		robot_front = -1;
	}

	float differ = angle - gyro_get_degrees() + offset;
	float difference = modifyAngle(differ);
	
	while (!(fabsf(difference) <= TURN_ANGLE_THRESHOLD)){
		differ = angle - gyro_get_degrees() + offset;
		difference = modifyAngle(differ);	
	
		//printf("loop differ =%.2f\n difference=%.2f", differ,difference);
 
		int speed = (int) difference*ROTATIONAL_GAIN;
		//printf("speed is = %d\n",speed);

		motor_set_vel(MOTORS_RIGHT_ONE,limitRotationalVelocity((int)(speed)));
		motor_set_vel(MOTORS_RIGHT_TWO,limitRotationalVelocity((int)(speed)));
		motor_set_vel(MOTORS_LEFT_ONE,-(limitRotationalVelocity((int)(speed))));
		motor_set_vel(MOTORS_LEFT_TWO,-(limitRotationalVelocity((int)(speed))));
		

	}
}



void navigateTo(Point target,int direction,int stop_decide,int speed_option,int away){

	float differ = targetAngle(target) - gyro_get_degrees() + offset;
	float difference = modifyAngle(differ);
	//printf("differ =%.2f difference=%.2f\n,offset = %.2f\n", differ,difference,offset);

	while (!(fabsf(difference) <= TURN_ANGLE_THRESHOLD)){
		differ = targetAngle(target) - gyro_get_degrees() + offset;
		difference = modifyAngle(differ);	
	
		//printf("loop differ =%.2f\n difference=%.2f", differ,difference);
 
		int speed = (int) difference*ROTATIONAL_GAIN;
		//printf("speed is = %d\n",speed);

		motor_set_vel(MOTORS_RIGHT_ONE,limitRotationalVelocity((int)(speed)));
		motor_set_vel(MOTORS_RIGHT_TWO,limitRotationalVelocity((int)(speed)));
		motor_set_vel(MOTORS_LEFT_ONE,-(limitRotationalVelocity((int)(speed))));
		motor_set_vel(MOTORS_LEFT_TWO,-(limitRotationalVelocity((int)(speed))));
		

	}
	//printf("done turning!\n");

	float logic_robot_x,logic_robot_y;

	copy_objects();
	float present_time = get_time();

	//for distance travelled
	int32_t diff = 0;
	float distance = distanceFromTarget(target);
	float current_robot_x = point_correction(game.coords[0].x);
	float current_robot_y = point_correction(game.coords[0].y);
	float distance_travelled = 0;
	float distance_multiplier = (distance - distance_travelled)/distance;
	
	int start_time = get_time();
	int dist = 0,counter = 0;
	int previous_x = point_correction(game.coords[0].x);
	int previous_y = point_correction(game.coords[0].y);
	int current_x =0,current_y =0;
	while(distance_travelled < distance - away){
		//newly added
		/*pause(20);
		counter += 1;
		
		if(counter == 25){
			copy_objects();
			current_x = point_correction(game.coords[0].x);
			current_y = point_correction(game.coords[0].y);
			if (abs(current_x - previous_x) < 25 && abs(current_y - previous_y) <25){
				stop();
				offset = 0;
				counter = 0;
				return;
			}
			else {
				previous_x = current_x;
				previous_y = current_y;
				counter =0;
			}
		}*/
		
		//printf("my x is = %d\n",point_correction(game.coords[0].x));
		//printf("my y is = %d\n",point_correction(game.coords[0].y));
	
		//for time out
		diff = get_time() - present_time;

		//for robot position and goal position, this controls the loop
		copy_objects();
		logic_robot_x = point_correction(game.coords[0].x);
		logic_robot_y = point_correction(game.coords[0].y);

		//for distance travelled, this controls the loop
		float x_square = ((current_robot_x - logic_robot_x)*(current_robot_x - logic_robot_x));
		float y_square = ((current_robot_y - logic_robot_y)*(current_robot_y - logic_robot_y));
		distance_travelled = (float)(sqrtf((x_square + y_square)));
		distance_multiplier = (distance - distance_travelled)/distance;
		
		
		float sep = targetAngle(target) - gyro_get_degrees() + offset;
		float error = modifyAngle(sep);
		//printf("sep =%.2f . error =%.2f\n", sep,error);
		//printf("target = %.2f, gyro = %.2f",targetAngle(target),gyro_get_degrees());

		int correction = error*ROTATIONAL_GAIN;
		if(speed_option == 0){
			if (direction == FORWARD){
				motor_set_vel(MOTORS_RIGHT_ONE,limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY + (int)correction)))));
				motor_set_vel(MOTORS_RIGHT_TWO,limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY + (int)correction)))));
				motor_set_vel(MOTORS_LEFT_ONE,limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY - (int)correction)))));
				motor_set_vel(MOTORS_LEFT_TWO,limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY - (int)correction)))));
				//printf("mot left =%d mot right =%d\n", robot_front*(limitVelocity(FORWARD_VELOCITY) - (int)correction),robot_front*(limitVelocity(FORWARD_VELOCITY) + (int)correction));
			}
			else{
				motor_set_vel(MOTORS_RIGHT_ONE,-limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY - (int)correction)))));
				motor_set_vel(MOTORS_RIGHT_TWO,-limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY - (int)correction)))));
				motor_set_vel(MOTORS_LEFT_ONE,-limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY + (int)correction)))));
				motor_set_vel(MOTORS_LEFT_TWO,-limitVelocity((int)(distance_multiplier*((FORWARD_VELOCITY + (int)correction)))));
			}
		}
		else {
			if (direction == FORWARD){
				motor_set_vel(MOTORS_RIGHT_ONE,limitVelocity(FORWARD_VELOCITY + (int)correction));
				motor_set_vel(MOTORS_RIGHT_TWO,limitVelocity(FORWARD_VELOCITY + (int)correction));
				motor_set_vel(MOTORS_LEFT_ONE,limitVelocity(FORWARD_VELOCITY - (int)correction));
				motor_set_vel(MOTORS_LEFT_TWO,limitVelocity(FORWARD_VELOCITY - (int)correction));
				//printf("mot left =%d mot right =%d\n", robot_front*(limitVelocity(FORWARD_VELOCITY) - (int)correction),robot_front*(limitVelocity(FORWARD_VELOCITY) + (int)correction));
			}
			else{
				motor_set_vel(MOTORS_RIGHT_ONE,-limitVelocity(FORWARD_VELOCITY - (int)correction));
				motor_set_vel(MOTORS_RIGHT_TWO,-limitVelocity(FORWARD_VELOCITY - (int)correction));
				motor_set_vel(MOTORS_LEFT_ONE,-limitVelocity(FORWARD_VELOCITY + (int)correction));
				motor_set_vel(MOTORS_LEFT_TWO,-limitVelocity(FORWARD_VELOCITY + (int)correction));
			}
		}
	}
	
	if(stop_decide == STOP){stop();}
	
	robot_front = 1;//change this
	offset = 0;
	//printf("reached point");
}




//gives sequence of movement tested working
void moveSequence(int current_territory,int target_territory,int opponent,int option){

    current_territory = (current_territory  == 0) ? 6 : current_territory;
    int flip = 0,preserve_current = current_territory,change = 0;
    int difference = target_territory - current_territory;

    flip = (target_territory>current_territory) ? 1 : 0;
    flip = ((abs(difference)>3) && flip == 1) ? 0:
            ((abs(difference)>3) && flip == 0) ? 1: flip;


    while (abs(current_territory % 6) != target_territory && change ==0){
        current_territory = (flip == 1) ? abs((current_territory + 1) % 6) : abs((current_territory - 1) % 6);
        current_territory = (current_territory == 0) ? 6 :current_territory;
        change = ((abs(current_territory)%6) == opponent) ? 1 : change;
    }

    while (abs(preserve_current % 6) != target_territory){
        if (change == 1){
        flip = (flip == 1) ? 0 : 1;
        change = 0;
        }
        preserve_current = (flip == 1) ? abs((preserve_current + 1) % 6) : abs((preserve_current - 1) % 6);
        preserve_current = (preserve_current == 0) ? 6: preserve_current;
        //replace the print statement by navigation and manipulation instrucions
		//printf("move to %d\n",abs(preserve_current) % 6);
		if (abs(preserve_current) % 6 == 0){
		navigateTo(T0,BACKWARD,DONT_STOP,1,0);
		}
		else if (abs(preserve_current) % 6 == 1){
		navigateTo(T1,BACKWARD,DONT_STOP,1,0);
		}
		else if (abs(preserve_current) % 6 == 2){
		navigateTo(T2,BACKWARD,DONT_STOP,1,0);
		}
		else if (abs(preserve_current) % 6 == 3){
		navigateTo(T3,BACKWARD,DONT_STOP,1,0);
		}
		else if (abs(preserve_current) % 6 == 4){
		navigateTo(T4,BACKWARD,DONT_STOP,1,0);
		}
		else if (abs(preserve_current) % 6 == 5){
		navigateTo(T5,BACKWARD,DONT_STOP,1,0);
		}
        
    }
	
	if (option == 1){
	//done moving to the territory, now start manipulation.
		if(robotCurrentTerritory() == 0){
			navigateTo(T0B,BACKWARD,STOP,1,100);
			gearbox();
			navigateTo(T0,BACKWARD,STOP,1,0);
			navigateTo(T0F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if(robotCurrentTerritory() == 1){
			navigateTo(T1B,BACKWARD,STOP,1,100);
			gearbox();
			navigateTo(T1,BACKWARD,STOP,1,0);
			navigateTo(T1F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if(robotCurrentTerritory() == 2){
			navigateTo(T2B,BACKWARD,STOP,1,100);
			gearbox();
			navigateTo(T2,BACKWARD,STOP,1,0);
			navigateTo(T2F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if(robotCurrentTerritory() == 3){
			navigateTo(T3B,BACKWARD,STOP,1,100);
			gearbox();
			navigateTo(T3,BACKWARD,STOP,1,0);
			navigateTo(T3F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if(robotCurrentTerritory() == 4){
			navigateTo(T4B,BACKWARD,STOP,1,100);
			gearbox();
			navigateTo(T4,BACKWARD,STOP,1,0);
			navigateTo(T4F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if(robotCurrentTerritory() == 5){
			navigateTo(T5B,BACKWARD,STOP,1,100);
			gearbox();
			navigateTo(T5,BACKWARD,STOP,1,0);
			navigateTo(T5F,FORWARD,STOP,1,100);
			collectBalls();
		}
	}
	else{
	stop();
	}
		
}



int next_territory(int opponent_territory){
copy_objects();
int i,j,imax = 0;
current_territory = (current_territory == 0) ?  6 : current_territory;
int possibilities[5] = {abs((current_territory + 1) % 6),abs((current_territory - 1) % 6),
                        abs((current_territory - 2) % 6),abs((current_territory + 2) % 6),
                        abs((current_territory + 3) % 6)};
    for(i = 0;i<5;i++){
        if(last_territory != possibilities[i]){
            if ((game.territories[possibilities[i]].remaining >= 5) && (opponent_territory != possibilities[i]) && (game.territories[possibilities[i]].rate_limit == 0)) {
                last_territory = abs(current_territory % 6);
                current_territory = possibilities[i];
                return possibilities[i];
            }
            else if ((game.territories[possibilities[i]].remaining >= 0) && (opponent_territory != possibilities[i]) && (game.territories[possibilities[i]].rate_limit == 0)) {
                for(j = 0;j<5;j++){
                    imax = (game.territories[possibilities[j]].remaining > imax) ? possibilities[j]: imax;
                }
				last_territory = abs(current_territory % 6);
				current_territory = imax;
				return imax;
            }

        }
    }
}



void gearbox(){
	copy_objects();
	int current_territory = robotCurrentTerritory();
	int start_time = get_time();
	int time_passed = get_time() - start_time;
	Point target = (current_territory == 0) ? T0B:
					(current_territory == 1) ? T1B:
					(current_territory == 2) ? T2B:
					(current_territory == 3) ? T3B:
					(current_territory == 4) ? T4B:T5B;
					
	while ((game.territories[current_territory].owner != 23) && (time_passed < 4000)){ //we time out if we exceed more than 1.5 seconds.
		
		turnTo(targetAngle(target),BACKWARD);
		glideTowardsTarget(BACKWARD);
		time_passed = get_time() - start_time;
		if (color == BLUE){
			motor_set_vel(MOTOR_FOR_GEARBOX,-150);
		}
		else{
			motor_set_vel(MOTOR_FOR_GEARBOX,150);
		}
		recaliberate();
	}
	
	motor_set_vel(MOTOR_FOR_GEARBOX,0);
	
	if ((time_passed >= 4000) && (game.territories[current_territory].owner != 23) && (timeout < 3)){
		//printf("timed out , current territory = %d",current_territory);
		timeout += 1;
		if (current_territory == 0){
			navigateTo(T0,FORWARD,STOP,1,0);
			navigateTo(T0B,BACKWARD,STOP,1,100);
			gearbox();
		}
		else if (current_territory == 1){
			navigateTo(T1,FORWARD,STOP,1,0);
			navigateTo(T1B,BACKWARD,STOP,1,100);
			gearbox();
		}
		else if (current_territory == 2){
			navigateTo(T2,FORWARD,STOP,1,0);
			navigateTo(T2B,BACKWARD,STOP,1,100);
			gearbox();
		}
		else if (current_territory == 3){
			navigateTo(T3,FORWARD,STOP,1,0);
			navigateTo(T3B,BACKWARD,STOP,1,100);
			gearbox();
		}
		else if (current_territory == 4){
			navigateTo(T4,FORWARD,STOP,1,0);
			navigateTo(T4B,BACKWARD,STOP,1,100);
			gearbox();
		}
		else if (current_territory == 5){
			navigateTo(T5,FORWARD,STOP,1,0);
			navigateTo(T5B,BACKWARD,STOP,1,100);
			gearbox();
		}
	}
	timeout = 0;
}

void collectBalls(){
	copy_objects();
	int current_territory = robotCurrentTerritory();
	int remaining = game.territories[current_territory].remaining;
	int goal = (remaining >= 5) ? remaining - 5 : 0; 
	int start_time = get_time();
	int time_passed = get_time() - start_time;
	Point target = (current_territory == 0) ? T0B:
					(current_territory == 1) ? T1B:
					(current_territory == 2) ? T2B:
					(current_territory == 3) ? T3B:
					(current_territory == 4) ? T4B:T5B;
					
	while((game.territories[current_territory].remaining != goal) && //until all balls not collected
			!((game.territories[current_territory].remaining == remaining) && (time_passed > 5000))&& (game.territories[current_territory].owner == 23) ){ //remaining balls dont change and we time out
			
			if ((game.territories[current_territory].remaining == remaining)){
				turnTo(targetAngle(target),FORWARD);
				glideTowardsTarget(FORWARD);
			}
			else{
				stop();
			}
			servo_set_pos(SWITCH_PRESS_SERVO,SWITCH_PRESS_SERVO_DOWN);
			pause(400);
			servo_set_pos(SWITCH_PRESS_SERVO,SWITCH_PRESS_SERVO_UP);
			pause(200);
	}
	
	if ((time_passed >= 2500) && (game.territories[current_territory].remaining == remaining) && (timeout < 3)){
		timeout += 1;
		if (current_territory == 0){
			navigateTo(T0,BACKWARD,STOP,1,0);
			navigateTo(T0F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if (current_territory == 1){
			navigateTo(T1,BACKWARD,STOP,1,0);
			navigateTo(T1F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if (current_territory == 2){
			navigateTo(T2,BACKWARD,STOP,1,0);
			navigateTo(T2F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if (current_territory == 3){
			navigateTo(T3,BACKWARD,STOP,1,0);
			navigateTo(T3F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if (current_territory == 4){
			navigateTo(T4,BACKWARD,STOP,1,0);
			navigateTo(T4F,FORWARD,STOP,1,100);
			collectBalls();
		}
		else if (current_territory == 5){
			navigateTo(T5,BACKWARD,STOP,1,0);
			navigateTo(T5F,FORWARD,STOP,1,100);
			collectBalls();
		}
	}
	timeout = 0;
	
}


void drop_balls(){
	int target;
	if (color == BLUE){
			target = (opponentCurrentTerritory() == 2 && (robotCurrentTerritory() == 4 || robotCurrentTerritory() == 5)) ? 3 :
						(opponentCurrentTerritory() == 2 && robotCurrentTerritory() == 0 ) ? 1 : 2; 
			moveSequence(robotCurrentTerritory(),target,opponentCurrentTerritory(),0);
			if (target == 1){
				navigateTo(T1D,BACKWARD,STOP,0,0);
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_OPEN);
				recaliberate();
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_CLOSE);
			}
			else if (target == 2){
				navigateTo(T2D,BACKWARD,STOP,0,0);
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_OPEN);
				recaliberate();
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_CLOSE);
			}
			else if (target == 3){
				navigateTo(T3D,BACKWARD,STOP,0,0);
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_OPEN);
				recaliberate();
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_CLOSE);
			}
			
	}
	else{
		    target = (opponentCurrentTerritory() == 4 && (robotCurrentTerritory() == 3 || robotCurrentTerritory() == 1)) ? 5 :
						(opponentCurrentTerritory() == 4 && robotCurrentTerritory() == 2 ) ? 0 : 4 ; 
			moveSequence(robotCurrentTerritory(),target,opponentCurrentTerritory(),0);
			if (target == 0){
				navigateTo(T0D,BACKWARD,STOP,0,0);
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_OPEN);
				recaliberate();
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_CLOSE);
			}
			else if (target == 4){
				navigateTo(T4D,BACKWARD,STOP,0,0);
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_OPEN);
				recaliberate();
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_CLOSE);
			}
			else if (target == 5){
				navigateTo(T5D,BACKWARD,STOP,0,0);
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_OPEN);
				recaliberate();
				servo_set_pos(THROW_BALL_SERVO,THROW_BALL_SERVO_CLOSE);
			}
	
	}
}


/*

For territory finding function

t0

Y ->   -887 to +887
x ->   443 to 2047

t1

y -> 256 to 1773
X -> 0 to 1536


t2

y -> 256 to 1773
x -> -1536 to 0

t3

y -> -887 to 887
x -> -2047 to -443

t4

y -> -1773 to -256
x -> -1536 to 0

t5

y -> -1773 to -256
x -> 0 to 1536


center points of every territory for exploration

t0

x -> 1024
y -> 0

t1

x -> 512
y -> 886

t2

x -> -512
y -> 886

t3

x -> -1024
y -> 0

t4

x -> -512
y -> -886

t5

x -> 512
y -> -886


*/