// occugrid.h
//
// programmer: Dave Hershberger
//
// This file is an attempt to pull the VFH code out of the
// whiteboard code and just make a nice stand-alone object
// out of it.

#ifndef OCCUGRID_H
#define OCCUGRID_H

#include "motors.h"
#include "vector.h"
#include "window.h"

// Define NO_INTERRUPT only when you won't be setting up the
// occupancy_grid_update() function as a timer callback, ie when
// you don't use occupancy_grid-> start() and stop().
// Defining it will let printf()s into the code for debugging.
//#define NO_INTERRUPT

struct IntPoint{
  int x, y;
};

struct GridElement{
  char occupancy;
  char history;
};

struct OGParameters{
  int graph_width;
  int graph_height;
  int grid_size;
  float grid_units_per_foot;
  float robot_radius_feet;
  
  /* care_dist is the distance that if any ultrasonic reading is
     greater than, it will be ignored.  This helps reduce the error in our
     global_field, but also doesn't allow the robot to correct for
     obstacles as soon as it normally would. */
  float care_dist_feet;
  int care_dist_grid;
  
  /* number_passes is the number of passes before the entire field is
     decreased by one */
  int number_passes;

  void read(char *filename);
	public:
	int gridsz() {return grid_size;}
	float grid_per_foot(void) {return grid_units_per_foot;}
	float robot_diameter_feet(void) {return 2.0*robot_radius_feet;}
};

// Same with these graph size definitions
class OccupancyGrid
{
protected:
  static OccupancyGrid *og;
  static Sonar *sonar;
  static Motors *motors;
  static OGParameters ogparms;
  
  static GridElement **grid;
  /* pass is used by the attrition part so that the field can be reduced
     only every "pass" number of ultrasonic reading. */
  static int pass;
  
  static int history_index;
  static IntPoint history[15];
  static int out_of_boundary;
  static float body_x, body_y, body_theta;
  // "body_theta" is the angle that the body sits relative to the
  // occupancy grid.  The body angle is different than the head angle,
  // remember, which is the angle of the wheels relative to the body.
  // The body angle is not supposed to change, but it does very gradually
  // as the robot turns its wheels.
  
  // where the robot is in the grid.
  static int grid_x, grid_y;
  static int running;
  
  void past_mapper();
  void reduce();
  void fill_ogparms(OGParameters *parms);
  friend void occupancy_grid_update(double elapsed_time);

public:
  OccupancyGrid(Sonar *sonar, Motors *motors);
  virtual void start();	 // Starts ultrasonics and sets up timer routine.
  virtual void stop();	 // Stops ultrasonics and stops timer routine.
  
  // Tell the OccupancyGrid where the robot is and what angle the
  // *body* is sitting at. Units are feet and radians.
  virtual void tell_pose(float x, float y, float body_angle);
  
  virtual int out_of_bounds() { return out_of_boundary; }
  virtual char read(int x, int y) { return grid[x][y].occupancy; }
  virtual void graph(Window *win);
	virtual int gridSize() {return ogparms.gridsz();}
	virtual float gridunitsperfoot(void) {return ogparms.grid_per_foot();}
	virtual float robot_diameter(void) {return ogparms.robot_diameter_feet();}
};

#endif				// OCCUGRID_H