/* vfh.cpp

   Defines a vector() function which reconciles a desired motion vector
   with obstacles sensed by sonar and returns a resulting vector
   or an indication that the path is BLOCKED.

   - by Dave Hershberger. Modified from code written by Elizabeth Nitz.
     The algorithm was published in the IEEE Transactions on Robotics
     and Automation, Vol 7, Number 3, June 1991: "The Vector Field
     Histogram---Fast Obstacle Avoidance for Mobile Robots".
   */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include <string.h>
#include "vfh.h"
#include "occugrid.h"
#include "vector.h"
#include "namevalu.h"
#include "window.h"

#ifdef MIN
#undef MIN
#endif

#define MIN(a, b) (((a) < (b)) ? (a) : (b))

struct VFHElement{
  int sector;
  float factor;
};

VFHParameters VFH::vfhparms;

void VFHParameters::read(char *filename)
{
  alpha = 5;
  threshold = 2000;
  smax = 30;
  hm = 3000;
  window_radius = 16;
  l = 5;
  b = 2;

  NameValueList pairs;
  NameValueListeLem *pair;

  if(!pairs.read(filename))
    fprintf(stderr,
	    "WARNING: Could not open vfhparm file %s. Using defaults.\n",
	    filename);
  else{
    for(pair = pairs.first(); pair; pair = pair->next){

      if(stricmp(pair->string, "alpha") == 0)
	alpha = atoi(pair->value);
      else if(stricmp(pair->string, "threshold") == 0)
	threshold = atof(pair->value);
      else if(stricmp(pair->string, "smax") == 0)
	smax = atoi(pair->value);
      else if(stricmp(pair->string, "hm") == 0)
	hm = atof(pair->value);
      else if(stricmp(pair->string, "window_radius") == 0)
	window_radius = atof(pair->value);
      else if(stricmp(pair->string, "l") == 0)
	l = atof(pair->value);
      else if(stricmp(pair->string, "b") == 0)
	b = atof(pair->value);

      else
	fprintf(stderr, "WARNING: %s in config file %s is not a valid\n"
		"parameter.\n", pair->string, filename);
    }
  }
  // Derived parameters get set after reading parameters from file.
  num_sectors = 360 / alpha;
  a = b * sqrt(2) * window_radius;
  window_size = window_radius * 2 + 1;
}

// "dist" is in grid-units from center of robot. Don't forget!
float VFH::weight_function(float dist)
{
  if(dist < 6)
    return 40000.0;
  else
    return vfhparms.a - vfhparms.b * dist;
}

// calc_polar() returns a pointer to an array of floats. The array
//              is static and is allocated by calc_polar(), so it
//              should not be freed by other routines.

float *VFH::calc_polar()
{

  static float *histogram, *smoothed_histogram;
  static int first_time = 1;
  static VFHElement *vfh_grid;

  int i, j, x, y, gx, gy, gx_limit, gy_limit;
  VFHElement *vfh_data;
  int c;

  if(first_time){
    // Allocate space and set up look-up table.
    first_time = 0;
    histogram = (float *) malloc(sizeof(float) * vfhparms.num_sectors);
    smoothed_histogram = (float *) malloc(sizeof(float) *
					  vfhparms.num_sectors);
    vfh_grid = (VFHElement *)malloc(sizeof(VFHElement) *
				    vfhparms.window_size *
				    vfhparms.window_size);
    
    for(y = 0; y < vfhparms.window_size; y++)
      for(x = 0; x < vfhparms.window_size; x++){
	double radians = atan2(y - vfhparms.window_radius,
			       x - vfhparms.window_radius);
	while(radians < 0)
	  radians += 2 * 3.14159;

	vfh_grid[x + y * vfhparms.window_size].sector =
	  (radians * 180 / 3.14159) / vfhparms.alpha;
	vfh_grid[x + y * vfhparms.window_size].factor =
	  weight_function(hypot(y - vfhparms.window_radius,
				x - vfhparms.window_radius));
      }
  }

  // clear polar histogram to start with.
  for (i = 0; i < vfhparms.num_sectors; i++){
    histogram[i] = 0;
  }

  // Prepare for loop.
  vfh_data = vfh_grid;
  gy_limit = grid_y + vfhparms.window_radius;
  gx_limit = grid_x + vfhparms.window_radius;

  // Loop over active window.
  for(gy = grid_y - vfhparms.window_radius;
      gy <= gy_limit;
      gy++)
    for(gx = grid_x - vfhparms.window_radius;
	gx <= gx_limit;
	gx++, vfh_data++){

      // Calculate the polar obstacle densities.
      c = read(gx, gy);
      if (c != 0){
	histogram[vfh_data->sector] += c * c * vfh_data->factor;
      }
    }

  // Now smooth the histogram.

  // NOTE: I did this smoothing differently than the paper described.
  // The paper's math didn't work out for me, so I did what I thought
  // they meant. - Dave Hershberger

  // Loop over histogram.
  for(i = 0; i <= vfhparms.num_sectors; i++) {
    smoothed_histogram[i] = 0;

    // Compute weighted sum over smoothing window.
    for(j = i - vfhparms.l + 1; j < i + vfhparms.l; j++) {
      if(j < 0)
	smoothed_histogram[i] += histogram[j + vfhparms.num_sectors] *
	  (vfhparms.l - abs(j - i));
      else if (j >= vfhparms.num_sectors)
	smoothed_histogram[i] += histogram[j - vfhparms.num_sectors] *
	  (vfhparms.l - abs(j - i));
      else
	smoothed_histogram[i] += histogram[j] * (vfhparms.l - abs(j - i));;
    }

    // Average summed values.
    smoothed_histogram[i] /= (vfhparms.l * vfhparms.l);
  }

  return smoothed_histogram;
}

VFH::VFH(Sonar *sonar, Motors *motors) : OccupancyGrid(sonar, motors)
{
  vfhparms.read("vfhparms.cfg");
}

Vector VFH::vector(Vector desired)
{
  
  float *h = calc_polar(); // h[] holds the polar obstacle densities.

  /* ktarg is the sector that the goal falls in.  This is used as the
     starting point to find the direction that the robot should travel */
  int	ktarg;

  /* knear is the sector that is closest to ktarg.  kfar is the sector
     that is farthest from ktarg.  The direction of travel is calculated
     by averaging knear and kfar. */
  int	knear, kfar;

  /* hcprime is the sector that the robot is going to go. It is used in
     calculating the velocity(vel) of the robot. */
  int	hcprime;

  static int number_of_loops = 0; // number of calls to vector().
  Vector retvec;
  float theta;
  int 	i,j,k;

  /* Calculate the sector the goal is in */
  ktarg = (int)(desired.dir / (2 * 3.14159) * vfhparms.num_sectors);
  if(ktarg < 0)
    ktarg += vfhparms.num_sectors;

  assert(ktarg >= 0 && ktarg < vfhparms.num_sectors);

  i = j = ktarg;
  k = 0;
  /* If not is a local valley, then find the nearest sector to the goal */
  while ((h[i] > vfhparms.threshold) &&
	 (h[j] > vfhparms.threshold) &&
	 (k <= vfhparms.num_sectors / 2)){
    k++;
    i = (++i <= (vfhparms.num_sectors - 1)) ? i : 0;
    k++;
    j = (--j >= 0) ? j : (vfhparms.num_sectors - 1);
  }

  // If there's no free path available, return a zero vector.
  if (k >= vfhparms.num_sectors / 2){
    retvec.dir = 0.0;
    retvec.mag = 0.0;
    return(retvec);
  }

  /* If there hasn't been at least two loops, don't move,
     let the field build up */
  if (number_of_loops++ < 2) 
    {
      retvec.dir = 0.0;
      retvec.mag = 0.000001; // Need to differentiate this from BLOCKED
      return(retvec);
    }

  /* If an obstacle is not in front of the robot, find a local valley
     so that the robot can center itself in the valley and go in between
     the two closest obstacles */
  if (i==j) {
    k = 0;
    knear = -5;
    kfar = -5;

    /* Search in both directions until an obstacle is found, then
       search the other way until one is found there. */
    while ((k < vfhparms.smax) && (kfar == -5)) {
      if (i != knear) {
	i = (++i <= (vfhparms.num_sectors - 1)) ? i : 0;
	k++;
	if (h[i] > vfhparms.threshold) {
	  if (knear != -5)
	    kfar = i;
	  else
	    knear = i;
	}
      }
      if (j != knear) {
	j = (--j >= 0) ? j : (vfhparms.num_sectors - 1);
	k++;
	if (h[j] > vfhparms.threshold) {
	  if (knear != -5)
	    kfar = j;
	  else
	    knear = j;
	}
      }
    }

    /* if the ends of the valley are not found, set knear and kfar
       to the current values of i and j */
    if (knear == -5) {
      knear = i;
      kfar = j;
    }

    /* if only one side is found, set kfar to the other */
    else if (kfar == -5)
      kfar = (i == knear) ? j : i;
  }
  /* If an obstacle was in front of the robot and the nearest valley is
     in the positive direction, set that to knear and then find kfar */
  else if (h[i] < h[j])
    {
      k=0;
      knear = i;
      kfar = -5;
      j = i;
      while ((k < vfhparms.smax) && (kfar == -5))
	{
	  k++;
	  j = (++j <= (vfhparms.num_sectors-1))?j:0;
	  if (h[j] > vfhparms.threshold)
	    kfar = j;
	}
      /* if kfar is not found within smax of knear, set kfar to j */
      if (kfar == -5) 
	kfar = j;
    }

  /* if obstacle is in front of robot and the nearest valley is on the
     negative side of ktarg, set that to knear and find kfar */
  else {
    k=0;
    knear = j;
    kfar = -5;
    i = j;
    while ((k < vfhparms.smax) && (kfar == -5)) {
      k++;
      i = (--i >= 0) ? i : (vfhparms.num_sectors - 1);
      if (h[i] > vfhparms.threshold)
	kfar = i; 
    }
    /* if not found within smax of knear, set i to kfar */
    if (kfar == -5)
      kfar = i;
  }

  /* find out if one sector went past 0 and correct for it */
  if (abs(knear - kfar) > vfhparms.smax)
    if (knear < kfar)
      knear += vfhparms.num_sectors;
    else
      kfar += vfhparms.num_sectors;

  /* theta is the average of the 2 sectors */
  theta = ((float)knear + (float)kfar) / 2.0;

  /* Make sure the sector is in the range 0 to vfhparms.num_sectors */
  if (theta > vfhparms.num_sectors - 1)
    theta -= vfhparms.num_sectors;
  hcprime = (int) theta;

  /* Convert the sector to an angle */
  theta = theta / (float) vfhparms.num_sectors * 2 * 3.14159;

  /* Calculate the return vector. */
  retvec.mag = desired.mag * (1 - (MIN(h[hcprime], vfhparms.hm) /
				     vfhparms.hm));
  retvec.dir = theta;

  return retvec;
}

void VFH::graph_polar(Window *win, float *histogram)
{
  static Color cyan(0, 63, 63);

  int scale = vfhparms.threshold / 70;

  win->set_color(cyan);
  for(int i = 0; i < vfhparms.num_sectors; i++){
    double radians = ((double) i) /
      ((double) vfhparms.num_sectors) * 2.0 * 3.14159;

    win->line(ogparms.graph_width / 2 * 4 + (int) (histogram[i] / scale *
						   cos(radians) * .5),
	      ogparms.graph_height / 2 * 4 + (int) (histogram[i] / scale *
						    sin(radians) * .5),
	      ogparms.graph_width / 2 * 4 + (int) (histogram[i] / scale *
						   cos(radians)),
	      ogparms.graph_height / 2 * 4 + (int) (histogram[i] / scale *
						    sin(radians)));
  }
}

void VFH::graph_threshold(Window *win, float *histogram)
{
  static Color cyan(0, 63, 63);
  int scale = vfhparms.threshold / 70;

  win->set_color(cyan);
  for(int i = 0; i < vfhparms.num_sectors; i++){
    if(histogram[i] > vfhparms.threshold){
      double radians1 = (((double) i) - 0.5) /
	((double) vfhparms.num_sectors) * 2.0 * 3.14159;
      double radians2 = (((double) i) + 0.5) /
	((double) vfhparms.num_sectors) * 2.0 * 3.14159;
      
      win->line(ogparms.graph_width / 2 * 4 + (int) (vfhparms.threshold /
						     scale * cos(radians1)),
		ogparms.graph_height / 2 * 4 + (int) (vfhparms.threshold /
						      scale * sin(radians1)),
		ogparms.graph_width / 2 * 4 + (int) (vfhparms.threshold /
						     scale * cos(radians2)),
		ogparms.graph_height / 2 * 4 + (int) (vfhparms.threshold /
						      scale * sin(radians2)));
    }
  }
}

void VFH::graph_vector(Window *win, Vector vec)
{
  static Color yellow(63, 63, 0);

  win->set_color(yellow);

  win->line(ogparms.graph_width / 2 * 4,
	    ogparms.graph_height / 2 * 4,
	    ogparms.graph_width / 2 * 4 + (int) (vec.mag *
						 ogparms.graph_width *
						 cos(vec.dir)),
	    ogparms.graph_height / 2 * 4 + (int) (vec.mag *
						  ogparms.graph_width *
						  sin(vec.dir)));
}

void VFH::graph(Window *win, Vector desired)
{
  float *h;

  OccupancyGrid::graph(win);

  h = calc_polar();

  graph_polar(win, h);
  graph_threshold(win, h);
  graph_vector(win, desired);
  graph_vector(win, vector(desired));
}