/*globals*/
int black, white, side;


void getValues() {
  while(!(stop_button())) {
  white = readSensor(6); /*rr*/
  black = readSensor(5); /*rl*/
  printf("\nwhite=%d, black=%d",white, black);
  }
}


void orient() {
  int fl, rr, rl;
  int us, them, countMax;

  fl = readSensor(3);
  rr = readSensor(6);
  rl = readSensor(5);

  /*how many are black?*/
  countMax = threshold(fl, white, black) + threshold(rr, white, black) + threshold(rl, white, black);

  /*printf("\nrl=%d, rr=%d, fl=%d", threshold(rl, white, black), threshold(rr, white, black), threshold(fl, white, black));*/
  
  
  if (countMax == 0) {
    /*white side*/
    us = white;
    them = black;
  } else if (countMax == 1) {
    /*we're on the white side*/
    us = white;
    them = black;
  } else if (countMax == 2) {
    /*we're on the black side*/
    us = black;
    them = white;
  } else if (countMax == 3) {
    /*black side*/
    us = black;
    them = white;
  } else {
    /*This should not happen*/
  }
  
  if (us > them) { /*black side*/
    side = 1;
    if (threshold(fl, us, them)) {
       /*facing board*/
       printf("\nblack facing board");
       left90(0);
    } else if (threshold(rr, us, them)) {
       /*facing away from board*/
      printf("\nblack away from board");
      right90(0);
    } else if (threshold(rl, us, them)) {
       /*facing down*/
      printf("\nblack down");
      left180(0);
    } else {
      /*facing up*/
      printf("\nblackfacing up");
    }   
   } else {  /*white side*/
     side = 0;
     if (threshold(fl, us, them)) {
        /*facing up*/
       printf("\nwhite up");
     } else if (threshold(rr, us, them)) {
        /*facing down*/
       printf("\nwhite down");
       left180(0);
     } else if (threshold(rl, us, them)) {
        /*facing away from board*/
       printf("\nwhite away from board");
       left90(0);
     } else {
       /*facing board*/
       printf("\nfacing board");
       right90(0);
     }
}
}


/*
input: sample value, false value, true value 
output: outputs a 1 if sample is closer to true value, 0 otherwise 
*/

int threshold(int sample, int falseValue, int trueValue) {
  int trueDist = abs(sample - trueValue);
  int falseDist = abs(sample - falseValue);
  if (trueDist < falseDist) {
    return 1;
  } else {
    return 0;
  }
}

int readSensor(int port) {
  int bad;
  bad = analog(port);
  return analog(port);
}