#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 );
};
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()
{
	Node *p, *q;
	q = head.next;
	while( q ){
		p = q;
		q = q->next;
		delete p;
	}
}

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;
	}
}

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;
	}
}

//用于擴展節點的輔助數據
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;
		}
		for( i=1; i<MSIZE-1; i++ ){
			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++, fx++ ){
			for( j=1, fx=sx-MSIZE/2+2; j<MSIZE-1; j++, fy++ ){
				if( maze[fy][fx].occupied )
					map[i][j].occupied = 1;
				else
					map[i][j].occupied = 0;
				map[i][j].wayPoint = 0xff;
			}
		}
	}
	else{// clip
		for( i=1, fy=sy-MSIZE/2+2; i<MSIZE-1; i++, fx++ ){
			for( j=1, fx=sx-MSIZE/2+2; j<MSIZE-1; j++, fy++ ){
				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;
				}
			}
		}
	}
	//交換起點與終點
	i = sx; sx = dx; dx = i;
	i = sy; sy = dy; dy = i;

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

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

#undef MSIZE
#undef MaxWayLen