/************************************************************************************************************/
/* Description : A star algorithm.                                                                          */
/* File Name : astar.cpp                                                                                    */
/* Date: author : DEC/18/2009                                                                               */
/* Author: Hermet Park (hermet@hermet.pe.kr)                                                                */
/************************************************************************************************************/

//A node structure 
typedef struct node_s {
	int degree;						//Depth info. Same with the depth in the tree data structure
	int distance;					//Distance between this node and the destination
	int value_factor;               //Evaluated value (degree + distance)
	int x, y;                       //Grid position
	struct node_s* direct[8];       //Neighbor nodes around this
	struct node_s* prev_node;		//Previous node in the linked list
	struct node_s* next_node;		//Next node in the linked list
} node_t; 

//A stack for node sorting
typedef struct node_stack_s {
	node_t*  node;                    //A node in this stack position
	struct node_stack_s*  next_node;  //Next node stack
} node_stack_t;

enum { DIRECTION_LEFT, DIRECTION_LEFTUP, DIRECTION_UP, DIRECTION_RIGHTUP,  DIRECTION_RIGHT, DIRECTION_RIGHTDOWN, DIRECTION_DOWN, DIRECTION_LEFTDOWN };

node_t *OPEN=NULL, *CLOSED=NULL;
node_stack_t* STACK=NULL;

const int FINDPATH_LIMIT = 200;       //limit loop
 
void init_astar() {
	
	node_t* temp;
	node_stack_t* stack;

	while (OPEN) {
		temp = OPEN->next_node;
		free(OPEN);
		OPEN = temp;
	}

	while (CLOSED) {
		temp = CLOSED->next_node;
		free(CLOSED);
		CLOSED = temp;
	}
	
	while (STACK) {
		stack = STACK->next_node;
		free(STACK);
		STACK = stack;
	}
}
 
bool is_available_grid(int x, int y)  {
 
	//if( BLOCKED ) {
	//	return 0;
	//}

	return true;
}
 
node_t* is_open(int x, int y) {
	
	node_t *temp = OPEN;

	while (temp) {
		if (temp->x == x && temp->y == y) 
			return temp;		
		temp = temp->next_node;
	}

	return NULL;
}

node_t* is_closed(int x, int y) {
	
	node_t *temp = CLOSED;

	while(temp) {
		if(temp->x == x && temp->y == y) 
			return temp;
		temp = temp->next_node;
	} 

	return NULL;
} 

void push_into_stack(node_t* node) {

	node_stack_t* temp;

	temp = (node_stack_t *) calloc(1, sizeof(node_stack_t));
	temp->node = node;
	temp->next_node = STACK;
	STACK = temp;

} 

node_t* pop_from_stack() {

	node_t *temp;
	node_stack_t* stack;

	stack = STACK;
	temp = stack->node;
	STACK = stack->next_node;

	free(stack);

	return temp;
}
 
void make_sort(node_t *old) {

	node_t *direct, *previousNode;
	int i;
	int degree = old->degree+1;

	for (i=0; i < 8; i++)	{
		if ((direct = old->direct[i]) == NULL) 
			continue;

		//Updates nodes. Use a stack for children
		if (direct->degree > degree) {
			direct->prev_node = old;
			direct->degree = degree;
			direct->value_factor = direct->distance+direct->degree;			
			push_into_stack(direct);
		}
	}

	//Updates nodes using a stack.
	while (STACK) {

		previousNode = pop_from_stack();

		for (i=0; i < 8; i++)	{

			if ((direct = previousNode->direct[i]) == NULL) 
				break;	

			if (direct->degree > previousNode->degree + 1) {
				direct->prev_node = previousNode;
				direct->degree = previousNode->degree+1;
				direct->value_factor = direct->distance+direct->degree;
				push_into_stack(direct);
			}
		}
	}
}
 
void insert_node(node_t* present) {
	
	node_t* old = NULL, *temp = NULL;

	if (OPEN == NULL) {
		OPEN = present;
		return;
	} 

	temp=OPEN;
 
	//Find a node which has a higher value factor than the present.
	while (temp && (temp->value_factor < present->value_factor)) {
		old=temp;		
		temp=temp->next_node;
	} 
	
	//Reconstruct the list. Now these list are sorted by distance.
	if (old) {
		present->next_node = temp;
		old->next_node = present;
	} else {
		present->next_node = temp;
		OPEN = present;
	}
} 

void extend_child_node(node_t* node, int x, int y, int dest_x, int dest_y, int cur_direct ) {

	node_t* old = NULL, *child = NULL;
	int i;
	int degree = node->degree+1;

	// If a extending node is existed in Open Node, pick it up.
	if (old = is_open(x, y)) {
	 
		node->direct[cur_direct] = old;

		//Reset the node. Notice that node order is reversed.
		if (degree<old->degree) {
			old->prev_node = node;
			old->degree = degree;
			old->value_factor = old->distance + old->degree;
		}

	//If a extending node is existed in Close Node.
	}else if (old=is_closed(x, y)) {
		
		node->direct[cur_direct] = old;

		//In some cases, degree won't be valid. Sort it again.   
		if (degree < old->degree) {
			old->prev_node = node;
			old->degree = degree;
			old->value_factor = old->distance + old->degree;
			make_sort(old);
		}

	//Create a child node and push it into Open Node.
	}else {

		if((child = (node_t* ) calloc(1, sizeof(node_t))) == NULL)
			return;				 

		child->prev_node=node;
		child->degree = degree;
		child->distance = (x - dest_x) * (x - dest_x) + (y - dest_y) * (y - dest_y);
		child->value_factor = child->distance + child->degree;
		child->x = x;
		child->y = y;

		insert_node(child);

		node->direct[cur_direct] = child; 
	}
}
 
char make_child(node_t* node, int dest_x, int dest_y) {

	int x, y;
	char flag=0;
	char checkis_available_grid[8] = {0, 0, 0, 0, 0, 0, 0, 0};

	x = node->x;
	y = node->y;

	//check thoe directions are available.
	checkis_available_grid[DIRECTION_RIGHTDOWN] = is_available_grid(x, y + 1);
	checkis_available_grid[DIRECTION_DOWN] = is_available_grid(x, y + 1);
	checkis_available_grid[DIRECTION_LEFTDOWN] = is_available_grid(x - 1, y + 1);
	checkis_available_grid[DIRECTION_LEFT] = is_available_grid(x - 1, y);
	checkis_available_grid[DIRECTION_LEFTUP] = is_available_grid(x - 1, y - 1);
	checkis_available_grid[DIRECTION_UP] = is_available_grid(x, y - 1);
	checkis_available_grid[DIRECTION_RIGHTUP] = is_available_grid(x + 1, y - 1);
	checkis_available_grid[DIRECTION_RIGHT] = is_available_grid(x + 1, y);

	//Create children nodes if they were available space.
	if(checkis_available_grid[DIRECTION_LEFT]) {
		extend_child_node(node, x-1, y, dest_x, dest_y);
		flag=1;
	}

	if(checkis_available_grid[DIRECTION_RIGHT]) {
		extend_child_node(node, x+1, y, dest_x, dest_y);
		flag=1;
	}

	if(checkis_available_grid[DIRECTION_UP]) {
		extend_child_node(node, x, y-1, dest_x, dest_y);
		flag=1;
	}

	if(checkis_available_grid[DIRECTION_DOWN]) {
		extend_child_node(node, x, y+1, dest_x, dest_y);
		flag=1;
	}

	if(checkis_available_grid[DIRECTION_RIGHTDOWN] && checkis_available_grid[DIRECTION_RIGHT] && checkis_available_grid[DIRECTION_DOWN]) {
		extend_child_node(node, x+1, y+1, dest_x, dest_y);
		flag=1;
	}

	if(checkis_available_grid[DIRECTION_LEFTUP] && checkis_available_grid[DIRECTION_LEFT] && checkis_available_grid[DIRECTION_UP]) {
		extend_child_node(node, x-1, y-1, dest_x, dest_y);
		flag=1;
	}

	if(checkis_available_grid[DIRECTION_RIGHTUP] && checkis_available_grid[DIRECTION_RIGHT] && checkis_available_grid[DIRECTION_UP]) {
		extend_child_node(node, x+1, y-1, dest_x, dest_y);
		flag=1;
	}

	if(checkis_available_grid[DIRECTION_LEFTDOWN] && checkis_available_grid[DIRECTION_LEFT] && checkis_available_grid[DIRECTION_DOWN]) {
		extend_child_node(node, x-1, y+1, dest_x, dest_y);
		flag=1;
	}

	return flag;

}
 
node_t* find_path(int start_x, int start_y, int dest_x, int dest_y) {

	node_t* present, *best=NULL;
	int count=0;

	//Add the starting point node. Reversely, starting point is the destination here.
	//That means, navigate from the destination to the starting point. 
	//Destination to Starting point as I mentioned above. We can rewind the completed list to figure out the path. 
	present = (node_t* ) calloc(1, sizeof(node_t));
	present->degree = 0;
	present->distance = (dest_x-start_x) * (dest_x-start_x) + (dest_y-start_y) * (dest_y-start_y); // Originally, it requires a square root but not necessary.
	present->value_factor = present->distance;    // distance + degree
	present->x = dest_x;
	present->y = dest_y;

	OPEN = present;

	while (count< FINDPATH_LIMIT) { 

		if(OPEN == NULL) 
			return best;

		best = OPEN;	//Begin with investigating a candidate in the Open Node.
		OPEN = best->next_node;     //The next node should be set to a next candidate for next step.
		best->next_node = CLOSED;  //Appends the Closed Nodes to a current best one so that we can keep a constructed path properly.
		CLOSED = best;	  //Move to the Closed Node because this best node will be visited in this time.

		//Failed at path finding.
		if(best==NULL) 
			return NULL;

		//Succeed!
		if(best->x == start_x && best->y == start_y) 
			return best;

		//Extends neighbor nodes from a current node.
 		if(make_child(best, start_x, start_y)==0 && count ==0) 
			return NULL;

		count++;	
	} 

	return best;
}