/*
	Ken Hughes's xwave.c, wind.c, and neighbor.c

	Graphics calls ported to Watcoms graphics library by
	Dave Hershberger.
*/


typedef unsigned int BIT;  // only 0 or 1 is stored in variables of this type


// Include Files

#include <stdio.h>
#include <conio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include "trulla.h"
#include "neighbor.h"
#include "window.h"
#include "loadws.h"

// External Declarations

extern int      showGrid;
extern int      wsSizeInCells;  // wsWidthInCells * wsHgtInCells
extern int      wsWidthInCells;
extern int      wsHgtInCells;
extern int      cellWidthInPix;
extern int      cellHgtInPix;
extern Color grays[MAXWGT + 1];

// Globals

int      maxInt;
float    maxFloat;
int      outputLine;
int      cycles[3];
BIT    * horzHistory;
GRIDEL * validCellPtr;
int      currRowCell; // offset from grid beginning to curr cell in curr row
int      lastRowCell; // offset from grid beginning to curr cell in last row processed
int      nextRowCell; // offset from grid beginning to curr cell in next row to be processed


// Function Prototypes

void DumpGrid();
void InitToRowBeginning ( int currRow );
void InitPropagationVars ();
void UpdateCurrCell ( int bestCellIndex );
void GeneratePaths ();
void InitGridelData ();
void InitXYLoc ();
void InitGoalGridel ();
int  Propagate_Horizontal ();
int  Propagate_Vertical ( int vertDirec );
void DisplayPaths (Window *win);
void DisplayStats ();


/////////////////////////////////////////////////////////////////////
//   Function:  GeneratePaths
//   Purpose:  This is the main propagation and path generation
//      function.  An initial horizontal propagation is performed
//      on the row containing the goal.  Thereafter, a vertical
//      propagation is performed to carry values from one row to
//      another followed by horizontal propagations until the new
//      row stabilizes.  The vertical and horizontal propagations are
//      performed throughout the grid until the grid stabilizes.
//      Finally, the paths and statistical data are displayed and
//      the data in each gridel are written to a file (dump.txt).
/////////////////////////////////////////////////////////////////////

void GeneratePaths(Window *win)
{
  int count, currRow, vertDirec;

  InitEnableConditions ();

  // Allocate memory
  horzHistory = (BIT *) malloc ( sizeof ( BIT ) * wsWidthInCells );
  validCellPtr = ( GRIDEL * ) malloc ( sizeof ( GRIDEL ) * 10 );
  
  assert(validCellPtr != NULL);

  // Initialization
  maxInt = 0x7fffffff;
  maxFloat = 999999999999.0;
  InitPropagationVars ();
  InitGridelData ();
  InitGoalGridel ();
  
  // Initial horizontal propagation
  do {
    ++cycles[0];
    InitToRowBeginning ( goalCellY );
  } while ( Propagate_Horizontal () );
  
  if ( goalCellY < wsHgtInCells - 1 )
    vertDirec = 1;		// begin vert propagation downwards (to bottom) of grid
  else
    vertDirec = -1;		// begin vert propagation upwards (to top) of grid
  
  currRow = goalCellY;
  
  
  // Perform vertical and horizontal propagations throughout grid
  for ( count = wsHgtInCells * 2; count ; count-- ) {
    currRow += vertDirec;		// move one row in appropriate vertical direction
    InitToRowBeginning ( currRow );
    
    ++cycles[0];
    ++cycles[1];
    
    if ( Propagate_Vertical ( vertDirec ) ) {
      count = wsHgtInCells * 2;
      do {
	++cycles[0];
	InitToRowBeginning ( currRow );
      } while ( Propagate_Horizontal () );
    }
    
    if ( currRow == wsHgtInCells - 1 ) // change vertical direction...
      vertDirec = -1;                  // ...if you have reached the...
      else if ( currRow == 0 )	       // ...top or bottom of the grid
	vertDirec = 1;
  }
  
  // Free dynamically allocated memory
  free ( horzHistory );

  // It's crashing occasionally on the free down here, but
  // this debug stuff isn't catching it. I haven't checked
  // to see if the *end* of validCellPtr gets hosed yet.

  if(*(((int *) validCellPtr) - 1) != 0x0000020d){
    fprintf(stderr, "FUCK! *0x%08x = 0x%08x.\n",
	    validCellPtr, *(((int *) validCellPtr) - 1));
  }
  free ( validCellPtr );
  
  // Display paths
  if ( showGrid ) {
    DisplayPaths (win);
    DisplayStats ();
  }
  // This was causing it to crash occasionally. I dont want to deal
  // with it right now and Im not using the dumped file anyway.
  //	DumpGrid();
}


/////////////////////////////////////////////////////////////////////
//   Function:  DumpGrid
//   Purpose:  Writes the gridel data to a file.  This may be useful
//      for debugging purposes, for example.
/////////////////////////////////////////////////////////////////////

void DumpGrid()
{
  GRIDEL * t;
  FILE * fp;
  int count;
  char buffer[150];
  
  if ( ( fp = fopen ( "dump.txt", "w" ) ) == NULL ) {
    printf ( "\nCannot open file dump.txt\n" );
    printf ( "Press any key to halt.");
    getch();
    exit (BADEXIT);
  }
  
  t = gridPtr;
  
  for ( count = 0; count < wsSizeInCells; count++ ) {
    if (t->xloc == 0) {
      fputs("\nxloc,yloc,normal,  s_dist,  t_dist, r_wgt,region,",fp);
      fputs("  delx,  dely, xgoal, ygoal\n", fp);
    }
    
    sprintf(buffer," %3d, %3d, %5d, ",t->xloc, t->yloc, t->normal);
    fputs (buffer,fp);
    
    if ( t->s_dist == maxFloat )
      sprintf (buffer,"%7.2f, ",15.0);
    else
      sprintf (buffer,"%7.2f, ",t->s_dist);
    fputs (buffer,fp);
    
    if (t->t_dist == maxFloat )
      sprintf (buffer,"%7.2f, ",15.0);
    else
      sprintf (buffer,"%7.2f, ",t->t_dist);
    fputs (buffer,fp);
    
    sprintf(buffer,"%5d, ",t->r_weight);
    fputs (buffer,fp);
    
    sprintf(buffer,"%5d, %5d, %5d, ",t->region,t->delx,t->dely);
    fputs (buffer,fp);
    
    sprintf(buffer,"%5d, %5d\n",t->xgoal,t->ygoal);
    fputs (buffer,fp);
    
    t++;
  }
  fclose(fp);
}


/////////////////////////////////////////////////////////////////////
//   Function:  InitToRowBeginning
//   Purpose:  Sets three global GRIDEL pointers to the beginning of
//      the current row, the beginning of the last (previous) row,
//      and the beginning of the next row.
/////////////////////////////////////////////////////////////////////

void InitToRowBeginning ( int currRow )
{
  // point to beginning of current, last, and next rows
  currRowCell = currRow * wsWidthInCells;
  lastRowCell = currRowCell - wsWidthInCells;
  nextRowCell = currRowCell + wsWidthInCells;
}


/////////////////////////////////////////////////////////////////////
//   Function:  InitPropagationVars
//   Purpose:  Initializes some variables that are needed during the
//      propagation of values throughout the grid.
/////////////////////////////////////////////////////////////////////

void InitPropagationVars ()
{
  int i;
  
  goalCellX = currCellX;
  goalCellY = currCellY;
  
  cycles[0] = cycles[1] = cycles[2] = 0;
  
  for ( i = wsWidthInCells ; i-- ; horzHistory[i] = 0 );

  assert(goalCellX >= 0 && goalCellX < wsWidthInCells);

  horzHistory [goalCellX] = 1;
}


/////////////////////////////////////////////////////////////////////
//   Function:  InitGridelData
//   Purpose:  Initializes many of the data items for each gridel in
//      the grid.
/////////////////////////////////////////////////////////////////////

void InitGridelData ()
{
  GRIDEL * tempGridPtr;
  
  tempGridPtr = gridPtr;
  for ( int i = 0; i < wsSizeInCells; i++ ) {
    tempGridPtr->vert_hist[1] = tempGridPtr->vert_hist[0] = 0;
    //	   xloc is initialized below in InitXYLoc
    //	   yloc is initialized below in InitXYLoc
    tempGridPtr->normal = -1;
    tempGridPtr->s_dist = maxFloat;
    tempGridPtr->t_dist = maxFloat;
    //    r_weight was initialized when the workspace file was read
    tempGridPtr->region = 0;
    tempGridPtr->delx = 0;
    tempGridPtr->dely = 0;
    //	   xgoal is initialized during propagation
    //	   ygoal is initialized during propagation
    
    tempGridPtr++;
  }
  
  InitXYLoc();
}


/////////////////////////////////////////////////////////////////////
//   Function:  InitXYLoc
//   Purpose:  Initializes the two data items in each gridel which
//      determines its x and y location in the grid.
/////////////////////////////////////////////////////////////////////

void InitXYLoc ()
{
  int i,j;
  GRIDEL * tempGridPtr;
  
  tempGridPtr = gridPtr;
  for ( j = 0; j < wsHgtInCells; j++ )
    for ( i = 0 ; i < wsWidthInCells ; i++ ) {
      tempGridPtr->xloc = i;
      tempGridPtr->yloc = j;
      tempGridPtr++;
    }
}


/////////////////////////////////////////////////////////////////////
//   Function:  InitGoalGridel
//   Purpose:  Initializes some data items in the goal gridel.
/////////////////////////////////////////////////////////////////////

void InitGoalGridel ()
{
  GRIDEL * goalPtr;
  
  goalPtr = gridPtr + goalCellX + goalCellY * wsWidthInCells;
  
  goalPtr->s_dist = 0;
  goalPtr->t_dist = 0;
  goalPtr->normal = -2;
  goalPtr->vert_hist[0] = 1;
  goalPtr->xgoal = goalPtr->ygoal = 0;
}


/////////////////////////////////////////////////////////////////////
//   Function:  Propagate_Horizontal
//   Purpose:  Perform a horizontal propagation on the current row
//      by beginning with the leftmost cell and working until the
//      end of the row is reached.  The following is performed on each
//      cell in the row.  Check the two horizontal neighbors of the
//      current cell for a recent change in their propagation values.
//      If either have changed, the propagation values of all eight
//      neighbors of the current cell are checked to see if they could
//      be used to update the values of the current cell.  If the values
//      of any of the cells in the current row are updated, TRUE (1) is
//      returned to the calling function by variable row_history.
/////////////////////////////////////////////////////////////////////

int Propagate_Horizontal ()
{
  int width, row_history;
  int historyOffset;
  BIT leftCellHist, currCellHist, rightCellHist;
  int bestCellIndex;
  GRIDEL * currCell, * validCell;
  
  historyOffset = leftCellHist = row_history = 0;
  currCellHist = *horzHistory;	// ptr should be pointing at the 1st element
  width = wsWidthInCells;
  
  while ( width-- ) {
    if ( width )
      rightCellHist = *(horzHistory+historyOffset+1);
    else
      rightCellHist = 0;
    
    *(horzHistory+historyOffset) = 0;
    
    // if either horizontal neighbor next to the current cell has changed
    // and if the current cell is not an obstacle, try to get the best
    // propagation values from the 8 neighbors of the current cell
    currCell = gridPtr + currRowCell;
    if ( currCell->r_weight != MAXWGT && (leftCellHist || rightCellHist) ) {
      ++(cycles[2]);
      bestCellIndex = ChooseBestValidCell ();
      
      // if there is a new propagation, see if its better than the current one
      if ( bestCellIndex >= 0 ) {
	
	// if the new propagation is closer than the current propagation or
	// if it is longer than the current one but the normal indicates a
	// "shadow subgoal", use the new propagation
	validCell = validCellPtr + bestCellIndex;
	if ( validCell->t_dist < currCell->t_dist ||
	    ( validCell->t_dist > currCell->t_dist &&
	     validCell->normal > -1 ) ) {
	  UpdateCurrCell ( bestCellIndex );
	  *(horzHistory+historyOffset) = row_history = 1;
	}			// if ( validCell->t_dist <...
      }				// if ( bestCellIndex >= 0 )...
    }				// if ( currCell->r_weight != MAXWGT &&...
    
    ++historyOffset;
    ++currRowCell;
    ++lastRowCell;
    ++nextRowCell;
    leftCellHist = currCellHist;
    currCellHist = rightCellHist;
  }				// 	while ( width--...
  
  return row_history;
}


/////////////////////////////////////////////////////////////////////
//   Function:  Propagate_Vertical
//   Purpose:  Perform a vertical propagation on the current row
//      by beginning with the leftmost cell and working until the
//      end of the row is reached.  The following is performed on each
//      cell in the row.  Check the three neighbors connected to the
//      current cell on either the row above or the row below the
//      current row, depending on which row was last run through the
//      vertical propagation function.  (If the vertical propagation
//      proceeds down the grid, then the neighbors on the row above
//      are checked since they were last run through the vertical
//      propagation; if the vertical propagation proceeds up the grid,
//      then the neighbors on the row below are checked since they were
//      last run through.)  If any of the propagation values of the
//      three relevant neighbors have changed, then the propagation
//      values of all eight neighbors of the current cell are checked
//      to see if they could be used to update the values of the
//      current cell.  If the values of any of the cells in the current
//      row are updated, TRUE (1) is returned to the calling function
//      by variable row_history.
/////////////////////////////////////////////////////////////////////

int Propagate_Vertical ( int vertDirec )
{
  BIT currCellHist, leftCellHist, rightCellHist;
  int row_history;
  int width;
  int bestCellIndex;
  int historyOffset;
  GRIDEL * cellAbove, * currCell, * cellBelow, * validCell;
  
  // prepare for the first pass
  
  historyOffset = leftCellHist = row_history = 0;
  cellAbove = gridPtr + lastRowCell;
  cellBelow = gridPtr + nextRowCell;
  
  if ( vertDirec == 1 )
    currCellHist = cellAbove->vert_hist[1] || cellAbove->vert_hist[0];
  else
    currCellHist = cellBelow->vert_hist[1] || cellBelow->vert_hist[0];
  
  width = wsWidthInCells;
  while ( width-- ) {
    cellAbove = gridPtr + lastRowCell;
    currCell = gridPtr + currRowCell;
    cellBelow = gridPtr + nextRowCell;
    
    // shift the change history from pass one to pass two
    currCell->vert_hist[1] = currCell->vert_hist[0];
    
    // this determines which neighors should be checked
    if ( width ) {
      if ( vertDirec == 1 )
	rightCellHist = (cellAbove+1)->vert_hist[1] ||
	  (cellAbove+1)->vert_hist[0];
      else
	rightCellHist = (cellBelow+1)->vert_hist[1] ||
	  (cellBelow+1)->vert_hist[0];
    }
    else
      rightCellHist = 0;
    
    // if one of the relevant neighbors has changed then
    //		recalculate the value for this gridel
    
    if ( currCell->r_weight != MAXWGT &&
	( leftCellHist || currCellHist || rightCellHist ) ) {
      ++cycles[2];
      bestCellIndex = ChooseBestValidCell ();
      
      // if there is a new propagation, and it is is closer than the current
      //		propagation or is longer than the current one but the normal
      //		indicates a shadow subgoal, use it
      
      currCell->vert_hist[0] = 0;
      *(horzHistory+historyOffset) = 0;
      
      if ( bestCellIndex >= 0 ) {
	validCell = validCellPtr + bestCellIndex;
	if ( validCell->t_dist < currCell->t_dist ||
	    ( validCell->t_dist > currCell->t_dist &&
	     validCell->normal > -1 ) ) {
	  UpdateCurrCell ( bestCellIndex );
	  *(horzHistory+historyOffset) = row_history = 1;
	}
      }
    }				// if ( leftCellHist...
    
    ++historyOffset;
    ++currRowCell;
    ++lastRowCell;
    ++nextRowCell;
    leftCellHist = currCellHist;
    currCellHist = rightCellHist;
  }				// while ( width-- )...
  
  return row_history;
}


/////////////////////////////////////////////////////////////////////
//   Function:  UpdateCurrCell
//   Purpose:  The current cell is updated with new propagation values.
/////////////////////////////////////////////////////////////////////

void UpdateCurrCell ( int bestCellIndex )
{
  GRIDEL * currCell;
  GRIDEL * validCell;
  
  currCell = gridPtr + currRowCell;

  assert(bestCellIndex >= 0);

  validCell = validCellPtr + bestCellIndex;
  
  currCell->xgoal = validCell->xgoal;
  currCell->ygoal = validCell->ygoal;
  currCell->delx = validCell->delx;
  currCell->dely = validCell->dely;
  currCell->s_dist = validCell->s_dist;
  currCell->t_dist = validCell->t_dist;
  currCell->normal = validCell->normal;
  currCell->region = validCell->region;
  currCell->vert_hist[0] = 1;
}


/////////////////////////////////////////////////////////////////////
//   Function:  DisplayPaths
//   Purpose:  Display an arrow in each cell that is not an obstacle,
//      that is not the goal, and that does not contain a zero for the
//      weighted distance in both the x and y directions.  The last
//      test prevents a divide-by-zero error.  If the last test is
//      not passed, an arrow will not be drawn in the cell that was
//      being tested.  The arrow in each cell consists of a line
//      segment with a circle drawn at one endpoint of the line.
//      The circle indicates the forward direction toward the goal.
/////////////////////////////////////////////////////////////////////

void DisplayPaths (Window *win)
{
  static Color black(0, 0, 0);
  static Color white(63, 63, 63);
  
  int count;
  GRIDEL * tempGridPtr;
  int cellXPos, cellYPos;
  int deltaX, deltaY;
  float dist;
  int distx, disty;
  
  int x,y,x1,y1;
  
  // radius (in pixels) of ellipse in x and y directions in pixels
  int xradius = cellWidthInPix * 0.1;
  if(xradius < 1)
    xradius = 1;
  
  int yradius = cellHgtInPix * 0.1;
  if(yradius < 1)
    yradius = 1;
  
  tempGridPtr = gridPtr;
  count = wsSizeInCells;
  
  while ( count--) {
    if ( tempGridPtr->r_weight != MAXWGT &&
	! (tempGridPtr->xloc == goalCellX &&
	   tempGridPtr->yloc == goalCellY ) ) {
      if ( ! ( tempGridPtr->delx == 0 && tempGridPtr->dely == 0 ) ) {
	cellXPos = tempGridPtr->xloc;
	cellYPos = tempGridPtr->yloc;
	deltaX = tempGridPtr->delx;
	deltaY = tempGridPtr->dely;
	
	dist = 2.0 * sqrt((double)deltaX*deltaX + (double)deltaY*deltaY);
	distx = 0.8 * cellWidthInPix * deltaX / dist;
	disty = 0.8 * cellHgtInPix * deltaY / dist;
	
	if ( tempGridPtr->r_weight <= 5 ) {
	  win->set_color ( black );
	}
	else {
	  win->set_color ( white );
	}
	
	x = cellWidthInPix * cellXPos + ( cellWidthInPix >> 1 ) - distx;
	y = cellHgtInPix * cellYPos + ( cellHgtInPix >> 1 ) - disty;
	x1 = cellWidthInPix * cellXPos + ( cellWidthInPix >> 1 ) + distx;
	y1 = cellHgtInPix * cellYPos + ( cellHgtInPix >> 1 ) + disty;
	win->line ( x, y, x1, y1);
	
	x = cellWidthInPix * cellXPos + ( cellWidthInPix >> 1 ) - distx;
	y = cellHgtInPix * cellYPos + ( cellHgtInPix >> 1 ) - disty;
	win->fill_ellipse (x - xradius, y - yradius,
			   x + xradius, y + yradius );
      }
    }
    ++tempGridPtr;
  }
}


/////////////////////////////////////////////////////////////////////
//   Function:  DisplayStats
//   Purpose:  Displays the total number of horizontal and vertical
//      propagations (cycles), the total number of vertical
//      propagations (passes), and the total number of gridels
//      whose neighbors were checked for values to possibly be
//      propagated to the current gridel (gridels).
/////////////////////////////////////////////////////////////////////

void DisplayStats ()
{
  // Dave here - I wont be needing this...
#if 0
  char buffer [80];
  
  setcolor ( WHITE );
  sprintf ( buffer,
	   "%5d cycles:horz/vert props    %5d passes:vert props   %5d gridels checked",
	   cycles[0], cycles[1], cycles[2] );
  outtextxy ( 0, outputLine, buffer);
#endif				// 0
}