#include<stdlib.h>
#include<stdio.h>
#include<string.h>
#include<limits.h>
#include<assert.h>

struct cell_s {
	int blocked:1;
	int closed:1;
	int opened:1;
	int route:1;
	int cost;
	int prevx, prevy;
};

struct node_s {
	int x, y;
	int totalcost;
	struct node_s *next;
};

typedef struct {
	int sizex, sizey;
	int startx, starty;
	int targetx, targety;
	struct cell_s *cells;
	struct node_s *open;
} ASTAR_ARRAY;

#define CELL(array, x, y) ((array)->cells[(y)*((array)->sizex)+(x)])

ASTAR_ARRAY *astar_create(size_t sizex, size_t sizey)
{
	ASTAR_ARRAY *array;
	
	if(!(array=calloc(1, sizeof(ASTAR_ARRAY)))) return NULL;
	if(!(array->cells=calloc(sizex*sizey, sizeof(struct cell_s)))) {
		free(array); 
		return NULL;
	}

	array->sizex=sizex; array->sizey=sizey;
	array->startx=0; array->starty=0;
	array->targetx=0; array->targety=0;

	return array;
}

void astar_destroy(ASTAR_ARRAY *array)
{
	struct node_s *node, *next;
	node=array->open;
	while(node) {
		next=node->next;
		free(node);
		node=next;
	}
	free(array->cells);
	free(array);
}

void astar_setpoints(ASTAR_ARRAY *array, int startx, int starty, int targetx, size_t targety)
{
	array->startx=startx; array->starty=starty;
	array->targetx=targetx; array->targety=targety;
}

void astar_block(ASTAR_ARRAY *array, int firstx, int firsty, int lastx, int lasty, int blocked)
{
	int x, y;
	for(y=firsty; y<=lasty; ++y) {
		for(x=firstx; x<=lastx; ++x) {
			CELL(array, x, y).blocked=blocked;
		}
	}
}

static int astar_targetcost(ASTAR_ARRAY *array, int x, int y)
{
/*	return 0;*/
	return 10*(abs(x-array->targetx)+abs(y-array->targety));
}

static struct node_s *astar_opencell(ASTAR_ARRAY *array, int x, int y, int cost, int prevx, int prevy)
{
	struct node_s *node, *nextnode=array->open, *prevnode=NULL;

	if(x<0 || x>=array->sizex || y<0 || y>=array->sizey) return NULL;

	if(!CELL(array, x, y).blocked && !CELL(array, x, y).closed && !CELL(array, x, y).opened) {
		/* Create and populate node */
		if(!(node=malloc(sizeof(struct node_s)))) return NULL;
		node->x=x; node->y=y;
		node->totalcost=cost+astar_targetcost(array, x, y);

		/* Update cell info. */
		CELL(array, x, y).cost=cost;
		CELL(array, x, y).prevx=prevx;
		CELL(array, x, y).prevy=prevy;
		CELL(array, x, y).opened=1;

		/* Insert node in open list */
		while(nextnode && node->totalcost>nextnode->totalcost) {
			prevnode=nextnode;
			nextnode=nextnode->next;
		}
		if(nextnode) node->next=nextnode;
		else node->next=NULL;
		if(prevnode) prevnode->next=node;
		else array->open=node;

		return node;
	} else if(cost<CELL(array, x, y).cost) {
		/* Not opened/closed */
		CELL(array, x, y).closed=0;
		CELL(array, x, y).opened=0;

		/* Find node */
		node=array->open;
		while(node && (node->x!=x || node->y!=y)) {
			prevnode=node;
			node=node->next;
		}
		assert(node);

		/* Remove node */
		if(prevnode) prevnode->next=node->next;
		else array->open=node->next;
		free(node);

		/* Reopen */
		astar_opencell(array, x, y, cost, prevx, prevy);

		return node;
	} return NULL;
}

static void astar_openadjacent(ASTAR_ARRAY *array, int x, int y, int cost)
{
	if(x-CELL(array, x, y).prevx!=0) {		/* Extra cost from x to y dir */
		astar_opencell(array, x-1, y, cost+10, x, y);
		astar_opencell(array, x, y-1, cost+15, x, y);
		astar_opencell(array, x, y+1, cost+15, x, y);
		astar_opencell(array, x+1, y, cost+10, x, y);
	} else if(y-CELL(array, x, y).prevy!=0) {	/* Extra cost from y to x dir */
		astar_opencell(array, x-1, y, cost+15, x, y);
		astar_opencell(array, x, y-1, cost+10, x, y);
		astar_opencell(array, x, y+1, cost+10, x, y);
		astar_opencell(array, x+1, y, cost+15, x, y);
	} else {					/* Start pos., no extra cost */
		printf("Initial\n");
		astar_opencell(array, x-1, y, cost+10, x, y);
		astar_opencell(array, x, y-1, cost+10, x, y);
		astar_opencell(array, x, y+1, cost+10, x, y);
		astar_opencell(array, x+1, y, cost+10, x, y);
	}
}

static int astar_closecell(ASTAR_ARRAY *array, struct node_s *node)
{
	CELL(array, node->x, node->y).closed=1;
	CELL(array, node->x, node->y).opened=0;

	if(array->open==node) {
		array->open=node->next;
		free(node);
		return 0;
	} else return -1;
}

int astar(ASTAR_ARRAY *array)
{
	struct node_s *best;
	int x, y;
	struct cell_s *cell;
	int n=0;

	/* Find target */
	best=astar_opencell(array, array->startx, array->starty, 0, array->startx, array->starty);
	while(best && (best->x!=array->targetx || best->y!=array->targety)) {
		astar_openadjacent(array, best->x, best->y, CELL(array, best->x, best->y).cost);
		astar_closecell(array, best);
		best=array->open;
		if(++n>100000) return -1;
	}
	if(!best) return -1;

	/* Find way back (And mark as "route") */
	x=best->x; y=best->y;
	while(x!=array->startx || y!=array->starty) {
		cell=&CELL(array, x, y);
		cell->route=1;
		x=cell->prevx;
		y=cell->prevy;
	}

	return 0;
}

void astar_print(ASTAR_ARRAY *array)
{
	struct cell_s *cell;
	int x, y;

	printf("    ");
	for(x=0; x<array->sizex; ++x) {
		printf(" %2d ", x);
	}
	putchar('\n');
	for(y=0; y<array->sizey; ++y) {
		printf("%2d: ", y);
		for(x=0; x<array->sizex; ++x) {
			cell=&CELL(array, x, y);
			if(x==array->startx && y==array->starty) printf("SSSS");
			else if(x==array->targetx && y==array->targety) printf("TTTT");
			else if(cell->route) printf(" ** ");
			else if(cell->blocked) printf("####");
			else if(cell->closed) printf(" CC ");
			else if(cell->opened) printf(" OO ");
			else printf("    ");
		}
		putchar('\n');
	}
/*	if(array->open) printf("Best open cell: %d,%d, totalcost=%d\n", 
			array->open->x, array->open->y, array->open->totalcost);*/
	printf("Cost: %d\n", CELL(array, array->targetx, array->targety).cost);
}

int main()
{
	ASTAR_ARRAY *array;

	if(!(array=astar_create(24, 24))) {
		fprintf(stderr, "ERROR: astar_create() failed\n");
		exit(EXIT_FAILURE);
	}

	/* Block areas */
	astar_block(array, 3, 7, 11, 20, 1);
	astar_block(array, 11, 10, 22, 20, 1);
	astar_block(array, 1, 15, 12, 22, 1);

	/* Unblock areas (make holes) */
	astar_block(array, 12, 10, 13, 16, 0);
	astar_block(array, 5, 16, 12, 16, 0);
	astar_block(array, 5, 17, 5, 22, 0);
	astar_block(array, 9, 7, 9, 15, 0);

	/* Set start and target points */
	astar_setpoints(array, 7, 2, 15, 22);

	/* Do it */
	if(astar(array)==-1) {
		fprintf(stderr, "ERROR: Did not find a route to target\n");
		astar_print(array);
		return EXIT_FAILURE;
	}

	/* And... print */
	astar_print(array);

	astar_destroy(array);

	return EXIT_SUCCESS;
}