void charge_forward()
{
  move_forward_full();
  run_roller();
}

void charge_forward_half()
{
  move_forward_half();
  run_roller();
}

int frontRight()
{
  return digital(FRONT_RIGHT) ;
}

int frontLeft()
{
  return digital(FRONT_LEFT);
}
int backRight()
{
  return digital(BACK_RIGHT) ;
}

int backLeft()
{
  return digital(BACK_LEFT);
}

int flushBack()
{
  return (digital(BACK_LEFT) && digital(BACK_RIGHT));;
}

int flushFront()
{
  return frontRight() && frontLeft();
}

void backStraight()
{
  float oldTime, newTime;

  oldTime = seconds();

  while(! flushBack() )
    {
      if( backLeft() )
	{
	  set_right_motors(-SPEED_FULL);
	  set_left_motors(SPEED_OFF);
	}
      else if ( backRight() )
	{
	  set_right_motors(SPEED_OFF);
	  set_left_motors(-SPEED_FULL);
	}
      else
	{
	  move_backward( SPEED_2_3 );
	}

      /* timeout of 1 second for backing up straight */
      newTime = seconds();
      if( (newTime - oldTime) > 1.0 )
	break;

    }
  
  motor_stop();
}

void findPlane( int whitePlane )
{
  int white = 0;

  if (whitePlane)
    {
      white = 0;
    }
  else
    {
      white = 1;
    }

  printf("Finding %d plane\n", whitePlane);

  while(1)
    {
      readSensors();
      
      if( front == white 
	  && front_left == white 
	  && front_right == white 
	  && center_left == white 
	  && center_right == white
	  ) {
	break;
      }
      else{
	move_forward_full();
      }

      printf("white=%d %d %d %d %d %d\n", white,
	     front, front_left, front_right,
	     center_left, center_right );

    }

  printf("Found %d plane\n", whitePlane);
}

void findLine(int blueLine)
{
    
  int blue = 0;
  int white = 0;
  int right = 0; /* signifies which side should hit first */
  int forward = 1; /* which direction to head to seek line */

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

  findPlane( blue );

  /*  keep moving until we hit the line */
  printf("Waiting until hitting line\n");

  /* wait until front sensors hit the line first */
  while( 1 )
    {
      readSensors();
      if( front_right == blue )
	{
	  right = 1;
	  forward = 1;
	  break;
	}
      else if ( front_left == blue )
	{
	  right = 0;	
	  forward = 1;
	  break;
	}
      else if( back_right == blue )
	{
	  right = 1;
	  forward = 0;
	  break;
	}
      else if ( back_left == blue )
	{
	  right = 0;	
	  forward = 0;
	  break;
	}  
      else if ( front == blue
		|| front_right == blue
		|| front_left == blue
		)
	break;
      
      else{
	/* keep searching */
	move_forward_full();
      }
    }
  
  printf( "Waiting until center passes line\n" );
  /* wait until center sensors hit the line */
  
  while( 1 )
    {
      readSensors();
    /* something has hit blue, so center has hit the line */
  
      if( front == blue
	  || front_right == blue
	  || front_left == blue
	  )
	break;

      /* haven't found anything, keep searching */
      
      else{
	if( forward )
	  move_forward_full();
	else
	  move_backward_full( );
      }
    }

  printf( "Rotating onto the line" );
  turn_triggered( right, blueLine );
  
  printf("Found line\n");

  backStraight();

  printf("Flushed against wall\n");
  motor_stop();
}

void turn_triggered( int right, int blueLine )
{
  int led_port = 0;

  if( right ){
    led_port = LED_PORT_4;
  }
  else{
    led_port = LED_PORT_3;
  }

  /* turn until a back sensor hits the line */
  while( 1 ){
    readSensors();

    if( isBlue( led_port ) == blueLine )
      break;

    if( right ){
      /*turn left*/
      set_left_turn();
    }
    else{
      /*turn right*/
      set_right_turn();
    } 
  }

  /* turn until that sensor is off the same line*/
  while( 1 ){
     readSensors();

    if( isBlue( led_port ) != blueLine )
      break;

    if( right ){
      /*turn left*/
      set_left_turn();
    }
    else{
      /*turn right*/
      set_right_turn();
    } 
  }

}