// Include Files

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include "trulla.h"
#include "loadws.h"
#include "tr2robot.h"
#include "window.h"
#include "propagat.h"
#include <iostream.h>

int      showGrid;

// External Declarations

extern float    feetPerGridel;
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 int			total_moves;
// Globals

float      toleranceInFeet;
LOCATION * goalLocation = new LOCATION;
Color grays[MAXWGT + 1];
int 			pastmoves[60];

// Function Prototypes

void ClearDisplay (Window *win);
int  GetNextCell ( Window *win, int haveCurrCell );
int DrawCurrentLocation ( Window *win, LOCATION * currLocInGridels );
void DrawPastMoves(Window* win);
void InitGraphics ();
void GetMaxCoords (Window *win);
void DefineGrayScale ();
void MoveLeft (Window *win);
void MoveRight (Window *win);
void MoveUp (Window *win);
void MoveDown (Window *win);


/////////////////////////////////////////////////////////////////////
//   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 (Window *win)
{
	InitGraphics ();
	GetMaxCoords (win);
	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 ()
{
  set_graphics_mode();
}


/////////////////////////////////////////////////////////////////////
//   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 (Window *win)
{
  maxXCoord = win->maxx();
  maxYCoord = win->maxy();
}


////////////////////////////////////////////////////////////////////
//   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 ()
{
  for ( int i = 0; i < MAXWGT; i++ )
    grays[i] = Color(0x3f / MAXWGT * i,
		     0x3f / MAXWGT * i,
		     0x3f / MAXWGT * i);
  grays[MAXWGT] = Color(0x3f, 0x3f, 0x3f);
}


////////////////////////////////////////////////////////////////////
//   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 (Window *win)
{
  if ( pathsAreDisplayed ) {
    ReDrawGrid (win);
    pathsAreDisplayed = 0;
  }
  ColorCell (win,
	     currCellXInPix, currCellYInPix,
	     currCellXInPix + cellWidthInPix, currCellYInPix + cellHgtInPix,
	     gridPtr + gridelOffset );
}


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

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


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

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


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

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


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

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


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

int DrawCurrentLocation ( Window *win, LOCATION * currLocInGridels )
{
  static Color orange(40, 20, 0);
	static int movecounter=0;
  int tempGridelOffset;
  int currXLocInPix, currYLocInPix;
  tempGridelOffset = (int) currLocInGridels->y * wsWidthInCells +
    (int) currLocInGridels->x;
	if (movecounter==0) pastmoves[movecounter++]=tempGridelOffset;
	else
  if (pastmoves[movecounter-1]!=tempGridelOffset)
			pastmoves[movecounter++]=tempGridelOffset;
  win->set_color ( orange);
  
  currXLocInPix = (int) currLocInGridels->x * cellWidthInPix + xOffsetInPix;
  currYLocInPix = (int) currLocInGridels->y * cellHgtInPix + yOffsetInPix;

  win->fill_ellipse(currXLocInPix - cellWidthInPix / 4,
		    currYLocInPix - cellWidthInPix / 4,
		    currXLocInPix + cellWidthInPix / 4,
		    currYLocInPix + cellWidthInPix / 4);
				return (movecounter);
}
void DrawPastMoves(Window* win){
	static Color orange(40,20,0);
  int currXLocInPix, currYLocInPix, x, y;
	win->set_color(orange);
	cout<<"I'm here with "<<total_moves<<endl;
	for (int i=0; i<total_moves; i++){
	x=pastmoves[i]%wsWidthInCells;
	y=pastmoves[i]/wsWidthInCells;
  currXLocInPix = x * cellWidthInPix + xOffsetInPix;
  currYLocInPix = y * cellHgtInPix + yOffsetInPix;
  win->fill_ellipse(currXLocInPix - cellWidthInPix / 4,
		    currYLocInPix - cellWidthInPix / 4,
		    currXLocInPix + cellWidthInPix / 4,
		    currYLocInPix + cellWidthInPix / 4);
	}
}
/////////////////////////////////////////////////////////////////////
//   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 ( Window *win, int haveCurrCell )
{
  static LOCATION currLocInFeet;
  LOCATION currLocInGridels;
  static DISPLACEMENT subgoalDispInFeet;
  int tempGridelOffset;
  
  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  &&
	! ( (gridPtr+tempGridelOffset)->delx == 0 &&
	   (gridPtr+tempGridelOffset)->dely == 0 ) ) {
      DrawCurrentLocation ( win, &currLocInGridels );
      haveCurrCell = 1;
    }
    printf("Enter tolerance: ");
    scanf("%f", &(toleranceInFeet));
  }
  
  if ( NextSubgoal ( &currLocInFeet, &subgoalDispInFeet ) ) {
    AdvanceCurrLocation ( &currLocInFeet, &subgoalDispInFeet );
    FeetToGridels ( &currLocInFeet, &currLocInGridels );
    DrawCurrentLocation ( win, &currLocInGridels );
    if ( IsGoal (goalLocation, &currLocInFeet, toleranceInFeet) ) {
      printf ("REACHED GOAL\n");
      haveCurrCell = 0;
    }
  }
  else {
    haveCurrCell = 0;
    printf ("NO SUBGOAL\n");
  }
  
  return ( haveCurrCell );
}

/////////////////////////////////////////////////////////////////////
//   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(char *filename)
{
  int choice;
  int haveCurrCell = 0;
  showGrid = 1;
  pathsAreDisplayed = 0;
  
  Window *trulla_win = new Window(320, 0, 620, 300);

  InitializeDisplay (trulla_win);
  
  LoadFile(trulla_win, filename);
  
  do {
    choice = getch();
    
    switch ( choice ) {
    case 'R' : case 'r' :
      printf ("Enter goal location: ");
      scanf ( "%f  %f", &(goalLocation->x), &(goalLocation->y) );
      SetGoal ( trulla_win, goalLocation );
      break;
      
    case 'N' : case 'n' :
      if ( pathsAreDisplayed ) {
	haveCurrCell = GetNextCell ( trulla_win, haveCurrCell ); //haveCurrCell could get changed
      }
      break;
      
    case 'L' : case 'l' :		// load a new workspace file
      LoadFile (trulla_win, NULL);
      break;
      
    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;
      
      // 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 ) {
	GeneratePaths (trulla_win,0);
	pathsAreDisplayed = 1;
	goalLocation->x = goalCellX * feetPerGridel;
	goalLocation->y = goalCellY * feetPerGridel;
      }
      break;
    }
    
  } while ( choice != 'Q' && choice != 'q' );
  
  set_text_mode();
}