/* graphtst.cpp

   main() function for testing the occupancy_grid class 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"

main(int argc, char *argv[])
{
  Sonar sonar;
  OccupancyGrid occupancy_grid(&sonar);
  float speed = 0.1;
  float fake_x = 0, fake_y = 0;
  int grid_x, grid_y;
  int done = 0;

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

#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();

    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
}