#include <joyos.h>
#include "turning.h"
#include "driving.h"
#include "sensing.h"

int16_t turn_degrees_on_axis(int8_t Direction,float degrees){   // Turn vehicle in desired direction. Returns 1 if succesful completion, 0 if timeout.
    
	uint32_t Timeout = get_time() + 5000;                              // Timeout after 5 seconds
	CoastMotors();
	pause(500);
	
	float angle_diff;
	int16_t counter =0;
	
	gyro_set_degrees(0.0);  
	
	if(Direction == Left){
		angle_diff =  degrees - gyro_get_degrees();
		
		while(angle_diff > AngleTolerance){
	
		if(get_time() > Timeout){
			printf("\nFailed to turn");
			CoastMotors();
			return 0;
		}
		if(angle_diff < 0){
			CoastMotors();
			printf("\nOvershoot %f   %d", (double) angle_diff, counter);
			return 1;
		}
		else{
			SetLeftVelocitybackward(100);
			SetRightVelocityforward(100);
			counter++;
			pause(10);
			angle_diff = degrees - gyro_get_degrees();
		}
		}
	}
	else{
			
		angle_diff = gyro_get_degrees() + degrees;
		
		while(angle_diff > AngleTolerance){
	
		if(get_time() > Timeout){
			printf("\nFailed to turn");
			CoastMotors();
			return 0;
		}
		if(angle_diff < 0){
			CoastMotors();
			printf("\nOvershoot %f  %d", (double) angle_diff, counter);
			return 1;
		}
		else{
			SetLeftVelocityforward(100);
			SetRightVelocitybackward(100);
			counter++;
			pause(10);
			angle_diff = gyro_get_degrees() + degrees;
		}
		}

	}
	
	CoastMotors();
	printf("\nFinished Turning %f   %d", (double) angle_diff, counter);
	return 1;
}

int16_t turn_degrees_off_axis(int8_t Direction1, int8_t Direction2, float degrees){   // Turn vehicle in desired direction. Returns 1 if succesful completion, 0 if timeout.
    
	uint32_t Timeout = get_time() + 7000;                              // Timeout after 5 seconds
	CoastMotors();
	pause(500);
	
	float angle_diff;
	
	gyro_set_degrees(0.0);
	
	if(Direction1 == Left){
		
		angle_diff = gyro_get_degrees() + degrees;
		
		if(Direction2 == FORWARD){
		
			while(angle_diff > AngleTolerance){
		
			if(get_time() > Timeout){
				printf("\nFailed to turn");
				CoastMotors();
				return 0;
			}
			if(angle_diff < 0){
				CoastMotors();
				return 1;
			}
			else{
				SetRightVelocityforward(100);
				pause(20);
				angle_diff = degrees - gyro_get_degrees();
			}
			}
		}
		else{
		
			while(angle_diff > AngleTolerance){
		
			if(get_time() > Timeout){
				printf("\nFailed to turn");
				CoastMotors();
				return 0;
			}
			if(angle_diff < 0){
				CoastMotors();
				return 1;
			}
			else{
				SetLeftVelocitybackward(130);
				pause(20);
				angle_diff = degrees - gyro_get_degrees();
			}
			}
		}	
		}

	else{
			
		angle_diff = degrees - gyro_get_degrees();

		if(Direction2 == FORWARD){
		
			while(angle_diff > AngleTolerance){
		
			if(get_time() > Timeout){
				printf("\nFailed to turn");
				CoastMotors();
				return 0;
			}
			if(angle_diff < 0){
				CoastMotors();
				return 1;
			}
			else{
				SetLeftVelocityforward(100);
				pause(20);
				angle_diff = degrees + gyro_get_degrees();
			}
			}
		}
		else{
		
			while(angle_diff > AngleTolerance){
		
			if(get_time() > Timeout){
				printf("\nFailed to turn");
				CoastMotors();
				return 0;
			}
			if(angle_diff < 0){
				CoastMotors();
				return 1;
			}
			else{
				SetRightVelocitybackward(100);
				pause(20);
				angle_diff = degrees + gyro_get_degrees();
			}
			}
		}
	}
	
	CoastMotors();
	return 1;
}

void turn_out_of_Starting_Square(){
	if(Orientation == West){
	printf("\nturning right 90 degrees");
	pause(3000);
		turn_degrees_on_axis(Right,90.0);
	}
	else if(Orientation == East){
	printf("\nturning left 90 degrees");
	pause(3000);
		turn_degrees_on_axis(Left,90.0);
	}
	else if(Orientation == South){
	printf("\nturning right 180 degrees");
	pause(3000);
		turn_degrees_on_axis(Right,180.0);
	}
}