// occugrid.cpp
//
// 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. We shall see.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "comp_dep.h"
#include "sonar.h"
#include "trig_tab.h"
#include "occugrid.h"
#include "namevalu.h"
#include "motors.h"
#include "window.h"
#include "misc.h"

#define DEBUG_ON 0

// Definition of OccupancyGrid static member data

OccupancyGrid *OccupancyGrid::og = NULL;
Sonar *OccupancyGrid::sonar;
Motors *OccupancyGrid::motors;
OGParameters OccupancyGrid::ogparms;

GridElement **OccupancyGrid::grid;
  /* pass is used by the attrition part so that the field can be reduced
     only every "pass" number of ultrasonic reading. */
int OccupancyGrid::pass;

int OccupancyGrid::history_index;
IntPoint OccupancyGrid::history[15];
int OccupancyGrid::out_of_boundary;
int OccupancyGrid::running;

float OccupancyGrid::body_x;
float OccupancyGrid::body_y;
float OccupancyGrid::body_theta;
// body_theta is not used yet, but it should be eventually. -Dave
  
// grid_x and grid_y store where the robot is in the grid.
int OccupancyGrid::grid_x;
int OccupancyGrid::grid_y;

void OGParameters::read(char *filename)
{
  graph_width = 100;
  graph_height = 100;
  grid_size = 500;
  grid_units_per_foot = 3;
  robot_radius_feet = 14.0 / 12.0; // Clem has 14 inch radius
  care_dist_feet = 7.1;
  number_passes = 1;

  NameValueList pairs;
  NameValueListeLem *pair;

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

      if(stricmp(pair->string, "graph_width") == 0)
	graph_width = atoi(pair->value);
      else if(stricmp(pair->string, "graph_height") == 0)
	graph_height = atoi(pair->value);
      else if(stricmp(pair->string, "grid_size") == 0)
	grid_size = atoi(pair->value);
      else if(stricmp(pair->string, "grid_units_per_foot") == 0)
	grid_units_per_foot = atof(pair->value);
      else if(stricmp(pair->string, "robot_radius_feet") == 0)
	robot_radius_feet = atof(pair->value);
      else if(stricmp(pair->string, "care_dist_feet") == 0)
	care_dist_feet = atof(pair->value);
      else if(stricmp(pair->string, "number_passes") == 0)
	number_passes = atoi(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.
  care_dist_grid = care_dist_feet * grid_units_per_foot;
}

OccupancyGrid::OccupancyGrid(Sonar *snr, Motors *mtrs)
{
  if(!og){
    og = this;
    sonar = snr;
    motors = mtrs;

    ogparms.read("ogparms.cfg");

    int i, j;

    // allocate and initialize the grid
    grid = (GridElement **) malloc(sizeof(GridElement *) *
				   ogparms.grid_size);
    for(i = 0; i < ogparms.grid_size; i++){
      grid[i] = (GridElement *) malloc(sizeof(GridElement) *
				       ogparms.grid_size);
      for(j = 0; j < ogparms.grid_size; j++){
	grid[i][j].occupancy = (char) 0; // Initialize the grid to 0.
	grid[i][j].history = (char) 0;
      }
    }
    
    for(i = 0; i < 15; i++){
      history[i].x = 0;
      history[i].y = 0;
    }
    
    pass = 0;
    history_index = 0;
    
    tell_pose(0, 0, 0);
    
    out_of_boundary = 0;
    running = 0;
  }
}

void OccupancyGrid::start()
{
  if(!running){
    out_of_boundary = 0;
    sonar->start();
    timer->add_callback(occupancy_grid_update, TICKS_PER_SONAR_FIRE, 2);
    running = 1;
  }
}

void OccupancyGrid::stop()
{
  if(running){
    timer->delete_callback(occupancy_grid_update, TICKS_PER_SONAR_FIRE);
    sonar->stop();
    running = 0;
  }
}

/* occupancy_grid_update():
   
   The update function is called from a timer interrupt, so
   it must be kept fairly fast and it should not have many
   local variables, since they mean less space on the stack,
   and an interrupt could happen while using a DOS function,
   which would imply a very small (512 byte?) stack.
   
   It updates the occupancy grid from the latest sonar data for
   the currently known robot position and orientation.
   
   Note: This routine now uses og->body_theta to find what angle the
   robot body sits at relative to the grid.  If you need to change
   this, like if you want to deal with the fact that the robot body
   gradually turns when the head and wheels turn a lot; or if you
   simply need to tell occupancy_grid that the robot body is at an
   angle to the grid; use tell_pose() to pass the body angle in. */

void occupancy_grid_update(double elapsed_time)
{
  // Making these variables static keeps them off the stack.
  static int i,j,x,y,u,s,t;
  static float dists[NUM_ULTRASONICS];
  static int grid_x;
  static int grid_y;
  static float dist_to_obs;
  static OccupancyGrid *og = OccupancyGrid::og;
  static Sonar *sonar = og->sonar;
  static Motors *motors = og->motors;
  static OGParameters *parms = &og->ogparms;
  static float fx, fy, junk;
  static int real_angle;  // angle a given sonar is facing, in degrees.
  
  // Wait until there's new sonar data before updating grid.
  if(!sonar->new_data_available())
    return;
  
  // Get the latest sonar distances
  sonar->read(dists);
//  motors->getxy(&fx, &fy, &junk);
//  og->tell_pose(fx, fy, 0);
  
  // Make our own copies to save references to og
  grid_x = og->grid_x;
  grid_y = og->grid_y;
  
  // Now update the grid with the distances from the sonars.
  if(grid_x - parms->care_dist_grid - 1 < 0 ||
     grid_x + parms->care_dist_grid + 1 >= parms->grid_size ||
     grid_y - parms->care_dist_grid - 1 < 0 ||
     grid_y + parms->care_dist_grid + 1 >= parms->grid_size){
    og->out_of_boundary = 1;
    return;
  }
  og->out_of_boundary = 0;
  
  og->past_mapper();
  
  for (u = 0; u < NUM_ULTRASONICS; u++)
    {
      static float slope;
      
      real_angle = int(FixAngle2(og->body_theta + u * 15.0 * R_PI / 180.0)
		       * 180.0 / R_PI);

      /* the distances[] array has integers representing units
	 of 10ths of feet in distance from the sensors. so,
	 Get a reading and add the radius of the robot. */
      
      dist_to_obs = dists[u] + parms->robot_radius_feet;
      
      if (dist_to_obs > parms->care_dist_feet)
	dist_to_obs = parms->care_dist_feet+1.0;
      
      x = (int) (table.cos_table(real_angle) * dist_to_obs *
		 parms->grid_units_per_foot);
      y = (int) (table.sin_table(real_angle) * dist_to_obs *
		 parms->grid_units_per_foot);
      
      /* Now reduce the values in the grid along a line from
	 the robot to the obstacle in the direction of this
	 sonar hit. */
      
      if ((real_angle >= 315.0 || real_angle < 45.0) ||
	  (real_angle >= 135.0 && real_angle <= 225.0)){

	if(real_angle >= 315.0 || real_angle < 45.0)
	  j =- 1;
	else
	  j = 1;
	i=0;
	slope = -((float) y / (float) x);
	  
	/* This loop reduces all of the values in the
	   global_field along the axis of the ultra-
	   sonic by 1. */
	while (table.sqrt_table(i * i + j * j) <
	       dist_to_obs * parms->grid_units_per_foot)
	  {
	    i = (int) (slope * (float) j);
	    s = i + grid_x;
	    t = j + grid_y;
	      
	    if (og->grid[s][t].occupancy > 0)
	      og->grid[s][t].occupancy--;
	      
	    if (real_angle >= 315.0 || real_angle < 45.0)
	      j--;
	    else
	      j++;
	  }
      }
      else
        {
	  if (real_angle >= 45 && real_angle <= 135)
	    i = 1;
          else
	    i = -1;
	  j = 0;
	  slope = -((float) x / (float) y);
	  
	  /* This does the same as above, only for
	     different ultrasonics */
	  while (table.sqrt_table(i * i + j * j) <
		 dist_to_obs * parms->grid_units_per_foot)
	    {
	      j = (int) (slope * (float) i);
	      s = i + grid_x;
	      t = j + grid_y;
	      
	      if (og->grid[s][t].occupancy > 0)
                og->grid[s][t].occupancy--;
	      
	      if (real_angle >= 45 && real_angle <= 135)
                i++;
	      else
                i--;
	    }
	}
      
      /* Now apply the mask to the global_field so that the
	 histogram will grow more quickly */
      if (dist_to_obs < parms->care_dist_feet)
        {
	  x += grid_x;
	  y += grid_y;
	  
	  // I'm taking this out for now. I think the "=" should be a "+=",
	  // otherwise this just copies the value of the one down and to
	  // the right. I am still not sure I want it even if it does work.
	  //  - Dave
#if 0
	  for(i = -1; i <= 1; i++)
	    for(j = -1; j <= 1; j++)
	      if ((i != 0) && (j != 0))
                og->grid[x][y].occupancy =
		  (int) 0.5 * og->grid[x+i][y+j].occupancy;
#endif				// 0	  
	  /* This change is being made to see if I can make the robot
	     handle objects that appear close to the robot easily
	     avoidable.  What I am going to do is check the distance
	     to the obstacle and if it is below a certain threshold,
	     update the field more quickly. */
	  
	  if (dist_to_obs < parms->grid_units_per_foot)
	    og->grid[x][y].occupancy = og->grid[x][y].occupancy + 15;
	  
          else if (og->grid[x][y].occupancy > 2)
	    og->grid[x][y].occupancy = og->grid[x][y].occupancy + 12;
	  
          else
	    og->grid[x][y].occupancy = og->grid[x][y].occupancy + 3;
	  
	  if (og->grid[x][y].occupancy > 15)
	    og->grid[x][y].occupancy = 15;
	}
    }
  
  /* reduce the global field */
  og->reduce();
}

/* past_mapper():
   
   Updates the last 15 steps of the path in the global_field in a
   gradient fashion.  After it gets to 15, the field is then lowered
   to 0 so that the robot will be allowed to repeat its previous
   path. */

void OccupancyGrid::past_mapper()
{
  // This runs in an interrupt handler, so keep the stack usage
  // smaller by making variables static.
  static int i, x, y;
  
  if (grid[grid_x][grid_y].history < 15)
    grid[grid_x][grid_y].history++;
  
  x = history[history_index].x;
  y = history[history_index].y;
  
  grid[x][y].history = 0;
  
  history[history_index].x = grid_x;
  history[history_index].y = grid_y;
  
  if (history_index < 14)
    history_index++;
  else
    history_index = 0;
  
  for (i = 14; i >= 0; i--)
    grid[history[i].x][history[i].y].history++;
}


/* reduce():
   
   reduce the occupancy values. */

void OccupancyGrid::reduce()
{
  static int i, j, x, y;	/* local counter variables */
  
  pass++;
  if (pass >= ogparms.number_passes)
    {
#ifdef NO_INTERRUPT
      if(DEBUG_ON)
	fprintf(stderr, "Reducing local area of grid around point %d, %d\n",
		grid_x, grid_y);
#endif
      for (i = -19; i <= 19; i++)
        for (j = -19; j <= 19; j++)
          {
	    x = grid_x + i;
	    y = grid_y + j;
            
	    if(grid[x][y].occupancy > 0)
              grid[x][y].occupancy--;
	  }
      pass = 0;
    }
}

void OccupancyGrid::tell_pose(float x, float y, float theta)
{
  body_x = x;
  body_y = y;
  body_theta = theta;
  
  // Find where we are in the grid
  grid_x = (int)(ogparms.grid_size / 2 + (body_x *
					  ogparms.grid_units_per_foot));
  grid_y = (int)(ogparms.grid_size / 2 + (body_y *
					  ogparms.grid_units_per_foot));
}

static Image *make_spots(Window *win)
{
  static Color red(63, 0, 0);
  static Color black(0, 0, 0);

  int pixel_value[4][4] = {{ 13,  8, 12, 16 },
			   {  9,  4,  3,  7 },
			   {  5,  1,  2, 11 },
			   { 14, 10,  6, 15 }};
  
  Image *spots = (Image *) malloc(sizeof(Image) * 17);
  
  for(int value = 0; value < 17; value++){
    for(int y = 0; y < 4; y++)
      for(int x = 0; x < 4; x++){
	win->set_color((value >= pixel_value[x][y]) ? red : black);
	win->set_pixel(x, y);
      }
    spots[value] = win->get_image(0, 0, 3, 3);
  }
  
  return spots;
}

void OccupancyGrid::graph(Window *win)
{
  static Color cyan(0, 63, 63);
  static Color black(0, 0, 0);

  static Image *spots;
  int x, y, gx, gy;
  static int first_time = 1;
  
  if(first_time){
    first_time = 0;
    spots = make_spots(win);
  }
  
  win->set_color(black);
  win->fill_rectangle(0, 0,
		      ogparms.graph_width * 4,
		      ogparms.graph_height * 4);
  
  gy = grid_y - ogparms.graph_height / 2;
  for(y = 0; y < ogparms.graph_height; y++){
    gx = grid_x - ogparms.graph_width / 2;
    for(x = 0; x < ogparms.graph_width; x++){
      if(gx >= 0 && gx < ogparms.grid_size &&
	 gy >= 0 && gy < ogparms.grid_size){
	if(grid[gx][gy].occupancy != 0)
	  win->put_image(spots[grid[gx][gy].occupancy], x * 4, y * 4);
	if(gx == grid_x && gy == grid_y){
	  win->set_color(cyan);
	  win->fill_rectangle(x * 4, y * 4, x * 4 + 3, x * 4 + 3);
	}
      }
      gx++;
    }
    gy++;
  }
}