/* McGruff is a watchdog that keeps track of important events in the background. */

void newthread_mcgruff(){ /* watchdog */
	int pid;

	if(MG_StopMG == true){
		MG_StopMG = false; /* allow McGruff to run */
		
		/* To stop McGruff, set MG_StopMG to True. */
		pid = start_process(start_mcgruff()); 	  
		return;
	}
	return;
}

void killthread_mcgruff(){
	MG_StopMG = true; /* Stop McGruff */
	return;
}

void start_mcgruff() {
	

	McGruffMain();  /* run watchdog loop */



	/* process should now exit */
}



void McGruffMain() {
	float distance = 0.0;
	float delta_dist = 0.0;

	while(!MG_StopMG){

		load_rf_pos();

		distance = MG_distance(rf_x0, rf_y0, rf_x1, rf_y1);
		delta_dist = MG_distance(MG_my_last_x, MG_my_last_y, MG_op_last_x, MG_op_last_y) - distance;
		/* -- check for opponent by GREEN Voting booth -- */
	/*	if(op_x < -200 && abs(op_y) < 100){
			MG_nearGREENvote = true;
			
		}
		else{
			MG_nearGREENvote = false;
		}
		/* -- check for opponent by RED Voting booth -- */
		/*if(op_x > 200 && abs(op_y) < 100){
			MG_nearREDvote = true;
		
		}
		else{
			MG_nearREDvote = false;
		}


        /* -- check if opponent has visited/occupies parts of the board. -- */
		if(op_x < -175 && op_y > 275){
			MG_1_visited = true;
			MG_1_present = true;
		}
		else{
			MG_1_present = false;
		}
		if(abs(op_x) < 75 && op_y > 275){
			MG_2_visited = true;
			MG_2_present = true;
		}
		else{
			MG_2_present = false;
		}
		if(op_x > 175 && op_y > 275){
			MG_3_visited = true;
			MG_3_present = true;
		}
		else{
			MG_3_present = false;
		}
		if(op_x < -175 && op_y < -275){
			MG_4_visited = true;
			MG_4_present = true;
		}
		else{
			MG_4_present = false;
		}
		if(abs(op_x) < 75 && op_y < -275){
			MG_5_visited = true;
			MG_5_present = true;
		}
		else{
			MG_5_present = false;
		}
		if(op_x > 175 && op_y < -275){
			MG_6_visited = true;
			MG_6_present = true;
		}
		else{
			MG_6_present = false;
		}


		/* calculate useful rf info if it's time (~every .25 seconds) */
		if(seconds() >= MG_last_rf_update + .25){
			load_rf_pos(); /* load latest values into my_x/y and op_x/y */

				/* heading is in absolute degrees (0 - 360) */
			/*MG_my_rf_heading = MG_heading_A_to_B(MG_my_last_x, MG_my_last_y, my_x, my_y);
			MG_op_rf_heading = MG_heading_A_to_B(MG_op_last_x, MG_op_last_y, op_x, op_y);*/
				/* speed is in units/second. = hundredths of feet per sec. */
			/*MG_my_rf_speed = 4.0 * sqrt((float)(my_x-MG_my_last_x)*(float)(my_x-MG_my_last_x) + (float)(my_y-MG_my_last_y)*(float)(my_y-MG_my_last_y));
			MG_op_rf_speed = 4.0 * sqrt((float)(op_x-MG_op_last_x)*(float)(op_x-MG_op_last_x) + (float)(op_y-MG_op_last_y)*(float)(op_y-MG_op_last_y));*/

			   /* update position values for next time. */
			MG_my_last_x = my_x;
			MG_my_last_y = my_y;
			MG_op_last_x = op_x;
			MG_op_last_y = op_y;
			MG_last_rf_update = seconds();
		}

		/* -- collision avoidence -- */
		if(distance < 150.0 && delta_dist < 0.0){ /* op. is close by and getting closer! */
			MG_op_near = true;
			/*newthread_alarm(); /* turn on alarm */
		}
		else{
			MG_op_near = false;
			/*killthread_alarm(); /* turn off alarm */
		}


		if(my_x == 0 && my_y ==0){ /*  */
			newthread_alarm(); /* turn on alarm */
		}
		else{
			killthread_alarm(); /* turn off alarm */
		}





		/* ------------------- */
		/* more Checking HERE! */
		/* ------------------- */
		defer(); /* only need to run through loop once per timeslot */
	}
	return;
}	


float MG_distance(int x0, int y0, int x1, int y1) {
	float dist;
	
	dist = sqrt( (float)(((float)y0-(float)y1)*((float)y0-(float)y1) + ((float)x0-(float)x1)*((float)x0-(float)x1)) );

	return(dist);
}


/*
float MG_heading_A_to_B(int Ax, int Ay, int Bx, int By){
	float A_to_B_heading = 0.0;
	float delta_x = 0.0;
	float delta_y = 0.0;
	
	delta_x = (float)(Bx - Ax);
	delta_y = (float)(By - Ay);

	if(delta_x > 0.0 && delta_y >= 0.0){  /* Quad. I */
/*		A_to_B_heading = atan(delta_y / delta_x)*180.0/pi;
	}
	if(delta_x == 0.0 && delta_y >= 0.0){  /* +x axis*/
/*		A_to_B_heading = 90.0;
	}
	if(delta_x < 0.0 && delta_y >= 0.0){  /* Quad. II */
/*		A_to_B_heading = 180.0 - atan(- delta_y / delta_x)*180.0/pi;
	}
	if(delta_x < 0.0 && delta_y < 0.0){  /* Quad. III */
/*		A_to_B_heading = 180.0 - atan(- delta_y / delta_x)*180.0/pi;
	}
	if(delta_x == 0.0 && delta_y < 0.0){  /* -x axis*/
/*		A_to_B_heading = 270.0;
	}
	if(delta_x > 0.0 && delta_y < 0.0){  /* Quad. IV */
/*		A_to_B_heading = 360.0 + atan(delta_y / delta_x)*180.0/pi;
	}
	return(A_to_B_heading);
	
}
*/


void newthread_alarm(){
	int pid;

	if(AL_StopAL == true){
		AL_StopAL = false; /* allow McGruff to run */
		
		/* To stop McGruff, set MG_StopMG to True. */
		pid = start_process(alarm()); 	  
		return;
	}
	return;
}

void killthread_alarm(){
	AL_StopAL = true; /* Stop Alarm */
	return;
}


void alarm(){
	while(!AL_StopAL){
		tone(1500.0, 0.2);
		defer();
		tone(2000.0, 0.2);
		defer();
	}
}