/* Spider (Pursuer) program for LEGO Robot
   Author - Sandip Bapat (bapat@cis.ohio-state.edu)
   Modified - July 8, 2002
*/


#define EYE2 SENSOR_2 
#define EYE1 SENSOR_1
#define EYE3 SENSOR_3
#define RIGHT OUT_C   
#define LEFT OUT_A   
#define TURN_TIME 40 
#define MANEUVER 40 
#define ALIGN 10
#define TURNING_TIME 160 

#define NORTH 0
#define EAST  1
#define SOUTH 2
#define WEST  3

#define YELLOW 0
#define BLACK 1

int line = 0; //This is a BOOLEAN value that indicates if the EYE2 is seeing the line or not
int i = 0; //An iterator variable
int line_found=0;
int last=1;
int start_line = 0;
int last_turn =0;
int where_to_go;
int line_width;
int numsearches;
int first_color_length;

int first_color,last_color;
int first_color_set = 0;
int last_color_set = 0;
int heading;
int direction;
int color;
int valid_count;
int ir_found;

task check() //constantly checks for the line
{
	for(;;)//Do while i have batteries
	if (EYE2<=32 || (EYE2>=44 && EYE2<50)) //if it sees the line
		line = 1; //i see the line
	else 
		line = 0; //i can not see the line
}


void search() //When i donīt see the line 
{
  last=1;
  for (;!line&&last>=0;){ //While i donīt see the line
      
      if (last){ 
      i=0;
      OnRev(LEFT); //Turn to left
      for(;i<TURN_TIME&&!line;i++); //Turn until my turn time is end or i see the line
      if (line) Wait(10);
      last--;
    }
    else{
      i=0;
      Toggle(LEFT+RIGHT); //Turn to the other side
      for(;i<2*TURN_TIME&&!line;i++); 
      //Turn until my turn time is end or i see the line
   if (line) Wait(10);
      last--;
    }
  }
  //restore the original heading if couldn't find the line...
  if(!line){
    i=0;
    Toggle(LEFT+RIGHT); //Turn to the other side
    for(;i<TURN_TIME+20;i++); //Turn until my turn time is end or i see the line
    line_width=Timer(0);
  }
}

void line_follow(){
  OnFwd(LEFT+RIGHT); //Start Engines
  line_found=1;
  for(;line_found>0;) //While i still have batteries
    {
      if (line){ //If i see the line
	line_found=1;
	if (!start_line)
	{
		 start_line=1;
		 ClearTimer(0);
	}
        OnFwd(LEFT+RIGHT); //Go fwd
	for(;line;)
	{
		if (EYE2<=32)
		{
			
			if (!first_color_set) 
			{
				first_color=BLACK;
				first_color_set=1;
				ClearTimer(1);
			}
			if (first_color_set && first_color == YELLOW && !last_color_set)
			{
				first_color_length=Timer(1);
				if (first_color_length < 10) first_color=BLACK;
				last_color=BLACK;
				last_color_set=1;
			}
		}
		else if (EYE2>=38)
		{
			
			if (!first_color_set) 
			{
				first_color=YELLOW;
				first_color_set=1;
				ClearTimer(1);
			}
			if (first_color_set && first_color== BLACK && !last_color_set)
			{
				first_color_length=Timer(1);
				if (first_color_length < 10) first_color=YELLOW;
				last_color=YELLOW;
				last_color_set=1;
			}
		}
		
	}
      }
      else{
	search(); //If i donīt see the line, start searching for it
	OnFwd(LEFT+RIGHT); //Go fwd
	if(!line) line_found--;
      }
    }
  Wait(ALIGN);
  Off(LEFT+RIGHT);
}


void goForward() 
{
  OnFwd(LEFT+RIGHT); 
  Wait(60);
  search();
}

void goLeft() 
{
  OnFwd(LEFT+RIGHT); 
  Wait(40);
  OnRev(LEFT);
  Wait(TURNING_TIME);
  OnFwd(LEFT+RIGHT); 
  search();
}

void goRight() 
{
  OnFwd(LEFT+RIGHT); 
  Wait(40);
  OnRev(RIGHT);
  Wait(TURNING_TIME);
  OnFwd(LEFT+RIGHT); 
  Wait(10);
  search();
}

void goBackwards() 
{
  OnFwd(LEFT+RIGHT); 
  Wait(40);
  OnRev(LEFT);
  Wait(2*TURNING_TIME);
  OnFwd(LEFT+RIGHT); //Start Engines
  Wait(10);
  search();
}

void searchLeft()
{
  OnFwd(LEFT+RIGHT); //Start Engines
  OnRev(LEFT);
  for (i=0;i<TURNING_TIME && !line;i++);
  Off(LEFT+RIGHT);
}


void searchRight()
{
  OnFwd(LEFT+RIGHT); //Start Engines
  OnRev(RIGHT);
  for (i=0;i<TURNING_TIME && !line;i++);
  Off(LEFT+RIGHT);
}


void searchBackwards()
{
  OnFwd(LEFT+RIGHT); //Start Engines
  OnRev(LEFT);
  Wait(2*TURNING_TIME);
  Off(LEFT+RIGHT);
}



void find_and_follow()
{
  if (last_turn)
	{
		Off(LEFT+RIGHT);
		numsearches=0;
		while(!line && numsearches < 3)
		{
			where_to_go=Random(2);
			if (where_to_go==0) 
				searchLeft();
			else if (where_to_go==1) 
				searchRight();
			else 
				searchBackwards();
			
			numsearches=numsearches+1;
			
			OnFwd(LEFT+RIGHT);
			search();
		}
		if (line) line_follow();
		else random_search();
	}
  else random_search();
}

void find_line()
{
  OnFwd(LEFT+RIGHT);
  for (i=0;i<75&&!line;i++);
  Off(LEFT+RIGHT);
  Wait(100);
}

void random_search()
{
int found_line=0;

  while (!found_line)
  {
	  find_line();
	  if (line) 
	  {
		found_line=1;
		line_follow();
	  }

	  else 
  	  {
        	OnRev(LEFT+RIGHT);
		for(i=0;i<120;i++);
		Off(LEFT+RIGHT);
		Wait(100);
		OnFwd(LEFT+RIGHT);
		OnRev(LEFT);
		Wait(100);
		Off(LEFT+RIGHT);
		Wait(100);
	 }
   }
}

void getfreq()
{
	ir_found=0;
		
	SetPower(LEFT+RIGHT,0);
	locate_ir();
	if (!ir_found) locate_ir();	
	
	valid_count=1;
	if(ir_found==1) countfreq();
	if(!valid_count)
	{
		valid_count=1;
		countfreq();
	}
	
	if (!valid_count || !ir_found) 
	{
		direction=Random(3);
 		PlaySound(SOUND_DOUBLE_BEEP);
	}
	
	SetPower(LEFT+RIGHT,1);
	
}

void locate_ir()
{
	for(i=0;i<130&&ir_found==0;i++)
	{
		OnFwd(LEFT+RIGHT);
		if (EYE1>60||EYE2>60||EYE3>60) 
		{
			Off(LEFT+RIGHT);
			ir_found=1;
			PlaySound(SOUND_DOUBLE_BEEP);
		}
		Off(LEFT+RIGHT);
	}
	Off(LEFT+RIGHT);
	Wait(200);
	
	if(ir_found==0)
	{
	 for(i=0;i<130&&ir_found==0;i++)
	 {
		OnRev(LEFT+RIGHT);
		if (EYE1>60||EYE2>60||EYE3>60) 
		{
			Off(LEFT+RIGHT);
			ir_found=1;
			PlaySound(SOUND_DOUBLE_BEEP);
		}
		Off(LEFT+RIGHT);
	 }
	 Off(LEFT+RIGHT);
	}
	Wait(200);
}

void countfreq()
{
int counter=0;	
	
	ClearTimer(0);
	do
	{	
		if (EYE1>60||EYE2>60||EYE3>60)
		{
			counter++;
			do 
			{}
			until (EYE1<60&&EYE2<60&&EYE3<60);
		}
	}
	while (Timer(0) <= 30);

	if (counter>=11 && counter<=13) direction=1;
	else if (counter>=8 && counter<=10) direction=3;
	else if (counter>=5&&counter<=7) direction=2;
	else if (counter>=2&&counter<=4) direction=0;
	else 
		{
		PlaySound(SOUND_DOUBLE_BEEP);
		valid_count=0;
		}
}



task main()
{
	SetSensor(EYE1,SENSOR_LIGHT); //Configure my EYE as a SENSOR_LIGHT
	SetSensor(EYE2,SENSOR_LIGHT);
	SetSensor(EYE3,SENSOR_LIGHT);
	SetDirection(LEFT+RIGHT,OUT_FWD);//COnfigure Engines to start Fwd
	SetPower(LEFT+RIGHT,1);//Not so fast buddy
	start check; //Run my check


	while(true){

		line_follow();
		if (!start_line) find_and_follow();		
		start_line=0;
		while (line_width<30) 
		{
			last_turn=0;
			find_and_follow();
		}
	
		if (first_color==YELLOW && last_color==BLACK) heading = NORTH;
		else if (first_color== YELLOW && (!last_color_set || last_color == YELLOW)) heading = SOUTH;
                else if (first_color==BLACK && last_color== YELLOW) heading = WEST;
		else heading = EAST;

		first_color_set=0;
		last_color_set=0;
		first_color=2;
		last_color=2;


		for (i=0;i<100;i++) SetUserDisplay(heading,0);

		getfreq();

		if (heading == direction)
		{
			 goForward(); 
			 last_turn=1;
		}
	      	else if(direction == (heading+1)%4)
		   {	
			 goRight();
			 last_turn=1;
		   }
	        else if(heading == (direction+1)%4)
		   { 
			goLeft();
			last_turn=1;
		   }
	        else if(heading == (direction+2)%4)
		   {
			goBackwards();
			last_turn=1;
		   }
		start_line=0;
	      	
	}
}