/********************
 * Find Orientation *
 ********************/

/*Functions:
  orientation        returns which orientation
  comparelight       returns sum of squares
  getlightreading    obtains light reading

/* 0 = Forward, 1 = Left, 2 = Back, 3 = Right; Forward is facing other robot*/

int reading[4];

int orientation() {
  int direction;
  int Fval,Lval,Bval,Rval;

  reading[0] = light0;
  reading[1] = light1;
  reading[2] = light2;
  reading[3] = light3;
  
  /*  Fval = comparelight(reading,F);
      Lval = comparelight(reading,L);
      Bval = comparelight(reading,B);
      Rval = comparelight(reading,R); */
    
  Fval = fabs(reading[0]-F[0]) + fabs(reading[1]-F[1]) + fabs(reading[2]-F[2]) + fabs(reading[3]-F[3]);
  Lval = fabs(reading[0]-L[0]) + fabs(reading[1]-L[1]) + fabs(reading[2]-L[2]) + fabs(reading[3]-L[3]);
  Bval = fabs(reading[0]-B[0]) + fabs(reading[1]-B[1]) + fabs(reading[2]-B[2]) + fabs(reading[3]-B[3]);
  Rval = fabs(reading[0]-R[0]) + fabs(reading[1]-R[1]) + fabs(reading[2]-R[2]) + fabs(reading[3]-R[3]);

  if (Fval < Lval && Fval < Bval && Fval < Rval)
    direction = 0;
  else
    if (Lval < Bval && Lval < Rval)
      direction = 1;
    else
      if (Bval < Rval)
	direction = 2;
      else
	direction = 3;
	return direction; 
}


int comparelight(int one[], int two[]) { 
  return (fabs(one[0]-two[0]) + fabs(one[1]-two[1]) + fabs(one[2]-two[2]) + fabs(one[3]-two[3]));
}

int fabs(int number) {
  if(number < 0)
    return -1*number;
  return number;
}