#include <helper.h>
#include <stdbool.h>
#include <string.h>
#include <stddef.h>
#include <darray.h>

#include <algorithm.h>

#define index(x) ((x)*2+1)
#define indexpoint(p) ((struct point){index(p.y), index(p.x)})
#define unindex(x) (((x)-1)/2)
#define unindexpoint(p) ((struct point){unindex(p.y), unindex(p.x)})


#define UNVISITED '#'
#define VISITED ' '


typedef unsigned long size_t;

static char *maze_at(char *m, size_t xs, struct point p)
{
	return &m[p.y*xs+p.x];
}

static bool maze_in_bounds(size_t ys, size_t xs, struct point p)
{
	return p.y > 0 && (size_t)p.y < ys-1 && p.x > 0 && (size_t)p.x < xs-1;
}

struct game make_maze(size_t y, size_t x)
{
	y += (y%2 == 0); //it must be an uneven number to look fine
	x += (x%2 == 0);
	assert(y >= 5);
	assert(x >= 5);
	struct game g = create_game(y, x);

	char *maze = xmalloc(y * x + 1);
	memset(maze, UNVISITED, y * x);
	maze[y * x] = '\0';
	darray(struct point) stack = darray_new();

	struct point current = {0, 0};
	*maze_at(maze, x, indexpoint(current)) = VISITED;

	while(true)
	{
		struct point nb[4]  =
		{
			{index(current.y), index(current.x-1)},
			{index(current.y), index(current.x+1)},
			{index(current.y-1), index(current.x)},
			{index(current.y+1), index(current.x)},
		};

		if((maze_in_bounds(y,x,nb[0]) && (*maze_at(maze,x,nb[0]) == UNVISITED)) ||
		   (maze_in_bounds(y,x,nb[1]) && (*maze_at(maze,x,nb[1]) == UNVISITED)) ||
		   (maze_in_bounds(y,x,nb[2]) && (*maze_at(maze,x,nb[2]) == UNVISITED)) ||
		   (maze_in_bounds(y,x,nb[3]) && (*maze_at(maze,x,nb[3]) == UNVISITED)))
		{
			int c;
			darray_append(stack, current);

			while(!maze_in_bounds(y,x,nb[c=randrange(0,3)]) || *maze_at(maze, x, nb[c]) != UNVISITED);
			*maze_at(maze, x, nb[c]) = VISITED;

			struct point choice = unindexpoint(nb[c]);
			struct point diff = {choice.y-current.y, choice.x-current.x};
			*maze_at(maze, x, (struct point){index(current.y)+diff.y, index(current.x)+diff.x}) = VISITED;
			current = choice;
		} else if(!darray_empty(stack)) {
			current = darray_pop(stack);
		} else {
			break;
		}
	}

	for(size_t i = 0; i < y; i++)
	{
		for(size_t j = 0; j < x; j++)
		{
			if(*maze_at(maze, x, (struct point){i,j}) == VISITED)
				game_at(&g, (struct point){i,j})->u->ground = EMPTYFIELD;
			else
				game_at(&g, (struct point){i,j})->u->ground = WALLFIELD;
		}
	}

	game_at(&g, (struct point){y-2,x-2})->u->ground.symbol = '*';
	game_at(&g, (struct point){y-2,x-2})->u->ground.type = TARGET;

	darray_free(stack);
	free(maze);
	return g;
}

struct node
{
	struct node *parent;
	struct point p;
	size_t g;
	size_t h;
	size_t f;
};

pointarr astar(struct game *g, struct point start, struct point target, size_t maxdist)
{
	darray(struct node*) open = darray_new();
	darray(struct node*) closed = darray_new();
	pointarr result = darray_new();
	struct node *startnode = xmalloc(sizeof(*startnode));
	startnode->p = start;
	startnode->parent = NULL;
	startnode->f = 0;
	startnode->g = 0;
	startnode->h = 0;

	darray_append(open, startnode);
	while(!darray_empty(open))
	{
		size_t currenti = 0;
		struct node *currentnode = darray_item(open, currenti);
		for(size_t i = 0; i < open.size; i++)
		{
			if(darray_item(open, i)->f < currentnode->f) {
				currenti = i;
				currentnode = darray_item(open, i);
			}
		}

		darray_append(closed, darray_item(open, currenti));
		darray_remove(open, currenti);
		if(pointeq(currentnode->p, target))
		{
			struct node *this = currentnode;
			darray_append(result, this->p);
			while((this = this->parent) != NULL)
			{
				darray_append(result, this->p);
			}
		}

		darray(struct point) children = darray_new();
		struct point up    = (struct point){currentnode->p.y-1, currentnode->p.x};
		struct point down  = (struct point){currentnode->p.y+1, currentnode->p.x};
		struct point left  = (struct point){currentnode->p.y, currentnode->p.x-1};
		struct point right = (struct point){currentnode->p.y, currentnode->p.x+1};

		if(game_at(g,left)->u->ground.type == NOOBJ)
			darray_append(children, left);
		if(game_at(g,right)->u->ground.type == NOOBJ)
			darray_append(children, right);
		if(game_at(g,up)->u->ground.type == NOOBJ)
			darray_append(children, up);
		if(game_at(g,down)->u->ground.type == NOOBJ)
			darray_append(children, down);


		for(size_t i = 0; i < children.size; i++)
		{
			struct point child = darray_item(children, i);
			struct node **n;
			darray_foreach(n, closed)
			{
				if(pointeq((*n)->p, child))
					goto next;
			}
			
			if(maxdist != 0 && currentnode->g+1 > maxdist)
				goto next;
			
			darray_foreach(n, open)
			{
				if(pointeq((*n)->p, child) && currentnode->g+1 > (*n)->g)
 					goto next;
			}
			darray_foreach(n, closed)
			{
				if(pointeq((*n)->p, child) &&currentnode->g+1 > (*n)->g)
					goto next;
			}
			
			struct node *childnode = xmalloc(sizeof(*childnode));
			childnode->p = child;
			childnode->parent = currentnode;
			childnode->g = currentnode->g + 1;
			childnode->h = ipow(childnode->p.y - target.y, 2) + ipow(childnode->p.x - target.x, 2);
			childnode->f = childnode->g + childnode->h;

			darray_append(open, childnode);


		next:;
		}
		darray_free(children);

	}

	struct node **n;
	darray_foreach(n, closed)
	{
		free(*n);
	}

	darray_free(open);
	darray_free(closed);

	return result;
}