int followLine(int blue_side)
{
  int pid = 0;

  /*
  pid = start_process( followingLine( blue_side ) );
  */
  followingLine( blue_side );

  sleep(1.0);

  /*
  while( 1 )
    {
      if( stopped )
	{
	  kill_process( pid );
	  ao();
	  break;
	}
    }
    */
}

int followingLine(int blue_side)
{
  
  int blue = 0;
  int white = 0;
  float speed_threshold;

  int oldReading, newReading;
  float oldTime, newTime;

  action = 0;

  if (blue_side)
    {
      blue = 1;
      white = 0;
    }
  else
    {
      blue = 0;
      white = 1;
    }

  oldReading = read_encoder(RIGHT_ENCODER) + read_encoder(LEFT_ENCODER);
  newReading = oldReading;

  oldTime = seconds();
  newTime = oldTime;

  /*
  move_forward_half();
  sleep(0.25);
  speed_threshold = speed * 0.6;
  */

  while(1)
    {
      readSensors();
      newTime = seconds();

      printf("%d %d %d ", center_left, front, center_right);

      if (front == blue
	  && center_right == white
	  && center_left == white)
	{
	  /* stay straight */
	  printf("stay straight\n");
	  action = 0;
	  move_action( action );
	}
      
      
      else if (center_right == blue
	       && front == white
	       && center_left == white)
	{
	  /* turn hard right */
	  printf("turn hard right\n");
	  action = 2;
	  move_action( action );
	}
      
      else if (center_left == blue
	       && front == white
	       && center_right == white)
	{
	  /* turn hard left */
	  printf("turn hard left\n");
	  action = -2;
	  move_action( action );
	}
      else if (center_right == blue
	       && front == blue
	       && center_left == white)
	{
	  /* turn soft right */
  	  printf("turn soft right\n");
	  action = 1;
	  move_action( action );
	}
      
      else if (center_left == blue
	       && front == blue
	       && center_right == white)
	{
	  /* turn soft left */
	  printf("turn soft left\n");
	  action = -1;
	  move_action( action );
	}
      /* we are lost */
      else
	{
	  printf("robot is lost\n");
	  move_action( action );

	}
	 

      /*
      if( (newTime - oldTime)
	  > 0.5 )
	{
	  oldTime = newTime;
	  newReading = read_encoder(RIGHT_ENCODER) + read_encoder(LEFT_ENCODER);
	  if( newReading - oldReading < 3 )
	    {
	      motor_stop();
	      sleep(0.3);
	      oldReading = newReading;
	    }
	}
      */

      if( flushFront() )
	{
	  motor_stop();
	  break;
	}
      /* As is, these can be triggered anywhere along the line. */
      
      else if( frontRight() )
	{
	  right_motor_stop();
	  set_left_motors(SPEED_FULL);

	  oldTime = seconds();

	  /* timeout code */
	  while(1){
	    newTime = seconds();
	    /* if more than 1 second of not moving */
	    if(( (newTime - oldTime) > 1.0 )  && stopped )
	      {
		step_backward();
		turn_right_tick( RIGHT_VEER );
		step_forward();
		break;
		
	      }
	    if( !frontRight() )
	      {
		break;
	      }
	    if( flushFront() )
	      {
		break;
	      }
	    
	  }
	    
	}
      else if( frontLeft()  )
	{
	  left_motor_stop();
	  set_right_motors(SPEED_FULL);

	  oldTime = seconds();

	  /* timeout code */
	  while(1){
	    newTime = seconds();
	    /* if more than 1 second of not moving */
	    if(( (newTime - oldTime) > 1.0 )  && stopped )
	      {
		step_backward();
		turn_left_tick( LEFT_VEER );
		step_forward();
		break;
		
	      }
	    if( !frontLeft() )
	      {
		break;
	      }
	    if( flushFront() )
	      {
		break;
	      }
	    
	  }
	  
	}
	
      /*
      else if( speed < speed_threshold )
	{
	  motor_stop();
	  break;
	}
      */

    }
}     

/*
  -2 hard left
  -1 soft left
   0 straight
   1 soft right
   2 hard right
 */

void move_action( int action )
{
  if( action == -2 )
    turn_hard_left();
  else if ( action == -1 )
    turn_soft_left();
  else if ( action == -0 )
    move_forward(SPEED_FULL);
  else if ( action == 1 )
    turn_soft_right();
  else if ( action == 2 )
    turn_hard_right();
  else
    ao();
}

void sweep( int blueLine )
{
  int new_right_encode = read_encoder(RIGHT_ENCODER);
  int new_left_encode = read_encoder(LEFT_ENCODER);
  int old_right_encode = read_encoder(RIGHT_ENCODER);
  int old_left_encode = read_encoder(LEFT_ENCODER);
  int counter = 0;
  int oldCount = 0;

  int headLeft = 1; /* start sweeping leftward */

  run_roller();
  while(1)
    {
      new_right_encode = read_encoder(RIGHT_ENCODER);
      new_left_encode = read_encoder(LEFT_ENCODER);

      /* COUNTER:
         Division with integers rounds down always.  I.e., 5/2 will round
	 down to 2.  oldCount is a variable which is only incremented when
	 the counter exceeds the modulus.*/
	 
      if ((counter / 5) != oldCount)
	{
	  printf("counter code \n");
	  oldCount++;
	  if ((new_right_encode == old_right_encode) &&
	      (new_left_encode == old_left_encode))
	    {
	      /* TIMEOUT/STALL CODE */
	      printf("wheel timeout\n");
	      wheel_timeout();
	    }
	  /* After if statement is passed, give the old values
	     new ones.
	   */
	  old_right_encode = new_right_encode;
	  old_left_encode = new_left_encode;
	}

      followLine( blueLine );
    
      step_backward();
      sleep(0.2);
      if( headLeft )
	{
	  turn_180_right();
	  headLeft = !headLeft;
	}
      else
	{
	  turn_180_left();
	  headLeft = !headLeft;
	}
	 
      sleep(0.2);
      
      followLine( blueLine );
      
      counter++;
  }
}