long time0 = 0L;
int t = 0;
int line_followID = 0;
int read_boardID = 0;

int RIGHT_MOTOR = 1;
int RIGHT_MOTOR_PWR = 60;
int LEFT_MOTOR = 2;
int LEFT_MOTOR_PWR = 100;

int FRONT_MOTOR = 3;
int FRONT_MOTOR_PWR = 100;

int FRONT_COLOR_SENSOR = 24;
int RIGHT_COLOR_SENSOR = 28;
int LEFT_COLOR_SENSOR = 26;
int SENSOR_QUERY_PER_READ = 10; 

int REAR_RIGHT_BUMP_SENSOR = 16;
int REAR_LEFT_BUMP_SENSOR = 30;
int REAR_MIDDLE_BUMP_SENSOR = 20;

int BALL_PLACE_SENSOR = 19;
int BALL_COLOR_SENSOR = 29;
int LEFT_DISTANCE_SENSOR = 5;
int RIGHT_DISTANCE_SENSOR = 4;

int BUMPER_FRONT_RIGHT = 18;
int BUMPER_FRONT_LEFT = 22;
int BUMPER_BACK_RIGHT = 17;
int BUMPER_BACK_LEFT = 21;


int RIGHT_GATE_SERVO = 0;
int LEFT_GATE_SERVO = 1;
int RIGHT_GATE_START = 615;
int RIGHT_GATE_READY = 2190;
int RIGHT_GATE_CRITICAL = 2580;
int RIGHT_GATE_DUMP = 2805;
int RIGHT_GATE_OPEN = 3090;
int LEFT_GATE_START = 3450;
int LEFT_GATE_READY = 2025;
int LEFT_GATE_CRITICAL = 1620;
int LEFT_GATE_DUMP = 1395;
int LEFT_GATE_OPEN = 1080;

#define START_LIGHT_PORT 31


int TRUE = 1;
int FALSE = 0;
int DEBUG = TRUE;

int LIGHT_DARK_THRESHOLD = 120;
int BALL_THRESHOLD = 75;
int DARK = 0;
int BLACK = 0;
int WHITE = 1;
int LIGHT = 1;
int TEAM_COLOR = 0;


int IR_N = 15;
int IR_E = 14;
int IR_S = 13;
int IR_W = 12;

long TICK = 500L;
long TURN_TICK = 500L;
long SERVO_TICK = 250L;

int WWW = 0;
int WWB = 1;
int WBW = 2;
int WBB = 3;
int BWW = 4;
int BWB = 5;
int BBW = 6;
int BBB = 7;

int READ_STATE = WWW;
int PREV_STATE = WWW;
int PHASE = 0;

int MOTOR_PWR = 100;

int getballID = 0;
int readboardID = 0;
int wallfollowID = 0;

int RIGHT_DISTANCE_THRESHOLD = 120;
int LEFT_DISTANCE_THRESHOLD = 120;

void main(){
  enable_servos();
  set_servos_start();
  msleep(500L);

  ir_transmit_off();
  start_machine(START_LIGHT_PORT);

  printf("GO!!!\n");
  routine();
}


void routine() {

  TEAM_COLOR = color_read(LEFT_COLOR_SENSOR);
  if (TEAM_COLOR == BLACK)
    printf("TEAM BLACK\n");
  else
    printf("TEAM WHITE\n");
  enable_servos();
  set_servos_ready();

  time0 = mseconds() + 2000L;
  while (time0 > mseconds()) {
    READ_STATE = color_read(FRONT_COLOR_SENSOR);
    if (TEAM_COLOR == BLACK){
      if (READ_STATE == WHITE)
	forward__(75,100);
      else
	forward__(100,75);
    } else {
      if (READ_STATE == WHITE)
	forward__(100,75);
      else
	forward__(75,100);
    }
  }
  
  ao();

  
  if (TEAM_COLOR == BLACK){
    hard_right();
    msleep(600L);  
  } else {
    hard_left();
    msleep(700L);
  }

  ao();

  
  /*adjust tendency*/
  forward__(100,90);

  while(sensor_read(LEFT_DISTANCE_SENSOR) < 60);
  ao();

  
  if (TEAM_COLOR == BLACK)
    hard_left_(75);
  else
    hard_right_(75);
  
  while(color_read(FRONT_COLOR_SENSOR) != BLACK);
  ao();
  
  if (TEAM_COLOR == BLACK) {
    while ( sensor_read(RIGHT_DISTANCE_SENSOR) < RIGHT_DISTANCE_THRESHOLD ) {
      if (color_read(FRONT_COLOR_SENSOR) == DARK) {
	right();
      } else if (digital(BUMPER_FRONT_RIGHT) == 1) {
	forward__(90, 100);
      } else if (digital(BUMPER_FRONT_LEFT) == 1) {
	forward__(100,90);
      }
      else 
	forward__(100,80);
    }
  } else {
    while ( sensor_read(LEFT_DISTANCE_SENSOR) < LEFT_DISTANCE_THRESHOLD ) {
      if (color_read(FRONT_COLOR_SENSOR) == DARK) {
	left();
      } else if (digital(BUMPER_FRONT_RIGHT) == 1) {
	forward__(90, 100);
      } else if (digital(BUMPER_FRONT_LEFT) == 1) {
	forward__(100,90);
      }
      else 
	forward__(80,100);
    }
  }
  
  stop();


  start_harvester();
  while (have_ball() == FALSE);
  stop_harvester();

  forward_(-80);
  msleep(800L);
  ao();

  if (TEAM_COLOR == BLACK)
    hard_left_(75);
  else
    hard_right_(100);
  
  while(color_read(FRONT_COLOR_SENSOR) == LIGHT);
  while(color_read(LEFT_COLOR_SENSOR) != color_read(RIGHT_COLOR_SENSOR));
  ao();
  forward__(90,100);

  msleep(3500L);
  ao();
}  


int have_ball(){
  if (sensor_read(BALL_PLACE_SENSOR) > 100)
    return TRUE;
  
  return FALSE;
}

int color_read(int port){
  if (sensor_read(port) > LIGHT_DARK_THRESHOLD)
    return DARK;
  else
    return LIGHT;
}

int ball_read(){
  if (sensor_read(BALL_COLOR_SENSOR) > BALL_THRESHOLD)
    return DARK;
  else
    return LIGHT;
}

int sensor_read(int port){
  int sum = analog(port);
  int i;
  sum = 0;
  for (i = 0; i < SENSOR_QUERY_PER_READ; i++) sum += analog(port);
  return (int) sum/SENSOR_QUERY_PER_READ;
}

int digital_read(int port){

  if (sensor_read(port) > 128)
    return TRUE;
  else
    return FALSE;
}


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 (start_button());
  return 1;
}

void forward() {
  motor(LEFT_MOTOR, LEFT_MOTOR_PWR);
  motor(RIGHT_MOTOR, RIGHT_MOTOR_PWR);
}

void forward_(int pwr) {
  motor(LEFT_MOTOR,pwr);
  motor(RIGHT_MOTOR,pwr);
}

void forward__(int lpwr, int rpwr) {
  motor(LEFT_MOTOR,lpwr);
  motor(RIGHT_MOTOR,rpwr);
}

void backward() {
  bk(LEFT_MOTOR);
  bk(RIGHT_MOTOR);
}

void backward_(int pwr) {
  motor(LEFT_MOTOR, -pwr);
  motor(RIGHT_MOTOR, -pwr);
}

void stop() {
  off(LEFT_MOTOR);
  off(RIGHT_MOTOR);
}

void right(){
  fd(LEFT_MOTOR);
  off(RIGHT_MOTOR);
}

void left(){
  fd(RIGHT_MOTOR);
  off(LEFT_MOTOR);
}

void hard_left(){
  bk(LEFT_MOTOR);
  fd(RIGHT_MOTOR);
}

void hard_right(){
  fd(LEFT_MOTOR);
  bk(RIGHT_MOTOR);
}

void hard_left_(int pwr){
  motor(LEFT_MOTOR,-pwr);
  motor(RIGHT_MOTOR,pwr);
}

void hard_right_(int pwr){
  motor(LEFT_MOTOR,pwr);
  motor(RIGHT_MOTOR,-pwr);
}

void start_harvester(){
  motor(FRONT_MOTOR, FRONT_MOTOR_PWR);
}

void stop_harvester(){
  off(FRONT_MOTOR);
}

void set_servos_start(){
  servo(RIGHT_GATE_SERVO, RIGHT_GATE_START);
  servo(LEFT_GATE_SERVO, LEFT_GATE_START);
}

void set_servos_ready(){
  servo(RIGHT_GATE_SERVO, RIGHT_GATE_READY);
  servo(LEFT_GATE_SERVO, LEFT_GATE_READY);
}

void servos_easy_dump(){
  servo(RIGHT_GATE_SERVO, RIGHT_GATE_CRITICAL);
  servo(LEFT_GATE_SERVO, LEFT_GATE_CRITICAL);
  msleep(SERVO_TICK);
  servo(RIGHT_GATE_SERVO, RIGHT_GATE_DUMP);
  servo(LEFT_GATE_SERVO, LEFT_GATE_DUMP);
  msleep(SERVO_TICK);
  servo(RIGHT_GATE_SERVO, RIGHT_GATE_OPEN);
  servo(LEFT_GATE_SERVO, LEFT_GATE_OPEN);
  msleep(SERVO_TICK);
}

void servos_fast_dump(){
  servo(RIGHT_GATE_SERVO, RIGHT_GATE_OPEN);
  servo(LEFT_GATE_SERVO, LEFT_GATE_OPEN);
  msleep(SERVO_TICK);
  msleep(SERVO_TICK); 
  msleep(SERVO_TICK);
  msleep(SERVO_TICK);
}

void forward_crack(){
  if (TEAM_COLOR == BLACK){
    if (PREV_STATE == WHITE)
      forward__(100,75);
    else
      forward__(75,100);
  } else {
    if (PREV_STATE == WHITE)
      forward__(75,100);
    else
      forward__(100,75);
  }
}