#include "gengine.h"

#define MSIZE		34
/*
class Path{
	BYTE wayPoint[MaxWayLen];
	int len;
	int head;

public:
	Path();
	bool IsEmpty( void ){
		return head == len;
	};

	int GetWayPoint( void ){
		return wayPoint[head++];
	};

	int FindPath( int sx, int sy, int dx, int dy );
};*/

struct Node{
	int x, y;
	int g, h;
	Node *prev, *next;

	Node( int X, int Y, int G, int H );
	Node();
};

struct Rally{
	Node head;

	Rally();
	~Rally();

	bool IsEmpty( void );
	void AddNode( Node* node );
	void DelNode( Node* node );
	Node* GetLeastCostNode( void );
	void Free( void );
};
static Rally Open, Closed;

struct Map{
	unsigned short wayPoint;
	unsigned short occupied;
	Node* node;
};
static Map map[MSIZE][MSIZE];

Path::Path( )
{
	len = 0;
	head = 0;
}


Node::Node( int X, int Y, int G, int H )
{
	x = X; y = Y;
	g = G; h = H;
	next = prev = NULL;
}

Node::Node()
{
	next = prev = NULL;
}

Rally::Rally()
{
	head.next = head.prev = NULL;
}

Rally::~Rally()
{
	Free();
}

inline bool Rally::IsEmpty( void )
{
	return head.next == NULL;
}

void Rally::AddNode( Node* node )
{
	if( node ){
		node->next = head.next;
		node->prev = &head;
		if( head.next )head.next->prev = node;
		head.next = node;
	}
}

void Rally::DelNode( Node* node )
{
	if( node ){
		node->prev->next = node->next;
		if( node->next )
			node->next->prev = node->prev;
	}
}

void Rally::Free( void )
{
	Node *p, *q;
	q = head.next;
	head.next = NULL;
	while( q ){
		p = q;
		q = q->next;
		delete p;
	}
}

Node* Rally::GetLeastCostNode( void )
{
	Node *p = head.next, *found = head.next;
	int cost = 10000;
	while( p ){
		if( p->g + p->h < cost ){
			cost = p->g + p->h;
			found = p;
		}
		p = p->next;
	}
	DelNode( found );
	return found;
}

//到當前點的耗散值
static inline int CostG( int g, int dir )
{
	if( dir & 1 ){
		if((dir - 1) % 4 )
			return g + 10 + 10;
		else
			return g + 10;
	}
	else
		return g + 12;
}

//估計耗散值
static inline int CostH( int sx, int sy, int dx, int dy )
{
	int deltax, deltay, c;
	deltax = dx - sx;
	deltay = dy - sy;
	if( deltax * deltay < 0 )
		c = 10 + 10;
	else
		c = 10 + 2;
	deltax = ABS( deltax );
	deltay = ABS( deltay );
	if( deltax > deltay ){
		deltax -= deltay;
		return deltay * c + deltax * 10;
	}
	else{
		deltay -= deltax;
		return deltax * c + deltay * 10;
	}
}

//用于擴展節點的輔助數據
static int adjacent[8][2] = {{ -1, 0 }, { -1, -1 }, { 0, -1 }, { 1, -1 }, 
					{ 1, 0 }, { 1, 1 }, { 0, 1 }, { -1, 1 }};
static bool mapInited = false;
//路徑搜索,返回路徑上的點數, 以sx,sy為中心點,搜索半徑MSIZE/2-1
int Path::FindPath( int sx, int sy, int dx, int dy )
{
	int i, j;
	int fx, fy, g, h;
	Node *current, *pnode;
	//int count = 0;

	if( !mapInited ){	//已經畫出邊框?
		for( i=0; i<MSIZE; i++ ){
			map[0][i].occupied = 1;
			map[MSIZE-1][i].occupied = 1;
			map[i][0].occupied = 1;
			map[i][MSIZE-1].occupied = 1;
		}
		mapInited = true;
	}
	// copy maze to map
	if( sx > MSIZE/2 && sx - MSIZE/2 < MAZE_SIZE - MSIZE
		&& sy >= MSIZE/2 && sy - MSIZE/2 < MAZE_SIZE - MSIZE*2 ){
		// no clip
		for( i=1, fy=sy-MSIZE/2+2; i<MSIZE-1; i++, fy++ ){
			for( j=1, fx=sx-MSIZE/2+2; j<MSIZE-1; j++, fx++ ){
				map[i][j].occupied = maze[fy][fx].occupied;
				map[i][j].wayPoint = 0xff;
				//map[i][j].node = NULL;
			}
		}
	}
	else{// clip
		/*int i1, j1;

		fx = sx-MSIZE/2+2;
		fy = sy-MSIZE/2+2;
		g = h = MSIZE - 2;
		i = j = 1;
		if( fx < 0 ){
			i = -fx+1; g -= i; fx = 0;
			for( j=1; j<MSIZE-1; j++ )
				map[j][i-1].occupied = 1;
		}
		if( fx + g > MAZE_SIZE ){
			g = MAZE_SIZE - fx;
			for( i1=1; i1<MSIZE-1; i1++ )
				map[i1][i+g].occupied = 1;
		}

		if( fy < 0 ){
			j = -fy+1; h -= j; fy = 0;
			for( i1=1; i1<MSIZE-1; i1++ )
				map[j][i1].occupied = 1;
		}
		if( fy + h >= MAZE_SIZE ){
			h = MAZE_SIZE - fy;
			for( i1=1; i1<MSIZE-1; i1++ )
				map[j+h][i1].occupied = 1;
		}

		for( i1=i; */
		for( i=1, fy=sy-MSIZE/2+2; i<MSIZE-1; i++, fy++ ){
			for( j=1, fx=sx-MSIZE/2+2; j<MSIZE-1; j++, fx++ ){
				if( fx<0 || fx>MAZE_SIZE-1 || fy<0 || fy>MAZE_SIZE-1){
					map[i][j].occupied = 1;
				}
				else{
					map[i][j].occupied = maze[fy][fx].occupied;
					map[i][j].wayPoint = 0xff;
					//map[i][j].node = NULL;
				}
			}
		}
	}
	//交換起點與終點
	i = dx - sx + MSIZE/2-1;	sx = dy - sy + MSIZE/2-1;
	sy = i;
	dx = MSIZE/2-1;				dy = MSIZE/2-1;

	// start to search
	current = new Node( sx, sy, 0, CostH( sx, sy, dx, dy ));
	map[sx][sy].node = current;
	map[sx][sy].wayPoint = 0;
	Open.AddNode( current );

	while( !Open.IsEmpty()){
		//count ++;
		current = Open.GetLeastCostNode();
		//adjacentand current node;
		for( i=0; i<8; i++ ){
			fx = current->x + adjacent[i][0];
			fy = current->y + adjacent[i][1];
			if( map[fx][fy].occupied )
				continue;
			g = current->g;
			h = current->h;
			if( map[fx][fy].wayPoint > 7 ){
				//hasn't been adjacentanded
				map[fx][fy].wayPoint = i;
				if( fx == dx && fy == dy ) 
					goto success;
				map[fx][fy].node = new Node( fx, fy, CostG( g, i ), CostH(fx, fy, dx, dy));
				Open.AddNode( map[fx][fy].node );
			}
			else{// has been adjacentanded
				if(( map[current->x][current->y].wayPoint + 4) % 8 == i )
					// parent node
					continue;
				pnode = map[fx][fy].node;
				if( pnode->g + pnode->h > h + CostG( g, i )){
					map[fx][fy].wayPoint = i;
					pnode->g = CostG( g, i );
				}
				else if( CostG(pnode->g, i) < g ){ 
					current->g = CostG(pnode->g, i);
					map[current->x][current->y].wayPoint = (i+4)%8;
				}
			}
		}
		Closed.AddNode( current );
	}
	//failed, Open is empty
	return 0;

success:
	head = len = 0;
	while( !(fx == sx && fy == sy) && len < MaxWayLen ){
		i = map[fx][fy].wayPoint;
		map[fx][fy].wayPoint = 8;
		wayPoint[len++] = (i+3) % 8;
		i = ( i+4 ) % 8;
		fx += adjacent[i][0];
		fy += adjacent[i][1];
	}
	Open.Free();
	Closed.Free();
#ifdef _DEBUG
/*	FILE* fp = fopen( "path.txt", "wt" );
	if( fp ){
		for( i=0;i<MSIZE; i++ ){
			for( j=0; j<MSIZE; j++ ){
				if( map[i][j].occupied )
					fputc( '8', fp );
				else if( map[i][j].wayPoint == 8 )
					fputc( '*', fp );
				else
					fputc( ' ', fp );
			}
			fprintf( fp, "\n" );
		}
		for( i=0; i<=len; i++ )
			fprintf( fp, "%d, ", wayPoint[i] );
		fclose( fp );
	}*/
#endif
	return len;
}

#undef MSIZE
#undef MaxWayLen