/* vfh.h

   Declares a vfh() 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. Isolated from code written by...
   */

#ifndef VFH_H
#define VFH_H

#include "sonar.h"
#include "occugrid.h"
#include "vector.h"
#include "window.h"

struct VFHParameters{

// alpha is the width in degrees of one sector.
  int alpha;
  int num_sectors;

/* threshold is a parameter that is used to determine if an obstacle
   is blocking our path.  If a smoothed polar obstacle density is
   less than this threshold, an obstacle is not in the way. */
  float threshold;

  /* smax is maximum size of a valley that the robot will try to enter.
     The larger this value the farther the robot will try to stay from
     an obstacle. */
  int smax;

  /* hm is a parameter that is chosen so as to reduce the velocity
     sufficiently when approaching an obstacle. */
  float hm;

  int window_radius;
  int window_size;
  int l; // half the width of the smoothing window
  float b;
  float a;

  void read(char *filename);
};

class VFH: public OccupancyGrid
{
protected:
  static VFHParameters vfhparms;
  
  // calc_polar() returns an array of num_sectors floats describing
  //              the smoothed polar obstacle density.
  float *calc_polar();
  float weight_function(float);
  void graph_vector(Window *win, Vector vec);
  void graph_threshold(Window *win, float *hist);
  void graph_polar(Window *win, float *hist);

public:
  VFH(Sonar *sonar, Motors *motors);
  // vector() returns a vector which is as close as possible to the desired
  //          vector but is on a clear path. It returns a zero-magnitude
  //          vector if the robot is totally blocked.
  Vector vector(Vector desired);
  virtual void graph(Window *win, Vector desired);
};

#endif // VFH_H