//---------------------------------------------------------------------------
//
// A simple wall following robot. 
//
// The robot drives mainly straight  until it encounters an obstacle, then 
// turns away. Biasing the forward motion to the left creates a robot that
// follows an external wall clockwise.
// 
// Simon Parsons
// City University of New York
// September 2004.
//
// Based on the rover demo by Markus L. Noga <markus@noga.de>
//
//---------------------------------------------------------------------------

#include <config.h>

// Give a sensible compiler message if we don't have the right files 
// accessible.

#if defined(CONF_DSENSOR) && defined(CONF_DMOTOR)

#include <conio.h>
#include <unistd.h>

#include <dsensor.h>
#include <dmotor.h>

#include <sys/lcd.h>
#include <tm.h>

//----------------------------------------------------------------------------
//
// Functions
//

wakeup_t sensor_press_wakeup(wakeup_t data);

//----------------------------------------------------------------------------
//
// Main
//

int main(int argc, char *argv[]) 
{
  int direction=1;              // Which side did we find the obstacle?

  // Set lower power to the lefthand motor to make the robot head to the
  // left.

  motor_a_speed(2*MAX_SPEED/5);
  motor_c_speed(2*MAX_SPEED/3);

  // Turn on the motors

  motor_a_dir(fwd);
  motor_c_dir(fwd);

  // Show what we are doing

  cputs("fwwd");
  
  while (!shutdown_requested()) {

    // If the sensors tell us there is something out there...

    if (wait_event(&sensor_press_wakeup, 0) != 0) 
      {
	// Record where we were in contact with an obstacle

	if(SENSOR_1<0xf000)
	  {
	    direction=0;     // Obstacle on left.
	  }
	if(SENSOR_3<0xf000)
	  {
	    direction=1;     // Obstacle on right;
	  }
	if((SENSOR_1<0xf000)&&(SENSOR_3<0xf000))
	  {
	    direction=2;     // Obstacle straight ahead;
	  }

	// Reverse away from the obstacle as fast as possible, reporting 
        // what we are doing

	motor_a_speed(MAX_SPEED);    
    	motor_c_speed(MAX_SPEED);
	motor_a_dir(rev);
	motor_c_dir(rev);

  	cputs("rev  ");
	msleep(500);

	// If contact is on the left, turn the lefthand motor on full
	// ahead for a while, to turn us right away from the obstacle.

	if(direction==0)    	     
	  {
	    motor_a_dir(fwd);
	    cputs("left");
	  }
	else

	// If contact is on the right, turn the righthand motor on full
	// ahead for a while, to turn left away from the obstacle.

	if(direction==1)
	  {
	    motor_c_dir(fwd);
	    cputs("right");
	  }
	else

	// If contact is right ahead, back up some more, then turn to the 
        // right.

	if(direction==2)
	  {
	    msleep(500);
	    motor_a_dir(fwd);
	    cputs("ahead");
	  }

	// Wait for a while to turn...

	msleep(500);

	// ... then switch the motors back how they were

	motor_a_speed(2*MAX_SPEED/5);
	motor_c_speed(2*MAX_SPEED/3);

	motor_a_dir(fwd);
	motor_c_dir(fwd);

      }
  }
    
  return 0;
}

//----------------------------------------------------------------------------
// 
// sensor_press_wakeup
//

// Return true if either touch sensor has been pressed.

wakeup_t sensor_press_wakeup(wakeup_t data) 
{
	lcd_refresh();
	return (SENSOR_1<0xf000) || (SENSOR_3<0xf000);
}

// If the right files aren't in place, print a warning and generate a null
// file.
// 

#else
#warning wall.c requires CONF_DMOTOR and CONF_DSENSOR
#warning wall follower will do nothing

int main(int argc, char *argv[]) 
{
  return 0;
}

#endif // CONF_DSENSOR, CONF_DMOTOR