int GYRO = 3;

int FRONT_SENSOR = 4;
int RIGHT_SENSOR = 5;
int LEFT_SENSOR = 6;

int RIGHT_MOTOR = 1;
int LEFT_MOTOR = 2;
int BRUSH_MOTOR = 3;

int RIGHT_ENCODER = 8;
int LEFT_ENCODER = 7;

int FRONT_THRESHOLD;
int RIGHT_THRESHOLD;
int LEFT_THRESHOLD;

int WHITE = 2;
int BLACK = 3;

int RED = 0;
int GREEN = 1;

int TEAM_COLOR;
int TEAM_NUMBER;
int BALL_COLOR;
int PATH;

int SPEED = 30;
int TWEAK = 5;
long TIME;


void main() {
   rf_team = 31;
   rf_enable();
   
   calibrate();
   printf("Press START to calibrate gyro...");
   wait_start();
   calibrate_gyro();

   start_machine();
 
   printf("GO!\n");
   
   start_process(update_angle());

   TEAM_COLOR = orient_and_get_color();
   TEAM_NUMBER = get_number();

   if (TEAM_COLOR == WHITE)
      printf("WHITE - TEAM %d\n", TEAM_NUMBER);
   else 
      printf("BLACK - TEAM %d\n", TEAM_NUMBER);

   drop_four();
   PATH = set_path();
   
   if (BALL_COLOR == RED)
      printf("PATH %d - COLOR RED\n", PATH);
   else 
      printf("PATH %d - COLOR GREEN\n", PATH);

   if (PATH == 1)
      path13();
   else if (PATH == 2)
      path24();
   else if (PATH == 3)
      path13();
   else
      path24();

   
   TIME = mseconds();

   while ((mseconds() - TIME) < 2000L) 
      drive_backward(degrees(), -70);
   
   moff();
   calibrate_gyro();

   display_stats();

   choose_bin();
}



void calibrate() {
   int frontBlack, rightBlack, leftBlack;
   int frontWhite, rightWhite, leftWhite;

   printf("place sensors on black...\n");

   wait_start();
   msleep(500L);

   frontBlack = analog(FRONT_SENSOR);
   leftBlack = analog(LEFT_SENSOR);
   rightBlack = analog(RIGHT_SENSOR);
 
   printf("BLACK F:%d R:%d L:%d\n", frontBlack, rightBlack, leftBlack); 

   wait_start();
   msleep(500L);

   printf("place sensors on white...\n");
   wait_start();
   msleep(500L);

   frontWhite = analog(FRONT_SENSOR);
   leftWhite = analog(LEFT_SENSOR);
   rightWhite = analog(RIGHT_SENSOR);

   printf("WHITE F:%d R:%d L:%d\n", frontWhite, rightWhite, leftWhite); 

   wait_start();
   msleep(500L);

   FRONT_THRESHOLD = (frontWhite + frontBlack) / 2;
   RIGHT_THRESHOLD = (rightWhite + rightBlack) / 2;
   LEFT_THRESHOLD = (leftWhite + leftBlack) / 2;

   printf("THOLD F:%d R:%d L:%d\n", FRONT_THRESHOLD, RIGHT_THRESHOLD, LEFT_THRESHOLD);
  
   wait_start();
   msleep(500L);
}



int orient_and_get_color() {
   int front, right, left;

   front = color(analog(FRONT_SENSOR),FRONT_THRESHOLD);
   right = color(analog(RIGHT_SENSOR),RIGHT_THRESHOLD);
   left = color(analog(LEFT_SENSOR),LEFT_THRESHOLD);

   if ((front == BLACK) && (right == WHITE) && (left == WHITE)) {
      turn_around();
      return WHITE;
   } else if ((front == WHITE) && (right == BLACK) && (left == WHITE)) {
      return WHITE;
   } else if ((front == WHITE) && (right == WHITE) && (left == BLACK)) {
	turn_right();
      return WHITE;
   } else if ((front == WHITE) && (right == WHITE) && (left == WHITE)) {
      turn_left();
      return WHITE;
   } else if ((front == WHITE) && (right == BLACK) && (left == BLACK)) {
      turn_right();
      return BLACK;
   } else if ((front == BLACK) && (right == WHITE) && (left == BLACK)) {
      turn_left();
      return BLACK;
   } else if ((front == BLACK) && (right == BLACK) && (left == WHITE)) {
      return BLACK;
   } else {
      turn_around();
      return BLACK;
   }
}



int get_number() {
   if (TEAM_COLOR == BLACK) {
      if (sign((float)rf_y0) > 0)
         return 0;
      else 
         return 1;
   } else {
      if (sign((float)rf_y0) > 0)
         return 1;
      else 
         return 0;
   }
}



void drop_four() {
   motor(BRUSH_MOTOR,-30);
   drive_forward(13,SPEED);
   msleep(500L);
}



int set_path() {
   int opp_x = his_x();
   
   motor(BRUSH_MOTOR,30);
   
   if ((TEAM_COLOR == BLACK) && (opp_x > 0)) {
      turn_left();
      BALL_COLOR = GREEN;
      return 1;
   } else if ((TEAM_COLOR == BLACK) && (opp_x <= 0)) {
      turn_right();
      BALL_COLOR = RED;
      return 2;
   } else if ((TEAM_COLOR == WHITE) && (opp_x < 0)) {
      turn_left();
      BALL_COLOR = RED;
      return 3;
   } else {
      turn_right();
      BALL_COLOR = GREEN;
      return 4;
   }
}



void path13() {
   drive_forward(20,SPEED);
   turn_wide_left();

   drive_forward(48,SPEED);
   turn_wide_left();

   drive_forward(6,SPEED);
}



void path24() {
   drive_forward(20,SPEED);
   turn_wide_right();

   drive_forward(48,SPEED);
   turn_wide_right();

   drive_forward(6,SPEED);
}



void choose_bin() {
   if (rf_vote_winner == BALL_COLOR) {
      if ((PATH == 1) || (PATH == 3)) {
         if (same_side()) {
	      switch13();
            middle_left();
         } else {
            far_right();
         }
      } else {
         if (same_side()) {
            switch24();
            middle_right();
         } else {
            far_left();
         }
      }
   } else {
      if ((PATH == 1) || (PATH == 3)) {
         if (same_side()) {
     	      switch13();
            far_left();
         } else {
            middle_right();
         }
      } else {
         if (same_side()) {
            switch24();
            far_right();
         } else {
            middle_left();
         }
	}
   } 
}



void switch13() {
   drive_forward(10,SPEED);
   turn_left();
   
   drive_forward(48,SPEED);
   turn_right();

   TIME = mseconds();

   while ((mseconds() - TIME) < 2000L) 
      drive_backward(degrees(), -SPEED);
   
   moff();
   calibrate_gyro();
   
   display_stats();
}



void switch24() {
   drive_forward(10,SPEED);
   turn_right();
   
   drive_forward(48,SPEED);
   turn_left();

   TIME = mseconds();

   while ((mseconds() - TIME) < 2000L) 
      drive_backward(degrees(), -SPEED);
   
   moff();
   calibrate_gyro();

   display_stats();
}



void middle_right() {
   drive_forward(31,SPEED);

   turn_right();
   drive_forward(20,SPEED);
   
}



void far_right() {
   drive_forward(54,SPEED);

   turn_wide_right();
   drive_forward(20,SPEED);
}



void middle_left() {
   drive_forward(31,SPEED);

   turn_left();
   drive_forward(20,SPEED);
}



void far_left() {
   drive_forward(54,SPEED);

   turn_wide_left();
   drive_forward(20,SPEED);
}



int my_x() {
   if (TEAM_NUMBER == 0)
      return rf_x0;
   else
      return rf_x1;
}



int my_y() {
   if (TEAM_NUMBER == 0)
      return rf_y0;
   else
      return rf_y1;
}
      


int his_x() {
   if (TEAM_NUMBER == 0)
      return rf_x1;
   else
      return rf_x0;
}



int his_y() {
   if (TEAM_NUMBER == 0)
      return rf_y1;
   else
      return rf_y0;
}



int color(int reading, int threshold) {
   if (reading < threshold)
      return WHITE;
   else
      return BLACK;
}



void wait_stop() {
   while (!press_stop());
}



void wait_start() {
   while (!press_start());
}



int press_start() {
   if (!start_button()) 
      return 0;

   while (start_button());

   return 1;
}



int press_stop() {
   if (!stop_button()) 
      return 0;

   while (stop_button());

   return 1;
}



float degrees() {
   return (theta / lsb_ms_per_deg);
}



int same_side() {
   if (sign((float)my_y()) == sign((float)his_y()))
      return 1;
   else 
      return 0;
}



void display_stats() {
   if (rf_vote_winner == BALL_COLOR)
      if (BALL_COLOR == RED)
         printf("COLOR: RED - WINNER: RED - Keeping\n");
      else
         printf("COLOR: GREEN - WINNER: GREEN - Keeping\n");
   else
      if (BALL_COLOR == RED)
         printf("COLOR: RED - WINNER: GREEN - Giving\n");
      else
         printf("COLOR: GREEN - WINNER: RED - Giving\n");
}