/* This will soon be the mappingcode. 4/30 */

/*
  Map takes a direction as an argument.  This is the original
  direction the robot must turn to begin mapping. The basic idea is to
  move forward for the length of a floor tile, then turn to face out
  and move forward, maintaining a constant ideal distance from the
  beacon.  The ideal distance is incremented in for loop by the length
  of a floor tile (30cm) until it reaches the maximum distance (300
  cm).  A while loop nested within the for loop continuously checks
  for obstacles.  When one is found, its position is obtained and
  recorded.  If the obstacle is a wall, the program breaks from the
  while loop and continues with the for loop.
*/

int map() {
  /* pids of threads */
  int forward_thread;
  int badspot_thread;
  int first_time_through = 1;

  /* the ideal distance */
  int ideal_dist;
  int distance;
  
  /* the type of obstacle returned by find_obstacle.  0 = nothing, 1 =
     obstacle, 2 = wall. */
  int object = 0;
  
  /* start loop */
  for (ideal_dist = (int) floor_tile_size;
       ideal_dist <= MAX_DISTANCE;
       ideal_dist += 30) {
    
    /* turn first, alongside wall */
    if (first_time_through) {
      if (orientation == 0) {
	turn_left( (time_for_90), 0);
      }
      else {
	turn_right( (time_for_90), 0);
      }
      first_time_through = 0;
    }
    /*
    else {
      if (orientation == 0) {
	turn_left( (time_for_90 * 2.0), 0);
      }
      else {
	turn_right( (time_for_90 * 2.0), 0);
      }
    }
    */
    /* move forward the length of a floor tile */
    /*    advance((int)floor_tile_size, 0);*/
    advance(floor_tile_size, 0);

    /* turn again, away from wall */
    if (orientation == 0) {
      turn_left(time_for_90 ,0);
    }
    else {
      turn_right(time_for_90, 0);
    }
  /* start finding bad spots */
    badspot_thread = start_process(find_bad_spots(ideal_dist));
    
    /* start moving forward for indeterminate length of time. */
    printf("starting forward thread.\n");
    /*start_press();*/
    /* move around in an arc */
    forward_thread = start_process(follow_arc(ideal_dist));
    /* as long as the object found is not a wall */
    while (1) {
      distance = pulse(1);
      object = find_obstacles(ideal_dist, distance);
      if (object != 0) {
	printf("storing obstacle.\n");
      }
      /* it's a wall */
      if ( object >= 2 ) {
	break;
      }
      /*      printf("no obstacle found.\n");*/
    }
    
    printf("killing forward.\n");
    /* since we're at a wall, we don't want to keep moving forward */
    kill_process(forward_thread);
    kill_process(badspot_thread);
    alloff();
    
    /* change orientation */
    if (orientation == 0) {
      orientation = 1;
    }
    else orientation = 0;
    
    printf("orientation: %d\n", orientation);
    printf("ideal distance: %d", ideal_dist+ideal_dist);
    /*start_press();*/
    if (start_button()) {
	kill_process(forward_thread);
	kill_process(badspot_thread)
	  return 1;
      }
  } /* end for */
  return_to_drop();
  return 1;  
} /* end map */

/*
 * Inherent problems: Some issues with thread management can be solved
 * with global blocking variables.  Some functions are going to have
 * to be modified to account for changes in this code, most notably
 * store_value and find_obstacls.  The fixes for those functions
 * should involve merely making *angle and *distance global ints
 * instead of pointers to ints.
 */

/* 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 ideal_dist) {
  while (1) {
    int bad_spot = redsense();
    if (bad_spot) 
      { 
	angle = -90;
	alloff();
	pause = 1;
	get_position(ideal_dist);
	store_value(0);
      }
    pause = 0;
    sleep(4.0);
  }
}

/* find_obstacles determins whether we are just in from of an
 * obstacle. If we are, then get_position is called. */
int find_obstacles(int ideal_dist, int old_dist) {
  int distance;
  int object = 0;

  sleep(.4);
  /* get the IR pulse */
  distance = pulse(1);
  
  /*distance = calibrate_ir(distance);*/
  
  /* if we are within obstacle_distance of the obstacle, record the
     position */
  /* reset angle */
  angle = -90;

  if (distance >= obstacle_distance) {
    /* double checking */
    if (distance >= obstacle_distance) {
    printf("distance: (find) %d", distance);
    printf("ob_dist: %d", obstacle_distance);
    pause=1;
    alloff();
    /*start_press();*/
    object = get_position(ideal_dist);
    }
  }
  pause = 0;
  return object;
}

void store_value(int type) {
  if (array_position >= 100) {
    printf("array full\n");
  }
  else { 
    printf("storing in %d\n", array_position);
    /*start_press();*/
    while(arraylock) {
      printf("array locked\n");
      sleep(.2);
    }
  
    arraylock = 1;
    angle_from_light[array_position] = angle;
      
    if (type == 0) {
      distance_from_light[array_position] = (distance * -1);
    }
    else {
      distance_from_light[array_position] = distance;
    }
    
    arraylock = 0;
    array_position++;
    printf("(%d, %d)\n", angle, distance);
    /*start_press();*/
  }
}

int get_position(int ideal_dist){

  int i=0;
  int j = 0;
  
  /* for divide-by-zero error checking */
  int error = 0;
  int pointing_threshold = 3; /* used for pointing back to the beacon */
  int number_of_mins = 0;
  int min_index[3] = {0,0,0};
  int flight_max = 250;
  int flight_max_index = -1;
  int uvalue_min = 0;
  int number_positive = 0;
  int threshold_positive = 20;
  float degrees_per_index = -1.0;
  int phi = -1;
  int angle_threshold;
  /* this is going to be a function call that grabs the appropriate
     calibration for this particular bot and this particular
     function. */
  int calibration = 0;
  int found = 0;
  
  /*populate the first SIZEARRAY elements of value_array*/
  /*turn an amount that will let us populate the whole array*/

  while(i<SIZEARRAY){
    motor(LMOTOR,100);
    motor(RMOTOR,-100); 
    msleep((long)(TURNSTEP));
    alloff();

    fvalue_array[i] = analog(FLIGHT);
    uvalue_array[i] = analog(PLIGHT);
    i++;
    
  }
      
  /*find if values are increasing or decreasing*/
  /*uvalue_array values are different from intensity values*/

  i=1;

  while(i < (SIZEARRAY)){

    if( (uvalue_array[i] - uvalue_array[(i-1)])>0){
      uslope_array[i] = 1;
    }
    
    if( (uvalue_array[i] - uvalue_array[(i-1)])<0){
      uslope_array[i] = -1;
    }
    if( (uvalue_array[i] - uvalue_array[(i-1)])==0){
      uslope_array[i] = 0;
    }
    
    i++;    
  }  

  i=1;

  while(i<SIZEARRAY){

    if((uvalue_array[i] > uvalue_min)&&(uslope_array[i]==1)){
      uvalue_min = uvalue_array[i];
      number_positive = 0;
    }    
    if(uslope_array[i]==-1){
      number_positive ++;
    }    
    if(number_positive>threshold_positive){
      if ((number_of_mins < 3) && (number_of_mins > -1)) {
	min_index[number_of_mins] = i-1-threshold_positive;
	number_of_mins ++; 
	uvalue_min = 0;
	number_positive = 0;
      }
    }
    if(number_of_mins>2){
      break;
    }
    i++;
  }
  
  /*now we should have the indices (in the uvalue_array) of three mins*/
  /*now we want to find the index of the max of the fvalue_array*/
  
  j = min_index[0];
  while(j < min_index[2]){ /* Really want = here? */
    
    if(fvalue_array[j] < flight_max){
      flight_max = fvalue_array[j];
      flight_max_index = j;
    }
    
    j++;
    
  }
 
  /*now we calculate the angle of the bot*/
  if ( (min_index[2] - min_index[0]) == 0)
    {
    printf("division by 0!\n");
    error = 1;
    }
  if ( (min_index[1] - min_index[0]) == 0)
    {
    printf("division by 0!\n");
    error = 1;
    }
  if ( (min_index[2] - min_index[1]) == 0)
    {
      printf("division by 0!\n");
      error = 1;
    }
  printf("calculating dpi\n");
  /*start_press();*/
  if (!error) {
    degrees_per_index = (1.0/3.0)*((180.0/((float)(min_index[1]-min_index[0])))+(180.0/((float)(min_index[2]-min_index[1])))+(360.0/((float)(min_index[2]-min_index[0]))));
    
    phi =(int)(((float)(flight_max_index-min_index[0]))*(degrees_per_index));
    
    if(phi>360){
      printf("calculating dpi again\n");
      /*start_press();*/
      degrees_per_index = 360.0/((float)(min_index[2]-min_index[0]));
      phi= (int)(((float)(flight_max_index-min_index[0]))* degrees_per_index) ;
    }
    
    
    if((0 <= phi)&&(phi<90)){
      angle = phi +90 + calibration;
    }
    
    if((90 <= phi)&&(phi<180)){
      angle = phi - 90 + calibration;
    }
    
    if((180 <= phi)&&(phi<270)){
      angle = phi - 90 + calibration;
    }
    
    if((270 <= phi)&&(phi<=360)){
      angle = phi - 270 + calibration;
    }
  } /* end if !error*/
  else {
    printf("Avoided divide by zero error.\n");
  }
  /*to get the radial distance we will have to calibrate this raw
    sensor value*/
  /*we will do this using a separate calibration function*/
  /*
    printf("distance: %d", distance);
    printf("angle: %d\n", angle);
    start_press();
    printf("flight_max_i = %d\n", flight_max_index);
    start_press();*/

  /* set the bot back facing the way it was */
  /* we already know max flight, so we can rotate directly to it. */

  while ( (analog(FLIGHT) - flight_max) > pointing_threshold) {
    motor(LMOTOR, 100);
    motor(RMOTOR, -100);
    msleep(90L);
    alloff();
  }  
  printf("\nfound light.\n");
  /*start_press();*/
 
  if (ideal_dist != 0) {
    angle_threshold=180-(int)(72.503*(float)ideal_dist^.1635);
  
    if (ideal_dist > MAX_DISTANCE) {
      if ((flight_max_index > -1) && (flight_max_index < 200))
	distance = light2distance(fvalue_array[flight_max_index],FLIGHT);
      else 
	printf("flight_max_index too high.  failed to compute.\n");
      store_value(1);
      return 3;
    }
    /* is it a wall?  which one? */
    if ( (angle < angle_threshold) && (orientation == 0) ) {
      if ((flight_max_index > -1) && (flight_max_index < 200))
	distance = light2distance(fvalue_array[flight_max_index],11);
      else 
	printf("flight_max_index too high.  failed to compute.\n");
      store_value(1);
      turn_right(time_for_90*2.0, 0);
      found = 1;
    }
    if ( (angle > 180-angle_threshold) && orientation == 1) {
      if ((flight_max_index > -1) && (flight_max_index < 200))
	distance = light2distance(fvalue_array[flight_max_index],10);
      else 
	printf("flight_max_index too high.  failed to compute.\n");
      store_value(1);
      turn_left(time_for_90*2.0, 0);
      found = 1;
    }
    if (found) {
      return 2;
    }
    /* it's an obstacle but not a wall */
    if ( !((angle < angle_threshold) || (angle > 180-angle_threshold)) ) {
      if ((flight_max_index > -1) && (flight_max_index < 200))
	distance = light2distance(fvalue_array[flight_max_index],FLIGHT);
      else 
	printf("flight_max_index too high.  failed to compute.\n");
      store_value(1);
      avoid_obstacle();
      return 1;
    }
    return 0;
  }
  else return 0;
} /* matches function */

int redsense(){
  bit_set(0x1000, 0x40);
  if (analog(0) <= red_threshold) {
    return 1;
  }
  else return 0;
}   

void avoid_obstacle() {
  alloff();
  if (orientation == 0) {
    turn_right(time_for_90, 0);
    advance(floor_tile_size, 0);
    turn_left(time_for_90, 0);
    advance(floor_tile_size, 0);
    turn_left(time_for_90, 0);
    advance(floor_tile_size, 0);
    turn_right(time_for_90, 0);
    alloff();
  } else {
    turn_left(time_for_90, 0);
    advance(floor_tile_size, 0);
    turn_right(time_for_90, 0);
    advance(floor_tile_size, 0);
    turn_right(time_for_90, 0);
    advance(floor_tile_size, 0);
    turn_left(time_for_90, 0);
    alloff();
  }
}

int follow_arc(int ideal_dist) {
  /* note for something to be added:  If we make on full turn without seeing
     the light with our side sensor (do to being to far away from an obstical)
     we may want to have it call a go back to radius function that will use
     the front light sensor to go back to the ideal_dist from the light and
     then turn (depending on which orientation we are going) and start up where
     we left off.
  */

  int turn_count = 0;
  int max_light = 300;
  
  /*printf("starting\n");*/
  sleep(1.0);

  while (1) {
    if (!pause) {
    if (orientation == 0) {
      /*printf("making left turn to light\n");*/
      /*  sleep(1.0); */
      /*turn left until we are perpendicular to the light*/
     
      motor(LMOTOR, 30);
      motor(RMOTOR, 100);
      /* sleep(0.2);*/
    
      while (!pause) {
	/*	printf("light: %d\n", analog(LLIGHT));*/
	
	/*look for the left light value*/
	/*if (analog(LLIGHT) <= dist_to_light(ideal_dist, my_llight)) {*/
	if (ideal_dist >= light2distance(analog(LLIGHT), LLIGHT)) {
	  turn_count = 0;
	  break;
	}
	
	turn_count++;

	
	/*after a while stop bot from taking a sharp turn go into a spin*/
	if(turn_count >= 200) {
	  motor(LMOTOR, 0);
	  motor(RMOTOR, 100);

	  /*keep track of our max light (smallist number)*/
	  if(analog(FLIGHT) < max_light) {
	    max_light = analog(FLIGHT);
	  }
	  
	}
	if(turn_count >=600) {
	  alloff();
	  turn_count = 0;
	  find_arc(ideal_dist, orientation, max_light);	  
	}
      }
      
    } else {  /* we are circling with the light on our right side*/

      /*printf("making right turn to light\n");*/
      /*turn left until we are perpendicular to the light*/
     
      motor(LMOTOR, 100);
      motor(RMOTOR, 30);
      /* sleep(0.2);*/
    
      while (!pause ) {
	printf("light: %d\n", analog(RLIGHT));

	/*look for the left light value*/
	/*	if (analog(RLIGHT) <= dist_to_light(ideal_dist, my_rlight)) {*/
	if (ideal_dist >= light2distance(analog(RLIGHT), RLIGHT)) {
	  turn_count = 0;
	  break;
	}
	
	turn_count++;

	
	/*after a while stop bot from taking a sharp turn go into a spin*/
	if(turn_count >= 200) {
	  motor(RMOTOR, 0);
	  motor(LMOTOR, 100);

	  /*keep track of our max light (smallist number)*/
	  if(analog(FLIGHT) < max_light) {
	    max_light = analog(FLIGHT);
	  }
	  
	}
	if(turn_count >=600) {
	  alloff();
	  turn_count = 0;
	  find_arc(ideal_dist, orientation, max_light);	  
	}
      }
    }
    
    /* start moving forward again */
    max_light=300;
    advance(3, 0);
    }/*end if !pause */
    else sleep(2.0);
  }
}

int find_arc(int ideal_dist, int orientation, int max_light) {
  
  int turn_count = 0;

  printf("starting find_arc\n");
  sleep(1.0);
  /*
    while(1){
    printf("1:%d\n", analog(FLIGHT) );

    }
  */

  if (orientation == 0) {  /*the light is to our left*/
    
    while(max_light <= analog(FLIGHT) ) {
      motor(LMOTOR, 100);
      motor(RMOTOR, -100);
      turn_count++;

      /*
	This is a round about way of avoiding some calabration.  Because all
	of the light sensors have different sensitiveity I didn't know what
	kind of thrish hold to use....so I'm just ++ max_light every time it
	does a full turn and didn't find the light.  This has the potential to
	take a while for very sensitive sensors, but max_light _should_ be
	close.
      */
      if ((turn_count % 600) == 0){
	ao();
	printf("haveing trouble\n");
	
	max_light++;
	turn_count=0;
      }
    }

    /*found the light moving towards it*/
    motor(LMOTOR, 100);
    motor(RMOTOR, 100); 
    
    while(1) {
      /*      if(analog(FLIGHT) <= dist_to_light(ideal_dist, my_flight)) {*/
      if (ideal_dist >= light2distance(analog(FLIGHT), FLIGHT)) {
	/* sleep for a tiny bit so we are a little inside of the ideal arc*/
	sleep(1.0);
	ao();
	printf("all done\n");
	sleep(1.0);
	/*
	  we are facing the light right now so we need to turn 90+
	  degrees so that the bot is perpindicular to the light again.

	*/
	turn_right(time_for_90, 0);
	return 0;
      }
    }
  } else {  /*We are circling with the light on our right side*/

    while(max_light <= analog(FLIGHT) ) {
      motor(LMOTOR, -100);
      motor(RMOTOR, 100);
      turn_count++;

      /*
	This is a round about way of avoiding some calabration.  Because all
	of the light sensors have different sensitiveity I didn't know what
	kind of thrish hold to use....so I'm just ++ max_light every time it
	does a full turn and didn't find the light.  This has the potential to
	take a while for very sensitive sensors, but max_light _should_ be
	close.
      */
      if ((turn_count % 600) == 0){
	ao();
	printf("haveing trouble\n");
	
	max_light++;
	turn_count=0;
      }
    }

    /*found the light moving towards it*/
    motor(LMOTOR, 100);
    motor(RMOTOR, 100); 
    
    while(1) {
      /*      if(analog(FLIGHT) <= dist_to_light(ideal_dist, my_flight)) {*/
      if (ideal_dist >= light2distance(analog(FLIGHT), FLIGHT)) {
	/* sleep for a tiny bit so we are a little inside of the ideal arc*/
	sleep(1.0);
	ao();
	printf("all done\n");
	sleep(1.0);
	/*
	  we are facing the light right now so we need to turn 90+
	  degrees so that the bot is perpindicular to the light again.

	*/
	turn_left(time_for_90, 0);
	return 0;
      }
    }
  }
}

/* returns back to the drop point. */
void return_to_drop() {
  int x, y;
  int drop_x, drop_y;
  int light_high;
  alloff();
  
  /* convert drop-off point to cartesian coordinates */
  x = (int)((float)distance*cos(((float)angle)*(3.1415926/180.0)));
  y = (int)((float)distance*sin(((float)angle)*(3.1415926/180.0)));
  
  drop_x = (int) ((float)drop_distance*cos(((float)drop_angle)*(3.1415926/180.0)));
  drop_y = (int) ((float)drop_distance*sin(((float)drop_angle)*(3.1415926/180.0)));
 
  printf("drop_y: %d   drop_x: %d\n", drop_y, drop_x);
  /*start_press();*/
  printf("x: %d    y: %d\n", x, y);
  /*start_press();*/
 
  printf("angle: %d  distance: %d\n", angle, distance);
  /*start_press();*/
  
  if (angle < 90) {
    /* do stuff */
    /* turn out 90 degrees */
    turn_left(time_for_90, 0);
    advance((drop_y-y), 0);
    turn_right(time_for_90, 0);
    advance((x-drop_x), 0);
  }
  else {
    /* do different stuff */
    turn_right(time_for_90, 0);
    /*    start_press();*/
    advance((drop_y-y), 0);
    /*    start_press();*/
    turn_left(time_for_90, 0);
    /*    start_press();*/
    advance((drop_x-x), 0);      
  }
  alloff();
}

 /*********************************************************************
This goes towards the light until the front sensor is saturated at
 a reading of 1.  The it turns either left or right depending on
what the 'bot is hard coded to do.
*********************************************************************/

void move_to_light(int orientation) {
  int stop_dist = 26;
  int current_dist= 30;
  int light_dist=300;
  int max_light=300;
  int after_max=0;
  int waddle=0;  /* 0 = waddle to the left */
  int counts_past=15;
  
  light_dist = light2distance(analog(FLIGHT), FLIGHT);
  while(stop_dist < light_dist) {
    if (waddle == 0) {
      motor(LMOTOR, 0);
      motor(RMOTOR, 100);
      if(analog(FLIGHT) < max_light) {
	max_light = analog(FLIGHT);
	after_max=0;
      } else {
	after_max++;
      }
      if(after_max == 1) {
	 current_dist = pulse(1);
	 if(current_dist > obstacle_distance) {
	   alloff();
	   avoid_obstacle();
	   after_max=0;
	   max_light = 300;
	   current_dist = 0;
	 }
      }
      if(after_max >= counts_past){
	after_max=0;
	max_light = 300;
	waddle = 1;
      }
    } else {
      motor(LMOTOR, 100);
      motor(RMOTOR, 0);
      if(analog(FLIGHT) < max_light) {
	max_light = analog(FLIGHT);
	after_max=0;
      } else {
	after_max++;
      }
      if(after_max == 1) {
	 current_dist = pulse(1);
	 if(current_dist > obstacle_distance) {
	   alloff();
	   avoid_obstacle();
	   after_max=0;
	   max_light = 300;
	   current_dist = 0;
	 }
      }      
      if(after_max >= counts_past){
	after_max=0;
	max_light=300;
	waddle = 0;
      }   
    }
    printf("light dist: %d\n", light_dist);
    light_dist = light2distance(analog(FLIGHT), FLIGHT);
  }
  alloff();

  /*old move_to_light when we thought we could use advance
    
  light_dist = light2distance(analog(FLIGHT), FLIGHT);
  while(stop_dist <= light_dist) {
    /*
      This will advance us blind for the first 3 cm but
      it keep us from advancing blind every time after a
      avoid_obstacle is called
      *
    advance(3,0);
    if (light_dist > 40) {
      current_dist = pulse(1);
      if(current_dist > obstacle_distance)
	avoid_obstacle();
    }
    
    advance(3,0);
    light_dist = light2distance(analog(FLIGHT), FLIGHT);
  }

  */
}