/* orient.c */

/*
   This file contains the code necessary to orient the robot
   from a random starting position.
*/

/* Turn the robot to face the block */
void face_block()
{
  int dir;

  dir = direction();

  if (dir != 1)
    {
      if (dir == 3)
	left_180();
      
      if (dir == 2)
	left_90();
      
      if (dir == 4)
	right_90();
    }
} /* END face_block() */


/* This function orients the robot to the direction
   indicated.*/
int orient()
{
  int dir;  
  int counter = 0;

  printf("\nOrienting...    ");
  
  if (color == 3)
    {
      printf("I am Black\n");
      /* start_press();
      dir = direction();
      
      printf("Direction is %d\n", dir);
      start_press();
      */
      dir = direction();

      if (dir != 4)
	{
	  if (dir == 1)
	    left_90();

	  if(dir == 2)
	    left_180();

	  if(dir == 3)
	    right_90();
	}
      
    }
  else
    {
      printf("I am White\n");

      dir = direction();

      if (dir != 2)
	{
	  if (dir == 1)
	    right_90();
	  
	  if(dir == 3)
	    left_90();

	  if(dir == 4)
	    right_180();
	}
      
    }
} /* END orient() function */


/* 
   This function returns a value that helps determine which
   player (black/white) the robot is.

   If the returned value is 1, the one of four sensors  is over
   a black area which would lead to the conclusion that the 
   robot is white player. 

   If the returned value is 3, the three of the four the sensors
   are over black areas, which would lead to the conclusions that 
   the robot is the black player.

   If any other value is return (0 or 2) then it is up to the
   caller of the function to determine what to do.
*/
int whoami()
{
  int total = 0;
  
  if (analog(FRONT_LEFT) > FL_threshold)
    total++;
  
  if (analog(FRONT_RIGHT) > FR_threshold)
    total++;
  
  if (analog(REAR_LEFT) > RL_threshold)
    total++;

  if (analog(REAR_RIGHT) > RR_threshold)
    total++;
  
  return total;
  
} /* END int whoami() function */


/* 
   This function returns a rough indicator of which direction the
   robot is facing

   arg: color = 0 indicates Black, 1 indicates White.
*/
int direction()
{
  if (color == 3)
    { /* determine direction for black player */
      /* this may need to be made more robust */

      if ( (analog(FRONT_LEFT) < FL_threshold) 
	   && (analog(FRONT_RIGHT) > FR_threshold)
           && (analog(REAR_LEFT) > RL_threshold) 
	   && (analog(REAR_RIGHT) > RR_threshold))
        return 1;
      
      if ( (analog(FRONT_LEFT) > FL_threshold) 
	   && (analog(FRONT_RIGHT) > FR_threshold)
           && (analog(REAR_LEFT) < RL_threshold) 
	   && (analog(REAR_RIGHT) > RR_threshold))
	return 2;

      if ((analog(FRONT_LEFT) > FL_threshold) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) > RL_threshold) 
	  && (analog(REAR_RIGHT) < RR_threshold))
        return 3;

      if ((analog(FRONT_LEFT) > FL_threshold) 
	  && (analog(FRONT_RIGHT) < FR_threshold)
	  && (analog(REAR_LEFT) > RL_threshold) 
	  && (analog(REAR_RIGHT) > RR_threshold))
        return 4;

      return 0;
    }
  else
    { /* determine direction for white player */
      /* this may need to be made more robust */
      
      if ((analog(FRONT_LEFT) < FL_threshold) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold) 
	  && (analog(REAR_RIGHT) < RR_threshold))
        return 1;
      
      if ((analog(FRONT_LEFT) > FL_threshold) 
	  && (analog(FRONT_RIGHT) < FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold) 
	  && (analog(REAR_RIGHT) < RR_threshold))
        return 2;

      if ((analog(FRONT_LEFT) < FL_threshold) 
	  && (analog(FRONT_RIGHT) < FR_threshold)
	  && (analog(REAR_LEFT) > RL_threshold) 
	  && (analog(REAR_RIGHT) < RR_threshold))
        return 3;

      if ((analog(FRONT_LEFT) < FL_threshold) 
	  && (analog(FRONT_RIGHT) < FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold) 
	  && (analog(REAR_RIGHT) > RR_threshold))
        return 4;

      return 0;
    }
  
} /* END int direction() function */