// testgoal.cpp
//
// by Dave Hershberger
//
// A simple program to test the MoveToGoal class 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"

main()
{
  Motors motors;
  MoveToGoal mtg;
  int done = 0;
  float x, y, theta;
  Vector goal_vector;
  float speed = 0.3;
  int i, j;
  
  motors.init();

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

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

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

	goal_vector = mtg.vector(x, y);

	if(fabs(FixAngle(theta - goal_vector.dir)) > .7){
	  motors.drive(0);
	  motors.turnAbs(goal_vector.dir);
	  motors.drive(goal_vector.mag * speed);
	}
	else{
	  motors.drive(goal_vector.mag * speed);
	  motors.turnAbs(goal_vector.dir);
	}
      }

      if(!mtg.at_goal() && getch() == 'q')
	done = 1;
    }
  }

  motors.halt();
}