// vfhmove.cpp
//
// by Dave Hershberger
//
// A simple program to test VFH and MoveToGoal with Clem's motors.

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <math.h>
#include "motors.h"
#include "mov2goal.h"
#include "misc.h"
#include "vfh.h"
#include "window.h"

main()
{
  Motors motors;
  MoveToGoal mtg;
  Sonar sonar;
  VFH vfh(&sonar, &motors);

  Window *vfh_win = new Window(0, 0, 400, 400);

  int done = 0;
  float x, y, theta;
  Vector motion;
  float speed = 0.3;
  int i, j;
  int drive_dir = 1;
  float angle;
  float body_angle_rads;
  int body_angle_degs;
  
  motors.init();
  motors.home();
  vfh.start();

  // enter graphics mode
  set_graphics_mode();

  printf("VFH/MoveToGoal test program.\n\n");

  printf("Clementine cannot determine her angle relative to the lab.\n");
  printf("Therefore, please enter the angle between clem's sonar 0\n");
  printf("and the positive X axis of the grid, in degrees.  The coordinate\n");
  printf("system is left-handed.\n\n");

  printf("  sonar 0 angle? => ");

  scanf("%d", &body_angle_degs);
  body_angle_rads = body_angle_degs * M_PI / 180.0;

  // Tell the motors that we're steered to "body_angle_rads" so that
  // when we turn to angle 0, sonar 0 will line up with the +X axis.
  motors.setxy(0, 0, body_angle_rads);

  while(!done){
    printf("Enter q to quit, any other key to continue.\n");

    if(getch() == 'q'){
      done = 1;
    }
    else{
      motors.getxy(&x, &y, &theta);

      fprintf(stderr, "current pose: x = %.4g, y = %.4g, theta = %.4g\n\n",
	     x, y, theta);

      mtg.read_from_console();

      while(!mtg.at_goal() && !kbhit()){

	motors.getxy(&x, &y, &theta);
	motion = mtg.vector(x, y);
	
	if(!mtg.at_goal()){
	  
	  vfh.tell_pose(x, y, body_angle_rads);
	  vfh.graph(vfh_win, motion);
	  
	  motion = vfh.vector(motion);
	  
	  if(vfh.out_of_bounds()){
	    fprintf(stderr, "Robot out of bounds.\n");
	    motors.drive(0);
	  }
	  else if(motion.mag == 0){
	    fprintf(stderr, "BLOCKED!\n");
	    motors.drive(0);
	  }
	  else{
	    if(fabs(FixAngle(theta - motion.dir)) > .7){
	      motors.drive(0);
	      motors.turnAbs(motion.dir);
	      motors.drive(motion.mag * speed);
	    }
	    else{
	      motors.drive(motion.mag * speed);
	      motors.turnAbs(motion.dir);
	    }
	  }
	}
      }

      motors.drive(0);

      if(mtg.at_goal())
	printf("\nArrived at goal!\n\n");
      
      if(!mtg.at_goal() && getch() == 'q')
	done = 1;
    }
  }
  
  // set graphics mode to text
  set_text_mode();
  
  vfh.stop();
  motors.halt();
}