/*
 * This is the mapping function for the group project. It is recursive,
 * calling itself after it has finished one pass.  The function takes the
 * direction it's going as an argument.  It returns nothing.  The function 
 * stops when the bot has mapped the outermost boundary.
 * 
 * vars (in order of appearance):
 * direction: 0 if left, 1 if right
 * distance: the distance that should be kept from the beacon on each
 * pass.
 * correction: the amount of correction that needs to made when the bot
 * gets too far away from or too close to the beacon
 * floor_tile_size: global int; length of time it takes to cross a floor tile
 * left: global int const = 0
 * right: global int const = 1
 */

/* definitions in binary terms of hazards */
int obstacle = 0;
int bad_spot = 1;

/* amount of time it takes to turn 90 degrees this is a guess and varies for each bot.*/
float time_for_90 = 1.5;

/* whether the bot goes left or right upon hitting the light */
int left_or_right;

/* arrays that store data.  the max is 100.*/
int distance_from_light[100];
int angle_from_light[100];
int array_position = 0;
/* locks for arrays */
int distancelock = 0;
int anglelock = 0;

/* drop off point coordinates */
int drop_angle = 90;
int drop_distance = 300;

/* directions */
int left = 0;
int right = 1;

/* outer bound in cm*/
int MAX_DISTANCE = 300;

void map(int direction, int distance) {
   
  /* difference between distance to beacon and ideal distance to beacon */
  int correction;

  /* process IDs for threads */
  int forward_thread;
  int find_obstacles_thread;
  int find_bad_spots_thread;
  
  /* angle and distance pointers */
  int *angle;
  int *distance;

  /* move forward for the amount of time it takes to cross a floor tile */
  forward(floor_tile_size);
  /* turn opposite direction for 90 degrees*/
  if (direction == right)
    turn_right(time_for_90);
  else
    turn_left(time_for_90);
  
  /* move forward until bot is under polarized light */
  /* at this point we will mess around with threads, because we want to move
   * forward while a bunch of other things are happining.*/
  forward_thread = start_process(forward(32000));
  find_obstacles_thread = start_process(find_obstacles());
  find_bad_spots_thread = start_process(find_bad_spots());
    
  while(1)
    {
      /* keep track of distance from unpolarized light */
      if (direction == left) {
	correction = distance - light_distance(right_sensor());
      }
      else {
	correction = distance - light_distance(left_sensor());
      }
      /* make correction */
      if (correction != 0) {
	correct(correction, direction);
      }

      /* are we still in bounds? if we aren't, then we're done. */
      if (distance < MAX_DISTANCE) {
	/* are we next to the wall, or far away from it? */
	get_position(&angle, &distance);
	if ( (angle < 10) || (angle > 170) ) {
	  /* next to the wall, call map one more time to move us back toward the drop point */
	  if (direction == left) 
	    map(right, distance + floor_tile_size);
	  else
	    map(left, distance + floor_tile_size);
	} 
      }
      else {
	/* we are at the drop point */
	/*  kill threads */
	kill_process(forward_thread);
	kill_process(find_obstacles_thread);
	kill_process(find_bad_spots_thread);
	/* and go back */
	break;
      }
    }
}
/* 
 * find_obstacles determines whether we are just in front of an obstacle.
 * if we are, then get_position is called.
 */
void find_obstacles() {
  /* distance bot must be from object */
  int obstacle_distance = 10;
  
  /* get the IR pulse */
  int distance = pulse(1);
  distance = minimize(distance);
  /* if we are within obstacle_distance of the obstacle, record the position */
   if (distance <= obstacle_distance) {
     get_position(&angle, &distance);
     if (anglelock) {
       sleep(.2);
     }
     else {
       anglelock = 1;
       angle_from_light[array_position] = &angle;
       anglelock = 0;
     }
     if (distancelock) {
       sleep(.2);
     }
     else {
       distancelock = 1;
       distance_from_light[array_position] = &distance;
       distancelock = 0;
     }
     array_position++;
   }
}

/* 
 * find_bad_spots looks on the ground and determines if the bot is rolling over
 * space that is harmful for people. 
 */
void find_bad_spots() {
  int bad_spot = redsense();

     get_position(&angle, &distance);
     if (anglelock) {
       sleep(.2);
     }
     else {
       anglelock = 1;
       angle_from_light[array_position] = &angle;
       anglelock = 0;
     }
     if (distancelock) {
       sleep(.2);
     }
     else {
       distancelock = 1;
       distance_from_light[array_position] = (&distance * -1);
       distancelock = 0;
     }
     array_position++;
}

/* 
 * correct makes the correction needed to keep the bot on track, as close as 
 * possible to the correct distance from the beacon.  essentially it is turning
 * briefly toward or away from the light, depending on which way it is off
 */
void correct(int correction, int direction) {
 
  /* if we're going left,*/
  if (direction == left) {
    /* and are too far away,*/ 
    if (correction > 0) {
      /* change the motor speed to produce an artificial turn */
      sleep(.3);
      /* change motor back */
    }
    /* or are too close */
    if (correction < 0) {
      /* change the motor speed to produce an artificial turn */
      sleep(.3);
      /* change motor back */
    }
  }
  /* if we're going right */
  if (direction == right) {
    /* change the motor speed to produce an artificial turn */
    if (correction > 0) {
      /* change the motor speed */
      sleep(.3);
      /* change motor back */
    }
    /* or are too close */
    if (correction < 0) {
      /* change the motor speed to produce an artificial turn */
      sleep(.3);
      /* change motor back */
    }
  }
}