/* testgrid.cpp

   main() function for testing the occupancy_grid class.

   author: Dave Hershberger */

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <math.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;

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

  clrscr();
  
#ifdef NO_INTERRUPT
  sonar.start();
#else
  occupancy_grid.start();
#endif

  while(!kbhit()){

    while(!sonar.new_data_available());

#ifdef NO_INTERRUPT
    occupancy_grid_update(0);
#endif

    gotoxy(1, 1);
    
    grid_x = grid_size / 2 + fake_x * grid_units_per_foot;
    grid_y = grid_size / 2 + fake_y * grid_units_per_foot;

    for(int y = -10; y < 10; y++){
      for(int x = -10; x < 10; x++){
	char c = occupancy_grid.read(x + grid_x, y + grid_y);
	if(x == 0 && y == 0)
	  printf("XX");
	else if(c == (char) 0)
	  printf("  ");
	else
	  printf("%2d", c);
      }
      printf("\n");
    }
    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);
  }
  
  printf("Exiting now\n");

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