/* cds.c */

/* defines must span 0, 1, 2 only */
#define CDS_WHITE 0
#define CDS_BLUE 1
#define CDS_UNKNOWN 2

int cds_white_val[5];
int cds_blue_val[5];

void calibrate_cds(char str[], int cds_col[], int n)
{
  int i, j;
  for (j=0; j<5; j++) cds_col[j]=0;

  printf("%s: press start\n", str);
  start_press();
  sleep(1.0);
  setup_drive(50);
  for (i=0; i<n; i++)
  {
    for (j=0;j<5;j++)
      cds_col[j] += analog(j + PORT_CDS_ARRAY_START);
    msleep(10L);
  }
  stop();

  for (j=0;j<5;j++)
    cds_col[j] /= n;

  printf("%d %d %d %d %d\n", cds_col[0], cds_col[1],
	 cds_col[2], cds_col[3], cds_col[4]);
  start_press();
}

void init_cds()
{
  int i,j;
  int n=25;

  set_digital_out(6);
  calibrate_cds("blue", cds_blue_val, 25);
  calibrate_cds("white", cds_white_val, 25);
  clear_digital_out(6);
}

int cds_get_color(int port, int factor)
{
  int i = port - PORT_CDS_ARRAY_START;
  int val = analog(port);

  if (abs(cds_white_val[i]-val) > factor*abs(cds_blue_val[i]-val))
    return CDS_BLUE;
  else if (abs(cds_blue_val[i]-val) > factor*abs(cds_white_val[i]-val))
    return CDS_WHITE;
  else
    return CDS_UNKNOWN;
}