#include "hexmap.h"

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

static int inversecantor(int cantor, int *x, int *y);

/*
 * This file is written by Nathan Wagner and dedicated to the public
 * domain
 */

double HL_vertexv[] = {
	.577350269189625764509148780502, 0.0,
	.288675134594812882254574390251, 0.5,
	-.288675134594812882254574390251, 0.5,
	-.577350269189625764509148780502, 0.0,
	-.288675134594812882254574390251, -0.5,
	.288675134594812882254574390251, -0.5};

/* these all are for a hex one unit across */
double          hexptvd[6][2] = {
	{.577350269189625764509148780502, 0.0},	/* 1.0/sqrt3 */
	{.288675134594812882254574390251, 0.5},	/* 0.5/sqrt3 */
	{-.288675134594812882254574390251, 0.5},
	{-.577350269189625764509148780502, 0.0},
	{-.288675134594812882254574390251, -0.5},
	{.288675134594812882254574390251, -0.5}
};

/* TODO how is this related? to the above? */
double          texptvd[6][2] = {
	{1.154700538379251529018297561004, 0.5},	/* 2.0/sqrt3 */
	{.866025403784438646763723170753, 1.0},	/* 1.5/sqrt3 */
	{.288675134594812882254574390251, 1.0},
	{0.0, 0.5},
	{.288675134594812882254574390251, 0.0},
	{.866025403784438646763723170753, 0.0}
};

double          hexpthd[6][2] = {
	{0.0, .577350269189625764509148780502},
	{0.5, .288675134594812882254574390251},
	{0.5, -.288675134594812882254574390251},
	{0.0, -.577350269189625764509148780502},
	{-0.5, -.288675134594812882254574390251},
	{-0.5, .288675134594812882254574390251}
};

void HL_vertices(int cantor, double *vc) {
	int i;
	double xc, yc;

	HL_hexcenter(cantor, &xc, &yc);

	for (i=0; i<6; i++) {
		*vc++ = hexptvd[i][0] + xc;
		*vc++ = hexptvd[i][1] + yc;
	}
	*vc++ = hexptvd[0][0] + xc;
	*vc++ = hexptvd[0][1] + yc;
}

void HL_trianglefan(int cantor, double *vc) {
	HL_hexcenter(cantor, vc, vc+1);
	HL_vertices(cantor, vc+2);
}

double HL_center_x(int cantor) {
	double x;

	HL_hexcenter(cantor, &x, 0);
	return x;
}

double HL_center_y(int cantor) {
	double y;

	HL_hexcenter(cantor, 0, &y);
	return y;
}

int HL_hexcenter(int cantor, double *xc, double *yc) {
	int x, y;
	double stride = 1.5/sqrt(3.0);

	inversecantor(cantor, &x, &y);

	if (xc) *xc = x * stride;
	if (yc && x >= 0) *yc = y + ((x + 1) % 2) / 2.0 - 0.5;
	if (yc && x < 0) *yc = y + ((-x + 1) % 2) / 2.0 - 0.5;

	return cantor;
}

/*
 * This function assumes that the hexes are one unit across, and vertically
 * oriented.  If that is not the case, you will need to transform
 * your input coordinates first.
 */
int HL_cantor_bin(double x, double y) {
	int i, j;

	HL_hexbin(1.0, x, y, &i, &j);
	return HL_cantor_xy(i, j);
}

static int xy2ijk(int x, int y, int *i, int *j, int *k) {
	int pi, pj, pk;

	pi = x;
	pj = -y;
	if (x < 0) {
		pj = pj + (-x + 1) / 2;
	} else {
		pj = pj - x/2;
	}
	pk = -pi - pj;

	if (i) *i = pi;
	if (j) *j = pj;
	if (k) *k = pk;

	return HL_cantor_xy(x,y);
}

static int ijk2xy(int i, int j, int k, int *x, int *y) {
	int px, py;

	px = i;

	/* py = -j - i/2; */
	py = -j;

	if (i < 0) {
		py += (-i + 1)/2;
	} else {
		py -= i/2;
	}

	if (x) *x = px;
	if (y) *y = py;

	return HL_cantor_xy(px,py);
}

int HL_cantor_ijk(int i, int j, int k) {
	return ijk2xy(i,j,k,0,0);
}

int HL_distance(int from, int to) {
	int dist = 0, i;;
	int fc[3], tc[3];

	HL_cantor_arrays(from, 0, fc);
	HL_cantor_arrays(to, 0, tc);

	for (i=0;i<=2;i++) {
		dist += abs(fc[i] - tc[i]);
	}

	return dist / 2;
}

int HL_hexes_within_range(int hex, int range, int *list, int size) {
	int count = 0;
	int i;

	if (range == 0) {
		return HL_hexes_at_range(hex, 0, list, size);
	}

	for (i=1;i<=range;i++) {
		count += HL_hexes_at_range(hex, i, count > size ? 0 : list+count, size-count);
	}
	return count;
}

int HL_hexes_at_range(int hex, int range, int *list, int size) {
	int q; /* p and q are count/loop vars */
	int c[3]; /* ijk coord array */

	if (range == 0) {
		if (list) {
			list[0] = hex;
		}
		return 1;
	} else if (range < 0) {
		return 0;
	}

	/* TODO don't bother to collect if the list isn't big enough? */
	/* i.e. if (!list || size < range * 6) */
	if (!list || size < 1) return range * 6;

	HL_cantor_arrays(hex, 0, c);
	c[0] += range;
	c[2] = -c[0] - c[1];
	hex = HL_cantor_ijkp(c);

	for(q=0; q<size && q < range * 6; q++) { 
		list[q] = hex;
		hex = HL_adjhex(hex, q/range+2);
	}

	return range * 6;
}

/* direction 0 is positive X , counter clockwise from there */
int HL_adjhex(int start, int dir) {
	int c[3];

	HL_cantor_arrays(start, 0, c);

	switch (dir%6) {
		case 2:
			c[0]--; c[1]++; break;
		case 1:
			        c[1]++; c[2]--; break;
		case 0:
			c[0]++;         c[2]--; break;
		case 5:
			c[0]++; c[1]--; break;
		case 4:
			        c[1]--; c[2]++; break;
		case 3:
			c[0]--;       ; c[2]++; break;
	}

	return HL_cantor_ijkp(c);
}

/* returns last hex in found path */
int HL_findpath(int start, int end, int *path, int size) {
	return 0;
}

int HL_cantor_xyp(int *xy) {
	return HL_cantor_xy(xy[0], xy[1]);
}

int HL_cantor_ijkp(int *ijk) {
	return HL_cantor_ijk(ijk[0], ijk[1], ijk[2]);
}

int HL_cantor_arrays(int can, int *xy, int *ijk) {
	return HL_cantor_decode(can, xy, xy ? xy+1 : 0,
			ijk, ijk ? ijk+1 : 0, ijk ? ijk+2 : 0);
}

int HL_cantor_decode(int can, int *x, int *y, int *i, int *j, int *k) {
	int px, py;

	inversecantor(can, &px, &py);
	if (x) *x = px;
	if (y) *y = py;

	xy2ijk(px, py, i, j, k);

	return can;
}

int HL_cantor_i(int cantor) {
	int i;

	HL_cantor_decode(cantor, 0,0, &i,0,0);
	return i;
}

int HL_cantor_j(int cantor) {
	int j;

	HL_cantor_decode(cantor, 0,0, 0,&j,0);
	return j;
}

int HL_cantor_k(int cantor) {
	int k;

	HL_cantor_decode(cantor, 0,0, 0,0,&k);
	return k;
}

int HL_cantor_x(int cantor) {
	int x;
	inversecantor(cantor, &x, 0);
	return x;
}

int HL_cantor_y(int cantor) {
	int y;
	inversecantor(cantor, 0, &y);
	return y;
}

/* Determine if a map with these dimensions will overflow */
int HL_map_bounds_ok(int xdim, int ydim) {
	
	/* return (x+y) * (x + y + 1) / 2 + y+1; */

	if (INT_MAX - xdim - 1 < ydim) return 0;
	if (INT_MAX / (xdim+ydim) < (xdim+ydim+1)) return 0;
	if ( (xdim+ydim) * (xdim+ydim+1) / 2 > INT_MAX - ydim - 1)
		return 0;

	return 1;
}

int HL_map_max_dimension(void) {
	int low, high, try;

	low = 1; high = INT_MAX/2;

        while (low != high - 1) {
                try = (low + high) / 2;
                if (HL_map_bounds_ok(try,try)) {
                        low = try;
                } else {
                        high = try;
                }
        }

        return low;
}

#ifdef ASTAR

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);
	}

}

#endif

static int inversenatcantor(int cantor, int *x, int *y) {
	int w, t, py;

	cantor -= 1;

	w = (int)floor((sqrt(8.0 * cantor + 1.0) - 1.0)/2.0);
	t = (w * w + w)/2;

	py = cantor - t;

	if (y) *y = py;
	if (x) *x = w - py;

	return cantor;
}

/*
 * map non negative integer pairs to their cantor pairing function
 * number, plus one.  We add one so that the result is never zero,
 * leaving zero to be "invalid" or "none" or what have you.
 */

static int natcantor(int x, int y) {
	return (x+y) * (x + y + 1) / 2 + y+1;
}

/* See http://en.wikipedia.org/wiki/Cantor_pairing_function */
/* see also http://szudzik.com/ElegantPairing.pdf */
/*
 * if either coordinate is negative, map the integers onto the
 * whole numbers, and then return the negative of the adjusted
 * cantor number.  As for most grids negative coordinates will
 * be invalid, this will allow for a <= 0 test for invalid
 * or out of bounds (on the negative side anyway, you'll
 * still have to test for out of range on the positive side).
 *
 * TODO figure out what the maximum supported coordinates are
 * for given integer sizes.
 */
int HL_cantor_xy(int x, int y) {
	if (x < 0 || y < 0) {
		x = abs(2 * x) - (x < 0);
		y = abs(2 * y) - (y < 0);
		return -natcantor(x, y);
	}
	return natcantor(x,y);
}

static int inversecantor(int cantor, int *x, int *y) {
	if (cantor < 0) {
		inversenatcantor(-cantor, x, y);
		if (x) {
			if (*x % 2) {
				*x = -(*x+1)/2;
			} else {
				*x = *x / 2;
			}
		}
		if (y) {
			if (*y % 2) {
				*y =  -(*y+1)/2;
			} else {
				*y = *y/2;
			}
		}
	} else {
		inversenatcantor(cantor, x, y);
	}

	return cantor;
}

struct hex {
        int iso;
        int x, y, z;
};

/* y *must* be positive down as the xy /iso conversion assumes this */
static int hex_xy(struct hex *h) {
	if (!h->iso) return 1;
	if (h->x >= 0) {
		h->y = -h->y - (h->x+1)/2;
	} else {
		/* need to round toward -inf, not toward zero, so x-1 */
		h->y = -h->y - h->x/2;
	}
	h->iso = 0;

	return 1;
}

static int hex_iso(struct hex *h) {
	if (h->iso) return 1;

	if (h->x >= 0) {
		h->y = (-h->y - (h->x+1)/2);
	} else {
		/* need to round toward -inf, not toward zero, so x-1 */
		h->y = (-h->y - (h->x)/2);
	}

	h->z = -h->x - h->y;
	h->iso = 1;
	return 1;
}

int HL_hexbin(double width, double x, double y, int *i, int *j) {
	double z, rx, ry, rz;
	double abs_dx, abs_dy, abs_dz;
	int ix, iy, iz, s;
	struct hex h;

	/* TODO just hard-code this cosine */
	x = x / cos(30 * M_PI / 180.0); /* rotated X coord */
	y = y - x / 2.0; /* adjustment for rotated X */

	/* adjust for actual hexwidth */
	x /= width;
	y /= width;

	z = -x - y;

	ix = rx = floor(x + 0.5);
	iy = ry = floor(y + 0.5);
	iz = rz = floor(z + 0.5);

	s = ix + iy + iz;

	if (s) {
		abs_dx = fabs(rx - x);
		abs_dy = fabs(ry - y);
		abs_dz = fabs(rz - z);

		if (abs_dx >= abs_dy && abs_dx >= abs_dz) {
			ix -= s;
		} else if (abs_dy >= abs_dx && abs_dy >= abs_dz) {
			iy -= s;
		} else {
			iz -= s;
		}
	}
	h.x = ix;
	h.y = iy;
	h.z = iz;
	h.iso = 1;

	hex_xy(&h);
	if (i) *i = h.x;
	if (j) *j = h.y;
	return HL_cantor_xy(h.x, h.y);
}