// Include Files

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include "trulla.h"
#include "loadws.h"
#include "tr2robot.h"
#include "window.h"
#include "propagat.h"
#include "motors.h"
#include "misc.h"
#include "vfh.h"

main(int argc, char *argv[])
{
  Motors motors;
  Sonar sonar;
  VFH vfh(&sonar, &motors);

  int choice;
  int haveCurrCell = 0;
  int have_goal = 0;
  
  LOCATION goal_location;
  LOCATION curr_location;
  DISPLACEMENT subgoal_displacement;
  Vector desired_vector, motion_vector;
  float magnitude = 0;
  float tolerance = 1;
  float x, y, theta;
  float speed = 0.3;

  showGrid = 1;
  pathsAreDisplayed = 0;
  
  Window *trulla_win = new Window(320, 0, 620, 300);
  Window *vfh_win = new Window(0, 0, 300, 300);

  InitializeDisplay (trulla_win);
  
  LoadFile(trulla_win, argv[1]);
  
  motors.init();
  motors.home();
  vfh.start();

  do {
    if(kbhit()){
      choice = getch();
    
      motors.drive(0);
      have_goal = 0;

      switch ( choice ) {

      case 'T' : case 't' :
	printf("Enter new tolerance in feet: ");
	scanf("%f", &tolerance);
	break;

      case 'M' : case 'm' :
	printf("Enter new magnitude: ");
	scanf("%f", &magnitude);
	if(magnitude > 5){
	  magnitude = 5;
	  printf("I won't accept magnitudes above 5. Using 5.\n");
	}
	break;

      case 'L' : case 'l' :	// load a new workspace file
	LoadFile (trulla_win, NULL);
	break;
	
      case 72 :			// up arrow
	MoveUp (trulla_win);
	break;
	
      case 75 :			// left arrow
	MoveLeft (trulla_win);
	break;
	
      case 77 :			// right arrow
	MoveRight (trulla_win);
	break;
	
      case 80 :			// down arrow
	MoveDown (trulla_win);
	break;
	
	// generate a path if the goal has changed or if the cell is not an obstacle
      case 'G' : case 'g' :
	if ( (gridPtr+gridelOffset)->r_weight != MAXWGT ) {
	  GeneratePaths (trulla_win);
	  pathsAreDisplayed = 1;
	  goal_location.x = goalCellX * feetPerGridel;
	  goal_location.y = goalCellY * feetPerGridel;
	  have_goal = 1;
	}
	break;
      }
    }
    else{
      // Robot control portion of the main loop.

      if(have_goal){

	motors.getxy(&x, &y, &theta);
	vfh.tell_pose(x, y, 0);

	curr_location.x = x;
	curr_location.y = y;

	if(IsGoal(&goal_location, &curr_location, tolerance)){
	  printf("I've reached the goal! Woo-HOO!\n");
	  have_goal = 0;
	}
	else if(NextSubGoal(&curr_location, &subgoal_displacement)){

	  desired_vector.dir = atan2(subgoal_displacement.y,
				     subgoal_displacement.x);

	  desired_vector.mag = magnitude;

	  vfh.graph(vfh_win, desired_vector);
	  
	  motion_vector = vfh.vector(desired_vector);
	  
	  if(vfh.out_of_bounds()){
	    fprintf(stderr, "Robot out of bounds.\n");
	    motors.drive(0);
	  }
	  else if(motion_vector.mag == 0){
	    fprintf(stderr, "BLOCKED!\n");
	    motors.drive(0);
	  }
	  else{
	    if(fabs(FixAngle(theta - motion_vector.dir)) > .7){
	      motors.drive(0);
	      motors.turnAbs(motion_vector.dir);
	      motors.drive(motion_vector.mag * speed);
	    }
	    else{
	      motors.drive(motion_vector.mag * speed);
	      motors.turnAbs(motion_vector.dir);
	    }
	  }
	}
	else{
	  fprintf(stderr, "Warning: No subgoal.\n");
	  motors.drive(0);
	}
      }      
    }
  } while ( choice != 'Q' && choice != 'q' );
  
  set_text_mode();
  vfh.stop();
  motors.halt();
}