/*
  quadrant_1 = front_right
  quadrant_2 = front_left
  quadrant_3 = back_left
  quadrant_4 = back_right
*/

persistent int led_threshold[LEDS];

int quadrant_1 = 0;
int quadrant_2 = 0;
int quadrant_3 = 0;
int quadrant_4 = 0;

int isBlue( int port )
{
  if( port == LED_PORT_1 )
    {
      return (analog(LED_PORT_1) > led_threshold[0]);
    }
  else if( port == LED_PORT_2 )
    {
      return (analog(LED_PORT_2) > led_threshold[1]);
    }
  else if( port == LED_PORT_3 )
    {
      return (analog(LED_PORT_3) > led_threshold[2]);
    }
  else if( port == LED_PORT_4 )
    {
      return (analog(LED_PORT_4) > led_threshold[3]);
    }
  else if( port == LED_PORT_5 )
    {
      return (analog(LED_PORT_5) > led_threshold[4]);
    }
  else if( port == LED_PORT_6 )
    {
      return (analog(LED_PORT_6) > led_threshold[5]);
    }
  else if( port == LED_PORT_7 )
    {
      return (analog(LED_PORT_7) > led_threshold[6]);
    }
  else if( port == LED_PORT_8 )
    {
      return (analog(LED_PORT_8) > led_threshold[7]);
    }
  else 
    {
      printf("BAD PORT\n");
      wait_start();
      return 0;
    }
}


void setLeds()
{
  quadrant_1 = isBlue(LED_PORT_1);
  quadrant_2 = isBlue(LED_PORT_2);
  quadrant_3 = isBlue(LED_PORT_3);
  quadrant_4 = isBlue(LED_PORT_4);

}  

/* returns 
   1 if on blue side
   0 if on white side
*/
   
   
int orientRobot()
{

  setLeds();
  sleep(0.2);
  setLeds();
  
  if (quadrant_1 + quadrant_2 + quadrant_3 + quadrant_4 > 1)
    {
      /* blue side  */
      if (DEBUG)
	{
	  printf("robot in blue side\n");
	}
      orientSide(1);
      return 1;
    }
  
  else
    {
      /* white side */
      if (DEBUG)
	{
	  printf("robot in white side\n");
	}
      orientSide(0);
      return 0;
    }

}


void orientSide(int blue_side)
{

  int white = 0;
  int blue = 0;

  setLeds();

  if (blue_side == 1)
    {
      white = 0;
      blue = 1;
    }
  else
    {
      white = 1;
      blue = 0;
    }
  
  if (quadrant_1 == blue &&
      quadrant_2 == white &&
      quadrant_3 == blue &&
      quadrant_4 == blue )
    {
      /* stay straight
	 1 0 1 1
      */
      if (DEBUG)
	{
	printf("robot stay straight\n");
	}
    }
  
  else if (quadrant_1 == white &&
      quadrant_2 == blue &&
      quadrant_3 == blue &&
      quadrant_4 == blue )
    {
      /* turn right 90
	 0 1 1 1
      */
      turn_right();
      if (DEBUG)
	{
	printf("robot turn right\n");
	}
    }      

  else if (quadrant_1 == blue &&
      quadrant_2 == blue &&
      quadrant_3 == blue &&
      quadrant_4 == white )
    {
      /* turn 180
	 1 1 1 0
      */
      turn_180();
      if (DEBUG)
	{
	printf("robot turn 180\n");
	}
    }

  else if (quadrant_1 == blue &&
      quadrant_2 == blue &&
      quadrant_3 == white &&
      quadrant_4 == blue )
    {
      /* turn left 90
	 1 1 0 1
      */
      turn_left();
      if (DEBUG)
	{
	printf("robot turn left\n");
	}
    }
  else
    {
      if (DEBUG)
	{      
	  printf("you're fucked\n");
	}
    }

}      

void calibrateThresholds()
{
  int i;
  int reading[LEDS] = { 0, 0, 0, 0, 0};
  int white_side[LEDS] = { 0, 0, 0, 0, 0 };
  int blue_side[LEDS] = { 0, 0, 0, 0, 0 };

  printf("Waiting for white side reading...\n");

  wait_start();

  for( i = 0; i < LEDS; i++ )
    reading[i] = 0;

  analog( LED_PORT_1 );
  analog( LED_PORT_2 );
  analog( LED_PORT_3 );
  analog( LED_PORT_4 );
  analog( LED_PORT_5 );
  analog( LED_PORT_6 );
  analog( LED_PORT_7 );
  analog( LED_PORT_8 );

  sleep(0.2);

  for( i = 0; i < SAMPLES; i++ )
    {
      reading[0] += analog( LED_PORT_1 );
      reading[1] += analog( LED_PORT_2 );
      reading[2] += analog( LED_PORT_3 );
      reading[3] += analog( LED_PORT_4 );
      reading[4] += analog( LED_PORT_5 );
      reading[5] += analog( LED_PORT_6 );
      reading[6] += analog( LED_PORT_7 );
      reading[7] += analog( LED_PORT_8 );
      sleep(0.1);
    }

  printf("Averages ");
  for( i = 0; i < LEDS; i++ )
    {
      reading[i] /= SAMPLES;
      printf("%d ", reading[i]);
    }
  printf("\n");

  for ( i = 0; i < LEDS; i++ )
    {
      white_side[i] = reading[i];
    }
    
  wait_start();

  printf("Waiting for blue side reading...\n");

  wait_start();

  for( i = 0; i < LEDS; i++ )
    reading[i] = 0;

  analog( LED_PORT_1 );
  analog( LED_PORT_2 );
  analog( LED_PORT_3 );
  analog( LED_PORT_4 );
  analog( LED_PORT_5 );
  analog( LED_PORT_6 );
  analog( LED_PORT_7 );
  analog( LED_PORT_8 );

  sleep(0.2);

  for( i = 0; i < SAMPLES; i++ )
    {
      reading[0] += analog( LED_PORT_1 );
      reading[1] += analog( LED_PORT_2 );
      reading[2] += analog( LED_PORT_3 );
      reading[3] += analog( LED_PORT_4 );
      reading[4] += analog( LED_PORT_5 );
      reading[5] += analog( LED_PORT_6 );
      reading[6] += analog( LED_PORT_7 );
      reading[7] += analog( LED_PORT_8 );
      sleep(0.1);
    }
  
  printf("Averages ");
  for( i = 0; i < LEDS; i++ )
    {
      reading[i] /= SAMPLES;
      printf("%d ", reading[i]);
    }
  printf("\n");
  
  for ( i = 0; i < LEDS; i++ )
    {
      blue_side[i] = reading[i];
    }
  
  wait_start();

  for ( i = 0; i < LEDS; i++ )
    {
      led_threshold[i] = (blue_side[i] + white_side[i]) / 2;
    }
  printf("Thresholds: %d %d %d %d %d %d %d %d %d\n", 
	 led_threshold[0], led_threshold[1], led_threshold[2],
	 led_threshold[3], led_threshold[4], led_threshold[5],
	 led_threshold[6], led_threshold[7] );
  wait_start();
}