/* orient.c */

/* 0: facing other side 
   1: facing ball
   2: facing own side
   3: facing out
   -1: cannot determine */

int our_color;

int find_orientation_led(int f)
{
  int n[3] = { 0, 0, 0 };
  int nw, ne, sw, se;

  nw = cds_get_color(PORT_CDS_FRONT_LEFT, f);
  n[nw]++;
  ne = cds_get_color(PORT_CDS_FRONT_RIGHT, f);
  n[ne]++;
  sw = cds_get_color(PORT_CDS_REAR_LEFT, f);
  n[sw]++;
  se = cds_get_color(PORT_CDS_REAR_RIGHT, f);
  n[se]++;

  if (f != 1)
  {
    printf("%d %d %d %d\n", nw, ne, sw, se);
    start_press();
  }

  if (n[CDS_WHITE] == 3 && n[CDS_BLUE] == 1)
    our_color = CDS_WHITE;
  else if (n[CDS_BLUE] == 3 && n[CDS_WHITE] == 1)
    our_color = CDS_BLUE;
  else
    return -1;

  if (nw != our_color)
    return 0;
  else if (ne != our_color)
    return 1;
  else if (se != our_color)
    return 2;
  else if (sw != our_color)
    return 3;
}

int find_orientation_ir()
{
  sleep(0.5);
  if (digital(PORT_IR_W))
    return 0;
  else if (digital(PORT_IR_N))
    return 1;
  else if (digital(PORT_IR_E))
    return 2;
  else if (digital(PORT_IR_S))
    return 3;
  else
    return -1;
}

void orient(int orientation)
{
  if (orientation == 0)
  {
    nav_set_pos(-DIST_CDS_START_X, -18.0, 0.0);
    drive_dist(100, 16.0, 1);
    setup_turn(0.0, 100);
    while(nav_get_angle() < PI_2 || nav_get_angle() > PI)
    {
      nav_update_angle();
      msleep(10L);
    }
  }
  else if (orientation == 1)
  {
    nav_set_pos(0.0, -18.0-DIST_CDS_START_X, PI_2);
    setup_turn(-1.0, 1);
    sleep(0.2);
    setup_turn(-1.0, 100);
    while(nav_get_angle() < 1.4*PI_2)
      nav_update_angle();
    stop();
    turn_to(PI, 50, 1);    
    drive_dist(-100, 15.0, 1);
  }
  else if (orientation == 2)
  {
    nav_set_pos(DIST_CDS_START_X, -18.0, PI);
    drive_dist(-100, 13.0, 1);
  }
  else
  {
    nav_set_pos(0.0, -18.0+DIST_CDS_START_X, 3.0*PI_2);
    setup_turn(1.0, 1);
    sleep(0.2);
    setup_turn(1.0, -100);
    while(nav_get_angle() > 2.4*PI_2)
      nav_update_angle();
    stop();
    turn_to(PI, 50, 1);
    drive_dist(-100, 15.0, 1);
    turn_to(PI_2, 100, 1);
    align_wall(3, 6000L);
    drive_dist(100, 4.0, 1);
  }
}