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



/*

OUT_A controls Left side wheels of the vehicle
OUT_B controls Right side wheels of the vehicle

There is a light sensor connected to the SENSOR_2 input pointing to 
the floor.

*/

#define EYE SENSOR_2 //We will refer the sensor 2 as the EYE
#define RIGHT OUT_C  //We will refer OUT_C as RIGHT(it controls right wheels)  
#define LEFT OUT_A   //We will refer OUT_A as LEFT(it controls left wheels)  
#define TURN_TIME 40 //This turn time is not used in a wait but in for cicles
#define MANEUVER 40 //This turn time is used in a wait 
#define ALIGN 95
#define TURNING_TIME 150 //This turn time is used in a wait 
#define TURN_SPEED 1  //Motor speed when turning for doing a scan of the line
#define NORMAL_SPEED 0 //Motor speed when following the line
#define SEARCH_TIME 100 //This turn time is not used in a wait but in for cicles
#define INTERACT_TIME 300 //This turn time is not used in a wait but in for cicles

int line = 0; //This is a BOOLEAN value that indicates if the EYE is seeing the line or not
int i = 0; //An iterator variable
int line_found=0;
int last =1;


task check() //constantly checks for the line
{
	for(;;)//Do while i have batteries
	if (EYE<=32 || (EYE>=40 && EYE < 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 
{
  PlaySound(SOUND_CLICK);
  last=1;
  for (;!line&&last>=0;){ //While i donīt see the line
    if (last){ 
      i=0;
      SetPower(LEFT+RIGHT,TURN_SPEED);
      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(5);
      SetPower(LEFT+RIGHT,NORMAL_SPEED);
      last--;
    }
    else{
      i=0;
      SetPower(LEFT+RIGHT,TURN_SPEED);
      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(5);
      SetPower(LEFT+RIGHT,NORMAL_SPEED);
      last--;
    }
  }
  //restore the original heading if couldn't find the line...
  if(!line){
    PlaySound(SOUND_DOUBLE_BEEP);
    i=0;
    SetPower(LEFT+RIGHT,TURN_SPEED);
    Toggle(LEFT+RIGHT); //Turn to the other side
    for(;i<TURN_TIME+18;i++); //Turn until my turn time is end or i see the line
    SetPower(LEFT+RIGHT,NORMAL_SPEED);
  }
}

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;
	OnFwd(LEFT+RIGHT); //Go fwd
	for(;line;);// While i see the line do nothing, just keep Fwd
      }
      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() //When i donīt see the line 
{
  OnFwd(LEFT+RIGHT); //Start Engines
  Wait(10);
  search();
}

void goLeft() //When i donīt see the line 
{
  OnFwd(LEFT+RIGHT); //Start Engines
  Wait(20);
  SetPower(LEFT+RIGHT,TURN_SPEED);
  OnRev(LEFT);
  Wait(TURNING_TIME);
  SetPower(LEFT+RIGHT,NORMAL_SPEED);
  OnFwd(LEFT+RIGHT); //Start Engines
  Wait(40);
  search();
}

void goRight() //When i donīt see the line 
{
  OnFwd(LEFT+RIGHT); //Start Engines
  Wait(20);
  SetPower(LEFT+RIGHT,TURN_SPEED);
  OnRev(RIGHT);
  Wait(TURNING_TIME);
  SetPower(LEFT+RIGHT,NORMAL_SPEED);
  OnFwd(LEFT+RIGHT); //Start Engines
  Wait(40);
  search();
}

void goBackwards() //When i donīt see the line 
{
  OnFwd(LEFT+RIGHT); //Start Engines
  PlaySound(SOUND_DOUBLE_BEEP);
  Wait(MANEUVER);
  OnRev(LEFT);
  Wait(2*TURNING_TIME);
  OnFwd(LEFT+RIGHT); //Start Engines
  Wait(MANEUVER+20);
  search();
}

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


	while(true){

		ClearMessage();
		for(;Message()==0;); //interact
	      
	      	if(Message()==1) goForward(); 
	      	else if(Message()==2) goLeft();
	      	else if(Message()==3) goRight();
/*	      	else if(Message()==3) goBackwards();*/

		line_follow();
	    
	  
	}
}