/*
 * Wallfollowing Code
 * RugWarrior Pro
 *
 * Mike Dickerson, Woody Hunt, Dave Ordal
 *
 */

/* Control Variables */

/* Control motor speed and direction */
int motDrive = 0;
int motRotate = 0;

int bumpDrive, bumpRotate;
int bump_act = 0;

int seegoalDrive, seegoalRotate;
int seegoal_act = 0;

int wallfollowDrive, wallfollowRotate;
int wallfollow_act = 0;
int wallfollow_override = 0;

int wallAlert = 0;

int wallfollowDirection = 1;	/* Direction of motion (forward (true) or reverse (false)) */

int ir_s;

/*
 * motors(): Constantly tell the motors the latest commands (as determined by 
 *          arbitrate()
 *
 */
void motors() {
	while(1) drive(motDrive, motRotate);
}

/*
 * arbitrate(): Wait to start the robot, then decide which task to do.
 *
 */
void arbitrate()
{ 
  int s_level = 0;		/* Instantaneous sound level */
  int snc_sound_delta = 30;	/* Pick a large number, then make a LOUD sound */

  sleep(1.0);

  /* Wait for sound to start moving */
  while (1) {
    s_level = abs(analog(microphone) - 128);	/* Abs difference from 128 */
    if ( s_level > snc_sound_delta) {
	break;
   }
   sleep(0.05);
  }

 while(1) {
	ir_s = ir_detect();

	if (ir_s == 3) wallAlert = 1;		/* Warn if we see a wall up ahead. */

	if (wallfollow_override) {
		motDrive = wallfollowDrive;
		motRotate = wallfollowRotate;
	} else if (bump_act) {
		motDrive = bumpDrive;
		motRotate = bumpRotate;
	} else if (seegoal_act) {
		motDrive = seegoalDrive;
		motRotate = seegoalRotate;
	} else if (wallfollow_act) {
		motDrive = wallfollowDrive;
		motRotate = wallfollowRotate;
	} else {	/* Stop */
		motDrive = 0;
		motRotate = 0;
	}
  }
}


/*
 * detectBump(): Figure out if we bump into something
 *
 */
void detectBump() {
int bmpr;
  while (1) {
      bmpr = bumper();

      if (bmpr != 0) {        /* Somehow, we were bumped */
	  bump_act = 1;		/* Take control of the robot */
	
	  if (bmpr == 0b100) {  /* Left Bump */
	    bumpDrive = 50;
	    bumpRotate = 0;
	    sleep(0.7);
	    bumpDrive = -50;
	    bumpRotate = -30;
	    sleep(0.3);
	  } else if (bmpr == 0b101) {	/* Left + Right = Front Bump */
	    bumpDrive = 50;
	    bumpRotate = 0;
	    sleep(0.7);
	    bumpDrive = -50;
	    bumpRotate = 10;
	    sleep(0.3);
	  } else if (bmpr == 0b001) {	/* Right Bump */
	    bumpDrive = 50;
	    bumpRotate = 0;
	    sleep(0.7);
	    bumpDrive = -50;
	    bumpRotate = 30;
	    sleep(0.3);
	  } else {				/* Some other bump */
	    bumpDrive = -25;
	    bumpRotate = 15;
	    sleep(0.7);
	    bumpDrive = -50;
	    bumpRotate = -10;
	    sleep(0.3);
	  }

	  bump_act = 0;		/* Bump gives up control */
	}
   }
}

/*
 * wallFollow(): Actually follow a wall
 *
 */
void wallFollow() {

  wallfollow_act = 1;

  while (1) {
    if (wallfollowDirection) {	/* We are going forward */

      if ((ir_s == 3) || (wallAlert == 1)) {
	  wallfollow_override = 1;
        wallfollowDrive = -30;
	  wallfollowRotate = 65; /* 55 */
        sleep(0.3);	/* 0.1 */
	  wallfollow_override = 0;
	  wallAlert = 0;
      } else if (ir_s == 1) {
        wallfollowDrive = -40;
	  wallfollowRotate = 15; /* 10 */
        sleep(0.4);	/* 0.1 */
      } else {
        wallfollowDrive = -40;
	  wallfollowRotate = -15;  /* 0 */
        sleep(0.6);	/* 0.5 */

  	  while (ir_s != 1 && ir_s != 3) {
	      wallfollowDrive = -40;
	  	wallfollowRotate = -30;
            sleep(0.1);	
	      
        }
      }
    } else {

       if ((ir_s == 3) || (wallAlert == 1)) {
	  wallfollow_override = 1;
        wallfollowDrive = -40;
	  wallfollowRotate = -65;
        sleep(0.5);
	  wallfollow_override = 0;
	  wallAlert = 0;
      } else if (ir_s == 2) {
        wallfollowDrive = -40;
	  wallfollowRotate = -20; /* -35 */
        sleep(0.8);		/* 0.1 */
      } else {
        wallfollowDrive = -40;
	  wallfollowRotate = 0;	/* -5 */
        sleep(0.6);

	  while (ir_s != 2 && ir_s != 3) {
	    	wallfollowDrive = -40;
	  	wallfollowRotate = 30;
            sleep(0.1);
	  }
      }
    }
  
  }
}

/*
 * seeGoal(): See a light, turn around, and set the direction variable to the
 *            opposite of what it is set at
 *
 */
void seeGoal() {

	while(1) {

	  /* Check to see if we saw a light */
	  while (1)
	  {
		if ((analog(photo_left) <= 4) || (analog(photo_right) <= 4)) {
	  		break;
	  	  }
		sleep(0.2);
	  }

  	  if (wallfollowDirection)
	  	  wallfollowDirection = 0;
	  else
		  wallfollowDirection = 1;

	  seegoal_act = 1;

	  /* Turn around and go the other direction */
	  /*while (ir_s != 2) {*/
	    seegoalDrive = 40;
	    seegoalRotate = 70;
	    sleep(2.0);
	  /*}*/

	  seegoal_act = 0;

	  sleep(3.0);
    }
}

void report() {
	while (1) {
		printf("WF B:%d W:%d IR:%d ", bump_act, wallfollow_act, ir_s);
		printf("Tr:%d Rt:%d %d\n", motDrive, motRotate, wallfollowDirection);
		sleep(0.2);  /* Don't overtax the LCD */
	} /* end while */
}
	  
void start_wallfollow()
{
  printf("Wall Follower\n"); 
  start_process(motors());
  start_process(arbitrate());
  start_process(report());
  start_process(seeGoal());
  start_process(wallFollow());
  start_process(detectBump());

}