//The distance to define whether an object is detected or not 
#define NEAR 30
// 0 for blue basket in front or 1 for blue basket in Back
#define DIRECTION 1 

bool object_detected; // boolean for ultra sonic; true when distance is smaller than NEAR
bool ball_found; // boolean for color; true when red or blue detected
bool hit_wall; // boolean for whether hit a wall;

long time_allstart; // The start time for each round
int angle_init; // The compass value of the start point
int angle_direction; // The compass value of the basket 

long time_init; //The start time for choice 2  
long time_duration; // The go straight duration for choice 2
 
bool make_choice_now; // The boolean for making choice or not; true for making a choice
bool timerstarted; // The timerstarted boolean for choice 2

bool justbegin; // The boolean for whether the beginning of the round or not

// set sensors ports
void set_sensors(){
	SetSensorColorFull(IN_4);
	SetSensorLowspeed(IN_1); // eye
	SetSensorTouch(IN_3);
	SetSensorLowspeed(IN_2); // compass
}

/*The task for checking color sensor, if red or blue is detected, set the 
  boolean ball_found to true and value of the angle_direction */
task check_color(){
    while(true){
	if(Sensor(IN_4)==INPUT_REDCOLOR){
		ball_found = true;
		angle_direction= (angle_init+(1-DIRECTION)*180)%360;
		TextOut(10,30,"Red ball");
	}else if(Sensor(IN_4)==INPUT_BLUECOLOR){
		ball_found = true;
		angle_direction= (angle_init+DIRECTION*180)%360;
		TextOut(10,30,"Blue ball");
		
	}else{
		ball_found = false;
	}	
    }
}

/*The task for checking hit wall, hit_wall is set to true when either the value of 
  touch sensor is 1 or the value of ultra sonic sensor stays smaller than 10cm for 5 seconds*/
task check_hit_wall(){
    while(true){
	if(Sensor(IN_3) == 1){
		hit_wall = true;
		ClearScreen();
		TextOut(10,30,"Hit_wall");
	}else if(SensorUS(IN_1)< 10 ){
		long time_t0 = CurrentTick();
		bool timeout = false;
		while(Sensor(IN_3)!=1 && SensorUS(IN_1)<10 && (timeout = !((CurrentTick()-time_t0)<5000)));
		if(Sensor(IN_3)==1 || timeout){
			timeout = false;
			hit_wall = true;
			ClearScreen();
			TextOut(10,30,"Hit_wall");
		}

	}else{
		hit_wall = false;
	}
    }
}

/* The task for checking the distance, set object_detected true when the value of 
   ultra sonic sensor is less than NEAR*/
task check_object(){
    while(true){
	if(SensorUS(IN_1)<=NEAR){
		object_detected = true;
	}else{
		object_detected = false;
	}
    }
}

/*Since our robot has a problem to go straight, so this function is to adapte motors'
  speed to make the robot go straight as possible*/
void go_straight(){
	OnFwd(OUT_A,98);
	OnFwd(OUT_B,100);
}

/*This function is to find the way to the right basket and throw the ball.After that the
  robot turns 180 and start for a new round*/
void move_back(){
	ClearScreen();
	TextOut(30,20,"move_back");
	NumOut(30,30,angle_direction);
	int angle = SensorHTCompass(IN_2)%360;
	int angle_diff =angle-angle_direction;
	NumOut(30,30,angle);
	NumOut(30,50,angle_diff);
	// find the direction
	while(abs(angle_diff)>=5){
		OnRev(OUT_A,50);
		OnFwd(OUT_B,50);
		Wait(20);
		NumOut(30,30,SensorHTCompass(IN_2)%360);
                NumOut(30,50, angle_diff);
		angle_diff= SensorHTCompass(IN_2)%360 - angle_direction;
	}
	Off(OUT_AB);
	Wait(500);
	// Forward until hit the wall
	go_straight();
	while(!hit_wall);
	Off(OUT_AB);
	// go back a little bit
	OnRev(OUT_AB,50);
	Wait(700);
	//turn left  
	Off(OUT_AB);
	ClearScreen();
	TextOut(20,30,"Turn to adjust the distance.");
	int angle_orthogonal = angle_direction+95;
	int angle_current = SensorHTCompass(IN_2)%360;
	while(abs(angle_current-angle_orthogonal)>5){
		OnFwd(OUT_A,50);
		OnRev(OUT_B,50);
		Wait(20);
		angle_current = SensorHTCompass(IN_2)%360;
	}
	Off(OUT_AB);
	// Adjust the distance to make the value of the ultra sonic be 44cm
	int dist_wall = SensorUS(IN_1);
	ClearScreen();
	TextOut(20,30,"Adjust the distance.");
	repeat(2){
		if(dist_wall<44){
			OnRev(OUT_A,49);
			OnRev(OUT_B,50);
			while(SensorUS(IN_1)<44);
			Off(OUT_AB);
		}else{
			OnFwd(OUT_A,49);
			OnFwd(OUT_B,50);
			while(SensorUS(IN_1)>44);
			Off(OUT_AB);
		}
	}
	//After finding the right distance, turn to the basket, 
	//forward until the wall
	angle_current = SensorHTCompass(IN_2)%360;
	long t0_turn = CurrentTick();
       //while(abs(angle_current-angle_direction-10)>5){
	while(CurrentTick()-t0_turn<=2000){
                OnRev(OUT_A,50);
                OnFwd(OUT_B,50);
                Wait(20);
                angle_current = SensorHTCompass(IN_2)%360;
        }
        Off(OUT_AB);
	go_straight();
	while(!hit_wall);

	Off(OUT_AB);
	Wait(200);
	// throw the ball
	RotateMotor(OUT_C,50,40);
	Wait(20);
	RotateMotor(OUT_C,50,100);
	RotateMotor(OUT_C,75,-140);
	Wait(500);

	//Turn 180 degrees 
	OnRev(OUT_A,50);
	OnFwd(OUT_B,50);
	Wait(4500);
	time_allstart = CurrentTick(); // restart from the beginning
	justbegin = true;
	
}

/*The moves of the arm after a ball is found to make sure the ball is well protected */
void ball_found_action(){
	Off(OUT_AB);
	OnFwd(OUT_C,50);
	Wait(200);
	Off(OUT_C);
	Wait(500);
}

/*The moves after an object detected. Turn left a little bit and forword */
void object_detected_action(){
	Off(OUT_AB);
	int dist = Sensor(IN_1);
			
	OnFwd(OUT_B,50);
	OnRev(OUT_A,50);
	Wait(200);	
	OnFwdSync(OUT_AB,80,0);
	OnFwd(OUT_C,50);
	long t0 = CurrentTick();
	while(!ball_found && (CurrentTick()-t0)<=3000 && !hit_wall);

}

/*The move to go along the wall, quit the function after hitting walls 8 times*/
void go_along_wall(){
    ClearScreen();
    TextOut(30,30,"go_along the wall !!!!");
    int hitwalltime = 0 ;
    while(hitwalltime <= 8){
	go_straight();
	OnFwd(OUT_C,50);
	if(ball_found){
		ball_found_action();
		move_back();
	}
	if(hit_wall){
		hitwalltime ++;
		ClearScreen();
                TextOut(20,20,"!!!!!!!Turn");
                RotateMotor(OUT_C,50,-180);
                Off(OUT_AB);

                int angle_before_turn = SensorHTCompass(IN_2)%360;
                Wait(200);
		int angle_after_turn = (angle_before_turn-55)%360;
		long time_1 = CurrentTick();
		while(abs(SensorHTCompass(IN_2)%360 - (angle_before_turn-55)%360)>=2 && (CurrentTick()-time_1)<=3000){
			OnFwd(OUT_B,50);
			OnRev(OUT_A,50);
		}
		
	}
    }
	
}

/*The function to grab balls on the middle line*/
void findBallinMiddle() {
    TextOut(1,30,"Move and search.");
    go_straight();
    OnFwd(OUT_C,50);
    long t0=CurrentTick();
    while( (CurrentTick()-t0)<4200 && !(hit_wall) && ball_found==false);
    Off(OUT_AB);
    if(ball_found) {
        Off(OUT_C);
       // move_back();
    } else {
        // turn to one direction
         OnFwd(OUT_A,80);
         OnRev(OUT_B,80);
         int one_direction = (angle_init + 95 )  % 360;
         while(abs(SensorHTCompass(IN_2) - one_direction ) >= 2 && ball_found==false);
         Off(OUT_AB);
         if (ball_found) {
           Off(OUT_C);
           //move_back();
         }
         else {
            OnFwdSync(OUT_AB,80,0);
            while(!(hit_wall) && ball_found==false);
            Off(OUT_AB);
            if(ball_found) {
              Off(OUT_C);
             // move_back();
            }
            else {
              // turn to another direction
              int other_direction = (angle_init-92 + 360) % 360;
              OnRev(OUT_A,80);
              OnFwd(OUT_B,80);
              while(abs(SensorHTCompass(IN_2) - other_direction ) >= 2 && ball_found==false);
              Off(OUT_AB);
              if(ball_found) {
                Off(OUT_C);
              //  move_back();
              }
              else {
                OnFwdSync(OUT_AB,80,0);
                while(!(hit_wall) && ball_found==false);
                Off(OUT_AB);
                if(ball_found) {
                  Off(OUT_C);
                //  move_back();
                }
              }
            }
         }
      }
      
}

/*The task for searching and moves*/
task move(){
	time_allstart = CurrentTick();
	while(true){
	    if(justbegin){ //At the beginning of each round, go check the middleline
		justbegin = false;
		findBallinMiddle();
	    }
	    if(make_choice_now){ //Make a random choice
		TextOut(30,30,"make choice now!!");
		int timerOrNot = Random(2);
		//NumOut(50,50,timerOrNot);
		if(timerOrNot == 1){
			time_duration = Random(5000)+3000;
			NumOut(20,30,time_duration);
			timerstarted = true;
			time_init = CurrentTick();
			make_choice_now = false;
		}
	    }
	    go_straight();
	    OnFwd(OUT_C,50);
	    // Check values of global variables and make moves
	    if(ball_found){
			make_choice_now = true;
			timerstarted = false;
			ball_found_action();
			move_back();	
	    }
		if(object_detected){
			TextOut(30,30,"See something!!");
			make_choice_now = true;
			timerstarted = false;
			object_detected_action();
		}	
		if(hit_wall){
			make_choice_now = true;
			timerstarted = false;
			int turn_time = Random(2500)+1000;

                        ClearScreen();
			TextOut(50,50,"hit a wall");
			OnRev(OUT_AB,50);
                        RotateMotor(OUT_C,50,-180);
                        Wait(1000);
                        Off(OUT_AB);

                        int angle_before_turn = SensorHTCompass(IN_2)%360;
                        Wait(200);
                        OnFwd(OUT_B,50);
                        OnRev(OUT_A,50);
                        Wait(500); // turn a little bit for deblock the arm
                        OnFwd(OUT_C,50);
                        Wait(turn_time-500);
                        Off(OUT_AB);
                        int angle_after_turn = SensorHTCompass(IN_2)%360;
                        Wait(200);
                        //If the left is blocked.turn right
                        if(abs(angle_after_turn-angle_before_turn)<20){
                                OnFwd(OUT_A,50);
                                OnRev(OUT_B,50);
                                turn_time = Random(3000)+500;
                                Wait(turn_time);
                                Off(OUT_AB);
                                angle_after_turn = SensorHTCompass(IN_2)%360;
                                Wait(200);
                                // if the right is blocked. go back.
                                if(abs(angle_after_turn-angle_before_turn)<20){
                                        OnRev(OUT_AB,70);
                                        Wait(3000);
                                        OnFwd(OUT_B,50);
                                        OnRev(OUT_A,50);
                                }
                        }

		}
		if((CurrentTick() - time_allstart)>=5000){ // After 5 seconds, go along the wall
		NumOut(30,30,time_allstart);
		NumOut(50,30,CurrentTick());
			make_choice_now = true;
			timerstarted = false;
			TextOut(30,30,"go_along the wall !!!!");
			time_allstart = CurrentTick();
			go_along_wall();
		}
		if(timerstarted && CurrentTick() - time_init > time_duration){
			TextOut(30,30,"scan !!!!");
			make_choice_now = true;
			timerstarted = false;
			bool stopturn = false;
			//scan and turn
			int angle_current = SensorHTCompass(IN_2)%360;
			int angle_turn = (SensorHTCompass(IN_2)+180)%360;
     			while(abs(angle_current-angle_turn)>5){
                		OnRev(OUT_A,50);
                		OnFwd(OUT_B,50);
                		Wait(20);
                		angle_current = SensorHTCompass(IN_2)%360;
				if(hit_wall || ball_found || object_detected){
					stopturn = true;
					Off(OUT_ABC);
				if(object_detected){
					TextOut(30,30,"Turn more  !!!!");
					OnRev(OUT_A,50);
					OnFwd(OUT_B,50);
					Wait(500);
				}
					break;
				}
        		}
			if(!stopturn){
			angle_turn = (angle_turn+180)%360;
			angle_current = SensorHTCompass(IN_2)%360;
     			while(abs(angle_current-angle_turn)>5){
                		OnRev(OUT_A,50);
                		OnFwd(OUT_B,50);
                		Wait(20);
                		angle_current = SensorHTCompass(IN_2)%360;
				if(hit_wall || ball_found || object_detected){
					stopturn = true;
					Off(OUT_ABC);
				if(object_detected){
					TextOut(30,30,"Turn more  !!!!");
					OnRev(OUT_A,50);
					OnFwd(OUT_B,50);
					Wait(500);
				}
					break;
				}
        		}
				
			}
			if(!stopturn){
				OnRev(OUT_A,50);
				OnFwd(OUT_B,50);
				Wait(1000);
			}	
			
		}
	}
}

/*Main task*/
task main(){
// Initialize variables
justbegin = true;
object_detected = false;
ball_found = false;
hit_wall = false;
make_choice_now = true;
timerstarted = false;
set_sensors();
angle_init = SensorHTCompass(IN_2);
Precedes(check_color,check_hit_wall,check_object,move);
}