#include "hexmap.h"

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

struct astar_hex {
	int hex;
	int f, g, h;
	int parent;
};

struct astar {
	int open, closed; /* how many in open or closed */
	int closedroom, openroom; /* how much is malloced */
	struct astar_hex *openlist;
	struct astar_hex *closedlist;
	int known, index;
	int error;
};

static void astar_init(struct astar *state) {
	state->open = 0;
	state->closed = 0;
	state->closedroom = 0;
	state->openroom = 0;
	state->openlist = 0;
	state->closedlist = 0;
	state->error = 0;
	state->known = 0;
	state->index = 0;

	return;
}

static void removeopen(struct astar *s, int hex) {
	int index;

	if (s->error) return;
	if (s->open == 0) return;

	if (s->known != hex) {
		for (index = 0; index < s->open; index++) {
			if (s->openlist[index].hex == hex) break;
		}
	} else {
		index = s->index;
		s->known = 0; s->index = 0;
	}
	s->openlist[index] = s->openlist[open];
	s->open--;
}

static void addopen(struct astar *s, int hex) {
	struct astar_hex *mem;
	if (s->error) return;

	if (s->open + 1 > s->openroom) {
		mem = realloc(s->openlist, s->openroom * 2 * sizeof *mem);
		if (mem == NULL) {
			s->error = errno;
			return;
		}
		state->openlist = mem;
		s->openroom = s->openroom * 2;
	}

	s->openlist[s->open].hex = hex;
	s->openlist[s->open].f = 0;
	s->openlist[s->open].g = 0;
	s->openlist[s->open].h = 0;
	s->known = hex;
	s->index = s->open;
	s->open = s->open + 1;
}

int lowrank(struct astar *s) {
	int f;
	int hex;
	int i;

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

	hex = s->openlist[0].hex;
	f = s->openlist[0].f;

	for(i=1; i < s->open; i++) {
		if (s->openlist[i].f < f) {
			hex = s->openlist[i].hex;
			f = s->openlist[i].f;
		}
		/* TODO check s->openlist[i].tiebreak */
	}

	return hex;
}

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

/*
 * find a path from start to end.  we use A*
 */
int HL_findpath(int start, int end, int psize, int *path,
		double (*metric)(int,int)) {
	int neighbors[6];
	int count;
	int cur = 0;
	struct astar state;
	int neighbor;

	if (start == end) return 0;
	if (!metric) metric = default_metric;

	astar_init(&state);
	addopen(&state, start);

	while (1) {
		cur = lowrank(&state);
		if (cur == end) {
			break;
		}
		addclosed(&state, cur);
		count = HL_hexes_at_range(cur->hex, 1, neighbors, 6);
		for (i=0;i<count;i++) {
			neighbor = neighbors[i];
			cost = g(&state, current) + metric(current, neighbor);
			if (inopen(&state, neighbor) && cost < g(&state, neighbor)) {
				removeopen(&state, neighbor);
			} else
			if (inclosed(&state, neighbor) && cost < g(&state, neighbor)) {
				/* should never happen with
				 * admissible heuristic
				 */
				removeclosed(&state, neighbor);
			} else
			if (!inopen(&state, neighbor) && !inclosed(&state, neighbor)) {
				addopen(&state, neighbor);
				setg(&state, neighbor, cost);
				setrank(&state, neighbor, cost + h(neighbor,end));
				setparent(&state, neighbor, current);
			} else {
				abort();
			}
		}
	}

	count = 0;
	while (parent(&state, cur) != start) {
		count++;
		cur = parent(&state, cur);
	}
	cur = end;
	while (parent(&state, cur) != start) {
		path[count--] = cur;
		cur = parent(&state, cur);
	}

}