void assessment(void)
{
long start;

// The robot is stopped for one reason or another. This state will 
// try to determine what to do next 

// INITIALIZE
	CLEAR_DISPLAY sprintf(string, "ASSESSMENT\0"); send_string(string);  
//	STOP
	start = current_time;

	steer(STRAIGHT, 25); 
 
	aim_sonar(AHEAD, 25);
	check_distances();

//  IF NO ROOM TO GO FORWARD OR TURN...
	if((LEFT_BUMPER OR RIGHT_BUMPER) AND lw < 15  AND rw < 15 AND zone[3] < 15 ){ 
		obsticle_ahead=YES; LED_2=0;
		next_state = DEAD_END; while (current_time < start + 500); return;
	}

//  IF TOO CLOSE TO SOMETHING
	if(LEFT_BUMPER OR RIGHT_BUMPER OR lw < 15  OR rw < 15 OR zone[3] < 10 ) 
		{
		if(obsticle_left + obsticle_ahead + obsticle_right == 3){
			say(OH_BROTHER);
			clear_obsticle_memory();
			pause(1000);
		}
		if(LEFT_BUMPER OR lw < 15  ){obsticle_left=YES; LED_4=0; }
		if(RIGHT_BUMPER OR rw < 15 ){obsticle_right=YES; LED_6=0; }
		if(zone[3] < 10){obsticle_ahead=YES; LED_2=0; }

		next_state = TOO_CLOSE;  while (current_time < start + 500); return;
	}

//	IF THERE'S SOME SPACE STRAIGHT AHEAD(AT LEAST 50CM), 
//	GO TO "FORWARD"
	if( zone[3] > 50  AND obsticle_ahead == NO ){	
		next_state = FORWARD;  while (current_time < start + 500); return;
	}  

//	obsticle_ahead = YES; LED_2 = 0; 

	STOP
//	LOOK AROUND	
	five_point_scan ();           // scan results are in "zone[]"
	aim_sonar(AHEAD, 25);

// IF THERE'S LOTS OF ROOM, OTHER THAN AHEAD,
// GO TO "SEMI_OPEN"
	if(	((zone[1]>80 OR zone[2]>80) AND zone[3]>20 AND zone[3]<50 AND obsticle_left==NO) OR 
		((zone[4]>80 OR zone[5]>80) AND zone[3]>20 AND zone[3]<50 AND obsticle_right==NO)
	  )
		{next_state = SEMI_OPEN;  while (current_time < start + 500); return;}


// hallway code does here
 
// DEAD-END?  TURN AROUND
	if( zone[1]<30 AND zone[3]<30 AND zone[5]<30 )
    {next_state = DEAD_END;  while (current_time < start + 500); return;}  

//  AND IF NOTHING ELSE APPLIES....
//  temporary
	say(BLEEP);
	next_state = FORWARD;  while (current_time < start + 500); return;  // temporary

}  //  end of state

//  ---------------------------------------------

void semi_open(void)
{
//  DETERMINE WHICH OF THE 5 ZONES HAS THE MOST CLEARANCE. 
//  MOVE IN THAT DIRECTION AND STOP. (REFER TO THE ZONE DIAGRAM)

	unsigned char dir;

	CLEAR_DISPLAY sprintf(string, "SEMI-OPEN SPACE\0"); send_string(string);
  	next_state = ASSESSMENT;

//	aim_sonar(AHEAD, 25);
//	check_distances();
	lw = left_ir(); rw = right_ir();  //  get info from IR sensors

//	if(obsticle_ahead = YES){}

	if     ( zone[1] > 80 AND lw > 40 AND obsticle_left == NO) { dir='l' ;} 
	else if( zone[2] > 80 AND lw > 60 AND obsticle_left == NO) { dir = 'l';  } 
	else if( zone[4] > 80 AND rw > 60 AND obsticle_right == NO) { dir = 'r';  }        
	else if( zone[5] > 80 AND rw > 40 AND obsticle_right == NO) { dir = 'r';  }          
	
	else if( zone[1] > 60 AND lw > 30 AND obsticle_left == NO) { dir = 'l';  }        
	else if( zone[2] > 60 AND lw > 40 AND obsticle_left == NO) { dir = 'l';  } 
	else if( zone[4] > 60 AND rw > 40 AND obsticle_right == NO) { dir = 'r';  }          
	else if( zone[5] > 60 AND rw > 30 AND obsticle_right == NO) { dir = 'r';  }         
 	
	else if( zone[1] > 40 AND lw > 20 AND obsticle_left == NO) { dir = 'l';  } 
	else if( zone[2] > 40 AND lw > 30 AND obsticle_left == NO) { dir = 'l';  } 
	else if( zone[4] > 40 AND rw > 30 AND obsticle_right == NO) { dir = 'r';  }          
	else if( zone[5] > 40 AND rw > 20 AND obsticle_right == NO) { dir = 'r';  } 

	clear_obsticle_memory(); 

	if(dir == 'l'){next_state = VEER_LEFT; return;}
	else {next_state = VEER_RIGHT; return;}

//  (if none of the above apply)
//	next_state = DEAD_END; return;
//	move_cursor(15,1); sprintf(string, "what?\0"); send_string(string);
//	pause(5000);

//	move(-30);             // back up 30cm
//	obsticle_ahead=YES; LED_2=0; 
//	reverse_attempts++;    // keep track to prevent endless loops
}

/*----------------------------------*/
void forward(void)
{
	long int start_time = current_time;

	CLEAR_DISPLAY sprintf(string, "FORWARD WITH CAUTION\0"); send_string(string);
	steer(STRAIGHT, 25);  
	motor_speed(40);  //  forward at 40% speed
	pause(200);       //  just in case ...
	
	aim_sonar(AHEAD, 25);

//  main loop
    while (1) {
	 
  		check_distances();
	    
//  if space < 15cm, take note of obsticle and go to ASSESSMENT

		if (lw < 15){
			STOP
			obsticle_left = YES; LED_4 = 0;
			next_state = ASSESSMENT; return;
		}

		if (LEFT_BUMPER){
			STOP
			obsticle_left = YES; LED_4 = 0;
			say(DOH);
			next_state = ASSESSMENT; return;
		}

		if (rw < 15){
			STOP
			obsticle_right = YES; LED_6 = 0;
			next_state = ASSESSMENT;  return;
		}
 
		if (RIGHT_BUMPER){
			STOP
			obsticle_right = YES; LED_6 = 0;
			say(BOING);
			LED_6 = 1;
			next_state = ASSESSMENT; return;
		}

		if (zone[3]<30){
			STOP
			obsticle_ahead = YES; LED_2 = 0;
			next_state = ASSESSMENT;  return;
		}

//  if there's a wall in front, turn
		if (zone[3] < 50 AND lw<50 AND rw<50){
			obsticle_ahead = YES; LED_2 = 0;
			if(lw > rw)next_state=VEER_LEFT;
			else next_state=VEER_RIGHT;

//			next_state = lw>rw ? VEER_LEFT : VEER_RIGHT;
//			say(AVOIDING WALL);	return;
		}

//  if lots of room, speed up
		if (zone[3] > 90 AND lw>50 AND rw>50) 
	       {/*say(YEEHAA);*/ next_state = CRUISE; return;}

//  if there's a wall to the right, try to follow it
		if (zone[3] > 30 AND lw>50 AND rw<50) 
	       {next_state = FOLLOW_WALL; return;}

//  if there's a wall to the left, turn away from it
		if (zone[3] > 30 AND rw>50 AND lw<50) 
	       {next_state = VEER_RIGHT; return;}

//  if out of room ...
        if (zone[3] < 30 AND lw < 30 AND  rw < 30) 
	       {next_state = ASSESSMENT; return;}

//  clear the obsticle memory after moving for a second and a half
		if(current_time > start_time + 1500){
			clear_obsticle_memory();
		}

//		if(IRR == 0){P2 = 0; turn_right(30); steer(STRAIGHT,25);}

	}  // end of loop

}  // end of "forward"

//-----------------------------

void cruise(void)
{
	CLEAR_DISPLAY sprintf(string, "CRUISE\0"); send_string(string);
	steer(STRAIGHT, 25);  
	motor_speed(70);  //  forward at 70% speed
	pause(200);       //  just in case ...

	clear_obsticle_memory();

	aim_sonar(AHEAD, 25);
//	check_distances();
	
	while (1) {
		check_distances();
		
//		if( LEFT_BUMPER OR RIGHT_BUMPER ) 
//		  {next_state = TOO_CLOSE; return; }
		
//		if (lw < 15) {next_state = TOO_CLOSE; return; }
//		if (rw < 15) {next_state = TOO_CLOSE; return; }

		if (LEFT_BUMPER OR lw < 15){
			STOP
			obsticle_left = YES;
			LED_4 = 0;
			say(SCREECH);
			next_state = TOO_CLOSE;
		    return;
		}

		if (RIGHT_BUMPER OR rw < 15){
			STOP
			obsticle_right = YES;
			LED_6 = 0;
			say(SCREECH);
			next_state = TOO_CLOSE;
		    return;
		}
 
		if (zone[3] < 15){
			STOP
			obsticle_ahead = YES;
			LED_2 = 0;
			say(SCREECH);
			next_state = TOO_CLOSE;
		    return;
		}

        if (zone[3] < 15 OR lw < 15 OR rw < 15) 
	       {next_state = ASSESSMENT; return;}

        if (zone[3] < 50 OR lw < 50 OR rw < 50) 
	       {LED_5=0; pause(2000);next_state = FORWARD; return;}

		if (zone[3] < 100 ){
			next_state = VEER; return; }

	} //end of loop
}  //  end of cruise

//  ----------------------------

void follow_wall(void)
{

	CLEAR_DISPLAY sprintf(string, "FOLLOW WALL\0"); send_string(string);
	motor_speed(40);
	aim_sonar(1700,15);  //  a little left of center
	
	while(1){

		check_distances();

		if (zone[3] < 20){
			STOP
			obsticle_ahead = YES;
			LED_5 = 0;
			tone(A3,200);
			next_state = FORWARD;
		    return;
		}

		if (LEFT_BUMPER OR lw < 15){
			STOP
			obsticle_left = YES;
			LED_4 = 0;
			//say(SCREECH);
			next_state = TOO_CLOSE;
		    return;
		}

		if (RIGHT_BUMPER OR rw < 10){
			STOP
			obsticle_right = YES;
			LED_6 = 0;
			next_state = TOO_CLOSE;
		    return;
		}

		if (lw>50 AND rw<18  )			//  if 10-18cm
		   {steer(LEFT,10);}
		else if(lw>50 AND rw<26 )		//  if 18-26cm
		   {steer(SLIGHTLY_LEFT,10);}
		else if(lw>50 AND rw<34 )		//  if 26-34cm
		   {steer(STRAIGHT,10);}
		else if(lw>50 AND rw<42 )		//  if 34-42cm
		   {steer(SLIGHTLY_RIGHT,10);}
		else if(lw>50 AND rw<50 )		//  if 42-50cm
		   {steer(RIGHT,10);}
		else							//	if >50cm
		   {next_state=FORWARD; return;}
//start_time = current_time;


	}  //  end of main loop
}

//  ----------------------------
void veer(void)
{
// An obsticle was encountered during CRUISE mode. There may be enough clearance for a turn 
// (a minimum of 60 cm is needed). This will try determine the best direction to turn then do it.
	CLEAR_DISPLAY sprintf (string, "VEER\0"); send_string(string);  
	
	motor_speed(40);

	aim_sonar(RIGHT,25);
	zone[5] = sonar() - FRONT_OFFSET;
	if(zone[5] < 0){zone[5] = 0;}
	if( zone[5] > 60 ) {next_state = VEER_RIGHT; return;}
		
	aim_sonar(LEFT,25);
	zone[1] = sonar() - FRONT_OFFSET;
	if(zone[1] < 0){zone[1] = 0;}
	if( zone[1] > 60 ) {next_state = VEER_LEFT; return;}
	else {pause(500);  next_state = FORWARD; return;}
} 

//  -----------------------------------------------

void veer_left(void)

// turn left until clear or time out
{
	unsigned int b;
	unsigned long int time_limit = current_time + 8000;  // give up after 8 seconds
  	
	CLEAR_DISPLAY sprintf(string, "VEER LEFT\0"); send_string(string);  
	
	steer (LEFT, 25);
	pause(250);
	motor_speed(40);
	//look_ahead();
	aim_sonar(AHEAD, 25);
	;
	obsticle_left = NO; LED_4 = 1;

	while (current_time < time_limit)
	{
		//lw = left_ir();
	    //zone[3] = sonar()-FRONT_OFFSET;
        //rw = right_ir();     //  gather info
		check_distances();


		if (LEFT_BUMPER OR lw < 10){
			obsticle_left = YES; LED_4 = 0;
			obsticle_right = YES; LED_6 = 0;
			obsticle_ahead = YES; LED_2 = 0;
			next_state = TOO_CLOSE;
		    return;
		}

		if (RIGHT_BUMPER OR rw < 10){
			obsticle_right = YES; LED_6 = 0;
			next_state = TOO_CLOSE;
		    return;
		}
 
		if (zone[3] < 20){
			obsticle_ahead = YES; LED_2 = 0;
			obsticle_right = YES; LED_6 = 0;
			next_state = TOO_CLOSE;
		    return;
		}


		if ((lw >= 30 ) AND ( zone[3] >= 30 ) AND ( rw >= 30 )) {
			clear_obsticle_memory();
			next_state = FORWARD; return;  //  a successful turn
		}  
	}
	next_state = ASSESSMENT;    //  time has expired
}

/*----------------------------*/

void veer_right(void)

// turn right until clear
{
	unsigned int b;
	unsigned long int start_time = current_time;  // give up after 8 seconds
	
	CLEAR_DISPLAY sprintf(string, "VEER RIGHT\0"); send_string(string);  
	steer (RIGHT, 25);
	pause(500);
	motor_speed(40);

	//look_ahead();
	aim_sonar(AHEAD,25);
	obsticle_right = NO; LED_6 = 1;

	while (current_time < start_time + 8000)  {

         //  gather info
		//lw = left_ir(); zone[3] = sonar()-FRONT_OFFSET; rw = right_ir(); 
		check_distances();

		if (LEFT_BUMPER OR lw < 10){
			obsticle_left = YES; LED_4 = 0;
			next_state = TOO_CLOSE;
		    return;
		}

		if (RIGHT_BUMPER OR rw < 10){
			obsticle_right = YES; LED_6 = 0;
			obsticle_left = YES; LED_4 = 0;
			obsticle_ahead = YES; LED_2 = 0;
			next_state = TOO_CLOSE;
		    return;
		}
 
		if (zone[3] < 20){
			obsticle_ahead = YES; LED_2 = 0;
			obsticle_left= YES; LED_4 = 0;
			next_state = TOO_CLOSE;
		    return;
		}


		if ((lw >= 30 ) AND ( zone[3] >= 30 ) AND ( rw >= 30 )) {
			clear_obsticle_memory();
			next_state = FORWARD; return;  //  a successful turn
		}  
	}
	next_state = ASSESSMENT;    //  time has expired
}

//  -----------------------------------------------  

void dead_end (void)
{
	// get turned around
	// I'll try using the compass later
	
	unsigned char a,b,current_bearing,p,q;

	next_state = ASSESSMENT;
	CLEAR_DISPLAY sprintf(string, "DEAD END\0"); send_string(string);  

//	clear_obsticle_memory();
	
//stop and back-up for 50 cm
	move (-30);	reverse_attempts++;

//  if room to either side, turn
	look_left(); if (lw<zone[1]) {zone[1]=lw;}
	look_right(); if (rw<zone[5]) {zone[5]=rw;}
	if(zone[1]>40 OR zone[5]>40){obsticle_ahead = YES; LED_2=0; return;}

//  if not, reverse and turn 4 times
	for(a=0; a<3; a++){
		reverse_right(); reverse_attempts++; LED_1=0;
		if(reverse_attempts > 8){
			next_state = STUCK;
			return;
		}
		turn_right(45);	obsticle_left = YES; LED_4 = 0;
	}  //  end of turning loop
}  // end of state


	//	look_left(); look_right();
	
//	a = bearing( ); 
//	prefered_bearing = a + 180; 
//	if (prefered_bearing > 360) {prefered_bearing -= 360;}
	
	//  if( a < 165 ) {steer (LEFT,25); motor_speed(40); veer_left(); goto DE;}
	//  else {steer (RIGHT,25); motor_speed(40); veer_right(); goto DE;}
//DE:
//	motor_speed(40);
//	turn_right(90);
//	
//	if( LEFT_BUMPER OR RIGHT_BUMPER ) {goto D2;}  // stuck?
	
//D1:
//	for( p = 0; p < 7; ++p )
//	{
//		turn_right(90); STOP pause(250);
//		reverse_right();
//		look_ahead(); //look_right();
//		current_bearing = bearing();
//		CLEAR_DISPLAY send_decimal_3(a);
//
//		b = is_bearing_within_range(current_bearing, prefered_bearing ,50);
//
//		if( b == 1) {return; }
//		if( LEFT_BUMPER OR RIGHT_BUMPER ) {break;}  // stuck?
//	}                         
	

//--------------------------------

void too_close(void)
{

	unsigned int time_limit, a, c, jammed = NO;

	CLEAR_DISPLAY sprintf(string, "TOO CLOSE\0"); send_string(string);  
	STOP
	next_state = ASSESSMENT;

// too many restrictions building up?
	if(reverse_attempts >= 8) {
	 	next_state = STUCK; return;
	}

//	aim_sonar(AHEAD, 25);
//	check_distances();	
//	five_point_scan();

//  is it jammed up against an obsticle on the left side?

	aim_sonar(LEFT, 25); zone[1]=sonar();
	if (zone[1] < 3) {jammed = YES;}

	if(obsticle_left == YES AND obsticle_right == NO AND jammed == NO) {
		reverse_right();  // steer left, back-up for 1s
		STOP
		pause(200);
		reverse_attempts++;
		motor_speed(40);
		next_state = VEER_RIGHT; return;
	}

	if(obsticle_left == YES AND obsticle_right == NO AND jammed == YES) {
 		steer(STRAIGHT,25);
 		move(-15);
		STOP
		pause(250);
		reverse_attempts++;
		motor_speed(40);
		next_state = VEER_RIGHT; return;
	}

//
//  is it jammed up against an obsticle on the left side?

	aim_sonar(RIGHT, 25); zone[5]=sonar();
	if (zone[5] < 3) {jammed = YES;}

	if(obsticle_right == YES AND obsticle_left == NO AND jammed == NO) {
		reverse_left();  // steer right, back-up for 1s
		STOP
		pause(250);
		reverse_attempts++;
		motor_speed(40);
		next_state = VEER_LEFT; return;
	}

	if(obsticle_right == YES AND obsticle_left == NO AND jammed == YES) {
 		steer(STRAIGHT,25);
 		move(-15);
		STOP
		pause(250);
		reverse_attempts++;
		motor_speed(40);
		next_state = VEER_LEFT; return;
	}

		move(-15);
		STOP
		pause(250);
		reverse_attempts++;
		say(BLEEP);
		next_state = SEMI_OPEN; return;
}


/*----------------------------*/

void stuck(){

	UBYTE n;

	CLEAR_DISPLAY sprintf(string, "STUCK\0"); send_string(string);  
	STOP
	say(AW_CRAP);
	clear_obsticle_memory();

//  blink the lights for a while
	for(n=0; n<20; n++){
		P2=0;
		pause(600);
		P2=255;
		pause(600);
	}
	say(SNORE);
	for(n=0; n<20; n++){
		P2=0;
		pause(600);
		P2=255;
		pause(600);
	}

	next_state = ASSESSMENT;

}

//===============================================================================