// 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 <iostream.h>
#include <stdlib.h>
#include <conio.h>
#include <math.h>
#include <time.h>
#include <sys\timeb.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"

MOVEMENT* first;						// linked queue of past movements
const int MAXMOVES=2;				// length of linked queue
extern int wsWidthInCells;	// size of workspace
extern int wsHgtInCells;
int reps;										
void ClearFirst();					// initializes linked list
int total_moves=0;
int cheating=0;
LOCATION curr_grid_loc;

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;
}
//////////////////////////////////////////////////////////////////////
//		Function: CheckCloseObs
//		Purpose: 	For each added unmodeled obstacle, CheckCloseObs
//				checks if there are any other obstacles within a distance
//				less than the robot's diameter. If an obstacle is found
//				within that distance, all of the gridels in between
//				are given obstacle status.
////////////////////////////////////////////////////////////////////

void CheckCloseObs(VFH vfh, int h, int w){
	int offs,x_coord,y_coord;
	float m,ox,oy;
	float r_diam=vfh.robot_diameter();
	float rat = r_diam/feetPerGridel;
	int range = (int) rat + 1;
	int stht = (h-range<0) ? 0:(h-range);		// checks for obstacles
	int stwdt = (w-range<0) ? 0:(w-range);	//  within a certain range
	int endht = (h+range>(wsHgtInCells-1)) ? (wsHgtInCells-1):(h+range);
	int endwdt = (w+range>(wsWidthInCells-1)) ? (wsWidthInCells-1):(w+range);
	for (int hc = stht; hc<=endht; hc++)
		for (int wc = stwdt; wc<=endwdt; wc++) {
			offs = wsWidthInCells*hc+wc;
			if ((abs(hc - h)>1) || (abs(wc - w)>1))
				if ((gridPtr+offs)->r_weight>=14){
					x_coord=wc; y_coord=hc;
					for (int i=1; i<=range; i++) {
					m = sqrt((h-y_coord)*(h-y_coord)+(w-x_coord)*(w-x_coord));
					if ((m<=r_diam)&&(m!=0.0)){
						ox = 1.5*(w-x_coord)/m; oy = 1.5* (h-y_coord)/m;
						x_coord+= (int) ox; y_coord+= (int) oy;
						if ((gridPtr+wsWidthInCells*y_coord+x_coord)->r_weight!=15)
						(gridPtr+wsWidthInCells*y_coord+x_coord)->r_weight=14;
					}
				}
				}
		}
	}
////////////////////////////////////////////////////////////////////////
//	Function:	RePlan
//	Purpose: 	Adds unmodeled obstacles to the map by finding the average
//				occupancy of the occupancy grid elements contained in each
//				map gridel. If any one occupancy grid element carries a value
//				above eleven, the whole gridel is deemed an obstacle.
//////////////////////////////////////////////////////////////////////

 void RePlan(VFH vfh, Window* win){
		int ratio,tag,start;
		float sum, avg;
		char occ;
		struct timeb s,f;
		time_t str,fir;
	 	int range = 8;
		int stht = ((!cheating)||((curr_grid_loc.y - range)<0)) ? 0:(curr_grid_loc.y-range);
		int stwdt = ((!cheating)||((curr_grid_loc.x - range)<0)) ? 0:(curr_grid_loc.x-range);
		int endht = ((!cheating)||((curr_grid_loc.y + range)>(wsHgtInCells-1))) ? (wsHgtInCells-1):(curr_grid_loc.y+range);
		int endwdt =((!cheating)||((curr_grid_loc.x + range)>(wsWidthInCells-1))) ? (wsWidthInCells-1):(curr_grid_loc.x+range);
		//cout<<"Replanning!\n";
		ftime(&s);
		str=time(NULL);
		start=vfh.gridSize()/2;
		ratio=(int) vfh.gridunitsperfoot()*feetPerGridel;
					// determines the number of occugrid cells per map grid cell
		for (int w=stwdt; w<=endwdt; w++)	//for every map cell,
		for (int h=stht; h<=endht; h++){
		sum=0.0; tag=0;
		for (int k=0; k<ratio; k++)				// it sums the occupancies of
			for (int l=0; l<ratio; l++){		// the contained occugrid cells
						occ=vfh.read(start+w*ratio+k,start+h*ratio+l);
						sum+=occ;
						if(occ>11) tag=1;}				// if any one occupancy is greater
			avg=sum/(ratio*ratio);					// than 11, the whole gridel is
			if (tag==1){ 										// deemed an obstacle
										avg=14.0;} 
			if (avg<1.0) avg=1.0;
			if( (avg>=1.0) &&((gridPtr+wsWidthInCells*h+w)->r_weight!=15))
		 (gridPtr+wsWidthInCells*h+w)->r_weight=(int) avg;
		 if (tag==1) CheckCloseObs(vfh,h,w);
		}
		ReDrawGrid(win);
		GeneratePaths(win,1);
		ftime(&f);
		fir=time(NULL);
		reps++;
		//DrawPastMoves(win);
		//cout<<total_moves<<endl;
		//cout<<difftime(fir,str)<<"."<<abs(f.millitm-s.millitm)<<endl;
}
////////////////////////////////////////////////////////////////////
//	Function: AddandCheck
//	Purpose:	Adds the MAXMOVES past moves in the linked list
// 				and compares that vector sum to the "desired vector"
//				as computed in the main function. If the dot product
//				is negative, the program replans.
//////////////////////////////////////////////////////////////////

int AddandCheck(float desx, float desy, VFH vfh, Window* win){
	DOT sum;
	MOVEMENT* temp;
	float dotprod;
	int a,b;
	sum.x=0.0; sum.y=0.0;
	temp=first;
	do {		// adds up the vectors in the movement vector queue
		sum.x+=temp->x; sum.y+=temp->y;
		temp=temp->next;
	} while (temp!=NULL);
	a=desx; b=desy;
	dotprod=(desx)*(sum.x) + (desy)*(sum.y); // computes dot product
	if (dotprod<0.0) {				// and replans if negative.
		RePlan(vfh,win);
		ClearFirst();
		return 1;
	}
	else return 0;
}
/////////////////////////////////////////////////////////////////////
//	Function: InsertMovement
//	Purpose:	Adds a past move to the movement vector queue.
///////////////////////////////////////////////////////////////////
void InsertMovement (float motx, float moty) {
	int j=1;
	MOVEMENT* temp;
	MOVEMENT* oldfirst;
	if ((first->x==0)&&(first->y==0)) {
		first->x=motx; first->y=moty; first->next=NULL;}
		else {
			MOVEMENT* node = new MOVEMENT;
			temp=first;
			node->x=motx; node->y=moty; node->next=NULL;
			while ((temp->next!=NULL)&&(j<MAXMOVES)) {
				temp=temp->next; j++;}
			temp->next=node;
			if (j==MAXMOVES) {
				oldfirst=first;
				first=first->next;
				delete oldfirst;}
		}
}
////////////////////////////////////////////////////////////////////
// Function: ChangeTail
// Purpose: If the program replans its path, the last movement in
//		the movement vector queue needs to be changed to the new
//  	value.
//////////////////////////////////////////////////////////////////////

void ChangeTail (float motx, float moty) {
	MOVEMENT* temp;
	temp=first;
	while (temp->next!=NULL) temp=temp->next;
	temp->x=motx; temp->y=moty;
}
///////////////////////////////////////////////////////////////////
// Functions: InitializeFirst and ClearFirst
// Purpose:	To set the movement vector queue to contain all zeros.
///////////////////////////////////////////////////////////////////

void InitializeFirst() {
	first->x=first->y=0.0;
	first->next=NULL;}

void ClearFirst(){
	MOVEMENT* temp=first;
	do {
	temp->x=temp->y=0.0;
	temp=temp->next;}
	while (temp!=NULL);
}

void RandObs(){
	float perc=0.05;
	int off, x, y, counter=0;
	int maxno=(int) (perc*wsWidthInCells*wsHgtInCells);
	cout<<maxno<<endl;
	srand(time(NULL));
	while(counter<maxno){
		x=rand() % wsWidthInCells;
		y=rand() % wsHgtInCells;
		off=wsWidthInCells*y+x;
		if((gridPtr+off)->r_weight!=15) 
			 if ((x!=goalCellX)&&(y!=goalCellY))
			 {(gridPtr+off)->r_weight=14;
			  counter++;}
		  }
	}

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

  enum {VFH_C, TRULLA_C} which_control = TRULLA_C;
  int choice;
  char algch;
	int haveCurrCell = 0;
  int have_goal = 0;
	int paths_started=0;
	int dot_prod_alg=0;
	int five_second=0;
	int continuous=0;
	float prevx,prevy,interval;
	float total_distance_traveled=0.0;
	time_t fi,st,check, prev_time;
  LOCATION goal_location;
  LOCATION curr_location;
  DISPLACEMENT subgoal_displacement;
  Vector desired_vector, motion_vector;
  float magnitude = 1;
  float tolerance = 1.5;
  float x, y, theta, body_angle;
  float speed_factor, speed = 0.3;
	float motx, moty, desx, desy;
  showGrid = 1;
  pathsAreDisplayed = 0;
  free (first);
	first=(MOVEMENT*) malloc (sizeof(MOVEMENT)*MAXMOVES);
	InitializeFirst();
  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");
	cout<<"Enter speed: "; cin>>speed_factor;
	speed*=speed_factor;
	cout<<"Algorithm choice (d, f, or c): "; cin>>algch;
	switch(algch){
		case 'd':
			dot_prod_alg=1;
			cheating=0;
			break;
		case 'D':
			dot_prod_alg=1;
			cheating=1;
			break;
		case 'f':
			five_second=1;
			cout<<"Enter time interval: "; cin>>interval;
			break;
		case 'c':
			continuous=1;
			cheating=0;
				break;
		case 'C':
			continuous=1;
			cheating=1;
			break;
	}
  ask_coords(&x, &y, &body_angle);
  prevx=x; prevy=y;
  // 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);
  total_moves=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);
	total_moves=DrawCurrentLocation(trulla_win, &curr_grid_loc);

	break;

      case 'L' : case 'l' :	// load a new workspace file
	LoadFile (trulla_win, NULL);
	InitializeFirst();
	break;
      case 'G' : case 'g' :
			st=time(NULL);
			paths_started=1;
			prev_time=st;
//			RandObs();
	if(which_control == TRULLA_C){
		InitializeFirst();
	  // generate a path if the cell is not an obstacle
	  if ( (gridPtr+gridelOffset)->r_weight != MAXWGT ) {
	    GeneratePaths (trulla_win,0);
	    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);
	total_distance_traveled+=sqrt((x-prevx)*(x-prevx)+(y-prevy)*(y-prevy));
	prevx=x;
	prevy=y;

	vfh.tell_pose(x, y, body_angle);
	
	curr_location.x = x;
	curr_location.y = y;
	
	FeetToGridels(&curr_location, &curr_grid_loc);
	total_moves=DrawCurrentLocation(trulla_win, &curr_grid_loc);

	if(IsGoal(&goal_location, &curr_location, tolerance)){
	  printf("AT goal in ");
		fi=time(NULL);
		cout<<difftime(fi,st)<<" s ";
		cout<<total_distance_traveled<<" ft "<<reps<<" reps"<<endl;
	  motors.drive(0);
	  have_goal = 0;
	}
	else{
	  if(which_control == TRULLA_C &&
	     !NextSubgoal(&curr_location, &subgoal_displacement)){
	    fprintf(stderr, "Warning: No subgoal ");
			fi=time(NULL);
			cout<<difftime(fi,st)<<" sec and "; 
			cout<<total_distance_traveled<<endl;
			RePlan(vfh,trulla_win);		// dot prod here, too :)
	    motors.drive(0);
	  }
	  else{
	    if(which_control == TRULLA_C){
				desx=subgoal_displacement.x;
				desy=subgoal_displacement.y;
			}
			else{
				desx=goal_location.x - curr_location.x;
				desy=goal_location.y - curr_location.y;
			}
			desired_vector.dir=atan2(desy,desx);
	    desired_vector.mag = magnitude;
	    vfh.graph(vfh_win, desired_vector);
	    motion_vector = vfh.vector(desired_vector);
//	 dot product method starts here!!
if (dot_prod_alg) {
	    motx=motion_vector.mag*cos(motion_vector.dir)/feetPerGridel;
		moty=motion_vector.mag*sin(motion_vector.dir)/feetPerGridel;
			InsertMovement(motx,moty);	// adds movement vector to queue
			if (AddandCheck(desx,desy,vfh,trulla_win)==1){
															// if dot product was negative,
			  if(which_control == TRULLA_C &&
	     !NextSubgoal(&curr_location, &subgoal_displacement)){
			 												// then find the next subgoal
		    fprintf(stderr, "Warning: No subgoal ");
				cout<<total_distance_traveled<<endl;
				RePlan(vfh,trulla_win);
		    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);
	    motx=motion_vector.mag*cos(motion_vector.dir)/feetPerGridel;
			moty=motion_vector.mag*sin(motion_vector.dir)/feetPerGridel;
			ChangeTail(motx,moty);		// change the queue to reflect the
		} }													// new movement vector.
}
// End of dot product method
				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);
				if (dot_prod_alg) RePlan(vfh, trulla_win); //oh, amd dot prod here, too
	    }
	    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);
		if (continuous)	RePlan(vfh,trulla_win); //continuous algorithm
		    }
	      else{
		motors.drive(motion_vector.mag * speed);
		motors.turnAbs(motion_vector.dir);
		if (continuous)		RePlan(vfh,trulla_win); //continuous algorithm
	      }
	    }
	  }
	}
      }
    }
		if (five_second) {
		check=time(NULL);			//5 second rule
				if ((paths_started)&&(difftime(check,prev_time)>=interval))	//5 s
					{RePlan(vfh,trulla_win);	prev_time=check;}		//5 s
		}
  } while ( choice != 'Q' && choice != 'q' );
  
  set_text_mode();
  vfh.stop();
  motors.halt();
}