// trulvfh.cpp
//
// A program to test the Trulla path planner in combination with
// VFH.
//
// by Dave Hershberger, but the trulla menu stuff was mostly lifted from
// Eva's trulla.cpp.


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

void ask_coords(float *x, float *y, float *body_angle_rads)
{
  int body_angle_degs;

  printf("Please enter the robot's current coordinates.\n"
	 "Current X (in feet) > ");
  scanf("%f", x);
  printf("Current Y (in feet) > ");
  scanf("%f", y);

  printf("\nPlease enter the angle between clem's sonar 0 and the\n");
  printf("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;
}

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

  enum {VFH_C, TRULLA_C} which_control = TRULLA_C;
  int choice;
  int haveCurrCell = 0;
  int have_goal = 0;
  
  LOCATION goal_location;
  LOCATION curr_location;
  LOCATION curr_grid_loc;
  DISPLACEMENT subgoal_displacement;
  Vector desired_vector, motion_vector;
  float magnitude = 1;
  float tolerance = 1.5;
  float x, y, theta, body_angle;
  float speed = 0.3;

  showGrid = 1;
  pathsAreDisplayed = 0;
  
  Window *trulla_win = new Window(510, 0, 1010, 500);
  Window *vfh_win = new Window(100, 0, 500, 400);

  motors.init();
  motors.home();

  printf("Using Trulla path planner with VFH to begin with.\n\n");

  ask_coords(&x, &y, &body_angle);
  
  // Since we just homed the robot, telling the motors they are turned
  // to body_angle radians means that angle 0 will line up with the
  // +X axis.
  motors.setxy(x, y, body_angle);

  vfh.start();

  vfh.tell_pose(x, y, body_angle);

  InitializeDisplay (trulla_win);
  LoadFile(trulla_win, argv[1]);
  
  curr_location.x = x;
  curr_location.y = y;
  FeetToGridels(&curr_location, &curr_grid_loc);
  DrawCurrentLocation(trulla_win, &curr_grid_loc);

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

      switch ( choice ) {

      case 'I' : case 'i' :
	// grab an image of the Trulla window and write it to a file.

	{
	  char *screen_shot = new char[500 * 500 * 3];
	  Color color;

	  for(int y = 0; y < 500; y++)
	    for(int x = 0; x < 500; x++){
	      color = trulla_win->get_pixel(x, y);
	      screen_shot[(500 * y + x) * 3] = color.red();
	      screen_shot[(500 * y + x) * 3 + 1] = color.green();
	      screen_shot[(500 * y + x) * 3 + 2] = color.blue();
	    }

	  char buf[100];
	  printf("Enter image filename: ");
	  scanf("%s", buf);

	  FILE *f = fopen(buf, "wb");
	  if(!f)
	    fprintf(stderr, "Can't open file %s for writing.\n", buf);
	  else
	    fwrite(screen_shot, 3, 500 * 500, f);

	  free(screen_shot);
	}
	break;

      case 'T' : case 't' :
	// switch to using Trulla and VFH.
	which_control = TRULLA_C;
	printf("Now using Trulla path planner with VFH.\n");
	break;

      case 'V' : case 'v' :
	// switch to using VFH only (with a move-to-goal).
	which_control = VFH_C;
	printf("Now using VFH with a simple move-to-goal.\n");
	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");
	}
	if(magnitude <= 0 ){
	  magnitude = .1;
	  printf("I won't accept non-positive magnitudes. Using 0.1.\n");
	}
	break;

      case 'C' : case 'c' :
	ask_coords(&x, &y, &body_angle);
	motors.setxy(x, y, body_angle);
	curr_location.x = x;
	curr_location.y = y;
	
	DrawGrid(trulla_win);
	FeetToGridels(&curr_location, &curr_grid_loc);
	DrawCurrentLocation(trulla_win, &curr_grid_loc);

	break;

      case 'L' : case 'l' :	// load a new workspace file
	LoadFile (trulla_win, NULL);
	break;
	
      case 'G' : case 'g' :
	if(which_control == TRULLA_C){
	  // generate a path if the cell is not an obstacle
	  if ( (gridPtr+gridelOffset)->r_weight != MAXWGT ) {
	    GeneratePaths (trulla_win);
	    pathsAreDisplayed = 1;
	    goal_location.x = goalCellX * feetPerGridel;
	    goal_location.y = goalCellY * feetPerGridel;
	    have_goal = 1;
	  }
	}
	else{
	  pathsAreDisplayed = 0;
	  trulla_win->clear();
	  DrawGrid(trulla_win);
	  DrawGoalIndicator(trulla_win);
	  goalCellX = currCellX;
	  goalCellY = currCellY;
	  goal_location.x = goalCellX * feetPerGridel;
	  goal_location.y = goalCellY * feetPerGridel;
	  have_goal = 1;
	}
	break;

      case 0:
	// The arrow keys generate 2 characters: a 0, then one of
	// the following numbers.

	choice = getch();
	switch (choice){

	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;
	}
	
      }
    }
    else{
      // Robot control portion of the main loop.
      
      if(have_goal){
	
	motors.getxy(&x, &y, &theta);
	vfh.tell_pose(x, y, body_angle);
	
	curr_location.x = x;
	curr_location.y = y;
	
	FeetToGridels(&curr_location, &curr_grid_loc);
	DrawCurrentLocation(trulla_win, &curr_grid_loc);

	if(IsGoal(&goal_location, &curr_location, tolerance)){
	  printf("I've reached the goal! Woo-HOO!\n");
	  motors.drive(0);
	  have_goal = 0;
	}
	else{
	  if(which_control == TRULLA_C &&
	     !NextSubgoal(&curr_location, &subgoal_displacement)){
	    fprintf(stderr, "Warning: No subgoal.\n");
	    motors.drive(0);
	  }
	  else{
	    if(which_control == TRULLA_C)
	      desired_vector.dir = atan2(subgoal_displacement.y,
					 subgoal_displacement.x);
	    else
	      desired_vector.dir = atan2(goal_location.y - curr_location.y,
					 goal_location.x - curr_location.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(motion_vector.mag * speed * .4);
		motors.turnAbs(motion_vector.dir);
		motors.drive(motion_vector.mag * speed);
	      }
	      else{
		motors.drive(motion_vector.mag * speed);
		motors.turnAbs(motion_vector.dir);
	      }
	    }
	  }
	}
      }
    }
  } while ( choice != 'Q' && choice != 'q' );
  
  set_text_mode();
  vfh.stop();
  motors.halt();
}