#ifndef HILBERT_H
#define HILBERT_H

#include <stdint.h>

#include "util.h"

/**
	\brief Calculates the (x,y) coordinates from the distance d
	\param d The distance from the starting point of the curve.
*/
static inline void
hil_xy_from_d(uint64_t d, uint32_t * x, uint32_t * y)
{
	int64_t dh, dl;
	uint64_t i, tx = 0, ty = 0, temp;
	for (i = 0; i < 64; i += 2) {
		dh = (d >> (i+1)) & 1;
		dl = (d >> i) & 1;

		if ((dh ^ dl) == 0) {
			temp = tx;
			tx = ty^(-dh);
			ty = temp^(-dh);
		}

		tx = (tx >> 1) | (dh << 63);
		ty = (ty >> 1) | ((dh ^ dl) << 63);
	}
	*x = tx >> 32;
	*y = ty >> 32;
}

/**
	\brief Calculates the distance from the (x,y) coordinates
	\param x x coordinate
	\param y y coordinate
*/
static inline uint64_t
hil_d_from_xy(uint32_t x, uint32_t y)
{
	int32_t i, xi, yi;
	uint32_t d, temp;

	d = 0;
	for (i = 31; i >= 0; i--) {
		xi = (x >> i) & 1;
		yi = (y >> i) & 1;

		if (yi == 0) {
			temp = x;
			x = y ^ (-xi);
			y = temp ^ (-xi);
		}

		d = 4*d + 2*xi + (xi^yi);
	}
	return d;
}

static inline uint64_t
hil_d_from_xy2(uint32_t x, uint32_t y)
{
	int32_t i;
	uint32_t state = 0, d = 0, row;
	for (i = 31; i >= 0; i--) {
		row = (4*state) | (2*((x >> i) & 1)) | ((y >> i) & 1);
		d = (d << 2) | ((0x361E9CB4 >> 2*row) & 3);
		state = (0x8FE65831 >> 2*row) & 3;
	}
	return d;
}

static inline uint64_t
hil_d_from_xy3(uint32_t x, uint32_t y)
{
	int32_t l;
	uint64_t s = 0, d = 0;
	for (l = 31; l >= 0; l--) {
		uint32_t xl = (x >> l) & 1;
		uint32_t yl = (y >> l) & 1;

		uint64_t shift = 4*((s & 0xC) | (xl << 1) | yl);
		s = (0x83DEC97A65B02F14ul & (0xFul << shift)) >> shift;
		d = (d << 2) | (s & 3);
	}
	return d;
}

static inline uint64_t
mor_z_from_xy(uint32_t x, uint32_t y)
{
	return (dilate(y) << 1) | dilate(x);
}

static inline void
mor_xy_from_z(uint64_t z, uint32_t * x, uint32_t *y)
{
	*y = undilate((z >> 1) & 0x5555555555555555ul);
	*x = undilate(z & 0x5555555555555555ul);
}

#endif