/* polartst.cpp

   main() function for testing the polar obstacle density calculation,
          and graphing the results.

   author: Dave Hershberger */

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <math.h>
#include <graph.h>
#include "comp_dep.h"
#include "sonar.h"
#include "occugrid.h"
#include "vfh.h"

main(int argc, char *argv[])
{
  float speed = 0.1;
  float fake_x = 0, fake_y = 0;
  int grid_x, grid_y;
  int done = 0;
  float *histogram;
  int i;

  if(argc > 1)
    speed = atof(argv[1]);

  if(argc > 2)
    occupancy_grid->number_passes = atoi(argv[2]);

#ifdef NO_INTERRUPT
  sonar->start();
#else
  occupancy_grid->start();
#endif

  // enter graphics mode
  _setvideomode(_MAXRESMODE);

  while(!done){

    if(kbhit())
      done = 1;

    while(!sonar->new_data_available());

#ifdef NO_INTERRUPT
    occupancy_grid_update(0);
#endif

    occupancy_grid->graph();
    histogram = calc_polar(occupancy_grid);

    _setcolor(CYAN);
    for(i = 0; i < num_sectors; i++){
      double radians = ((double) i) / ((double) num_sectors) * 2.0 * 3.14159;
      _moveto(graph_width / 2 * 4, graph_height / 2 * 4);
      if(histogram[i] / 30 > graph_width / 2 * 4)
	histogram[i] = graph_width / 2 * 4 * 30;
      _lineto(graph_width / 2 * 4 + (int) (histogram[i] / 30.0 *
					   cos(radians)),
	      graph_height / 2 * 4 + (int) (histogram[i] / 30.0 *
					    sin(radians)));
    }

    if(occupancy_grid->out_of_bounds()){
      printf("Robot out of bounds.\n");
      speed = -speed;
    }
    fake_x += speed;
    occupancy_grid->tell_pose(fake_x, fake_y, 0);
  }
  
  // set graphics mode to text
  _setvideomode(_DEFAULTMODE);

  printf("Exiting now\n");

#ifdef NO_INTERRUPT
  sonar->stop();
#else
  occupancy_grid->stop();
#endif
}