int rPhotoMax;
int lPhotoMax;
int lPhotoMin;
int rPhotoMin;

int lPhoto = 1;
int rPhoto = 0;
int rMotor = 1;
int lMotor = 0;

/* abs value */
int abs(int n)
{
  if (n<0) return (-1 * n);
  return(n);
}

/********** utilities for display ***********/

/* Print character ch, rep times */
void repeat_char(int ch, int rep)
{ int j = 0;
  for (j = 0; j < rep; j++)
    printf("%c", ch); }

/* Count the number of digits in an integer (negative numbers add 1 for - sign) */
int digits(int signed_val)
{ int digs, val;
  val = abs(signed_val);
  if (val < 100)
    { if (val < 10)
	digs = 1;
        else digs = 2; }
    else if (val < 10000)
      { if (val < 1000)
	  digs = 3;
          else digs = 4; }
      else digs = 5;
  if (signed_val >= 0)
    return digs;
    else return (digs + 1);
}

/* Print val in the given number of columns if possible */
void printn(int val, int cols)	
{ int digs;			/* Number of digits */
  digs = digits(val);
  if (digs >= cols)
    printf("%d", val);	/* May not be possible to print in required number of cols */
  else
   { repeat_char(32,cols - digs);
     printf("%d",val); }
}

/* display the L/R values passed in, and the max and mins */
void displayLevels(int l, int r)
{
  printf("L="); printn(l,4);
  printn(lPhotoMax,5); printf("/"); printn(lPhotoMin,3);
  printf(" R="); printn(r,4);
  printn(rPhotoMax,5); printf("/"); printn(rPhotoMin,3);
  printf(" \n");
}


/* darkness should register a high number, lightness low */


/* return a normalized value from -100 (light) to +100 (dark)
   also adjusts min/max levels as new ranges are encountered */

/* This code works best if you expose the sensors to the full range of light 
before putting the robot down */

float darknessLevel(int sensor)
{ 
  int min, max, val, l, r;
  
  l = analog(lPhoto);
  r = analog(rPhoto);
  if (l > lPhotoMax) lPhotoMax = l;
  if (r > rPhotoMax) rPhotoMax = r;
  if (l < lPhotoMin) lPhotoMin = l;
  if (r < rPhotoMin) rPhotoMin = r;

  if (sensor == lPhoto) {
    val = l;
    min = lPhotoMin;
    max = lPhotoMax; }
  else {
    val = r;
    min = rPhotoMin;
    max = rPhotoMax; }  
  
  return ((200. * ((float) (val - min)) / ((float) (max - min))) - 100.);

}

/* turn L if dir==0, else R */
void backupAndTurn(int dir)
{
  motor(lMotor, -100.0);
  motor(rMotor, -100.0);
  sleep(1.0);  
  if (dir) {
    motor(lMotor, 100.);
    sleep(2.0);
    motor(rMotor, 100.); }
  else {
    motor(rMotor, 100.);
    sleep(2.0);
    motor(lMotor, 100.); }
}

/* if dir==0 rotate L, else R */
void rotate (int dir)
{
if (dir) {
    motor(lMotor, 50.);
    motor(rMotor, -50.); }
  else {
    motor(rMotor, 50.);
    motor(lMotor, -50.); }
}
  
/* set the min/max values to something reasonable */
void initLevels()
{
  lPhotoMax = 150;
  lPhotoMin = 50;
  rPhotoMax = 150;
  rPhotoMin = 50;
}

/* V3 works.  Goes to the light and then sits there.  Will follow the
   light if it moves. */
int main()
{ 
  float lp, rp;
  sleep(0.75);
  test_number++;
  
  if (test_number >= 3) test_number = 1;
  
  if (test_number != 1) { /* exit on alternate resets */
    
    initLevels();
    while (1) {
  
      lp = darknessLevel(lPhoto);
      rp = darknessLevel(rPhoto);
      displayLevels((int) lp, (int) rp);
  
      if (bumper())
        backupAndTurn(lp > rp); /* if left is darker, turn R */
    
      if (abs((int) (lp - rp)) > 20) /* if there's a big difference, rotate */
        rotate(lp > rp);
      else if (lp + rp > -180.){ /* go to the light */
        motor(lMotor, 80. - (rp - lp));
        motor(rMotor, 80. - (lp - rp)); }
      else /* at the light */
        stop();
     }
  }
}