/* Team 13 Strategy File */

void execute_strategy () {
	float angletemp,angletemp2;
	float NS_TO = 0.0;
	float EW_TO = 0.0;
	float inittime = seconds();
	int enemymovedx = 0;
	int enemymovedy = 0;
	int TOreached = 0;
	float TOchecktime = 0.0;
	int neededcorrect = false;
	int Avisited = 0;  /* A is enemy scoring area corresp to 1 for case 0; B is 3, C is 5*/
	int Bvisited = 0;
	int Cvisited = 0;
	float casefactor = 1.0;
	float backup_enough = 0.14;
	float temp_gyro_angle = 0.0;

	/*
	Case 0: Black side, want to go to red (+x)
	Case 1: Black side, want to go to green (-x)
	Case 2: White side, want to go to red (+x)
	Case 3: White side, want to go to green (-x)*/
	int CASE = -1;

	
	if (initial_orient == 0 || initial_orient == 1) {
		/*North or south to start with */
		while (enemymovedx == 0 && (seconds()-inittime<NS_TO)) {
			load_rf_pos();
			if (op_x > 100)
				enemymovedx = 1;
			if (op_x < -100)
				enemymovedx = -1;
		}
		if (enemymovedx == 0)
			TOreached = 1;
		/* now, know which way enemy went or that timeout reached*/

		angletemp = angle;

		/* do stuff based on white or black */
		if (initial_orient == 0 && teamisblack) {
			if (enemymovedx == 1) {
				gyro_go_to_angle(angle+90.0, 2.5);
				angletemp += 90.0;
				CASE = 1;
			}
			if (enemymovedx == -1) {
				gyro_go_to_angle(angle-90.0, 2.5);
				angletemp -= 90.0;
				CASE = 0;
			}
			if (TOreached) {
				if (rf_vote_winner == 0) {
					gyro_go_to_angle(angle+90.0, 2.5);
					angletemp += 90.0;
					CASE = 1;
				}
				else {
					gyro_go_to_angle(angle-90.0, 2.5);
					angletemp -= 90.0;
					CASE = 0;
				}
			}
		}
		if (initial_orient == 1 && teamisblack) {
			if (enemymovedx == 1) {
				gyro_go_to_angle(angle-90.0, 2.5);
				angletemp -= 90.0;
				CASE = 1;
			}
			if (enemymovedx == -1) {
				gyro_go_to_angle(angle+90.0, 2.5);
				angletemp += 90.0;
				CASE = 0;
			}
			if (TOreached) {
				if (rf_vote_winner == 0) {
					gyro_go_to_angle(angle-90.0, 2.5);
					angletemp -= 90.0;
					CASE = 1;
				}
				else {
					gyro_go_to_angle(angle+90.0, 2.5);
					angletemp += 90.0;
					CASE = 0;
				}
			}
		}
		if (initial_orient == 0 && !teamisblack) {
			if (enemymovedx == 1) {
				gyro_go_to_angle(angle+90.0, 2.5);
				angletemp += 90.0;
				CASE = 3;
			}
			if (enemymovedx == -1) {
				gyro_go_to_angle(angle-90.0, 2.5);
				angletemp -= 90.0;
				CASE = 2;
			}
			if (TOreached) {
				if (rf_vote_winner == 0) {
					gyro_go_to_angle(angle+90.0, 2.5);
					angletemp += 90.0;
					CASE = 3;
				}
				else {
					gyro_go_to_angle(angle-90.0, 2.5);
					angletemp -= 90.0;
					CASE = 2;
				}
			}
		}
		if (initial_orient == 1 && !teamisblack) {
			if (enemymovedx == 1) {
				gyro_go_to_angle(angle-90.0, 2.5);
				angletemp -= 90.0;
				CASE = 3;
			}
			if (enemymovedx == -1) {
				gyro_go_to_angle(angle+90.0, 2.5);
				angletemp += 90.0;
				CASE = 2;
			}
			if (TOreached) {
				if (rf_vote_winner == 0) {
					gyro_go_to_angle(angle-90.0, 2.5);
					angletemp -= 90.0;
					CASE = 3;
				}
				else {
					gyro_go_to_angle(angle+90.0, 2.5);
					angletemp += 90.0;
					CASE = 2;
				}
			}
		}
	}
	else {
		/*East or west to start with */

		while (enemymovedx == 0 && (seconds()-inittime<EW_TO)) {
			load_rf_pos();
			if (op_x > 100)
				enemymovedx = 1;
			if (op_x < -100)
				enemymovedx = -1;
		}
		if (enemymovedx == 0)
			TOreached = 1;

		angletemp = angle;

		/* now, know which way enemy went or that timeout reached */

		if (initial_orient == 2 && teamisblack) {
			if (enemymovedx == 1) {
				gyro_go_to_angle(angle+180.0, 2.5);
				angletemp += 180.0;
				CASE = 1;
			}
			if (enemymovedx == -1) {
				CASE = 0;
			}
			if (TOreached) {
				CASE = 0;
			}
		}
		if (initial_orient == 3 && teamisblack) {
			if (enemymovedx == 1) {
				CASE = 1;
			}
			if (enemymovedx == -1) {
				gyro_go_to_angle(angle+180.0, 2.5);
				angletemp += 180.0;
				CASE = 0;
			}
			if (TOreached) {
				CASE = 1;
			}
		}
		if (initial_orient == 2 && !teamisblack) {
			if (enemymovedx == 1) {
				gyro_go_to_angle(angle+180.0, 2.5);
				angletemp += 180.0;
				CASE = 3;
			}
			if (enemymovedx == -1) {
				CASE = 2;
			}
			if (TOreached) {
				CASE = 2;
			}
		}
		if (initial_orient == 3 && !teamisblack) {
			if (enemymovedx == 1) {
				CASE = 3;
			}
			if (enemymovedx == -1) {
				gyro_go_to_angle(angle+180.0, 2.5);
				angletemp += 180.0;
				CASE = 2;
			}
			if (TOreached) {
				CASE = 3;
			}
		}

	}

	/* set casefactor before doing any turns below this */
	/* Cases 0 and 3 have same turning angles, 1 and 2 have same,
	and all have same distances. */
	if (CASE == 0 || CASE == 3) {
		casefactor = 1.0;
	}
	else {
		casefactor = -1.0;
	}


	/* by here, should be rotated and ready to go for each case */
	
	/* angletemp currently stores what the angle should be */

	/* Case 0: Black to red side, and Case 3: White to green side */

	/* Line up with far balls */
	forward_se((int)(1.42*clicksperfoot),3.0);

	/* Rotate 90 deg to face far balls */
	gyro_go_to_angle(angle-90.0*casefactor+(angletemp-angle), 3.0);
	angletemp -= 90.0*casefactor;

	/* Are we going to hit them? NEEDS TO BE CHECKED FOR EACH CASE!*/
	load_rf_pos();
	if (CASE == 0 && op_x>175) {
		/*turn 90 back towards home, then go twice the dist we came */
		CASE = 1;
		gyro_go_to_angle(angle-90.0+(angletemp-angle), 3.0);
		/*angletemp -= 90.0;*/
		forward_se((int)(2.0*1.45*clicksperfoot),3.0);
		gyro_go_to_angle(angle+90.0+(angletemp-90.0-angle), 3.0);
		casefactor *= -1.0;
	}
	else {
		if (CASE == 1 && op_x<-175) {
			/*turn 90 back towards home, then go twice the dist we came */
			CASE = 0;
			gyro_go_to_angle(angle+90.0+(angletemp-angle), 3.0);
			/*angletemp += 90.0;*/
			forward_se((int)(2.0*1.45*clicksperfoot),3.0);
			gyro_go_to_angle(angle-90.0+(angletemp+90.0-angle), 3.0);
			casefactor *= -1.0;
		}
		else {
			if (CASE == 2 && op_x>175) {
				printf("\nin third check");
				/*turn 90 back towards home, then go twice the dist we came */
				CASE = 3;
				gyro_go_to_angle(angle+90.0+(angletemp-angle), 3.0);
				/*angletemp += 90.0;*/
				forward_se((int)(2.0*1.45*clicksperfoot),3.0);
				gyro_go_to_angle(angle-90.0+(angletemp+90.0-angle), 3.0);
				casefactor *= -1.0;
			}
			else {
				if (CASE == 3 && op_x<-175) {
					printf("\nin fourth check");
					/*turn 90 back towards home, then go twice the dist we came */
					CASE = 2;
					gyro_go_to_angle(angle-90.0+(angletemp-angle), 3.0);
					/*angletemp -= 90.0;*/
					forward_se((int)(2.0*1.45*clicksperfoot),3.0);
					gyro_go_to_angle(angle+90.0+(angletemp-90.0-angle), 3.0);
					casefactor *= -1.0;
				}
			}
		}
	}

	angletemp += 90.0*casefactor;
	threadsafe_sleep(0.2);
	gyro_go_to_angle(angle-90.0*casefactor+(angletemp-angle), 3.0); /* doublecheck critical angle! */

	/* Go get 'em - far.. timeout might be important */
	angletemp = angle;
	intake_pull(100);
	forward_se((int)((48.5/12.0)*clicksperfoot), 5.0);

	/* Rotate towards wall */
	gyro_go_to_angle(angle+90.0*casefactor+(angletemp-angle), 3.0);
	angletemp += 90.0*casefactor;

	/* hit wall, timeout */
	/*intake_pull(60);*/
	forward_se((int)((10.0/12.0)*clicksperfoot),2.0);
	/*angletemp = angle;*/
	intake_pull(100);

	/* back up enough to turn, same as below */
	/*backward_se((int)(backup_enough*clicksperfoot),4.0);*/

	/* Rotate back towards home */
	gyro_go_to_angle(angle+82.0*casefactor+(angletemp-angle), 3.0);
	angletemp += 82.0*casefactor;

	/*  sweep all balls near voting area wall */
	forward_se((int)(((48.0)/12.0)*clicksperfoot),5.0);

	TOchecktime = seconds() + 4.0;	
	if(seconds() >= TOchecktime){
		if(CASE ==1 || CASE == 2){
			gyro_go_to_angle(angle+15.0,3.0); /* rotate away from wall */			
		}
		else{
			gyro_go_to_angle(angle-15.0,3.0); /* rotate away from wall */
		}

		forward_se((int)(((12.0)/12.0)*clicksperfoot),4.0);
	}

	threadsafe_sleep(.2);

	/* sweeping servo turn to vote*/
	temp_gyro_angle = angle;
	TOchecktime = seconds() + 5.0;
	if(CASE == 1 || CASE == 2 && TOchecktime > seconds()){ /* black and red */
		servo(servoport,(servoTURN + servoBK)/2 + 500); /* was + 500 */
		while(angle <  temp_gyro_angle + 60.0){
			Lmotor(-100);
			Rmotor(-5);
		}
		Lmotor(0);
		Rmotor(0);
	}
	else{ /* black and green */
		if(TOchecktime > seconds()){
			servo(servoport,(servoTURN + servoBK)/2 + 1600 - 500); /* was - 500 */
			while(angle > temp_gyro_angle - 60.0){
				Lmotor(-5);
				Rmotor(-100);
			}
			Lmotor(0);
			Rmotor(0);
		}
	}

	/* Voting sequence */
	intake_push(100);
	forward_se((int)(3.0*clicksperfoot),1.0);
	backward_se((int)(3.0*clicksperfoot),0.3);
	forward_se((int)(3.0*clicksperfoot),0.7);
	backward_se((int)(3.0*clicksperfoot),0.3);
	forward_se((int)(3.0*clicksperfoot),1.0);
	angletemp = angle;

	/* Back up enough to turn, same as above */
	backward_se((int)(backup_enough*clicksperfoot),1.5);

	/* Rotate towards home */

	gyro_go_to_angle(angle+88.0*casefactor+(angletemp-angle), 4.0);

	/* Forward to get first batch of score balls */
	angletemp = angle;
	intake_pull(100);
	/*forward_se((int)(((60.0)/12.0)*clicksperfoot),5.0); /* hit wall now */
	/*if(!neededcorrect){
		forward_se((int)((33.0/12.0)*clicksperfoot),3.0);
	}
	else{
		forward_se((int)((31.0/12.0)*clicksperfoot),3.0);
	}*/

	forward_se((int)((27.0/12.0)*clicksperfoot),3.0);


	/* Rotate towards home again, if stuck, move forward, try again */

	/*backup to line up if hitting far wall */
	/*backward_se((int)(((15.0)/12.0)*clicksperfoot),3.5);*/

	neededcorrect = false;
	TOchecktime = seconds() + 2.5;
	gyro_go_to_angle(angle+93.0*casefactor+(angletemp-angle), 2.5);
	if(seconds() >= TOchecktime && fabs(angle-(angletemp+93.0*casefactor)) > 5.0){ /* timed out, backup */
		forward_se((int)((12.0/12.0)*clicksperfoot),1.0);
		gyro_go_to_angle(angle+93.0*casefactor+(angletemp-angle), 2.5);
		neededcorrect = true;
	}

	/* Go forward to line up w/ score bin near home */
	angletemp = angle;
	forward_se((int)((3.0-0.14-0.5 + (2.0/12.0))*clicksperfoot),3.0);

	/* Rotate towards score bin */
	gyro_go_to_angle(angle-90.0*casefactor+(angletemp-angle), 2.5);

	/* Score! */
	intake_pull(40);
	forward_se((int)(3.0*clicksperfoot),1.5);
	intake_off();
	threadsafe_sleep(0.3);
	angletemp = angle;

	
	/* done scoring, hopefully */

	load_rf_pos();

	printf("\nmy:%d,%d",my_x, my_y);
	threadsafe_sleep(1.0);

	if ((((CASE == 0 || CASE == 1) && (my_x<-275||my_x>275||my_y<0))) || ((CASE == 2 || CASE == 3) && (my_x<-275||my_x>275||my_y>0))) {
		/* if we're nowhere near close, take crazy action (start w/ intake push and backwards just in case, then goalset to sweep enemy main area */
		intake_push(100);
		backward_se((int)(200.0*clicksperunit),3.0);

		if(CASE == 0 || CASE == 2) {
			setgoal(250.0,0.0);
			update_start_coords();
			rotate_towards_goal();
			intake_pull(100);
			forward_se((int)(distance(start_x,start_y,goal_x,goal_y)),5.0);
		}
		else {
			setgoal(-250.0,0.0);
			update_start_coords();
			rotate_towards_goal();
			intake_pull(100);
			forward_se((int)(distance(start_x,start_y,goal_x,goal_y)),5.0);
		}

		/* that was a preliminary move to make sure we don't hit the mid wall, now do the main timeout one to get to the far wall */
		/* make sure to timeout! also reset angletemp after */
		load_rf_pos();
		if (CASE == 0) {
			setgoal(250.0,-500.0);
			update_start_coords();
			rotate_towards_goal();
			forward_se((int)(distance(start_x,start_y,goal_x,goal_y)),4.0);
			angletemp = angle;
			backward_se((int)(backup_enough*clicksperfoot),3.0);
			gyro_go_to_angle(angle-90.0+(angletemp-angle), 3.0);
			forward_se((int)(800.0*clicksperunit),3.5);
		}
		if (CASE == 1) {
			setgoal(-250.0,-500.0);
			update_start_coords();
			rotate_towards_goal();
			forward_se((int)(distance(start_x,start_y,goal_x,goal_y)),4.0);
			angletemp = angle;
			backward_se((int)(backup_enough*clicksperfoot),3.0);
			gyro_go_to_angle(angle+90.0+(angletemp-angle), 3.0);
			forward_se((int)(800.0*clicksperunit),3.5);
		}
		if (CASE == 2) {
			setgoal(250.0,500.0);
			update_start_coords();
			rotate_towards_goal();
			forward_se((int)(distance(start_x,start_y,goal_x,goal_y)),4.0);
			angletemp = angle;
			backward_se((int)(backup_enough*clicksperfoot),3.0);
			gyro_go_to_angle(angle+90.0+(angletemp-angle), 3.0);
			forward_se((int)(800.0*clicksperunit),3.5);
		}
		if (CASE == 3) {
			setgoal(-250.0,500.0);
			update_start_coords();
			rotate_towards_goal();
			forward_se((int)(distance(start_x,start_y,goal_x,goal_y)),4.0);
			angletemp = angle;
			backward_se((int)(backup_enough*clicksperfoot),3.0);
			gyro_go_to_angle(angle-90.0+(angletemp-angle), 3.0);
			forward_se((int)(800.0*clicksperunit),3.5);
		}
		intake_off();
		/* add more crazy stuff if needed*/
	}
	else {
	
		/* CHECK ON WHETHER OR NOT TO LEAVE HERE /////////////////////////////*/

		/* First set Avisited, Bvisited, Cvisited */

		if (CASE == 0) {
			Avisited = MG_1_visited;
			Bvisited = MG_3_visited;
			Cvisited = MG_5_visited;
			our_color = 0;
		}
		else {
			if (CASE == 1) {
				Avisited = MG_3_visited;
				Bvisited = MG_1_visited;
				Cvisited = MG_5_visited;
				our_color = 1;
			}
			else {
				if (CASE == 2) {
					Avisited = MG_4_visited;
					Bvisited = MG_6_visited;
					Cvisited = MG_2_visited;
					our_color = 0;
				}
				else {
					if (CASE == 3) {
						Avisited = MG_6_visited;
						Bvisited = MG_4_visited;
						Cvisited = MG_2_visited;
						our_color = 1;
					}
				}
			}
		}
		
		/* if we want to leave */
		if(!(our_color==rf_vote_winner && (!Avisited && !Bvisited)) || (our_color==rf_vote_winner && MG_op_near)) {
			/* Only leave if NOT (winning vote and opponent hasn't done shit, OR winning vote and opponent is close by)	*/

			if (our_color != rf_vote_winner && (abs(rf_vote_red-rf_vote_green) <= 1)) {
				/* get 2 strays and vote */
				intake_push(100);
				backward_se((int)(250.0*clicksperunit),3.0);
				gyro_go_to_angle(angle-175.0+(angletemp-angle), 3.0);
				intake_pull(100);
				forward_se((int)(4.0*clicksperfoot),2.0);
				angletemp = angle;
				backward_se((int)(backup_enough*clicksperfoot),3.0);
				gyro_go_to_angle(angle+87.0*casefactor+(angletemp-angle), 3.0);
				forward_se((int)(4.0*clicksperfoot),3.0);
				/* Voting sequence */
				intake_push(100);
				backward_se((int)(3.0*clicksperfoot),0.3);
				forward_se((int)(3.0*clicksperfoot),0.7);
				backward_se((int)(3.0*clicksperfoot),0.3);
				forward_se((int)(3.0*clicksperfoot),1.7);
			}
			else  {
				if (Avisited || Bvisited || Cvisited) {
					/* charge A! might get some other balls on the way, then vote */
					intake_push(100);
					backward_se((int)(200.0*clicksperunit),3.0);
					gyro_go_to_angle(angle+90.0*casefactor+(angletemp-angle), 3.0);
					intake_pull(100);
					forward_se((int)(400.0*clicksperunit),3.0);
					angletemp = angle;
					backward_se((int)(backup_enough*clicksperfoot),3.0);
					gyro_go_to_angle(angle-90.0*casefactor+(angletemp-angle), 3.0);
					forward_se((int)(250.0*clicksperunit),2.0);
					/*now against wall in enemy area, have balls */
					angletemp = angle;
					backward_se((int)(200.0*clicksperunit),3.0);
					gyro_go_to_angle(angle-90.0*casefactor+(angletemp-angle), 3.0);
					angletemp -= 90.0*casefactor;
					forward_se((int)(400.0*clicksperunit),2.0);
					gyro_go_to_angle(angle-90.0*casefactor+(angletemp-angle), 3.0);
					angletemp -= 90.0*casefactor;
					forward_se((int)(250.0*clicksperunit),2.0);
					gyro_go_to_angle(angle+90.0*casefactor+(angletemp-angle), 3.0);
					forward_se((int)(3.0*clicksperfoot),1.5);
					/* Voting sequence */
					intake_push(100);
					backward_se((int)(3.0*clicksperfoot),0.3);
					forward_se((int)(3.0*clicksperfoot),0.7);
					backward_se((int)(3.0*clicksperfoot),0.3);
					forward_se((int)(3.0*clicksperfoot),1.7);
				}
				else {
					/* our_color is losing, opponent hasnt visited any areas, so descore them, hahaha */
					intake_pull(100);
					backward_se((int)(200.0*clicksperunit),3.0);
					/*go to area B!*/
					gyro_go_to_angle(angle-90.0*casefactor+(angletemp-angle), 3.0);
					forward_se((int)(400.0*clicksperunit),2.5);
					angletemp = angle;
					backward_se((int)(backup_enough*clicksperfoot),3.0);
					gyro_go_to_angle(angle+90.0*casefactor+(angletemp-angle), 3.0);
					forward_se((int)(250.0*clicksperunit),2.5);
					/*deposit balls*/
					/*get in front of our_color booth, face it*/
					backward_se((int)(4.5*clicksperfoot),5.0);
				}
			}
		}
	}
		
	

	/* that's all the time we have for today */

	ao();

	/*printf("\nMG:%d,%d,%d,%d,%d,%d",MG_1_visited,MG_2_visited,MG_3_visited,MG_4_visited,MG_5_visited,MG_6_visited);*/

	return;
	/* remember to:
	add while loop to turn stuck correction
	if we haven't reached our scoring area, set goal off table so you run into opp wall, sweep opp area into ours
	*/
}



void test_turn(){
	float temp_gyro_angle = angle;
	servo(servoport,(servoTURN + servoBK)/2 + 1600 - 500);
	while(angle > temp_gyro_angle - 60.0){
		Lmotor(0);
		Rmotor(-100);
	}
	Lmotor(0);
	Rmotor(0);
}