#include "mylist.h"
#include "mygrid.h"
#include "myastar.h"
#include "myestimate.h"

int OpenList[N][NI];
int OpenListSize=0;
int ClosedList[N][NI];
int ClosedListSize=0;

//const int dir=8; // number of possible directions
/*
dr,dc

-1,-1 |-1,0  |-1,1
(5)   |(6)   |(7)
------|------|-----
0,-1  |	  .  |0,1
(4)   |      |(0)
------|------|-----
1,-1  |1,0   |1,1
(3)   |(2)   |(1)
      |      |
*/
//static int dr[dir]={0, 1, 1,  1,  0, -1, -1, -1};
//static int dc[dir]={1, 1, 0, -1, -1, -1,  0,  1};

const int dir=4; // number of possible directions
/*
dr,dc
      |-1,0  |
      |(3)   |
------|------|-----
0,-1  |	  .  |0,1
(2)   |      |(0)
------|------|-----
      |1,0   |
      |(1)   |
      |      |

*/
static int dr[dir]={0, 1,  0, -1};
static int dc[dir]={1, 0, -1,  0};


void node_init(int v[NI],int r, int c,int G,int H,int parent=-1){
	v[INFO_ROW] = r;
	v[INFO_COL] = c;
	v[INFO_PARENT] = parent;
	v[INFO_G] = G;
	v[INFO_F] = G + H;
}

void updateG(int n[NI],const int & i) // i: direction
{
	n[INFO_G]+=(dir==8?(i%2==0?10:14):10);
}

void updateH(int n[NI], const int & rDest, const int & cDest)
{
	int r=n[INFO_ROW],c=n[INFO_COL];
	n[INFO_F]=n[INFO_G] + estimate(undefined,r,c,rDest, cDest);
}


void myswap(int x[NI],int y[NI])
{
	int t[NI];
	for(int i=0; i < NI; i++)
		t[i]=x[i];
	for(int i=0; i < NI; i++)
		x[i]=y[i];
	for(int i=0; i < NI; i++)
		y[i]=t[i];
}

void BubbleSort(int a[][NI], int array_size, bool bAsc=true)
{
	int i, j;
	for (i = 0; i < (array_size - 1); ++i)
	{
		for (j = 0; j < array_size - 1 - i; ++j )
		{
			if (bAsc)
			{
				if((a[j][INFO_F]) > (a[j+1][INFO_F]))
				{
					myswap(a[j],a[j+1]);
				}
			}
			else
				if((a[j][INFO_F]) < (a[j+1][INFO_F]))
				{
					myswap(a[j],a[j+1]);
				}
		}
	}
}

// A-star algorithm.
bool AStarPathFind( int grid[_NR][_NC], int NR, int NC, const int & rStart, const int & cStart, 
				const int & rFinish, const int & cFinish, int lstPath[N][2],int &lstPathSize )
{
	if(_NR<=0 || _NC <= 0 || NR <=0 | NC <=0 || rStart<0 || cStart <0 || rFinish < 0 || cFinish <0)
		return false;
	int n0[NI];
	int iLow=0;
	OpenListSize=0;
	ClosedListSize=0;
	
        // create the start node and push into list of open nodes
	node_init(n0,rStart,cStart,0,0);
	updateH(n0,rFinish,cFinish);
	List_Add(OpenList,N,OpenListSize, n0);

	// A* search
	while(OpenListSize >0)
	{
	       // get the current node with the lowest F cost from the list of open nodes
#if SORT
		iLow=OpenListSize-1;
#else
		iLow = List_FindLowCost(OpenList,N,OpenListSize);
		if (iLow<0)
			break;
#endif
	 	List_Item(OpenList,N,OpenListSize,n0,iLow);
		if (iLow<0)
			break;
// Add node to the open list
		List_Add(ClosedList,N,ClosedListSize,n0);
 
// remove the node from the open list
		List_Remove(OpenList,N,OpenListSize,iLow);

		int cdc,rdr;
		int r=n0[INFO_ROW],c=n0[INFO_COL];
 
// Stop when you:
//    Add the target square to the closed list, in which case the path has been found (see note below), or
//    Fail to find the target square, and the open list is empty. In this case, there is no path.   
		if(r==rFinish && c==cFinish)
		{
// Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path
			//string path="";
			char ch;
			int ix;
			int j;
			lstPathSize=0;
			while(!(r==rStart && c==cStart))//Backtrace
			{
				ix=List_FindItem(ClosedList,N,ClosedListSize,r,c);
				if (ix >=0 )
				{
					int np[NI];
					List_Item(ClosedList,N,ClosedListSize,np,ix);
					j=np[INFO_PARENT];
					r+=dr[j];
					c+=dc[j];
					lstPath[lstPathSize][0]=r;
					lstPath[lstPathSize][1]=c;
					lstPathSize++;
				}
			}
			if (lstPathSize >0 ) 
				lstPathSize--;

			return true;
		}

// generate moves (child nodes) in all possible directions (8 or 4)
		for(int i=0;i<dir;i++)
		{
			rdr=r+dr[i];
			cdc=c+dc[i]; 
//If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.           
			if(!(cdc<0 || cdc>NC-1 || rdr<0 || rdr>NR-1 || grid[rdr][cdc]==1 
				|| List_FindItem(ClosedList,N,ClosedListSize,rdr,cdc)>=0))
			{
				// generate a child node
				int m0[NI];
				node_init(m0,rdr,cdc,n0[INFO_G],n0[INFO_F]);
				updateG(m0,i);
				updateH(m0,rFinish,cFinish);

				int ix=List_FindItem(OpenList,N,OpenListSize,rdr,cdc);
				if(ix < 0)
				{
				//If it isn’t on the open list, add it to the open list. 
				// mark its parent node direction
				//Record the F, G, and H costs of the square. 
					m0[INFO_PARENT]=(i+dir/2)%dir;
					List_Add(OpenList,N,OpenListSize,m0);
				}
				else
				{
//If it is on the open list already, 
//check to see if this path to that square is better, using G cost as the measure.
//A lower G cost means that this is a better path. 
//If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. 
//If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.
					int mi[NI];
					List_Item(OpenList,N,OpenListSize,mi,ix);
					if(mi[INFO_F] > m0[INFO_F])
					{
						mi[INFO_F] = m0[INFO_F];
						mi[INFO_G] = m0[INFO_G];
						mi[INFO_PARENT] = (i+dir/2)%dir;
					}
				}
			}
		}

#if SORT
		BubbleSort(OpenList,OpenListSize,false);
#endif
	}

	return false; // no route found
}