int cs_nw_w=0, cs_nw_b=0,
    cs_ne_w=0, cs_ne_b=0,
    cs_sw_w=0, cs_sw_b=0,
    cs_se_w=0, cs_se_b=0;

void color_sensor_config() {
  /* Calibrate color sensors */

  int i;
  long start_time;

  printf("\nPut NW on whitePut SE on black.\n");
  button(START);
  threadsafe_sleep(0.4);
  for (i = 0; i < 6; i++) {
    threadsafe_sleep(0.1);
    cs_nw_w += analog(COLOR_NW);
    cs_se_b += analog(COLOR_SE);
  }
  cs_nw_w /= 6; cs_se_b /= 6;

  printf("\nPut SW on whitePut NE on black.\n");
  button(START);
  threadsafe_sleep(0.4);
  for (i = 0; i < 6; i++) {
    threadsafe_sleep(0.1);
    cs_sw_w += analog(COLOR_SW);
    cs_ne_b += analog(COLOR_NE);
  }
  cs_sw_w /= 6; cs_ne_b /= 6;

  printf("\nPut SE on whitePut NW on black.\n");
  button(START);
  threadsafe_sleep(0.4);
  for (i = 0; i < 6; i++) {
    threadsafe_sleep(0.1);
    cs_se_w += analog(COLOR_SE);
    cs_nw_b += analog(COLOR_NW);
  }
  cs_se_w /= 6; cs_nw_b /= 6;

  printf("\nPut NE on whitePut SW on black.\n");
  button(START);
  threadsafe_sleep(0.4);
  for (i = 0; i < 6; i++) {
    threadsafe_sleep(0.1);
    cs_ne_w += analog(COLOR_NE);
    cs_sw_b += analog(COLOR_SW);
  }
  cs_ne_w /= 6; cs_sw_b /= 6;
}

int pick_color(int port, int w, int b) {
  int reading = analog(port);
  if (abs(reading - w) < abs(b - reading))
    return WHITE;
  else
    return BLACK;
}

void orient() {
  int nw, sw, ne, se;
  int count[2];
  long st;

  /* Determine color */
  nw = pick_color(COLOR_NW, cs_nw_w, cs_nw_b);
  ne = pick_color(COLOR_NE, cs_ne_w, cs_ne_b);
  sw = pick_color(COLOR_SW, cs_sw_w, cs_sw_b);
  se = pick_color(COLOR_SE, cs_se_w, cs_se_b);

  printf("\n%d %d            %d %d\n", nw, ne, sw, se);
  /* stop_press();*/

  count[nw]++; count[ne]++; count[sw]++; count[se]++;
  if (count[BLACK] > count[WHITE]) {
    /* We're black. */
    MYCOLOR = BLACK;
    /*
    if (abs(rf_y0 - START_BY) < abs(rf_y1 - START_BY)) {
      rf_x_us = &rf_x0; rf_y_us = &rf_y0;
      rf_x_them = &rf_x1; rf_y_them = &rf_y1;
    } else {
      rf_x_us = &rf_x1; rf_y_us = &rf_y1;
      rf_x_them = &rf_x0; rf_y_them = &rf_y0;
    }
    */
    if (nw == WHITE) {
      printf("\nBlack - turn L\n");
      turn_left();
    } else if (se == WHITE) {
      printf("\nBlack - turn R\n");
      turn_right();
    } else if (sw == WHITE) {
      printf("\nBlack - turn\n");
      turn_around();
    }
  } else {
    /* We're white. */
    MYCOLOR = WHITE;
    if (abs(rf_y0 - START_WY) < abs(rf_y1 - START_WY)) {
      our_num = 0;
    } else {
      our_num = 1;
    }
    if (ne == BLACK) {
      printf("\nWhite - turn R\n");
      turn_right();
    } else if (sw == BLACK) {
      printf("\nWhite - turn L\n");
      turn_left();
    } else if (se == BLACK) {
      printf("\nWhite - turn\n");
      turn_around();
    }
  }
}