// Include Files

#include <math.h>
#include "trulla.h"
#include "tr2robot.h"
#include <iostream.h>

// Function Prototypes

void FeetToGridels (LOCATION * locationInFeet, LOCATION * locationInGridels);
void GridelsToFeet(DISPLACEMENT*, DISPLACEMENT*);
void ReDrawGrid ();
int CheckAhead(LOCATION, float, float);
int CheckStuck(LOCATION, float &, float &);

// External Declarations

extern int  showGrid;
extern float feetPerGridel;
extern int  pathsAreDisplayed;
extern int  goalCellX, goalCellY;
extern int  currCellX, currCellY;
extern int  currCellXInPix, currCellYInPix;
extern int  cellWidthInPix, cellHgtInPix;
extern int  gridelOffset;
extern int  wsWidthInCells, wsHgtInCells;
extern GRIDEL * gridPtr;
extern OBST* head;
extern void DisplayOptions ();
extern void DrawGrid ();
extern void GeneratePaths (int);
extern void DrawGoalIndicator ();
extern void ColorCell (int left, int top, int right, int bottom,
				GRIDEL * tempGridPtr );
extern void RePlan();

/////////////////////////////////////////////////////////////////////
//   Function:  SetGoal
//   Purpose:  Sets the goal position on the grid.  It converts the
//      input goal location in feet to a goal location in gridels.
//      If the goal has been successfully set, it returns OKAY (1);
//      otherwise, it returns NOT OKAY (0).
/////////////////////////////////////////////////////////////////////

int SetGoal ( LOCATION * goalLocInFeet )
{
	LOCATION goalLocInGridels;
	int newGridOffset, oldGridOffset;
	GRIDEL * newGridPtr, * oldGridPtr;
	OBST* newObsPtr;

// convert goal location from feet to gridels (the x and y values
// representing the location in gridels are integers although they
// are stored in floats -- LOCATION has float data items)
	FeetToGridels ( goalLocInFeet, &goalLocInGridels );

// check if goal selection is on the grid
	if ( (int) goalLocInGridels.x >= wsWidthInCells ||
		  (int) goalLocInGridels.y >= wsHgtInCells )
		return (!OKAY);

	newGridOffset = (int) goalLocInGridels.y * wsWidthInCells +
						  (int) goalLocInGridels.x;
	newGridPtr = gridPtr + newGridOffset;
	newObsPtr=head+newGridOffset;
// check if goal selection is an obstacle
	if (( newGridPtr->r_weight == MAXWGT )||(newObsPtr->full==1))
		return (!OKAY);

// check if paths are already displayed
	if ( (int) goalLocInGridels.x == currCellX &&
		  (int) goalLocInGridels.y == currCellY && pathsAreDisplayed )
		return (OKAY);

// erase old paths
	if ( pathsAreDisplayed && showGrid )
		ReDrawGrid ();

// remove old goal indicator
	if ( showGrid ) {
		oldGridOffset = currCellY * wsWidthInCells + currCellX;
		oldGridPtr = gridPtr + oldGridOffset;
		ColorCell (currCellXInPix, currCellYInPix,
			currCellXInPix + cellWidthInPix, currCellYInPix + cellHgtInPix,
			oldGridPtr );
	}

// change current cell variables to goal cell
	currCellX = (int) goalLocInGridels.x;
	currCellY = (int) goalLocInGridels.y;
	currCellXInPix = currCellX * cellWidthInPix;
	currCellYInPix = currCellY * cellHgtInPix;

// set pointer to goal in gridel array
	gridelOffset = currCellY * wsWidthInCells + currCellX;

// draw new goal indicator
	if ( showGrid )
		DrawGoalIndicator ();

// generate paths to the new goal
	GeneratePaths (0);

	pathsAreDisplayed = 1;

	return OKAY;
}


/////////////////////////////////////////////////////////////////////
//   Function:  FeetToGridels
//   Purpose:  Converts the first location parameter from feet to
//      gridels and stores the result in the second parameter.
/////////////////////////////////////////////////////////////////////

void FeetToGridels ( LOCATION * locationInFeet,	LOCATION * locationInGridels)
{
	float gridelX, gridelY;

	gridelX = locationInFeet->x / feetPerGridel;
	gridelY = locationInFeet->y / feetPerGridel;

	locationInGridels->x = gridelX;
	locationInGridels->y = gridelY;
}

void GridelsToFeet(DISPLACEMENT* dispInFeet, DISPLACEMENT* dispInGridels)
{
	dispInFeet->x=dispInGridels->x*feetPerGridel;
	dispInFeet->y=dispInGridels->y*feetPerGridel;
}
/////////////////////////////////////////////////////////////////////
//   Function:  NextSubgoal
//   Purpose:  Finds the next subgoal from the current cell.  If a
//      subgoal has been found, it returns OKAY (1); otherwise, it
//      returns NOT OKAY (0).
/////////////////////////////////////////////////////////////////////

int NextSubgoal (LOCATION* currLocInFeet, DISPLACEMENT* subgoalDispInGridels,
					int &tempGridelOffset, int &stuck)
{
	float magn;
	float onex=0.0;
	float oney=0.0;
	LOCATION currLocInGridels;
	int currx, curry, offset;
	GRIDEL * tempGridPtr;
	OBST* tempObsPtr;
	stuck=0;
	FeetToGridels (currLocInFeet, &currLocInGridels);
	tempGridelOffset=(int) currLocInGridels.y * wsWidthInCells +
		(int) currLocInGridels.x;
	tempGridPtr=gridPtr+tempGridelOffset;
	tempObsPtr=head+tempGridelOffset;
	if ((tempGridPtr->delx!=0)||(tempGridPtr->dely!=0)){
		magn=hypot(tempGridPtr->delx, tempGridPtr->dely);
		onex=(tempGridPtr->delx)/magn;
		oney=(tempGridPtr->dely)/magn;
		}
// check if the current location is on the grid
	if ( (int) currLocInGridels.x >= wsWidthInCells ||
		  (int) currLocInGridels.y >= wsHgtInCells )
		return (!OKAY);

// check if there is a subgoal or goal to go to from the current cell
	if ( tempGridPtr->delx == 0 && tempGridPtr->dely == 0 )
		return ( !OKAY );

// check if the current cell is an obstacle
	if(( tempGridPtr->r_weight == MAXWGT )||(tempObsPtr->full==1))
		return (!OKAY);

// check if the next cell is an obstacle or is off the grid
if (CheckAhead(currLocInGridels,onex,oney)==0){
	stuck=CheckStuck(currLocInGridels, onex, oney);
}
	if (stuck==1) {return(OKAY);}
	else{
	subgoalDispInGridels->x=-onex;
	subgoalDispInGridels->y=-oney;
	return ( OKAY );
	}
}



/////////////////////////////////////////////////////////////////////
//   Function:  ReDrawGrid
//   Purpose:  Redraws the grid, the options line at the bottom of
//      the screen, and the goal indicator.  Initializes the goal
//      variables to indicate there is no valid goal at the moment.
/////////////////////////////////////////////////////////////////////

void ReDrawGrid ()
{
	DrawGrid ();
	DisplayOptions ();
	DrawGoalIndicator ();
	goalCellX = goalCellY = -1;
}

///////////////////////////////////////////////////////////////////
// Function: CheckAhead
// Purpose: Checks the next cell to see if it is a modeled obstacle,
// 	an unmodeled obstacle, or if it is off the grid.
//////////////////////////////////////////////////////////////////

int CheckAhead(LOCATION current, float onex, float oney){
	int currx,curry, offset;
	current.x-=onex;
	current.y-=oney;
	currx=(int) current.x;
	curry=(int) current.y;
	offset=curry*wsWidthInCells + currx;
	if (((head+offset)->full==1)||((gridPtr + offset)->r_weight==MAXWGT)||
		(abs(currx)>=wsWidthInCells)||(abs(curry)>=wsHgtInCells)){
		 return 0;}
	else return 1;
}

/////////////////////////////////////////////////////////////////////
//   Function:  IsGoal
//   Purpose:  Determines if the current location is the goal location.
//      If so, TRUE (1) is returned; if not, FALSE (0) is returned.
//      The tolerance is the number of feet between which the current
//      and goal locations may differ and still allow the function to
//      conclude that the goal and current locations are the same.
/////////////////////////////////////////////////////////////////////

int IsGoal ( LOCATION * goalLocInFeet, LOCATION * currLocInFeet,
	float tolerance )
{
//	LOCATION goalLocInGridels, currLocInGridels;
//	float toleranceInGridels;
	float xDispl, yDispl, hypotenuse;

//	FeetToGridels ( goalLocInFeet, &goalLocInGridels );
//	FeetToGridels ( currLocInFeet, &currLocInGridels );

//	toleranceInGridels = tolerance / feetPerGridel;
	xDispl = goalLocInFeet->x - currLocInFeet->x;
	yDispl = goalLocInFeet->y - currLocInFeet->y;
	hypotenuse = hypot ( xDispl, yDispl );
	if ( hypotenuse <= tolerance )
		return (TRUE);
	else
		return (FALSE);
/*
	if ( (int) goalLocInGridels.x == (int) currLocInGridels.x &&
		  (int) goalLocInGridels.y == (int) currLocInGridels.y )
		return (TRUE);
	else
		return (FALSE);
*/
}


/////////////////////////////////////////////////////////////////////
//   Function:  AdvanceCurrLocation
//   Purpose:  Advances the current location to the next subgoal.
//      The current location in feet and the displacement in feet to
//      the next subgoal are passed as parameters.  The displacement
//      is added to the current location to advance the current
//      location to the next subgoal.
/////////////////////////////////////////////////////////////////////

void AdvanceCurrLocation (LOCATION * currLocInFeet,
	DISPLACEMENT * subgoalDispInFeet){
	currLocInFeet->x += subgoalDispInFeet->x;
	currLocInFeet->y += subgoalDispInFeet->y;
}

//////////////////////////////////////////////////////////////////////
// Function: CheckStuck
// Purpose: If the next sequential gridel is occupied or off the
//			grid, CheckStuck checks the two closest cells to the left
// 		and right of the occupied cell. If both of these are
//			occupied, the robot is "stuck" and the function returns 1.
/////////////////////////////////////////////////////////////////////

int CheckStuck(LOCATION currLocInGridels, float &onex, float &oney){
	const float sin45=sqrt(2.0)/2.0;
	float sine=oney;
	 if (abs(onex)==abs(oney)){
		if (onex<0){
			if (oney>0)
			  if(CheckAhead(currLocInGridels,-1.0,0.0)==1)
								{onex=-1; oney=0;}
				 else if(CheckAhead(currLocInGridels,0.0,1.0)==1)
								{onex=0; oney=1;}
					  else {cout<<"STUCK1!!\n"; return 1;}

			if (oney<=0)
			  if(CheckAhead(currLocInGridels,-1.0,0.0)==1)
								{onex=-1; oney=0;}
				else if (CheckAhead(currLocInGridels,0.0,-1.0)==1)
								{onex=0; oney=-1;}
				  else {cout<<"STUCK2!!\n"; return 1;}

		}else if (onex>0){
		 if (oney>0)
			if (CheckAhead(currLocInGridels,0.0,1.0)==1)
								{onex=0; oney=1;}
			 else if(CheckAhead(currLocInGridels,1.0,0.0)==1)
								{onex=1; oney=0;}
			  else {cout<<"STUCK3!!\n"; return 1;}

		  else if (oney<=0)
			if (CheckAhead(currLocInGridels,1.0,0.0)==1)
								{onex=1; oney=0;}
			 else if(CheckAhead(currLocInGridels,0.0,-1.0)==1)
								{onex=0; oney=-1;}
				else {cout<<"STUCK4!!\n";  return 1; }
		}
		}else{
		if(abs(sine)<sin45){
			  if(onex<0)
				 if (CheckAhead(currLocInGridels,-sin45, sin45)==1)
								{onex=-sin45; oney=sin45;}
					else if(CheckAhead(currLocInGridels,-sin45,-sin45)==1)
								{onex=-sin45; oney=-sin45;}
					 else {cout<<"STUCK5!!\n"; return 1;}

			  else if (onex>=0)
				 if(CheckAhead(currLocInGridels,sin45,-sin45)==1)
								{onex=sin45; oney=-sin45;}
					else if(CheckAhead(currLocInGridels,sin45,sin45)==1)
								{onex=sin45; oney=sin45;}
					 else {cout<<"STUCK6!!\n"; return 1;}
		}
		else {
			 if(abs(sine)>sin45){
			  if(sine>0)
				if(CheckAhead(currLocInGridels,-sin45, sin45)==1)
								{onex=-sin45; oney=sin45;}
				 else if(CheckAhead(currLocInGridels,sin45,sin45)==1)
								{onex=sin45; oney=sin45;}
				  else {cout<<"STUCK7!!\n"; return 1;}

			  else if (sine<=0)
				if(CheckAhead(currLocInGridels,-sin45,-sin45)==1)
								{onex=-sin45; oney=-sin45;}
				 else if(CheckAhead(currLocInGridels,sin45,-sin45)==1)
								{onex=sin45; oney=-sin45;}
				  else {cout<<"STUCK8!!\n";  return 1;}
			}
		}
	}
return 0;
}