// Include Files

#include <math.h>
#include "trulla.h"
#include "tr2robot.h"
#include "loadws.h"
#include "propagat.h"
#include "window.h"

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

/////////////////////////////////////////////////////////////////////
//   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 ( Window *win, LOCATION * goalLocInFeet )
{
	LOCATION goalLocInGridels;
	int newGridOffset, oldGridOffset;
	GRIDEL * newGridPtr, * oldGridPtr;

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

// check if goal selection is an obstacle
	if ( newGridPtr->r_weight == MAXWGT )
		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 (win);

// remove old goal indicator
	if ( showGrid ) {
		oldGridOffset = currCellY * wsWidthInCells + currCellX;
		oldGridPtr = gridPtr + oldGridOffset;
		ColorCell ( win,
			   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 (win);

// generate paths to the new goal
	GeneratePaths (win,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;
}


/////////////////////////////////////////////////////////////////////
//   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 * subgoalDispInFeet)
{
	LOCATION currLocInGridels;
	int tempGridelOffset;
	GRIDEL * tempGridPtr;

	FeetToGridels ( currLocInFeet, &currLocInGridels );
	tempGridelOffset = (int) currLocInGridels.y * wsWidthInCells +
		(int) currLocInGridels.x;
	tempGridPtr = gridPtr + tempGridelOffset;

// 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 )
//		return ( !OKAY );

	subgoalDispInFeet->x = - tempGridPtr->xgoal * feetPerGridel;
	subgoalDispInFeet->y = - tempGridPtr->ygoal * feetPerGridel;
	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 (Window *win)
{
	DrawGrid (win);
	DisplayOptions ();
	DrawGoalIndicator (win);
	goalCellX = goalCellY = -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;
}