void main()
{
  int i, pid;

  init();

  while(1)
    {
      /* display menu until stop button depressed */
      while(!stop_trigger() )
	{
	  i = knob() / 10;
	  
	  if( i == 0 )
	    {
	      printf("blues\n");
	    }
	  else if ( i == 1 )
	    {
	      printf("turn right\n");
	    }
	  else if ( i == 2 )
	    {
	      printf("turn left\n");
	    }
	  else if ( i == 3 )
	    {
	      printf("move forward shafted\n");
	    }
	  else if ( i == 4 )
	    {
	      printf("charge forward half speed\n");
	    }
	  else if ( i == 5 )
	    {
	      printf("charge forward input speed\n");
	    }
	  else if ( i == 6 )
	    {
	      printf("orientation\n");
	    }
	  else if ( i == 7)
	    {
	      printf("calibrate\n");
	    }
	  else if ( i == 8 )
	    {
	      printf("turn right half speed\n");
	    }
	  else if ( i == 9 )
	    {
	      printf("turn left half speed\n");
	    }
	  else if ( i == 10 )
	    {
	      printf("step forward\n");
	    }
	  else if ( i == 11 )
	    {
	      printf("qualify\n");
	    }
	  else if ( i == 12 )
	    {
	      printf("linefollow\n");
	    }
	  else if ( i == 13 )
	    {
	      printf("go straight with encoders\n");
	    }
	  else if ( i == 14 )
	    {
	      printf("stopped sensor\n");
	    }
	  else if ( i == 15 )
	    {
	      printf("find line\n");
	    }
	  else if ( i == 16 )
	    {
	      printf("sweep\n");
	    }
	  
	  else 
	    {
	      printf("alloff\n");
	    }
	  sleep( 0.1 );
	  
	}	  
      
      wait_stop();
      
      if( i == 0 )
	{
	  pid = start_process(showBlues());
	}
      else if ( i == 1 )
	{
	  pid = start_process(turn_right());
	}
      else if ( i == 2 )
	{
	  pid = start_process(turn_left());
	}
      else if ( i == 3 )
	{
	  pid = start_process(forwardTest());
	}
      else if ( i == 4 )
	{
	  pid = start_process(chargeForwardHalfTest());
	}
      else if ( i == 5 )
	{
	  pid = start_process(chargeForwardTest());
	}
      else if ( i == 6 )
	{
	  pid = start_process(orient());
	}
      else if ( i == 7)
	{
	  pid = start_process(calibrateThresholds());
	}
      else if ( i == 8 )
	{
	  pid = start_process(rightHalfTest());
	}
      else if ( i == 9 )
	{
	  pid = start_process(leftHalfTest());
	}
      else if ( i == 10 )
	{
	  pid = start_process(step_forward());
	}
      else if ( i == 11 )
	{
	  pid = start_process(qualify());
	}
      else if ( i == 12 )
	{
	  /* followingLine with arg = 1
	     means we are blue, and assumes we're in the white side
	     ready to follow the blue line.
	     */
	  pid = start_process(followLine(0));
	}
      else if ( i == 13 )
	{
	  pid = start_process(fd_encoder(1000));
	}
      else if ( i == 14 )
	{
	  pid = start_process(stoppedDisplay() );
	}
      else if ( i == 15 )
	{
	  pid = start_process(findLine( 1 ) );
	}
      else if ( i == 16 )
	{
	  pid = start_process(sweep( 1 ) );
	}
      else 
	{
	  pid = start_process(ao());
	}
      wait_stop();
      kill_process(pid);
      ao();
    }
}

void showAnalogs()
{
  while(1)
    {
      printf("%d %d %d %d %d %d\n",analog(LED_PORT_1), analog(LED_PORT_2), 
	     analog(LED_PORT_3), analog(LED_PORT_4), analog(LED_PORT_5), analog(START_SENSOR));
      sleep(0.1);
    }
}

void showBlues()
{
  while(1)
    {
      printf("%d %d %d %d %d %d %d %d\n",
	     isBlue(LED_PORT_1), isBlue(LED_PORT_2), 
	     isBlue(LED_PORT_3), isBlue(LED_PORT_4), 
	     isBlue(LED_PORT_5), isBlue(LED_PORT_6), 
	     isBlue(LED_PORT_7), isBlue(LED_PORT_8));
      sleep(0.1);
    }
}


void stoppedDisplay()
{
  while(1)
    {
      printf("%d \n", stopped );
      sleep(0.1);
    }
}

void rightTest()
{
  float time;
  int encoder_start_right, encoder_start_left;

  wait_start();

  enable_encoder(LEFT_ENCODER);
  enable_encoder(RIGHT_ENCODER);
  encoder_start_left = read_encoder(LEFT_ENCODER);
  encoder_start_right = read_encoder(RIGHT_ENCODER);

  while(!start_trigger())
    {
      motor(LEFT_MOTOR_1, FULL_SPEED);
      motor(RIGHT_MOTOR_1, -FULL_SPEED);
      motor(LEFT_MOTOR_2, FULL_SPEED);
      motor(RIGHT_MOTOR_2, -FULL_SPEED);
      printf("%d %d\n",read_encoder(LEFT_ENCODER) - encoder_start_left, 
	     read_encoder(RIGHT_ENCODER) - encoder_start_right );
      sleep(0.05);
    }

  off(LEFT_MOTOR_1);
  off(LEFT_MOTOR_2);
  off(RIGHT_MOTOR_1);
  off(RIGHT_MOTOR_2);
}

void rightHalfTest()
{
  float time;
  int encoder_start_right, encoder_start_left;

  wait_start();

  enable_encoder(LEFT_ENCODER);
  enable_encoder(RIGHT_ENCODER);
  encoder_start_left = read_encoder(LEFT_ENCODER);
  encoder_start_right = read_encoder(RIGHT_ENCODER);

  while(!start_trigger())
    {
      motor(LEFT_MOTOR_1, HALF_SPEED);
      motor(RIGHT_MOTOR_1, -HALF_SPEED);
      motor(LEFT_MOTOR_2, HALF_SPEED);
      motor(RIGHT_MOTOR_2, -HALF_SPEED);
      printf("%d %d\n",read_encoder(LEFT_ENCODER) - encoder_start_left, 
	     read_encoder(RIGHT_ENCODER) - encoder_start_right );
      sleep(0.05);
    }

  off(LEFT_MOTOR_1);
  off(LEFT_MOTOR_2);
  off(RIGHT_MOTOR_1);
  off(RIGHT_MOTOR_2);
}

void leftTest()
{
  float time;
  int encoder_start_right, encoder_start_left;

  wait_start();

  enable_encoder(LEFT_ENCODER);
  enable_encoder(RIGHT_ENCODER);
  encoder_start_left = read_encoder(LEFT_ENCODER);
  encoder_start_right = read_encoder(RIGHT_ENCODER);

  while(!start_trigger())
    {
      motor(LEFT_MOTOR_1, -FULL_SPEED);
      motor(RIGHT_MOTOR_1, FULL_SPEED);
      motor(LEFT_MOTOR_2, -FULL_SPEED);
      motor(RIGHT_MOTOR_2, FULL_SPEED);
      printf("%d %d\n",read_encoder(LEFT_ENCODER) - encoder_start_left, 
	     read_encoder(RIGHT_ENCODER) - encoder_start_right );
      sleep(0.05);
    }

  off(LEFT_MOTOR_1);
  off(LEFT_MOTOR_2);
  off(RIGHT_MOTOR_1);
  off(RIGHT_MOTOR_2);
}

void leftHalfTest()
{
  float time;
  int encoder_start_right, encoder_start_left;

  wait_start();

  enable_encoder(LEFT_ENCODER);
  enable_encoder(RIGHT_ENCODER);
  encoder_start_left = read_encoder(LEFT_ENCODER);
  encoder_start_right = read_encoder(RIGHT_ENCODER);

  while(!start_trigger())
    {
      motor(LEFT_MOTOR_1, -HALF_SPEED);
      motor(RIGHT_MOTOR_1, HALF_SPEED);
      motor(LEFT_MOTOR_2, -HALF_SPEED);
      motor(RIGHT_MOTOR_2, HALF_SPEED);
      printf("%d %d\n",read_encoder(LEFT_ENCODER) - encoder_start_left, 
	     read_encoder(RIGHT_ENCODER) - encoder_start_right );
      sleep(0.05);
    }

  off(LEFT_MOTOR_1);
  off(LEFT_MOTOR_2);
  off(RIGHT_MOTOR_1);
  off(RIGHT_MOTOR_2);
}

void orient()
{
  printf("orient\n");
  if (DEBUG)
    {
      printf("orienting robot\n");
    }
  orientRobot();
  if (DEBUG)
    {
      printf("robot oriented\n");
    }

}

void forwardTest()
{
  float time;

  wait_start();

  time = seconds();

  while(!start_trigger())
    {
      fd_encoder( 20 );
    }

  off(LEFT_MOTOR_1);
  off(LEFT_MOTOR_2);
  off(RIGHT_MOTOR_1);
  off(RIGHT_MOTOR_2);

  return;
}

void chargeForwardTest()
{
  int i;

  while(1)
    {
      
      while( !start_trigger() )
	{
	  i = ( knob() * 100 ) / 255;
	  printf( "%d\n", i );
	  sleep( 0.1 );
	}
      
      wait_start();
      /* move_forward( i ); */
      
      run_roller();
      
      fd_encoder( 200 );
      
      wait_start();
      motor_stop();
      stop_roller();
      
    }
}

void chargeForwardHalfTest()
{
  while(1)
    {
      wait_start();
      move_forward_half();
      run_roller();
      wait_start();
      motor_stop();
      stop_roller();
    }
}

void chargeForwardFullTest()
{
  while(1)
    {
      wait_start();
      move_forward_full();
      run_roller();
      wait_start();
      motor_stop();
      stop_roller();
    }
}

void qualify()
{
  int color;

  /* wait_start(); */
  
  start_machine( START_SENSOR );
  start_process(checkStopped());

  color = orientRobot();
  run_roller();
  fd_encoder( 200 );

  step_backward();
  sleep(0.2);
  turn_left();
  backStraight();
  run_roller();

  sweep( color );

}

void qualifyTest()
{
  int color;

  /* wait_start(); */
  
  color = orientRobot();
  run_roller();
  fd_encoder( 200 );
  
  step_backward();
  sleep(0.2);
  turn_left();
  backStraight();
  run_roller();

  sweep( color );

}


void strategy()
{
  int color;
  start_machine( START_SENSOR );

  sleep(0.5);
  color = orientRobot();

  /* move forward 5 ticks */
  fd_encoder( 5 );
  
  run_roller();

  findLine( color );
  sweep( color );

}