#ifndef ASTARPATHFINDER_H
#define ASTARPATHFINDER_H
#include "Layer.h"

//AstarPathfinder.h
//#include"Unitconfig.h"
#define SHIFT           5

class AstarPathfinder
{
private:
	struct NODE 
	{
		long f, h;
		int g, tmpg;
		int x, y;
		int NodeNum;
		NODE *Parent;
		NODE *Child[8];
		NODE *NextNode;
	};

	NODE *OPEN;
	NODE *CLOSED;
	NODE *PATH;
	struct STACK 
	{
	   	NODE *NodePtr;
	    STACK *NextStackPtr;
	};	
	STACK *Stack;
    bool isPath;   
    int HEIGHT,
        WIDTH,
        TOTAL_TILES;
    int *TileMap;
public:   
   	AstarPathfinder(Layer &Map,unsigned short);
    ~AstarPathfinder();
   	void InitAstarTileMap(Layer &Map,unsigned short);      
   	void RedoAstarTileMap(Layer &Map,unsigned short);   
  
	bool NewPath(int sx, int sy, int dx, int dy);
	bool ReachedGoal(void);
	void PathNextNode(void);
	int NodeGetX(void);
	int NodeGetY(void);
    int TileNum(int x, int y);
	bool FreeTile(int x, int y);
private:

	void BoundaryTiles(void);
    void FreeNodes(void);
    void FindPath(int sx, int sy, int dx, int dy);
	NODE *ReturnBestNode(void);
	void GenerateSuccessors(NODE *BestNode, int dx, int dy);
   	void GenerateSucc(NODE *BestNode,int x, int y, int dx, int dy);
	NODE *CheckOPEN(int tilenum);
	NODE *CheckCLOSED(int tilenum);
	void Insert(NODE *Successor);
	void PropagateDown(NODE *Old);
    void Push(NODE *Node);
	NODE *Pop(void);
};


//AstarPathfinder.cpp

AstarPathfinder::AstarPathfinder(Layer &Map,unsigned short lay)
{
   Stack = ( STACK* )calloc(1,sizeof( STACK ));
   isPath = false;
   OPEN = NULL;
   CLOSED = NULL;
   PATH = NULL;

   InitAstarTileMap(Map,lay);
}

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

void AstarPathfinder::InitAstarTileMap(Layer &Map,unsigned short lay)
{
	int i,j,index=0;

	WIDTH = WORLD_X;
	HEIGHT = WORLD_Y;
	TOTAL_TILES = HEIGHT*WIDTH;

	TileMap = new int[TOTAL_TILES];

	for (j=0; j<HEIGHT; j++)
	{
		for (i=0; i<WIDTH; i++)
		{
			TileMap[index]=0;

			if (Map.cells[j][i].get_passable(lay)==1)
				TileMap[index] = 1; 

			index++;
		}
	}
}

void AstarPathfinder::RedoAstarTileMap(Layer &Map,unsigned short lay)
{
	int i,j,index= 0;

	for (j=0; j<HEIGHT; j++)
	{
		for (i=0; i<WIDTH; i++)
		{
			TileMap[index]=0;

			if (Map.cells[j][i].get_passable(lay)==1)
				TileMap[index] = 1; 

			index++;
		}
	}
}

bool AstarPathfinder::NewPath(int sx,int sy, int dx,int dy)
{
	if ( FreeTile(dx,dy)&&FreeTile(sx,sy) && (TileNum(sx,sy)!=TileNum(dx,dy)) )
	{
		isPath=true;
		FreeNodes();
   		FindPath(sx,sy,dx,dy);
   		return (isPath);
	}
	else
	{
		return (isPath=false);
	}
}

bool AstarPathfinder::ReachedGoal(void)
{
   if ( !isPath ) return true;
   if ( PATH->Parent != NULL )
   	return false;
   else
   	return true;
}

int AstarPathfinder::TileNum(int x, int y)
{
	return ((y>>SHIFT)*WIDTH + (x>>SHIFT));
}

bool AstarPathfinder::FreeTile(int x, int y)
{
   if (TileMap[(y>>SHIFT)*WIDTH + (x>>SHIFT)]==0) return true;
   return false;
}

void AstarPathfinder::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 AstarPathfinder::FindPath(int sx, int sy, int dx, int dy)
{
	NODE *Node, *BestNode;
   int TileNumDest;

   isPath=true;
   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);
   Node->f = Node->g+Node->h;
   Node->NodeNum = TileNum(dx, dy);
   Node->x = dx;
   Node->y = dy;

   OPEN->NextNode=Node;
   for (;;)
   {
		BestNode=ReturnBestNode();

		if (BestNode==NULL) break;

		if (BestNode->NodeNum == TileNumDest) 
			break;

		GenerateSuccessors(BestNode,sx,sy);
   }
   PATH = BestNode;
}

void AstarPathfinder::PathNextNode(void) 
{ 
	PATH=PATH->Parent; 
}

int AstarPathfinder::NodeGetX(void) 
{ 
	return PATH->x; 
}

int AstarPathfinder::NodeGetY(void)   
{ 
	return PATH->y; 
}

AstarPathfinder::NODE*AstarPathfinder::ReturnBestNode(void)
{
   NODE *tmp;

   if ( OPEN->NextNode == NULL )
   {
		isPath=false;
		tmp=NULL;
		return tmp;
   }

   tmp = OPEN->NextNode;
   OPEN->NextNode = tmp->NextNode;

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

   return(tmp);
}

void AstarPathfinder::GenerateSuccessors(NODE *BestNode, int dx, int dy)
{
   int x, y;

   // Upper-Left
   if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->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) )
      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) )
      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) )
      GenerateSucc(BestNode,x,y,dx,dy);

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

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

   g = BestNode->g+1;
   TileNumS = TileNum(x,y);

   if ( (Old=CheckOPEN(TileNumS)) != NULL )
   {
		for( c = 0; c < 8; c++)
	   	if( BestNode->Child[c] == NULL )
	   		break;
		BestNode->Child[c] = Old;

		if ( g < Old->g )
		{
			Old->Parent = BestNode;
	   	Old->g = g;
	   	Old->f = g + Old->h;
		}
   }
   else if ( (Old=CheckCLOSED(TileNumS)) != NULL )
   {
   for( c = 0; c< 8; c++)
		if ( BestNode->Child[c] == NULL )
	   		break;
		BestNode->Child[c] = Old;

		if ( g < Old->g )
		{
	   	    Old->Parent = BestNode;
	   	    Old->g = g;
	   	    Old->f = g + Old->h;
	   	    PropagateDown(Old);
					   	        
		}
   }
   else
   {
		Successor = ( NODE* )calloc(1,sizeof( NODE ));
		Successor->Parent = BestNode;
		Successor->g = g;
		Successor->h = (x-dx)*(x-dx) + (y-dy)*(y-dy);
		Successor->f = g+Successor->h;
		Successor->x = x;
		Successor->y = y;
		Successor->NodeNum = TileNumS;
		Insert(Successor);
		for( c =0; c < 8; c++)
			if ( BestNode->Child[c] == NULL )
	  		    break;
		BestNode->Child[c] = Successor;
   }
}


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

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


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

   tmp = CLOSED->NextNode;

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


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

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

   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 AstarPathfinder::PropagateDown(NODE *Old)
{
   int c, g;
   NODE *Child, *Father;

   g = Old->g;
   for ( c = 0; c < 8; c++)
   {
   	if ( (Child=Old->Child[c]) == NULL )
          break;
		if ( g+1 < Child->g)
		{
	   	 Child->g = g+1;
	   	 Child->f = Child->g + Child->h;
	  	    Child->Parent = Old;
	       Push(Child);
		}
   }

   while ( Stack->NextStackPtr != NULL )
   {
		Father = Pop();
		for (c = 0; c<8; c++)
		{
	   	if ( (Child=Father->Child[c]) == NULL )
	  			break;
	      if ( Father->g+1 < Child->g )
	  		{
	      	Child->g = Father->g+1;
	    		Child->f = Child->g+Child->h;
	    		Child->Parent = Father;
	    		Push(Child);
	      }
	   }
   }
}


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

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

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

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

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

#endif