#include <joyos.h>
#include <lib/pid.h>
#include <happylib.h>
#include <math.h>
#include <goaltogoal.h>

#define right_forward 1
#define left_forward 1
#define right_motor 0
#define left_motor 2
#define right_IR 13
#define mid_IR 14
#define left_IR 15
#define LSB_MS_PER_DEG	1330
#define gyro_port 11
#define right_bumper 0
#define left_bumper 2
#define servo 0
#define gateservo 2
#define RightEncoder 24
#define LeftEncoder 25
#define front_dist_sensor 22
#define left_dist_sensor 20
#define right_dist_sensor 23
#define back_dist_sensor 21
#define PI 3.14

uint8_t team_number[2] = {16,0};

/*	Orient bot, then go forward until blue ball line is reached, turn and try crashing into other bot, 
	if no bot go backwards and block goal
*/


//check whether IR sensor is black (0) or white (1)
int right_IR_on() {
	if (analog_read(right_IR) >= 0 && analog_read(right_IR) <600) { //IR values
	return (1);
	}
	
	else {
	return (0);
	}
}

int left_IR_on() {
	if (analog_read(left_IR) >= 0 && analog_read(left_IR) <600) { //check IR values for each LED white values
	return (1);
	}
	
	else {
	return (0);
	}
}

int mid_IR_on() {
	if (analog_read(mid_IR) >= 0 && analog_read(mid_IR) <600) { //check IR values for each LED
	return (1);
	}
	
	else {
	return (0);
	}
}



void follow_line(float stop_dist) {
	/*uint8_t prev_right_IR=1;
	uint8_t prev_mid_IR=1;
	uint8_t prev_left_IR=1;*/
	//while((irdist_read(front_dist_sensor)/2.54) < 20)
	
	while(/*(irdist_read(front_dist_sensor)/2.54) < stop_dist*/    1)
	{	//printf("\nleft=%d mid=%d right=%d",analog_read(left_IR), analog_read(mid_IR),analog_read(right_IR));
		printf("\n distance = %f", (double) irdist_read(front_dist_sensor));
	
		if 	(left_IR_on()==1 && mid_IR_on()==1 && right_IR_on()==0) { //sharp right to center line
			motor_set_vel(right_motor, right_forward * 70);			
			motor_set_vel(left_motor, left_forward * 100);
			pause(50);
		}
	
		else if (left_IR_on()==1 && mid_IR_on()==0 && right_IR_on()==1) { //go straight
			motor_set_vel(right_motor, right_forward * 100);
			motor_set_vel(left_motor, left_forward * 100);
			pause(50);
		}
	
		else if (left_IR_on()==1 && mid_IR_on()==0 && right_IR_on()==0) { //gradual right
			motor_set_vel(right_motor,right_forward * 80);
			motor_set_vel(left_motor,left_forward * 100);
			pause(50);
		}
	
		else if (left_IR_on()==0 && mid_IR_on()==1 && right_IR_on()==1) { //sharp left
			motor_set_vel(right_motor, right_forward * 100);			
			motor_set_vel(left_motor, left_forward *70);
			pause(50);
		}
	
		else if (left_IR_on()==0 && mid_IR_on()==0 && right_IR_on()==1){ //gradual left
			motor_set_vel(right_motor,right_forward * 100);
			motor_set_vel(left_motor,left_forward * 80);
			pause(50);
		}
	}
}

/*void follow_start()
{	
	while(left_IR_on
	do {follow_line();}
	while (irdist_read(front_dist_sensor) > (20 * 2.54));
}*/

void back_up(int drive_time) {
	int enddrive_time, startdrive_time;
	
	motor_set_vel(right_motor, right_forward * -110);
	motor_set_vel(left_motor, left_forward * -110);
	pause(50);
	startdrive_time = get_time();
	do {
		enddrive_time = get_time();
	} while (enddrive_time - startdrive_time < drive_time); 
	
	motor_set_vel(right_motor, right_forward * 0);
	motor_set_vel(left_motor, left_forward * 0);
	pause(50);
}

//define turns
//left turn - motors move in opposite directions
void turn_left (float drive_angle) {
	float enddrive_angle, startdrive_angle;
	startdrive_angle = gyro_get_degrees();
	motor_set_vel(right_motor, 120);
	motor_set_vel(left_motor, -120);
	pause(40);
	
	do {
		enddrive_angle = gyro_get_degrees();
		pause(10);
	} while (enddrive_angle - startdrive_angle < drive_angle);
	
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
}

//left turn - left motor moves backwards
void turn_left_one(float drive_angle) {

	float enddrive_angle, startdrive_angle;
	startdrive_angle = gyro_get_degrees();
	/*printf( "\n%f" , (float) startdrive_angle);
	go_click();*/
	motor_set_vel(right_motor,0);
	motor_set_vel(left_motor, -120);
	pause(50);
	
	do {
		enddrive_angle = gyro_get_degrees();
		pause(10);
		//printf("\n%f", (float) enddrive_angle);
	} while (enddrive_angle - startdrive_angle < drive_angle); 
	
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
}
// left turn - right motor moves forward
void turn_left_two(float drive_angle) {

	float enddrive_angle, startdrive_angle;
	startdrive_angle = gyro_get_degrees();
	/*printf( "\n%f" , (float) startdrive_angle);
	go_click();*/
	motor_set_vel(right_motor,120);
	motor_set_vel(left_motor, 0);
	pause(50);
	
	do {
		enddrive_angle = gyro_get_degrees();
		pause(10);
		//printf("\n%f", (float) enddrive_angle);
	} while (enddrive_angle - startdrive_angle < drive_angle); 
	
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
}

void turn_right_one(float drive_angle) {

	float enddrive_angle, startdrive_angle;
	startdrive_angle = gyro_get_degrees();
	/*printf( "\n%f" , (float) startdrive_angle);
	go_click();*/
	motor_set_vel(right_motor,-110);
	motor_set_vel(left_motor, 110);
	pause(50);
	
	do {
		enddrive_angle = gyro_get_degrees();
		pause(10);
		//printf("\n%f", (float) enddrive_angle);
	} while (enddrive_angle - startdrive_angle > drive_angle); 
	
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
}


void zigzag(){
	motor_set_vel(right_motor, right_forward * 100);
	motor_set_vel(left_motor, left_forward * 100);
	float start_time=get_time();
	while(!(get_time()>start_time +500 && (left_IR_on()==0 || mid_IR_on()==0 || right_IR_on()==0))){
		motor_set_vel(right_motor, right_forward * 100 + (30 * sin( (get_time()-start_time) /800 * 2 * PI  )));
		motor_set_vel(left_motor, left_forward * 100);	
	}
	motor_set_vel(right_motor, right_forward * 0);
	motor_set_vel(left_motor, left_forward * 0);

}

void drive_straight(int drive_time) {
	int enddrive_time, startdrive_time;
	
	motor_set_vel(right_motor, right_forward * 110);
	motor_set_vel(left_motor, left_forward * 110);
	pause(50);
	startdrive_time = get_time();
	do {
		enddrive_time = get_time();
	} while (enddrive_time - startdrive_time < drive_time); 
	
	motor_set_vel(right_motor, right_forward * 0);
	motor_set_vel(left_motor, left_forward * 0);
	pause(50);
}



void init_decision() {

	if (analog_read(22)<(300) && analog_read(23)<(300)) {
		printf("\nfront=%d right=%d",analog_read(22), analog_read(23));
		turn_right_one(-83);
		turn_left_two(85);
		turn_right_one(-83);
	} else if (analog_read(22)<(300) && analog_read(23)>(300)) {
		printf("\nfront=%d right=%d",analog_read(22), analog_read(23));
		turn_left_two(82);
	} else if (analog_read(22)>(300) && analog_read(23)<(300)) {
		printf("\nfront=%d right=%d",analog_read(22), analog_read(23));
		back_up(700);
		pause(10);
		turn_right_one(-83);
	} else if (analog_read(22)>(300) && analog_read(23)>(300)) {
		printf("\nfront=%d right=%d",analog_read(22), analog_read(23));
		back_up(650);
		pause(10);
		turn_left_two(170);
		pause(10);
	
	}
}

//scores a point!! straight, right, pushes ball into goal


void go_forward(float turn_dist) // go forward while back distance sensor is less than 14 inches
{
	do
	{	motor_set_vel(right_motor, 120);
		motor_set_vel(left_motor, 120);
	}
		while (irdist_read(back_dist_sensor)/2.54 < turn_dist);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
}

void go_backward (float turn_dist) // go forward while back distance sensor is less than 14 inches
{
	do
	{	motor_set_vel(right_motor, -120);
		motor_set_vel(left_motor, -120);
	}
		while (irdist_read(back_dist_sensor)/2.54 > turn_dist);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
}

/*void continue_forward(float stop_dist) // go forward until a certain dist
{
	do
	{	motor_set_vel(right_motor, 120);
		motor_set_vel(left_motor, 120);
	}
		while (irdist_read(front_dist_sensor)/2.54 > stop_dist);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
}*/

void continue_forward(float stop_dist) // go forward until a certain dist
{	motor_set_vel(right_motor, 120);
	motor_set_vel(left_motor, 120);
	do
	{	pause(1);		
	}
		while ((((float) irdist_read(front_dist_sensor))/2.54) > stop_dist);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
		
}

int continue_forward2(float stop_dist) // go forward until a certain dist
{
	do
	{	motor_set_vel(right_motor, 120);
		motor_set_vel(left_motor, 120);		
	}
		while (irdist_read(front_dist_sensor)/2.54 > stop_dist);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
		if (irdist_read(back_dist_sensor)/2.54 < 26) {
		return 1;
		}
		else {
		return 0;
		}
		
}

	

//turn right 90 works with skid
void turn_right(float drive_angle) {

	float enddrive_angle, startdrive_angle;
	startdrive_angle = gyro_get_degrees();
	/*printf( "\n%f" , (float) startdrive_angle);
	go_click();*/
	motor_set_vel(right_motor,-110);
	motor_set_vel(left_motor, 110);
	pause(50);
	
	do {
		enddrive_angle = gyro_get_degrees();
		pause(10);
	} while ((enddrive_angle - startdrive_angle) > drive_angle); 
	
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
}

//check angle
void check_angle(float wanted_angle)
{	float beg_angle, fin_angle;	
	beg_angle = gyro_get_degrees();
	if ((beg_angle > wanted_angle-3) && (beg_angle <wanted_angle+3)){
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
	}
	else if (beg_angle < wanted_angle-3) {
		motor_set_vel(right_motor, 60);
		motor_set_vel(left_motor, -60);
		do{ fin_angle = gyro_get_degrees();	
		} while ( fin_angle < wanted_angle - 3);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
	}
	else if (beg_angle > wanted_angle+3) {
		motor_set_vel(right_motor, -60);
		motor_set_vel(left_motor, 60);
		do{ fin_angle = gyro_get_degrees();	
		} while ( fin_angle > wanted_angle + 3);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
	}
}

//check position
void check_position()
{	float start_angle, end_angle;
	start_angle = gyro_get_degrees();
	if (start_angle > -2.0 && start_angle < 2.0)
	{	motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
		pause(30);
	}
	else if (start_angle < -2.1)
	{	
		motor_set_vel(right_motor, 80);
		motor_set_vel(left_motor, -80);
		do{ end_angle = gyro_get_degrees();
			pause(20);
		}
		while(end_angle < -7);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
	}
	else if (start_angle > 2.1)
	{	
		motor_set_vel(right_motor, -80);
		motor_set_vel(left_motor, 80);
		do{ end_angle = gyro_get_degrees();
			pause(20);
		}
		while(end_angle > 7);
		
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
	}
	printf("\n pos=%f", (double) gyro_get_degrees());
	
}

void backleft(float drive_angle) {

	float enddrive_angle, startdrive_angle;
	startdrive_angle = gyro_get_degrees();
	/*printf( "\n%f" , (float) startdrive_angle);
	go_click();*/
	motor_set_vel(right_motor,-110);
	motor_set_vel(left_motor, -130);
	pause(50);
	
	do {
		enddrive_angle = gyro_get_degrees();
		pause(10);
	} while ((enddrive_angle - startdrive_angle) > drive_angle); 
	
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
}

//open arm
void open_arm() {
	servo_set_pos(servo, 85);
	pause(50);
}

//lift gate
void lift_gate() {
	servo_set_pos(gateservo, 465);
	pause(50);
}

// get to goal
void goal_drive(){ // drive until bumper senses wall then stop (works)
	do 
	{	motor_set_vel(right_motor,120);
		motor_set_vel(left_motor,103);}
		while (digital_read(left_bumper) == 0 && digital_read(right_bumper) == 0);
	
		motor_set_vel(right_motor, 0);
		motor_set_vel(left_motor, 0);
}

float rightwalldist() {
	
	return fabs(irdist_read(right_dist_sensor) * cos(-gyro_get_degrees()-90.0));
}

void follow_wall(uint32_t stoptime)
{	while (get_time() < stoptime) {
		if 	(rightwalldist() >= 19.5*2.54 && rightwalldist() <= 20.5*2.54 )
		{	motor_set_vel(right_motor, 110);
			motor_set_vel(left_motor, 110);
		}
		else if (rightwalldist() < 19.4*2.54)
		{	motor_set_vel(right_motor, 115);
			motor_set_vel(left_motor, 110);
		}
		else if (rightwalldist() > 20.6*2.54)
		{	motor_set_vel(right_motor, 110);
			motor_set_vel(left_motor, 115);
		}
		else if (rightwalldist() < 16*2.54)
		{	break;
		}
	}
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
	return;
}	

void bump_bot() //go until bump sensor is pushed (is there code to do a code for whichever comes first? or if back dist sensor is greater than 36 inches)
{	do 
	{	motor_set_vel(right_motor, 130);
		motor_set_vel(left_motor, 130);
	}
	while ((right_bumper == 0 && left_bumper == 0)  || irdist_read(back_dist_sensor) > (36 * 2.54) );
	motor_set_vel(right_motor, 0);
	motor_set_vel(left_motor, 0);
}



/* void check_wall() //need to call check angle before calling this
 {	float front_dist=irdist_read(front_dist_sensor)/2.54;
	float back_dist=irdist_read(back_dist_sensor)/2.54;
	float right_dist=irdist_read(right_dist_sensor)/2.54;
	float left_dist=irdist_read(left_dist_sensor)/2.54;
}*/

int usetup (void) {
	set_auto_halt(0);
	printf_P (PSTR("\nPlace robot,    press go."));
	go_click ();
	printf_P (PSTR("\nStabilizing..."));
	//pause (500);
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (gyro_port, LSB_MS_PER_DEG, 500L);
	irdist_set_calibration(12452, 0);
	return 0;
}

int	umain (void) 
{
	init_decision();
	pause(50);
	gyro_set_degrees(0);
	//gyro_get_degrees();
	//printf("\n%f", (double) gyro_get_degrees());
	//go_click();

/*while (1) {
		printf("\n%f, %f", (double) rightwalldist(), (double) gyro_get_degrees());
	}*/
	go_forward(15.5);
	pause(50);
	turn_right(-78);
	pause(40);
	go_forward(18.6);
	pause(40);
	
	zigzag();
	pause(50);
	printf("\n robot");
	//drive_straight(1500);
	continue_forward(20);
	
	//follow_line(20);//need to stop after certain distance
	pause(40);
	check_angle(-90.0);
	printf("\n angle=%f", (double) gyro_get_degrees());
	turn_left(78.0);
	pause(30);
	int backdist=continue_forward2(20.0);
		if (backdist==0) {
	
		pause(30);
		check_angle(0.0); // end of other side
		pause(30);
		turn_right(-80.0);
		}
		
		else if (backdist==1) {
		pause(30);
		go_backward(12.2);
		pause(20);
		check_position();
		pause(30);
		turn_left(80.1);
		pause(20);
		drive_straight(4000);
		pause(20);
		gyro_set_degrees(90);
		pause(20);
		back_up(700);
		pause(20);
		turn_right(-83);
		pause(30);
		go_forward(23);
		pause(20);
		continue_forward(23);
		pause(30);
		check_angle(0);
		pause(30);
		continue_forward(20);
		pause(50);
		turn_right(-76);
		}
		
	pause(30);
	continue_forward(9.2);
	pause(30);
	turn_right(-73.5);
	pause(30);
	open_arm();
	lift_gate();
	pause(30);
	goal_drive();
	
	gyro_set_degrees(0);
	timed_backup();
	close_gate();
	wide_right(-90);
	wide_right(-90);
	drive_straight(1000);
	back_up(900);
	wide_left(80);
	while(1) {
		timed_drivestraight();
		turn_right_one(-90);
		drive_straight(500);
		backleft(90);
	}
	stop();
		
	//follow_start();
	pause(50);
	return 0;
}