/* Movement Routines */



void timeout_se(int clicks, float timeout){ /* shaft encoding with timeout.. work very well with servo on rear caster wheel */
	float initial_angle = angle; /* keep track of theta's starting value */
	int currentservo = 2300;
	int goingforward = true;
	float angle_thresh = 10.0; /* threshold for steering. */
	int servo_main_delta = 0;
	int been_movin = false;
	int total_clicks = clicks;

	float se_start_time = seconds();
	float se_end_time = se_start_time + timeout;
/*
	if(MotL_direction == 1 && MotR_direction == 1){
		currentservo = servoFWD;
		goingforward = true;
	}
	else{
		currentservo = servoBK;
		goingforward = false;
	}
*/
	enable_encoder(7);
	enable_encoder(8);
	while((float)(read_encoder(7)+read_encoder(8))/2.0 < (float)(clicks-150)  &&  seconds() < se_end_time ){
		Lmotor(MotL_direction*100);
		Rmotor(MotR_direction*100);
	}

	Lmotor(MotL_direction*0);
	Rmotor(MotR_direction*0);
	threadsafe_sleep(.30); /* let the bot coast */ /* was 0.3*/

	while((float)(read_encoder(7)+read_encoder(8))/2.0 < (float)(clicks-70)  &&  seconds() < se_end_time ){
		if(MotL_direction == 1 && MotR_direction == 1){
			Lmotor(MotL_direction*60);
			Rmotor(MotR_direction*60);
		}
		else{
			Lmotor(MotL_direction*50);
			Rmotor(MotR_direction*50);
		}
		been_movin = true;
	}

	Lmotor(MotL_direction*0);
	Rmotor(MotR_direction*0);

	if(seconds() < se_end_time){
		threadsafe_sleep(.20); /* was 0.2 */ 
	}

	if(total_clicks > 100){
		bad_Lmotor_brake(-MotL_direction*40, 0);
		bad_Rmotor_brake(-MotR_direction*40, 0);
		/* Lmotor(MotL_direction*0);
		   Rmotor(MotR_direction*0); */
		threadsafe_sleep(.005);
	}

	while((float)(read_encoder(7)+read_encoder(8))/2.0 < (float)(clicks-45)  &&  seconds() < se_end_time ){
		if(MotL_direction == 1 && MotR_direction == 1){
			Lmotor(MotL_direction*40);
			Rmotor(MotR_direction*40);
		}
		else{
			Lmotor(MotL_direction*30);
			Rmotor(MotR_direction*30);
		}
		been_movin = true;
	}
	
	if(been_movin){
		bad_Lmotor_brake(-MotL_direction*40, 0);
		bad_Rmotor_brake(-MotR_direction*40, 0);
		Lmotor(MotL_direction*0);
		Rmotor(MotR_direction*0);
		threadsafe_sleep(.002);
	}

	while((float)(read_encoder(7)+read_encoder(8))/2.0 < (float)(clicks-6)  &&  seconds() < se_end_time ){

		Lmotor(MotL_direction*20);
		Rmotor(MotR_direction*20);
		
		been_movin = true;
	}


	/*if(seconds() < se_end_time){
		threadsafe_sleep(.3);
	}*/

	if(been_movin){
		bad_Lmotor_brake(-MotL_direction*40, 0);
		bad_Rmotor_brake(-MotR_direction*40, 0);
		Lmotor(MotL_direction*0);
		Rmotor(MotR_direction*0);
		threadsafe_sleep(.002);
	}

	if(seconds() >= se_end_time){
		printf("\nMove T.O.!      %f ", timeout);
	}

	/*printf("\n%d,%d         err:%f",read_encoder(7),read_encoder(8),(angle - initial_angle));*/
	return;
}



void forward_se(int clicks, float timeout){

    servo(servoport,servoFWD);
	if(!last_was_straight){
		threadsafe_sleep(.35);
	}

	MotL_direction = 1;
	MotR_direction = 1;
	timeout_se(clicks, timeout);
	last_was_straight = true;
	return;
}


void backward_se(int clicks, float timeout){

    servo(servoport,servoBK);
	if(!last_was_straight){
		threadsafe_sleep(.35);
	}

	MotL_direction = -1;
	MotR_direction = -1;
	timeout_se(clicks, timeout);
	last_was_straight = true;
	return;

}



void rotate_towards_goal(){
	float goal_heading = 0.0;
	goal_heading = heading_towards_goal();
	gyro_go_to_angle(goal_heading, 3.5);
	return;
}

/*float get_heading(){
	return(angle);
}*/


void load_rf_pos(){
	if(rf_our_team == 0){
		my_x = rf_x0;
		my_y = rf_y0;
		op_x = rf_x1;
		op_y = rf_y1;
	}
	else{
		op_x = rf_x0;
		op_y = rf_y0;
		my_x = rf_x1;
		my_y = rf_y1;
	}
	return;
}

void setgoal (float inputx, float inputy) {
	goal_x = (int)inputx;
	goal_y = (int)inputy;
}

void update_start_coords () {
	start_x = my_x;
	start_y = my_y;
}


float shortest_delta_angle_to_goal(float current_heading){
	float goal_heading = 0.0;
	float error = 0.0;
	goal_heading = heading_towards_goal();

	/*printf("\nhtg: %f", goal_heading);
	wait_start();
	printf("\ncurrhead: %f",current_heading);
	wait_start();*/

	error = goal_heading - current_heading;

	while(error < -360.0){
		error += 360.0;
	}
	error = error + 360.0;
	while(error > 360.0){
		error -= 360.0;
	}
	if(error < 180.0){
		return(error);
	}
	else{
		return(-(360.0 - error));
	}
}





float heading_towards_goal(){
	float goal_heading = 0.0;
	float delta_x = 0.0;
	float delta_y = 0.0;
	
	load_rf_pos();
	delta_x = (float)(goal_x - my_x);
	delta_y = (float)(goal_y - my_y);

	if(delta_x > 0.0 && delta_y >= 0.0){  /* Quad. I */
		goal_heading = atan(delta_y / delta_x)*180.0/pi;
		/*printf("\nQ1:");*/
	}
	if(delta_x == 0.0 && delta_y >= 0.0){  /* +x axis*/
		goal_heading = 90.0;
		/*printf("\n+x:");*/
	}
	if(delta_x < 0.0 && delta_y >= 0.0){  /* Quad. II */
		goal_heading = 180.0 - atan(- delta_y / delta_x)*180.0/pi;
		/*printf("\nQ2:");*/
	}
	if(delta_x < 0.0 && delta_y < 0.0){  /* Quad. III */
		goal_heading = 180.0 - atan(- delta_y / delta_x)*180.0/pi;
		/*printf("\nQ3:");*/
	}
	if(delta_x == 0.0 && delta_y < 0.0){  /* -x axis*/
		goal_heading = 270.0;
		/*printf("\n-x:");*/
	}
	if(delta_x > 0.0 && delta_y < 0.0){  /* Quad. IV */
		goal_heading = 360.0 + atan(delta_y / delta_x)*180.0/pi;
		/*printf("\nQ4:");*/
	}
	
	/*printf(" %f,%f",delta_x,delta_y);
	wait_start();*/
	return(goal_heading);
	
}


float distance(int x0,int y0,int x1,int y1){
	float dist = 0.0;
	dist = sqrt( ((float)x0 - (float)x1)*((float)x0 - (float)x1) + ((float)y0 - (float)y1)*((float)y0 - (float)y1) );
	return(dist);
}

float fdistance(float x0,float y0,float x1,float y1){
	float dist = 0.0;
	dist = sqrt( (x0 - x1)*(x0 - x1) + (y0 - y1)*(y0 - y1) );
	return(dist);
}


void Lmotor (int speed) {
	motor(leftmotorA,speed);
	motor(leftmotorB,speed);
}

void Rmotor (int speed) {
	motor(rightmotorA,speed);
	motor(rightmotorB,speed);
}

void Imotor (int speed) {
	motor(intakemotorA,speed);
	motor(intakemotorB,speed);
}

void bad_Lmotor_brake(int speed, int brake){
	motor(leftmotorA,speed);
	motor(leftmotorB,-brake);
}

void bad_Rmotor_brake(int speed, int brake){
	motor(rightmotorA,speed);
	motor(rightmotorB,-brake);
}