/* calibrate.c */
/* This file contains the functions necessary to calibrate
   light sensors.
*/

/* 
   setting up easy macros so that we can readily move
   the sensors around without messing anything up 
*/

#define FRONT_LEFT 26
#define FRONT_RIGHT 24
#define REAR_LEFT 30
#define REAR_RIGHT 28
#define START_LIGHT 22
#define LINE_LEFT 20
#define LINE_RIGHT 18

#define FRONT_LEFT_BUMPER_1 3
#define FRONT_LEFT_BUMPER_2 9
#define FRONT_RIGHT_BUMPER_1 2
#define FRONT_RIGHT_BUMPER_2 16
#define BACK_LEFT_BUMPER 11
#define BACK_RIGHT_BUMPER 10

#define RIGHT_BALL_SENSOR 5
#define LEFT_BALL_SENSOR 4
#define ENCODER 7

#define LEFT_ONEEIGHTY 41
#define RIGHT_ONEEIGHTY 55
#define LEFT_NINETY 20
#define RIGHT_NINETY 29

#define DIST_ONE 60
#define DIST_TWO 30

/* These are threshold values for the photoresistors */
int FR_threshold, FL_threshold, RR_threshold, RL_threshold;
int LL_threshold, LR_threshold, SL_threshold;

/* Indicates the configuration of the Board */
int config, color;

void calibrate(int version)
{
  printf("Press Start to Begin Cals\n");
  start_press();

  if( version == 0)
    {
      cal_value_rev2(0);
      start_press();

      cal_value_rev2(1);
	start_press();

      cal_value_rev2(2);
      start_press();
    }
  else
    {
      printf("Ready to Cal FL");

      FL_threshold = cal_value_rev(FRONT_LEFT);
      printf("FL Threshold: %d\n", FL_threshold);

      start_press();

      printf("Ready to Cal FR");

      FR_threshold = cal_value_rev(FRONT_RIGHT);
      printf("FR Threshold: %d\n", FR_threshold);

      start_press();

      printf("Ready to Cal RL");

      RL_threshold = cal_value_rev(REAR_LEFT);
      printf("RL Threshold :%d\n", RL_threshold);

      start_press();

      printf("Ready to Cal RR");

      RR_threshold = cal_value_rev(REAR_RIGHT);
      printf("RR Threshold :%d\n", RR_threshold);

      start_press();

      printf("Ready to Cal LL");

      LL_threshold = cal_value_rev(LINE_LEFT);
      printf("LL Threshold :%d\n", LL_threshold);

      start_press();

      printf("Ready to Cal LR");

      LR_threshold = cal_value_rev(LINE_RIGHT);
      printf("LR Threshold :%d\n", LR_threshold);

      start_press();

      printf("Ready to Cal SL");

      SL_threshold = cal_value_rev(START_LIGHT);
      printf("SL Threshold: %d\n", SL_threshold);

      start_press();

    }
}


int cal_value(int sensor)
{
  int count = 0;
  int value = 0;
  int white_average;
  int black_average;

  printf(" Place over White\n");
  start_press();
  printf("Working...\n");

  while (count < 10)
   {
     value += analog(sensor); 
     count++;
   }

  white_average = value / (count + 1);

  printf("White Average: %d\n", white_average);
  start_press();

  printf("Place Sensor Over Black\n");
  start_press();
  printf("Working...\n");

  /* reset the value and count variables */
  value = 0;
  count = 0;

  while (count < 10)
   {
     value += analog(sensor);
     count++;
   }
  
  black_average = value / (count + 1);

  printf("Black average: %d\n", black_average);
  start_press();

  /* 
     We now have the white/black averages, so now we can
     find the threshold
  */

  value = black_average - white_average;
  value /= 2;
  
  value += white_average;

  return value;

}


/* Another threshold value calculation function */
/* differs from cal_value() in that it uses the max white
   and min black values instead of the averages
*/
int cal_value_rev(int sensor)
{
  int count = 0;
  int value = 0;
  int white_max;
  int black_min;
  int sensor_val;

  printf(" Place over White\n");
  start_press();
  printf("Working...\n");

  while (count < 50)
   {
     sensor_val = analog(sensor);

     if( sensor_val > white_max)
        white_max = sensor_val;

     count++;
   }

  printf("White Max: %d\n", white_max);
  start_press();

  printf("Place Sensor Over Black\n");
  start_press();
  printf("Working...\n");

  /* reset the value and count variables */
  value = 0;
  count = 0;
  black_min = 256;

  while (count < 50)
   {
     sensor_val = analog(sensor);

     if (sensor_val < black_min)
        black_min = sensor_val;

     count++;
   }
  
  printf("Black Min: %d\n", black_min);
  start_press();

  /* 
     We now have the white max and black min, so now we can
     find the threshold
  */

  value = black_min - white_max;
  value /= 2;
  
  value += white_max;

  return value;

}


/* Calibrates Sensors in Groups, to Speed things up */
void cal_value_rev2(int group)
{
  int count = 0;
  int i = 0;
  int value[4];
  int members = 0;
  int white_max[] = {0, 0, 0, 0};
  int black_min[4];
  int sensor_val[4];
  int sensor[4];

  printf("\n");
  /* the front group */
  if (group == 0)
    {
      printf("Front Group Cal ");
      members = 2;
	sensor[0] = FRONT_LEFT;
      sensor[1] = FRONT_RIGHT;      
    }

  /* the middle group */
  if (group == 1)
    {
      printf("Middle Group Cal");
      members = 3;
      sensor[0] = LINE_LEFT;
      sensor[1] = START_LIGHT;
      sensor[2] = LINE_RIGHT;
    }

  /* the back group */
  if (group == 2)
    {
       printf("Rear Group Cal  ");
       members = 2; 
       sensor[0] = REAR_LEFT;
       sensor[1] = REAR_RIGHT;
    }

  printf("Place over White\n");
  start_press();

  printf("White Maxs:     ");
  while(i < members)
   {
     while (count < 50)
      {
        sensor_val[i] = analog(sensor[i]);

        if( sensor_val[i] > white_max[i])
          white_max[i] = sensor_val[i];

        count++;
      }
      count = 0;
      printf("%d ", white_max[i]); 
      i++;
    }

  start_press();

  printf("\nPlace Sensors Over Black\n");
  start_press();

  /* reset the value and count variables */
  count = 0; i = 0;
  black_min[0] = 256; black_min[1] = 256;
  black_min[2] = 256; black_min[3] = 256;

  printf("Black Mins:     ");
  while ( i < members )
    {
      while (count < 50)
       {
         sensor_val[i] = analog(sensor[i]);

         if (sensor_val[i] < black_min[i])
           black_min[i] = sensor_val[i];

         count++;
       }
       count = 0;
       printf("%d ", black_min[i]);
       i++;
     }

  start_press();

  /* time to compute the thresholds */
  i = 0;
  printf("\nThresholds:     ");
  while( i < members)
   {
    value[i] = black_min[i] - white_max[i];
    value[i] /= 2;
    value[i] += white_max[i];
    printf("%d ", value[i]);
    i++;
   }

  if (group == 0)
   {
     FL_threshold = value[0];
     FR_threshold = value[1];
   }

  if (group == 1)
   {
     LL_threshold = value[0];
     SL_threshold = value[1];
     LR_threshold = value[2];
   } 

  if (group == 2)
   {
     RL_threshold = value[0];
     RR_threshold = value[1];
   }
}