#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <errno.h>

#include "hexagon.h"

/*
 * TODO use binary heap for the openlist and closed list.
 * open list should be a min-heap on the f value
 * closed list is just tested for existence, so a sort on
 * the hex id is fine
 */

#if 0
#define DEBUG_ASTAR 1 
#endif

static double default_metric(int a, int b) {
	return 1.0;
}

static double default_heuristic(int a, int b) {
	return (double)HL_distance(a, b);
}

struct HL_astar *HL_astar_init(struct HL_astar *state) {
	if (!state) {
		state = malloc(sizeof *state);
		if (!state) return NULL;
		state->malloced = 1;
	}
	state->sets = 0;
	state->open = 0;
	state->closed = 0;
	state->nodes = 0;
	state->allocated = 0;
	state->error = 0;
	state->known = 0;
	state->index = 0;
	state->pathlen = 0;

	state->metric = default_metric;
	state->heuristic = default_heuristic;
	state->neighbor = HL_adjacent_hex;

	return state;
}

/*
 * Reset a state to enpty.  Doesn't free memory, so it
 * can be reused.
 */
struct HL_astar *HL_astar_clear(struct HL_astar *s) {
	if (!s) return NULL;

	s->open = 0;
	s->closed = 0;
	s->nodes = 0;
	s->pathlen = 0;

	return s;
}

void HL_astar_free(struct HL_astar *s) {
	free(s->sets);
	if (s->malloced) {
		free(s);
	}
}

static struct HL_astar_hex *add_open(struct HL_astar *s, int hex) {
	struct HL_astar_hex *node;

	if (!s || s->error) return NULL;

	/* TODO check in open or closed */

	if (s->nodes == s->allocated) {
		int newalloc;
		struct HL_astar_hex *alloc;
		if (s->allocated) {
			newalloc = s->allocated * 2;
		} else {
			newalloc = 16;
		}
		alloc = realloc(s->sets, newalloc * sizeof *node);
		if (!alloc) {
			s->error = 1;
			return NULL;
		}
		s->sets = alloc;
		s->allocated = newalloc;
	}

	node = &s->sets[s->nodes++];

	node->hex = hex;
	node->g = 0.0;
	node->f = 0.0;
	node->h = 0.0;
	node->open = 1;
	node->parent = 0;
	if (s->open) s->open->prev = node;
	node->next = s->open;
	node->prev = NULL;
	s->open = node;

	return node;
}

static struct HL_astar_hex *closehex(struct HL_astar *s, struct HL_astar_hex *h) {
	if (!s || s->error) return NULL;
	if (!h) {
		s->error = 1;
		return NULL;
	}

#ifdef DEBUG_ASTAR
	fprintf(stderr, "Closing hex: (%d,%d)\n",  HL_cantor_x(h->hex), HL_cantor_y(h->hex));
#endif
	h->open = 0;
	if (h->prev) h->prev->next = h->next;
	if (h->next) h->next->prev = h->prev;
	if (s->closed) s->closed->prev = h;
	if (s->open == h) s->open = h->next;
	h->next = s->closed;
	s->closed = h;
	h->prev = NULL;

	return h;
}

struct HL_astar_hex *in_closed(struct HL_astar *s, int hex) {
	struct HL_astar_hex *h;
	if (!s) return 0;

	for(h = s->closed; h; h = h->next) {
		if (h->hex == hex) return h;
	}
	return NULL;
}

struct HL_astar_hex *in_open(struct HL_astar *s, int hex) {
	struct HL_astar_hex *h;
	if (!s) return 0;

	for(h = s->open; h; h = h->next) {
		if (h->hex == hex) return h;
	}
	return NULL;
}

static struct HL_astar_hex *lowrank(struct HL_astar *s) {
	struct HL_astar_hex *n, *r;

	if (!s || s->open == 0) return 0;

	r = s->open;
	for(n = r; n; n = n->next) {
		if (n->f < r->f) {
			r = n;
		}
	}

	return r;
}

/*
 * find a path from start to end.  we use A*
 */
int HL_findpath(struct HL_astar *s, int loops) {
	int dir, y;
	struct HL_astar_hex *node, *x, *yopen;
	int tent_better;
	double cost;

	s->error = 0;

	if (s->start == s->goal) return 0;

	node = add_open(s, s->start);
	node->g = 0.0;
	node->h = s->heuristic(s->start, s->goal);
	node->f = node->h;

#ifdef DEBUG_ASTAR
	fprintf(stderr, "A* findpath: ");
	fprintf(stderr, "(%d,%d)",  HL_cantor_x(s->start), HL_cantor_y(s->start));
	fprintf(stderr, " -> ");
	fprintf(stderr, "(%d,%d)",  HL_cantor_x(s->goal), HL_cantor_y(s->goal));
	fprintf(stderr, "\n");
#endif

	while (s->open) {

#ifdef DEBUG_ASTAR
	fprintf(stderr, "open list: ");
	for(x = s->open; x; x = x->next) {
		fprintf(stderr, "(%d,%d)",  HL_cantor_x(x->hex), HL_cantor_y(x->hex));
	}
	fprintf(stderr, "\n");
	fprintf(stderr, "closed list: ");
	for(x = s->closed; x; x = x->next) {
		fprintf(stderr, "(%d,%d)",  HL_cantor_x(x->hex), HL_cantor_y(x->hex));
	}
	fprintf(stderr, "\n");
#endif

		x = lowrank(s);
#ifdef DEBUG_ASTAR
		fprintf(stderr, "checking lowest f hex = (%d,%d) f = %f, g = %f, h = %f\n",
			HL_cantor_x(x->hex), HL_cantor_y(x->hex),
				x->f, x->g, x->h
				);
#endif
		if (x->hex == s->goal) {
			break;
		}
		closehex(s, x);
		for (dir = 0; y = s->neighbor(x->hex, dir); dir++) {
			if (in_closed(s, y)) {
				continue;
			}

			cost = x->g + s->metric(x->hex, y);

			yopen = in_open(s, y);
			if (! yopen) {
				yopen = add_open(s, y);
				tent_better = 1;
			} else if (cost < yopen->g) {
				tent_better = 1;
			} else {
				tent_better = 0;
			}

			if (tent_better) {
				if (yopen->parent) {
					if (x->f < yopen->parent->f) {
						yopen->parent = x;
					}
				} else {
					yopen->parent = x;
				}
				yopen->g = cost;
				yopen->h = s->heuristic(y, s->goal);
				yopen->f = yopen->g + yopen->h;
#ifdef DEBUG_ASTAR
		fprintf(stderr, "maybe better hex = (%d,%d) f = %f, g = %f, h = %f\n",
			HL_cantor_x(yopen->hex), HL_cantor_y(yopen->hex),
				yopen->f, yopen->g, yopen->h
				);
#endif
			}
		}
	}

	/* error no path ? */
	if (x->hex != s->goal) {
		s->error = 1;
		return 0;
	}

#ifdef DEBUG_ASTAR
	fprintf(stderr, "found path: ");
#endif

	/* reconstruct path */
	s->to = x;
	x->next = 0;
	for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
		if (yopen->parent) {
			yopen->parent->next = yopen;
		}
		if (yopen->hex == s->start) s->from = yopen;
#ifdef DEBUG_ASTAR
		fprintf(stderr, "(%p %d %d,%d)",
				yopen, yopen->hex,
				HL_cantor_x(yopen->hex),
				HL_cantor_y(yopen->hex)
				);
#endif
		s->pathlen++;
	};
	s->pathlen--;

#ifdef DEBUG_ASTAR
	fprintf(stderr, "\n");
#endif

	return s->pathlen;
}