/* RETURN VALUES */
#define WHITE_NORTH 0
#define WHITE_SOUTH 1
#define WHITE_EAST 2
#define WHITE_WEST 3
#define BLACK_NORTH 4
#define BLACK_SOUTH 5
#define BLACK_EAST 6
#define BLACK_WEST 7
#define WHITE_UNKNOWN 8
#define BLACK_UNKNOWN 9
#define CRITICAL_FAILURE 10

#define SIDE_WHITE 0
#define SIDE_BLACK 1


/* ORIENT/SPIN HARDCODES */
#define ANGLE_90 10 /* 10 */
#define ANGLE_180 20 /* 22 */
#define ANGLE_270 33
#define SPIN_SPEED 50
#define SPIN_ANGLE 35

/*SENSOR HARDCODES */
#define SENSOR_THRESHOLD 70
#define SENSOR_LEFT_WHEEL 5
#define SENSOR_RIGHT_WHEEL 6
#define BUMP_LEFT_WHEEL 11
#define BUMP_RIGHT_WHEEL 10

/* PLANS */

#define PLAN_A 1
#define PLAN_B 2
#define PLAN_C 3


int orient() {
  /* Spins robot and returns side number */
  int state=-1;
  
  state = orient_direction();

  enable_servos();
  init_drive();
  init_hinge();
  close_hinge();

  if (state==WHITE_NORTH || state==BLACK_NORTH) {
    orient_spin(ANGLE_180,1);
  }
  else if (state==WHITE_EAST || state==BLACK_EAST) {
    orient_spin(ANGLE_90,1);
  }
  else if (state==WHITE_SOUTH || state==BLACK_SOUTH) {
    /* Woo-hoo, we've got it easy this round. */
  }
  else if (state==WHITE_WEST || state==BLACK_WEST) {
    orient_spin(ANGLE_270,1);
  }
  else {
    /* We're Fucked.  Might as well assume south! */
  }

  if (state==WHITE_NORTH ||
      state==WHITE_SOUTH ||
      state==WHITE_EAST ||
      state==WHITE_WEST) 
    {
      return SIDE_WHITE;
    }
  else if (state==BLACK_NORTH ||
	   state==BLACK_SOUTH ||
	   state==BLACK_EAST ||
	   state==BLACK_WEST) 
    {
      return SIDE_BLACK;
    }
  else {
    /* We're still fucked.  Assume White.  Hope for the best! */
    printf("CRITICAL       FAILURE.\n");
    return SIDE_WHITE;
  }
}

void orient_spin(int endval, int direction) {
  /* Orients the robot based on a shaft encoder count.
     use 90_DEG and 180_DEG, respectively.  
     Direction is best positive, but works negative.
  */
  
  int count = 0 ;;

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);
  
  servo(DRIVE_RIGHT_SERVO,DRIVE_RIGHT_SERVO_CENTER-(20*(SPIN_ANGLE)));
  servo(DRIVE_LEFT_SERVO,DRIVE_LEFT_SERVO_CENTER+(21*(SPIN_ANGLE)));
  drive_spin(SPIN_SPEED * direction);
  while(read_encoder(7)+read_encoder(8) < endval && count++ < 200) {
    sleep(.05);
  }
  drive_spin(0);
}

int orient_direction() {
  /* Returns orientation from sensor array.

     sv[] is actual values
     rv[] is boolean values:
       0 is White.
       1 is Black.
     
     *******************************
     ASSUMES SENSOR ARRAY USES 17-20
     *******************************
  */
 
  int sv[4];
  int rv[4];
  int i;

  
  printf("Reading. . .\n");
  for (i=0;i<4;i++) {
    analog(17+i); /* Change this if sensor plugs change */
    sv[i] = analog(17+i);
    if (sv[i] > SENSOR_THRESHOLD) { 
      rv[i] = 1;
	}
    else {
      rv[i] = 0;
	}
  }
  printf("%d %d %d %d | ", sv[0], sv[1], sv[2], sv[3]);
  if (rv[0]==rv[1] && rv[1]==rv[2] && rv[2]==rv[3]) {
    printf("All Same.\n");
    return CRITICAL_FAILURE;
  }
  else if (sum4(rv)==2) {
    printf("2 & 2 Same.\n");
    return CRITICAL_FAILURE;
  }
  else if (sum4(rv)==1) {
    for (i=0;i<4;i++) {
      if (rv[i]==1)
	break;
    }
    
    if(i == 0) {
      printf("White. East.\n");
      return WHITE_EAST;
    }
    else if(i == 1) {
      printf("White. North.\n");
      return WHITE_NORTH;
    }
    else if(i == 2) {
      printf("White. West.\n");
      return WHITE_WEST;
    }
    else if(i == 3) {
      printf("White. South.\n");
      return WHITE_SOUTH;
    }
    else {
      printf("White. ???\n", i);
      return WHITE_UNKNOWN;
    }
  }
  else if (sum4(rv)==3) {
    for(i = 0; i < 4; i++) {
      if(rv[i] == 0)
	break;
    }

    if(i == 0) {
      printf("Black. North.\n");
      return BLACK_NORTH;
    }
    else if(i == 1) {
      printf("Black. West.\n");
      return BLACK_WEST;
    }
    else if(i == 2) {
      printf("Black. South.\n");
      return BLACK_SOUTH;
    }
    else if(i == 3) {
      printf("Black. East.\n");
      return BLACK_EAST;
    }
    else {
      printf("Black. ???\n", i);
      return BLACK_UNKNOWN;
    }
  }
}

void sensor_bumpstop(int sensnum) {
  while(!digital(sensnum)) {}
  ao();
}

int sum4(int array[]) {
  int i;
  int sum = 0;
  for (i=0;i<4;i++) {
    sum = sum + array[i];
  }
  return sum;
}