/* vfhtest.cpp

   main() function for testing the vfh() function,
          and graphing the results.

   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"
#include "vfh.h"
#include "window.h"
#include "motors.h"

main(int argc, char *argv[])
{
  Sonar sonar;
  Motors motors;
  VFH vfh(&sonar, &motors);
  Window *win = new Window(0, 0, 300, 300);

  float speed = 0.1;
  float fake_x = 0, fake_y = 0;
  int grid_x, grid_y;
  int done = 0;
  float *histogram;
  int i;
  Vector desired_vector;

  desired_vector.mag = 1;
  desired_vector.dir = 0;

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

  motors.init();

#ifdef NO_INTERRUPT
  sonar.start();
#else
  vfh.start();
#endif

  // enter graphics mode
  set_graphics_mode();

  // I'm putting these turns in to try to find if there's some
  // interaction between motors, sonars, and kbhit(). sigh.

  while(!done){

    if(kbhit())
      done = 1;

    while(!sonar.new_data_available());

#ifdef NO_INTERRUPT
    occupancy_grid_update(0);
#endif

    vfh.graph(win, desired_vector);

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

  printf("Exiting now\n");

#ifdef NO_INTERRUPT
  sonar.stop();
#else
  vfh.stop();
#endif

  motors.halt();
}