// Include Files                                                     4

#include <stdio.h>
#include <iostream.h>
#include <stdlib.h>
#include <conio.h>
#include <graphics.h>
#include <time.h>
#include "trulla.h"
#include "loadws.h"
#include "tr2robot.h"


// External Declarations

extern float    feetPerGridel;
extern int      showGrid;
extern int      maxInt;
extern float    maxFloat;
extern int      outputLine;
extern int      sizeOfCharInPix;
extern int      optionsLine;
extern int      gridelOffset;
extern int      xOffsetInPix, yOffsetInPix;  // offset to cell center from upper left of cell
extern int      currCellXInPix, currCellYInPix;
extern int      maxXCoord, maxYCoord;
extern int      cellWidthInPix, cellHgtInPix;
extern int      currCellX, currCellY;
extern int      goalCellX, goalCellY;
extern int      wsWidthInCells, wsHgtInCells;
extern int      pathsAreDisplayed;
extern GRIDEL * gridPtr;

extern void ReDrawGrid ();
extern void DrawGrid ();
extern void DisplayOptions ();
extern void DrawGoalIndicator ();
extern void ColorCell (int left, int top, int right, int bottom,
	GRIDEL * tempGridPtr );
extern void FeetToGridels ( LOCATION * locationInFeet,
	LOCATION * locationInGridels );
extern void GridelsToFeet(DISPLACEMENT*, DISPLACEMENT*);
extern void GeneratePaths (int);
extern void RePlan();

// Globals

float      toleranceInFeet;
LOCATION * goalLocation;
struct     textsettingstype far textinfo;
struct     palettetype far graypal;
OBST* 	  head;
MOVEMENT*   first;
const int MAXMOVES=4;
float 	  robot_diameter=2.4;

// Function Prototypes

void ClearDisplay ();
int  GetNextCell ( int haveCurrCell );
void DrawCurrentLocation ( LOCATION * currLocInGridels );
void InitGraphics ();
void GetMaxCoords ();
void DefineGrayScale ();
void MoveLeft ();
void MoveRight ();
void MoveUp ();
void MoveDown ();
void clearobs(int);
void InsertMovement(float, float);
int AddandCheck(int, int);
void InitializeFirst(void);
void ClearFirst(void);
void ChangeTail(float, float);
void RandObs(float);
/////////////////////////////////////////////////////////////////////
//   Function:  InitializeDisplay
//   Purpose:  Initializes the graphics system, calculates the maximum
//      allowable coordinates for the grid display, and defines the
//      gray scale for use in coloring the cells of the grid.
/////////////////////////////////////////////////////////////////////

void InitializeDisplay ()
{
	InitGraphics ();
	GetMaxCoords ();
	DefineGrayScale ();
}


/////////////////////////////////////////////////////////////////////
//   Function:  InitGraphics
//   Purpose:  Automatically detects the graphics driver and initializes
//      the graphics system.  If an error occurs during initialization,
//      report this and exit the program.
/////////////////////////////////////////////////////////////////////

void InitGraphics ()
{
	int gmode, errorcode;
	int gdriver = DETECT;

// initialize graphics mode
	initgraph(&gdriver, &gmode, "C:\\BORLANDC\\BGI");

// read result of initialization
	errorcode = graphresult();

	if (errorcode != grOk)  // an error occurred
	{
		printf("Graphics error: %s\n", grapherrormsg(errorcode));
		printf("Press any key to halt:");
		getch();
		exit (BADEXIT);             // return with error code
	}
}


/////////////////////////////////////////////////////////////////////
//   Function:  GetMaxCoords
//   Purpose:  The maxXCoord and maxYCoord variables define the maximum
//      allowable coordinates on the screen for drawing the workspace
//      grid.  The rectangle bounding the grid extends from (0,0) (upper
//      left) to (maxXCoord,maxYCoord) (lower right).  Lines at the
//      bottom of the screen are used to display the user options and
//      the statistics line; this space, therefore, is removed from the
//      allowable grid space.  In addition, the y coordinates
//      for these two lines are calculated.
/////////////////////////////////////////////////////////////////////

void GetMaxCoords ()
{
	gettextsettings (&textinfo); // gets charsize needed for DisplayOptions()
// The charsize data item of the textinfo structure is multiplied by 8
// because the bit-mapped fonts is 8x8.  The value of textinfo.charsize
// indicates how many times bigger than this size the character is.  If,
// for example, textinfo.charsize is 1, then the characters display in an
// 8x8 box; if it's 2, then a 16x16 size box is used...  See the Borland
// help information on "textsettingstype" then "charsize".
	sizeOfCharInPix = 8 * textinfo.charsize;
	maxXCoord = getmaxx();
	maxYCoord = getmaxy() - 3 * sizeOfCharInPix; // remove 3 lines from bottom
	optionsLine = maxYCoord + sizeOfCharInPix;
	outputLine = optionsLine + sizeOfCharInPix;
}


////////////////////////////////////////////////////////////////////
//   Function:  DefineGrayScale
//   Purpose:  Define the palette colors to be a gray scale from
//      black (0) to white (MAXWGT).  MAXWGT is currently set to 15.
//      This accommodates the current allowable gridel weights of 1 to 9
//      and 'x' ('x', indicating an obstacle, is read from the workspace
//      file and is stored as 15 in the weight variables.)  No provisions
//      have been made in the program to allow for more than 15 different
//      weights.
//      The gray scale lightens as the palette color numbers increase;
//      however, traversal of gridels becomes more difficult with
//      increasing weights.  An attempt to define the gray scale from
//      white (0) to black (MAXWGT) resulted in an unattractive white
//      border at the edges of the screen.  A simpler fix was to leave
//      the gray scale as currently defined (black to white) and do a
//      calculation where necessary to obtain a gray color which more
//      appropriately represents the gridel weight.
/////////////////////////////////////////////////////////////////////

void DefineGrayScale ()
{
	getpalette (&graypal);
	for ( int i = 0; i < MAXWGT; i++ )
		setrgbpalette ( graypal.colors[i], i*4, i*4, i*4 );
}


////////////////////////////////////////////////////////////////////
//   Function:  ClearDisplay
//   Purpose:  Erases paths on the grid by redrawing the grid and
//      erases the goal indicator in the current goal cell.  This
//      function is used when the goal indicator is being moved
//      to another location.
/////////////////////////////////////////////////////////////////////

void ClearDisplay ()
{
	if ( pathsAreDisplayed ) {
		ReDrawGrid ();
		pathsAreDisplayed = 0;
	}
	ColorCell (currCellXInPix, currCellYInPix,
		currCellXInPix + cellWidthInPix, currCellYInPix + cellHgtInPix,
		gridPtr + gridelOffset );
}


/////////////////////////////////////////////////////////////////////
//   Function:  MoveUp
//   Purpose:  Moves the goal indicator up the grid one cell.
/////////////////////////////////////////////////////////////////////

void MoveUp ()
{
	if ( currCellY > 0 ) {
		ClearDisplay ();
		gridelOffset -= wsWidthInCells;
		currCellY--;
		currCellYInPix -= cellHgtInPix;
		DrawGoalIndicator ();
	}
}


/////////////////////////////////////////////////////////////////////
//   Function:  MoveDown
//   Purpose:  Moves the goal indicator down the grid one cell.
/////////////////////////////////////////////////////////////////////

void MoveDown ()
{
	if ( currCellY + 1 < wsHgtInCells ) {
		ClearDisplay();
		gridelOffset += wsWidthInCells;
		currCellY++;
		currCellYInPix += cellHgtInPix;
		DrawGoalIndicator ();
	}
}


/////////////////////////////////////////////////////////////////////
//   Function:  MoveLeft
//   Purpose:  Moves the goal indicator to the left by one cell.
/////////////////////////////////////////////////////////////////////

void MoveLeft ()
{
	if ( currCellX > 0 ) {
		ClearDisplay();
		gridelOffset--;
		currCellX--;
		currCellXInPix -= cellWidthInPix;
		DrawGoalIndicator ();
	}
}


/////////////////////////////////////////////////////////////////////
//   Function:  MoveRight
//   Purpose:  Moves the goal indicator to the right by one cell.
/////////////////////////////////////////////////////////////////////

void MoveRight ()
{
	if ( currCellX + 1 < wsWidthInCells ) {
		ClearDisplay();
		gridelOffset++;
		currCellX++;
		currCellXInPix += cellWidthInPix;
		DrawGoalIndicator ();
	}
}


/////////////////////////////////////////////////////////////////////
//   Function:  DrawCurrentLocation
//   Purpose:  This function was intended to test the robot interface
//      functions.  It draws an indicator "==" in the current cell
//      to show the path that the robot follows to move from one
//      subgoal to another.
/////////////////////////////////////////////////////////////////////

void DrawCurrentLocation ( LOCATION * currLocInGridels )
{
	int tempGridelOffset;
	int currXLocInPix, currYLocInPix;

	tempGridelOffset = (int) currLocInGridels->y * wsWidthInCells +
		(int) currLocInGridels->x;

	if ( (gridPtr+tempGridelOffset)->r_weight <= 5 )
		setcolor ( BLACK );
	else
		setcolor ( WHITE );

	currXLocInPix = (int) currLocInGridels->x * cellWidthInPix + xOffsetInPix;
	currYLocInPix = (int) currLocInGridels->y * cellHgtInPix +
		yOffsetInPix - 10;
	outtextxy ( currXLocInPix, currYLocInPix, "==" );
}


/////////////////////////////////////////////////////////////////////
//   Function:  GetNextCell
//   Purpose:  This function was intended to test the robot interface
//      functions.  It allows manual input of the robot's current
//      location.  Each time this function is called, it finds the
//      next subgoal and advances the current location to this subgoal
//      until the goal is reached.
/////////////////////////////////////////////////////////////////////

int GetNextCell (int haveCurrCell)
{
	LOCATION * currLocInFeet;
	LOCATION* temp;
	LOCATION currLocInGridels;
	DISPLACEMENT* subgoalDispInFeet;
	DISPLACEMENT* subgoalDispInGridels;
	int tempGridelOffset, offset, stuck;
	if ( !haveCurrCell ) {
		printf("Enter curr cell: ");
		scanf("%f   %f", &(currLocInFeet->x), &(currLocInFeet->y));
		FeetToGridels ( currLocInFeet, &currLocInGridels );
		tempGridelOffset = (int) currLocInGridels.y * wsWidthInCells +
			(int) currLocInGridels.x;
		if ( ((gridPtr+tempGridelOffset)->r_weight != MAXWGT)  &&
			((head+tempGridelOffset)->full==0) &&
		! ( (gridPtr+tempGridelOffset)->delx == 0 &&
			 (gridPtr+tempGridelOffset)->dely == 0 ) ) {
			DrawCurrentLocation ( &currLocInGridels );
			haveCurrCell = 1;
		}
		printf("Enter tolerance: ");
		scanf("%f", &(toleranceInFeet));
	}

 if (NextSubgoal( currLocInFeet, subgoalDispInGridels, offset, stuck)){
		GridelsToFeet(subgoalDispInFeet, subgoalDispInGridels);
		InsertMovement(subgoalDispInGridels->x, subgoalDispInGridels->y);
		if (AddandCheck(offset,stuck)==1){
			if(!NextSubgoal(currLocInFeet,subgoalDispInGridels,offset,stuck))
				return 0;
			else{
				GridelsToFeet(subgoalDispInFeet, subgoalDispInGridels);
				ChangeTail(subgoalDispInGridels->x, subgoalDispInGridels->y);
			}
		//if (AddandCheck(offset)==1) {
		//				cout<<"Do we ever get here?\n";
		//				return 0;}
		}
		//cout<<"Advancing "<<subgoalDispInGridels->x<<" "<<subgoalDispInGridels->y<<endl;
		AdvanceCurrLocation ( currLocInFeet, subgoalDispInFeet );
		FeetToGridels ( currLocInFeet, &currLocInGridels );
		DrawCurrentLocation(&currLocInGridels);
		}
else{
		if ( IsGoal (goalLocation, currLocInFeet, toleranceInFeet) ) {
			printf ("REACHED GOAL\n");
			haveCurrCell = 0;
		}
		else {haveCurrCell=0; printf("No subgoal\n");}
								}
	return ( haveCurrCell );
}
///////////////////////////////////////////////////////////////////////
// Function: clearobs
// Purpose: This function clears the obstacle grid of a given size.
// 	This function is called when reloading a new workspace.
//////////////////////////////////////////////////////////////////////

void clearobs (int size){
	OBST* tempObsPtr;
	tempObsPtr=head;
	for(int s=0; s<size; s++){
		tempObsPtr->full=0;
		//tempObsPtr->vir=0;
		tempObsPtr++;}
}

////////////////////////////////////////////////////////////////////
// Function: InsertMovement
// Purpose:  Adds movement vectors to the linked queue of movement
// 		vectors; the number of vectors in the queue may not exceed
//			MAXMOVES.
///////////////////////////////////////////////////////////////////

void InsertMovement(float x, float y){
	int j=1;
	MOVEMENT* temp;
	MOVEMENT* oldfirst;
	//cout<<"inserting "<<x<<" "<<y<<endl;
	if ((first->x==0)&&(first->y==0)){
		first->x=x; first->y=y; first->next=NULL;}
	else{
		MOVEMENT* node=new MOVEMENT;
		temp=first;
		node->x=x; node->y=y; 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;
				}
		}
}

void ChangeTail(float x, float y){
	MOVEMENT* temp;
	temp=first;
	while(temp->next!=NULL) temp=temp->next;
	temp->x=x;  temp->y=y;
}
////////////////////////////////////////////////////////////////////
// Function: AddandCheck
// Purpose: Adds up the vectors in the movement vector queue and
// computes the dot product of the sum with the vector as generated
// by GeneratePaths().
//////////////////////////////////////////////////////////////////

int AddandCheck(int offset, int stuck){
	VECT sum,delta;
	MOVEMENT* temp;
	float dot;
	//int col;
	sum.x=0.0; sum.y=0.0;
	temp=first;
	do{
		sum.x+=temp->x; sum.y+=temp->y;
		temp=temp->next;
	 }while (temp!=NULL);
	 delta.x=-1.0*(gridPtr+offset)->delx;
	 delta.y=-1.0*(gridPtr+offset)->dely;
	 dot=(delta.x)*(sum.x)+(delta.y)*(sum.y);
	 //col=offset % wsWidthInCells;
	 //cout<<"s "<<sum.x<<" "<<sum.y<<"d "<<delta.x<<" "<<delta.y<<endl;
	 if ((dot<0.0)||(stuck==1)) { cout<<"Replanning at "<<offset<<endl;
		RePlan();
		ClearFirst();
		return 1;
		}
		else return 0;
	}

//////////////////////////////////////////////////////////////////
// Functions: InitializeFirst & ClearFirst
// Purpose: Initialize and clear movement vector queue.
/////////////////////////////////////////////////////////////////

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

void ClearFirst(){
	MOVEMENT* temp=first;
	do{
	temp->x=temp->y=0;
	temp=temp->next;
	}while(temp!=NULL);
	}
void RandObs(float perc){
	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) && ((head+off)->full==0))
			 if ((x!=goalCellX)&&(y!=goalCellY))
			 {(head+off)->full=1;
			  counter++;}
		  }
	}
/////////////////////////////////////////////////////////////////////
//   Function:  Trulla
//   Purpose:  This is the main trulla function.  It can be called
//      by a driver program.  (See modules RUNTRULLA.CPP for an example
//      of how this is done.)
/////////////////////////////////////////////////////////////////////

void Trulla ()
{
	int choice,sz;
	int haveCurrCell = 0;
	int x, y;
	float perc;
	MOVEMENT* temp;
	showGrid = 1;
	pathsAreDisplayed = 0;
	InitializeDisplay ();
	LoadFile(NULL);
	sz=wsWidthInCells*wsHgtInCells;
	free(head);
	head=(OBST*) malloc(sizeof(OBST)* sz);
	clearobs(sz);
	free(first);
	first=(MOVEMENT*) malloc(sizeof(MOVEMENT)*MAXMOVES);
	InitializeFirst();
	do {
		choice = getch();

		switch ( choice ) {
			case 'R' : case 'r' :
				printf ("Enter goal location: ");
				scanf ( "%f  %f", &(goalLocation->x), &(goalLocation->y) );
				SetGoal ( goalLocation );
				break;

			case 'N' : case 'n' :
				if ( pathsAreDisplayed ) {
					haveCurrCell = GetNextCell ( haveCurrCell ); //haveCurrCell could get changed
				}
				break;

			case 'L' : case 'l' :  // load a new workspace file
				LoadFile (NULL);
				sz=wsWidthInCells*wsHgtInCells;
				clearobs(sz);
				InitializeFirst();
				break;
			case 'O': case 'o':  // unmodeled obstacle
				printf("Enter percentage of unmodeled obstacles: ");
				scanf("%d %d", &x, &y);
				(head+wsWidthInCells*y+x)->full=1;
				ReDrawGrid();
				break;
		  case 'c': case 'C':
				cout<<"Enter percentage: ";
				cin>>perc;
				RandObs(perc);
				//ReDrawGrid();
				break;
		case 'A': case 'a':
				clearobs(sz);
				break;
		case 'Y': case 'y':
				RePlan();
				break;
		 case 72:         // up arrow
				MoveUp ();
				break;

			case 75 :        // left arrow
				MoveLeft ();
				break;

			case 77 :            // right arrow
				MoveRight ();
				break;

			case 80 :        // down arrow
				MoveDown ();
				break;

// generate a path if the goal has changed or if the cell is not an obstacle
			case 'G' : case 'g' :
				if ( (goalCellX != currCellX || goalCellY != currCellY) &&
				(gridPtr+gridelOffset)->r_weight != MAXWGT ) {
					InitializeFirst();
					GeneratePaths (0);
					pathsAreDisplayed = 1;
					goalLocation->x = goalCellX * feetPerGridel;
					goalLocation->y = goalCellY * feetPerGridel;
				}
				break;
		}

	} while ( choice != 'Q' && choice != 'q' );

	closegraph();
}