//////////////////////////////////////////////////////////////////////////////////
// $Source: /cvsroot/cdx/cdx3.0/addons/CDXAStar/CDXASTAR.CPP,v $
// $Author: hebertjo $
//
// $Log: CDXASTAR.CPP,v $
// Revision 1.1.1.1  2000/04/22 16:51:24  hebertjo
// Initial checkin of v3.0 to SourceForge CVS.
//
// Revision 1.3  1999/05/30 02:23:11  mseibert
// Removed out-commented code...
//
// Revision 1.2  1999/05/29 16:17:07  mseibert
// Corrected a bug and enhanced the diagonal pathfinding routine
//
// Revision 1.1.1.1  1999/05/04 23:59:23  bsimser
// Imported addons
//
// $Revision: 1.1.1.1 $
//////////////////////////////////////////////////////////////////////////////////
#include <CDX.h>
#include <cdxmap.h>
#include "CDXAStar.h"

////////////////////////////////////////////////////////////////////////////////
//                           Constructor Destructor                           //
////////////////////////////////////////////////////////////////////////////////
CDXAStar::CDXAStar(CDXMap* pMap, int forbiddenTiles)
{
   Stack = ( STACK* )calloc(1,sizeof( STACK ));
   isPath = FALSE;
   OPEN = NULL;
   CLOSED = NULL;
   PATH = NULL;

   // Sets the A* Tilemap append on CDXMap
   InitAstarTileMap(pMap, forbiddenTiles);
}

////////////////////////////////////////////////////////////////////////////////
CDXAStar::~CDXAStar()
{
   FreeNodes();
   free(Stack);
   if ( TileMap )
		delete [] TileMap;
}

////////////////////////////////////////////////////////////////////////////////
//                             Public Member Functions                        //
////////////////////////////////////////////////////////////////////////////////
void CDXAStar::InitAstarTileMap(CDXMap* pMap, int forbiddenTiles)
{
  COLS = pMap->m_Width+2;    // note the cols should be 2 more than the map size
  ROWS = pMap->m_Height+2;   // because if we have a point at the extremes and we check
  TOTAL_TILES = ROWS*COLS;   // to see if we can go in all directions, we don't need a
                              // special case to check these. (same for rows)
  TileMap = new int[TOTAL_TILES];
  int index = COLS+1; // A* map index
  int CDXMapIndex = 0;
  int tile = 0;

  BoundaryTiles(); // sets 1's arround the map area

  for (int i = 0; i < ROWS-2; i++)       // fill up the area with current map data
  {                                      // 1 = obstacle
	  for (int j = 0; j < COLS-2; j++)     // 0 = free
		{
      tile = pMap->GetTile(j, i);

      if ( tile < forbiddenTiles )   // the first "forbidden" tiles
				TileMap[index] = 1;          // in the CDX-tiles-bitmap are not reachable

      index++;
    }
    index += 2; // jump boundary
  }
}

////////////////////////////////////////////////////////////////////////////////

BOOL CDXAStar::NewPath(int sx,int sy, int dx,int dy)
{
   if ( FreeTile(dx,dy)&&FreeTile(sx,sy) && (TileNum(sx,sy)!=TileNum(dx,dy)) )
   {
      FreeNodes(); // clear node lists
   	FindPath(sx,sy,dx,dy);
   	return (isPath=TRUE);
   }
   else
  	   return (isPath=FALSE);
}

////////////////////////////////////////////////////////////////////////////////

BOOL CDXAStar::ReachedGoal(void) // check it's return value before getting
{                                      // the node entries
   if ( !isPath ) return TRUE;  // this looks a little bit strange
   if ( PATH->Parent != NULL )  // but prevents illegal calls of ::PathNextNode()
   	return FALSE;             // or ::PathGet*()
   else
   	return TRUE;
}

////////////////////////////////////////////////////////////////////////////////

int CDXAStar::TileNum(int x, int y)
{
	return((y>>SHIFT)*COLS + (x>>SHIFT)+COLS+1); // The reason I add COLS+1 to
   // tile number is because I want the tile number to start at the 2nd
   // the column and the 2nd row. Because if we do this we don't have to have
   // a special case to test if at a boundary. In the function BoundaryTiles
   // I place 1's around the boundary, in this way when I call the function
   // FreeTile() it returns false because we can't go there.
}

////////////////////////////////////////////////////////////////////////////////

int CDXAStar::FreeTile(int x, int y)
{         //  returns 1 if tile is free(nothing on it).
	      //  Note: if TileMap[equation]==0 it means free so just !(not) it.
   return(!TileMap[(y>>SHIFT)*COLS + (x>>SHIFT)+COLS+1]);
}

////////////////////////////////////////////////////////////////////////////////
//								      Private Member Functions                        //
////////////////////////////////////////////////////////////////////////////////

void CDXAStar::FreeNodes(void)
{
  	NODE *Node, *OldNode;

   if ( OPEN != NULL )
   {
   	Node = OPEN->NextNode;
   	while ( Node != NULL )
	   {
      	OldNode = Node;
      	Node = Node->NextNode;
         free(OldNode);
	   }
   }

   if ( CLOSED != NULL )
   {
   	Node = CLOSED->NextNode;
   	while ( Node != NULL )
      {
         OldNode = Node;
      	Node = Node->NextNode;
     	   free(OldNode);
	   }
   }
}

////////////////////////////////////////////////////////////////////////////////

void CDXAStar::BoundaryTiles(void) // This function places 1 around the boundary
{                             // so that we don't go there. Also we don't
   int c, index = 0;         // have to treat the boundaries as a special case

   for (c = 0; c < TOTAL_TILES; c++)      // zero out tiles.
       TileMap[c] = 0;

   for (c = 0; c < COLS; c++)         // place 1's at boundaries.
   	 TileMap[c] = 1;

   for (c = 1;c < ROWS-1; c++)
   {
      index += COLS;
      TileMap[index] = 1;
      TileMap[index+COLS-1] = 1;
   }
   index += COLS;

   for (c = index; c < TOTAL_TILES; c++)
       TileMap[c]=1;
}

////////////////////////////////////////////////////////////////////////////////
//                               A* Algorithm                                 //
////////////////////////////////////////////////////////////////////////////////

void CDXAStar::FindPath(int sx, int sy, int dx, int dy)
{
	NODE *Node, *BestNode;
   int TileNumDest;

   TileNumDest = TileNum(sx, sy);

   OPEN=( NODE* )calloc(1,sizeof( NODE ));
   CLOSED=( NODE* )calloc(1,sizeof( NODE ));

   Node=( NODE* )calloc(1,sizeof( NODE ));
   Node->g = 0;
   Node->h = (dx-sx)*(dx-sx) + (dy-sy)*(dy-sy);  // should really use sqrt().
   Node->f = Node->g+Node->h;
   Node->NodeNum = TileNum(dx, dy);
   Node->x = dx;
   Node->y = dy;

   OPEN->NextNode=Node;        // make Open List point to first node
   for (;;)
   {
		BestNode=ReturnBestNode();
		if (BestNode->NodeNum == TileNumDest)    // if we've found the end, break and finish
	   	break;
		GenerateSuccessors(BestNode,sx,sy);
   }
   PATH = BestNode;
}

////////////////////////////////////////////////////////////////////////////////

CDXAStar::NODE
*CDXAStar::ReturnBestNode(void)
{
   NODE *tmp;
   char message[128];

   if ( OPEN->NextNode == NULL )
   {
		sprintf(message,"No more nodes on OPEN: Perhaps tilenum destination not reachable!");
      MessageBox(0, message, "Error A* Pathfinder", MB_OK | MB_ICONERROR);
      PostQuitMessage(0);
   }

// Pick Node with lowest f, in this case it's the first node in list
// because we sort the OPEN list wrt lowest f. Call it BESTNODE.

   tmp = OPEN->NextNode;   // point to first node on OPEN
   OPEN->NextNode = tmp->NextNode;    // Make OPEN point to nextnode or NULL.

// Next take BESTNODE (or temp in this case) and put it on CLOSED

   tmp->NextNode = CLOSED->NextNode;
   CLOSED->NextNode = tmp;

   return(tmp);
}

////////////////////////////////////////////////////////////////////////////////

void CDXAStar::GenerateSuccessors(NODE *BestNode, int dx, int dy)
{
   int x, y;
	// If diagonal movements are not allowed, remove the "if" cases for diagonal pathfinding
  // That means that the pathfinding routine only allows left/right/up/down movements

	//
	// Case study by Martin Seibert: 
	//
	// If your sprite cannot reach a diagonal Tile because it's size, you must find
	// out, if there is at least one other Tile passable to reach to diagonal Tile
	// Example:
	// 
	// # = Not passable
	// . = passable
	// X = Destination
	// * = Player
	//
	// This example won't work!
	// 
	// ....#.....  This would cause the Player stuck at the shown position when using the 
	// ....###*..  original version of the GenerateSuccessor code.
	// ......X#.#  I've added some additional code to find out if at least one other Tile
	// ..........  is passable to move directly (diagonal) to the destination point.
	// ..........  PS: This example assumes that the Player doesn't fit between two diagonal Tiles
	//
	// This example works!
	//
	// ....#.....  Here the player can move one Tile left, and then one Tile down to reach 
	// ....##.*..  the destination Tile!
	// ......X#.#  
	// ..........  
	// ..........
	//
	//

	// Upper-Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y-TILESIZE) && (FreeTile(x+TILESIZE,y) || FreeTile(x,y+TILESIZE)))
		GenerateSucc(BestNode,x,y,dx,dy);

	// Upper
	if ( FreeTile(x=BestNode->x, y=BestNode->y-TILESIZE))
		GenerateSucc(BestNode,x,y,dx,dy);

  	// Upper-Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y-TILESIZE) && (FreeTile(x-TILESIZE,y) || FreeTile(x,y+TILESIZE)))
		GenerateSucc(BestNode,x,y,dx,dy);

	// Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y))
		GenerateSucc(BestNode,x,y,dx,dy);

	// Lower-Right
 	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y+TILESIZE) && (FreeTile(x-TILESIZE,y) || FreeTile(x,y-TILESIZE)))
		GenerateSucc(BestNode,x,y,dx,dy);

	// Lower
	if ( FreeTile(x=BestNode->x, y=BestNode->y+TILESIZE))
		GenerateSucc(BestNode,x,y,dx,dy);

	// Lower-Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y+TILESIZE) && (FreeTile(x+TILESIZE,y) || FreeTile(x,y-TILESIZE)))
		GenerateSucc(BestNode,x,y,dx,dy);

	// Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y))
		GenerateSucc(BestNode,x,y,dx,dy);

/*
	// Upper-Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y-TILESIZE) && FreeTile(x-1,y-1))
		GenerateSucc(BestNode,x,y,dx,dy);
	// Upper
	if ( FreeTile(x=BestNode->x, y=BestNode->y-TILESIZE) && FreeTile(x,y-1))
		GenerateSucc(BestNode,x,y,dx,dy);
	// Upper-Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y-TILESIZE) && FreeTile(x+1,y-1))
		GenerateSucc(BestNode,x,y,dx,dy);
	// Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y) && FreeTile(x+1,y))
		GenerateSucc(BestNode,x,y,dx,dy);
	// Lower-Right
	if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y+TILESIZE) && FreeTile(x+1,y+1))
		GenerateSucc(BestNode,x,y,dx,dy);
	// Lower
	if ( FreeTile(x=BestNode->x, y=BestNode->y+TILESIZE) && FreeTile(x,y+1))
		GenerateSucc(BestNode,x,y,dx,dy);
	// Lower-Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y+TILESIZE) && FreeTile(x-1,y+1))
		GenerateSucc(BestNode,x,y,dx,dy);
	// Left
	if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y) && FreeTile(x-1,y))
		GenerateSucc(BestNode,x,y,dx,dy);
*/
}

////////////////////////////////////////////////////////////////////////////////

void CDXAStar::GenerateSucc(NODE *BestNode,int x, int y, int dx, int dy)
{
   int g, TileNumS, c = 0;
   NODE *Old, *Successor;

   g = BestNode->g+1;	    // g(Successor)=g(BestNode)+cost of getting from BestNode to Successor
   TileNumS = TileNum(x,y);  // identification purposes

   if ( (Old=CheckOPEN(TileNumS)) != NULL ) // if equal to NULL then not in OPEN list, else it returns the Node in Old
   {
		for( c = 0; c < 8; c++)
	   	if( BestNode->Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
	   		break;
		BestNode->Child[c] = Old;

		if ( g < Old->g )  // if our new g value is < Old's then reset Old's parent to point to BestNode
		{
			Old->Parent = BestNode;
	   	Old->g = g;
	   	Old->f = g + Old->h;
		}
   }
   else if ( (Old=CheckCLOSED(TileNumS)) != NULL ) // if equal to NULL then not in OPEN list, else it returns the Node in Old
   {
   for( c = 0; c< 8; c++)
		if ( BestNode->Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
	   		break;
		BestNode->Child[c] = Old;

		if ( g < Old->g )  // if our new g value is < Old's then reset Old's parent to point to BestNode
		{
	   	    Old->Parent = BestNode;
	   	    Old->g = g;
	   	    Old->f = g + Old->h;
	   	    PropagateDown(Old);  // Since we changed the g value of Old, we need
					   	        // to propagate this new value downwards, i.e.
					 	          // do a Depth-First traversal of the tree!
		}
   }
   else
   {
		Successor = ( NODE* )calloc(1,sizeof( NODE ));
		Successor->Parent = BestNode;
		Successor->g = g;
		Successor->h = (x-dx)*(x-dx) + (y-dy)*(y-dy);  // should do sqrt(), but since we don't really
		Successor->f = g+Successor->h;     // care about the distance but just which branch looks
		Successor->x = x;                 // better this should suffice. Anyayz it's faster.
		Successor->y = y;
		Successor->NodeNum = TileNumS;
		Insert(Successor);     // Insert Successor on OPEN list wrt f
		for( c =0; c < 8; c++)
			if ( BestNode->Child[c] == NULL ) // Add Old to the list of BestNode's Children (or Successors).
	  		    break;
		BestNode->Child[c] = Successor;
   }
}

////////////////////////////////////////////////////////////////////////////////

CDXAStar::NODE
*CDXAStar::CheckOPEN(int tilenum)
{
   NODE *tmp;

   tmp = OPEN->NextNode;
   while ( tmp != NULL )
   {
		if ( tmp->NodeNum == tilenum )
	   	return (tmp);
		else
	  		tmp = tmp->NextNode;
   }
   return(NULL);
}

////////////////////////////////////////////////////////////////////////////////

CDXAStar::NODE
*CDXAStar::CheckCLOSED(int tilenum)
{
   NODE *tmp;

   tmp = CLOSED->NextNode;

   while ( tmp != NULL )
   {
		if ( tmp->NodeNum == tilenum )
      	return(tmp);
		else
	   	tmp = tmp->NextNode;
   }
   return(NULL);
}

////////////////////////////////////////////////////////////////////////////////

void CDXAStar::Insert(NODE *Successor)
{
   NODE *tmp1, *tmp2;
   int f;

   if ( OPEN->NextNode == NULL )
   {
		OPEN->NextNode = Successor;
		return;
   }

       // insert into OPEN successor wrt f
   f = Successor->f;
   tmp1 = OPEN;
   tmp2 = OPEN->NextNode;

   while ( (tmp2 != NULL) && (tmp2->f < f) )
   {
   	 tmp1 = tmp2;
       tmp2 = tmp2->NextNode;
   }

   Successor->NextNode = tmp2;
   tmp1->NextNode = Successor;
}

////////////////////////////////////////////////////////////////////////////////

void CDXAStar::PropagateDown(NODE *Old)
{
   int c, g;
   NODE *Child, *Father;

   g = Old->g;            // alias.
   for ( c = 0; c < 8; c++)
   {
   	if ( (Child=Old->Child[c]) == NULL )   // create alias for faster access.
          break;
		if ( g+1 < Child->g)
		{
	   	 Child->g = g+1;
	   	 Child->f = Child->g + Child->h;
	  	    Child->Parent = Old;           // reset parent to new path.
	       Push(Child);                 // Now the Child's branch need to be
		}     // checked out. Remember the new cost must be propagated down.
   }

   while ( Stack->NextStackPtr != NULL )
   {
		Father = Pop();
		for (c = 0; c<8; c++)
		{
	   	if ( (Child=Father->Child[c]) == NULL )  // we may stop the propagation 2 ways: either
	  			break;
	      if ( Father->g+1 < Child->g ) // there are no children, or that the g value of
	  		{                          // the child is equal or better than the cost we're propagating
	      	Child->g = Father->g+1;
	    		Child->f = Child->g+Child->h;
	    		Child->Parent = Father;
	    		Push(Child);
	      }
	   }
   }
}


////////////////////////////////////////////////////////////////////////////////
//                              STACK FUNCTIONS                               //
////////////////////////////////////////////////////////////////////////////////

void CDXAStar::Push(NODE *Node)
{
   STACK *tmp;

   tmp =( STACK* )calloc(1,sizeof( STACK ));
   tmp->NodePtr = Node;
   tmp->NextStackPtr = Stack->NextStackPtr;
   Stack->NextStackPtr = tmp;
}

////////////////////////////////////////////////////////////////////////////////

CDXAStar::NODE
*CDXAStar::Pop(void)
{
   NODE *tmp;
   STACK *tmpSTK;

   tmpSTK = Stack->NextStackPtr;
   tmp = tmpSTK->NodePtr;

   Stack->NextStackPtr = tmpSTK->NextStackPtr;
   free(tmpSTK);
   return(tmp);
}
