#pragma once

#include <limits>
#include <cassert>
#include <stdexcept>

#define MORTON_CODE_MAX_LEVEL 32

#define UINT8                unsigned char
#define UINT32               unsigned int
#define UINT64               unsigned long long
#define UINT32_NBITS         (sizeof(UINT32) * 8)
#define MORTON_CODE_UINT32_N ((3 * MORTON_CODE_MAX_LEVEL + UINT32_NBITS - 1) / UINT32_NBITS)

#pragma pack(push, 1)

class MortonCode {
protected:

	static UINT32 expandBits(UINT32 v)
	{
		assert(v < ((UINT32) 1 << 11));
		// _______32 bits_______
		// |000.............vvv| - 11-v significant bits
		//
		// convert to:
		//
		// _______32 bits_______
		// |0v00v........00v00v|

		// See https://stackoverflow.com/a/1024889
		v = (v | (v << 16)) & 0x070000FF; // 0000 0111 0000 0000  0000 0000 1111 1111
		v = (v | (v <<  8)) & 0x0700F00F; // 0000 0111 0000 0000  1111 0000 0000 1111
		v = (v | (v <<  4)) & 0x430C30C3; // 0100 0011 0000 1100  0011 0000 1100 0011
		v = (v | (v <<  2)) & 0x49249249; // 0100 1001 0010 0100  1001 0010 0100 1001

		assert(v < ((UINT32) 1 << 31));
		return v;
	}

	static UINT64 expandBits(UINT64 v)
	{
		assert(v < ((UINT64) 1 << 22));
		// _______32 bits_____________32 bits_______
		// |000.............000|000.............vvv| - 22-v significant bits
		//
		// convert to:
		//
		// _______32 bits_____________32 bits_______
		// |v00............00v0|0v...........00v00v|

		v = (v | (v << 16)) & 0x0000003F0000FFFFull; // 0000 0000 0000 0000  0000 0000 0011 1111   0000 0000 0000 0000  1111 1111 1111 1111
		v = (v | (v << 16)) & 0x003F0000FF0000FFull; // 0000 0000 0011 1111  0000 0000 0000 0000   1111 1111 0000 0000  0000 0000 1111 1111
		v = (v | (v <<  8)) & 0x300F00F00F00F00Full; // 0011 0000 0000 1111  0000 0000 1111 0000   0000 1111 0000 0000  1111 0000 0000 1111
		v = (v | (v <<  4)) & 0x30C30C30C30C30C3ull; // 0011 0000 1100 0011  0000 1100 0011 0000   1100 0011 0000 1100  0011 0000 1100 0011
		v = (v | (v <<  2)) & 0x9249249249249249ull; // 1001 0010 0100 1001  0010 0100 1001 0010   0100 1001 0010 0100  1001 0010 0100 1001
		return v;
	}

	static UINT32 shrinkBits(UINT32 v)
	{
		assert((v & (~0x49249249)) == 0);
		// _______32 bits_______
		// |0v00v........00v00v|
		//
		// convert to:
		//
		// _______32 bits_______
		// |000.............vvv| - 11-v significant bits

		v =  v              & 0x49249249; // 0100 1001 0010 0100  1001 0010 0100 1001
		v = (v | (v >>  2)) & 0x430C30C3; // 0100 0011 0000 1100  0011 0000 1100 0011
		v = (v | (v >>  4)) & 0x0700F00F; // 0000 0111 0000 0000  1111 0000 0000 1111
		v = (v | (v >>  8)) & 0x070000FF; // 0000 0111 0000 0000  0000 0000 1111 1111
		v = (v | (v >> 16)) & 0x000007FF; // 0000 0000 0000 0000  0000 0111 1111 1111

		assert(v < ((UINT32) 1 << 11));
		return v;
	}

	static UINT32 shrinkBits(UINT64 v)
	{
		assert((v & (~0x9249249249249249ull)) == 0);
		// _______32 bits_____________32 bits_______
		// |v00............00v0|0v...........00v00v|
		//
		// convert to:
		//
		// _______32 bits_____________32 bits_______
		// |000.............000|000.............vvv| - 22-v significant bits

		v =  v              & 0x9249249249249249ull; // 1001 0010 0100 1001  0010 0100 1001 0010   0100 1001 0010 0100  1001 0010 0100 1001
		v = (v | (v >>  2)) & 0x30C30C30C30C30C3ull; // 0011 0000 1100 0011  0000 1100 0011 0000   1100 0011 0000 1100  0011 0000 1100 0011
		v = (v | (v >>  4)) & 0x300F00F00F00F00Full; // 0011 0000 0000 1111  0000 0000 1111 0000   0000 1111 0000 0000  1111 0000 0000 1111
		v = (v | (v >>  8)) & 0x003F0000FF0000FFull; // 0000 0000 0011 1111  0000 0000 0000 0000   1111 1111 0000 0000  0000 0000 1111 1111
		v = (v | (v >> 16)) & 0x0000003F0000FFFFull; // 0000 0000 0000 0000  0000 0000 0011 1111   0000 0000 0000 0000  1111 1111 1111 1111
		v = (v | (v >> 16)) & 0x00000000003FFFFFull; // 0000 0000 0000 0000  0000 0000 0000 0000   0000 0000 0011 1111  1111 1111 1111 1111

		assert(v < ((UINT64) 1 << 22));
		return (UINT32) v;
	}

	static void encodeKey(unsigned int x_index, unsigned int y_index, unsigned z_index, unsigned int level, UINT32 key[MORTON_CODE_UINT32_N])
	{
		if (level == 0) {
			assert(x_index == 0 && y_index == 0 && z_index == 0);
			key[0] = 0;
			key[1] = 0;
			key[2] = 0;
			return;
		}

		assert(x_index < ((UINT64) 1 << level));
		assert(y_index < ((UINT64) 1 << level));
		assert(z_index < ((UINT64) 1 << level));

		static_assert(MORTON_CODE_UINT32_N == 3, "Unexpected MortonCode key size!");

		// _______32 bits_____________32 bits_____________32 bits_______
		// |xyz..............xy|z.................x|yz..............xyz|
		// |  11-x  11-y  10-z |           21-x  21-y  22-z            |

		const UINT32 LAST_21_BITS = ((1 << 21) - 1);
		const UINT32 LAST_22_BITS = ((1 << 22) - 1);

		const UINT32 FULL_32_BITS = 0xFFFFFFFF;
		const UINT32 FRST_11_BITS = FULL_32_BITS - LAST_21_BITS;
		const UINT32 FRST_10_BITS = FULL_32_BITS - LAST_22_BITS;

		assert(MORTON_CODE_MAX_LEVEL - level < 32); // due to https://stackoverflow.com/a/9860578
		x_index = x_index << (MORTON_CODE_MAX_LEVEL - level);
		y_index = y_index << (MORTON_CODE_MAX_LEVEL - level);
		z_index = z_index << (MORTON_CODE_MAX_LEVEL - level);
		UINT64 x12 = expandBits((UINT64) (x_index & LAST_21_BITS));
		UINT64 y12 = expandBits((UINT64) (y_index & LAST_21_BITS));
		UINT64 z12 = expandBits((UINT64) (z_index & LAST_22_BITS));
		UINT32 x0  = expandBits((x_index & FRST_11_BITS) >> 21);
		UINT32 y0  = expandBits((y_index & FRST_11_BITS) >> 21);
		UINT32 z0  = expandBits((z_index & FRST_10_BITS) >> 22);
		UINT64 key12 = (x12 << 2) | (y12 << 1) | (z12 << 0);
		UINT32 key0  = (z0  << 2) | (x0  << 1) | (y0  << 0);
		key[2] = (UINT32)  (key12 & 0x00000000FFFFFFFFull);
		key[1] = (UINT32) ((key12 & 0xFFFFFFFF00000000ull) >> 32);
		key[0] = key0;
	}

	static void decodeKey(const UINT32 key[MORTON_CODE_UINT32_N], unsigned int level, unsigned int &x_index, unsigned int &y_index, unsigned int &z_index)
	{
		if (level == 0) {
			assert(key[0] == 0 && key[1] == 0 && key[2] == 0);
			x_index = 0;
			y_index = 0;
			z_index = 0;
			return;
		}

		static_assert(MORTON_CODE_UINT32_N == 3, "Unexpected MortonCode key size!");

		UINT64 key12 = ((UINT64) key[1] << 32) | key[2];
		UINT32 key0  = key[0];

		// _______32 bits_____________32 bits_____________32 bits_______
		// |xyz..............xy|z.................x|yz..............xyz|
		// |  11-x  11-y  10-z |           21-x  21-y  22-z            |

		const UINT32 LAST_21_BITS = ((1 << 21) - 1);
		const UINT32 LAST_22_BITS = ((1 << 22) - 1);

		const UINT32 FULL_32_BITS = 0xFFFFFFFF;
		const UINT32 FRST_11_BITS = FULL_32_BITS - LAST_21_BITS;
		const UINT32 FRST_10_BITS = FULL_32_BITS - LAST_22_BITS;

		UINT32 x12 = shrinkBits((key12 >> 2) & 0x9249249249249249ull); // 1001 0010 0100 1001  0010 0100 1001 0010   0100 1001 0010 0100  1001 0010 0100 1001
		UINT32 y12 = shrinkBits((key12 >> 1) & 0x9249249249249249ull); // 1001 0010 0100 1001  0010 0100 1001 0010   0100 1001 0010 0100  1001 0010 0100 1001
		UINT32 z12 = shrinkBits((key12 >> 0) & 0x9249249249249249ull); // 1001 0010 0100 1001  0010 0100 1001 0010   0100 1001 0010 0100  1001 0010 0100 1001
		UINT32 x0  = shrinkBits((key0  >> 1) & 0x49249249); // 0100 1001 0010 0100  1001 0010 0100 1001
		UINT32 y0  = shrinkBits((key0  >> 0) & 0x49249249); // 0100 1001 0010 0100  1001 0010 0100 1001
		UINT32 z0  = shrinkBits((key0  >> 2) & 0x49249249); // 0100 1001 0010 0100  1001 0010 0100 1001
		assert(MORTON_CODE_MAX_LEVEL - level < 32); // due to https://stackoverflow.com/a/9860578
		x_index = ((x0 << 21) | x12) >> (MORTON_CODE_MAX_LEVEL - level);
		y_index = ((y0 << 21) | y12) >> (MORTON_CODE_MAX_LEVEL - level);
		z_index = ((z0 << 22) | z12) >> (MORTON_CODE_MAX_LEVEL - level);

		assert(x_index < ((UINT64) 1 << level));
		assert(y_index < ((UINT64) 1 << level));
		assert(z_index < ((UINT64) 1 << level));
	}

	UINT32 key[MORTON_CODE_UINT32_N];
	UINT8  level;

public:

	MortonCode()
	{
		key[0] = std::numeric_limits<UINT32>::max();
		key[1] = std::numeric_limits<UINT32>::max();
		key[2] = std::numeric_limits<UINT32>::max();
		level = std::numeric_limits<UINT8>::max();
	}

	MortonCode(unsigned int x_index, unsigned int y_index, unsigned z_index, unsigned char lvl)
	{
		level = lvl;

		encodeKey(x_index, y_index, z_index, lvl, key);
	}

	/**
	 * x, y, z are in unit cube
	 */
	MortonCode(double x, double y, double z, unsigned char lvl)
	{
		level = lvl;

		if (level == 0) {
			key[0] = 0;
			key[1] = 0;
			key[2] = 0;
			return;
		}

		const double cube_width = ((UINT64) 1 << MORTON_CODE_MAX_LEVEL);
		x = std::min(std::max(x * cube_width, 0.0), cube_width - 1.0);
		y = std::min(std::max(y * cube_width, 0.0), cube_width - 1.0);
		z = std::min(std::max(z * cube_width, 0.0), cube_width - 1.0);

		assert((UINT64) x < ((UINT64) 1 << MORTON_CODE_MAX_LEVEL));
		assert((UINT64) y < ((UINT64) 1 << MORTON_CODE_MAX_LEVEL));
		assert((UINT64) z < ((UINT64) 1 << MORTON_CODE_MAX_LEVEL));

		UINT32 x_index = (UINT32) x;
		UINT32 y_index = (UINT32) y;
		UINT32 z_index = (UINT32) z;

		assert(level > 0);
		assert(level <= MORTON_CODE_MAX_LEVEL);
		assert(MORTON_CODE_MAX_LEVEL - level < 32);

		x_index /= 1 << (MORTON_CODE_MAX_LEVEL - level);
		y_index /= 1 << (MORTON_CODE_MAX_LEVEL - level);
		z_index /= 1 << (MORTON_CODE_MAX_LEVEL - level);

		encodeKey(x_index, y_index, z_index, level, key);
	}

	void decodeIndices(unsigned int &x_index, unsigned int &y_index, unsigned int &z_index) const
	{
		decodeKey(key, level, x_index, y_index, z_index);
	}

	void decodeIndices(unsigned long long &x_index, unsigned long long &y_index, unsigned long long &z_index) const
	{
		unsigned int x, y, z;
		decodeKey(key, level, x, y, z);
		x_index = x;
		y_index = y;
		z_index = z;
	}

	vector3d center() const
	{
		const double cube_width = ((UINT64) 1 << level);
		unsigned int x_index, y_index, z_index;

		decodeKey(key, level, x_index, y_index, z_index);

		return vector3d(x_index + 0.5, y_index + 0.5, z_index + 0.5) / cube_width;
	}

	unsigned char getLevel() const
	{
		return level;
	}

	MortonCode copy() const
	{
		MortonCode that;
		that.key[0] = key[0];
		that.key[1] = key[1];
		that.key[2] = key[2];
		that.level = level;
		return that;
	}

	MortonCode createParent() const
	{
		if (level == 0)
			throw std::runtime_error("Root node at zero level has no parent!");

		MortonCode parent = copy();
		parent.level--;

		// (1 lvl) key[0]   (11 lvl)   key[1]    (22 lvl)  key[2]
		// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
		// |xyz..............xy|z.................x|yz..............xyz|
		// |  11-x  11-y  10-z |  11-x  10-y  11-z |  10-x  11-y  11-z |
		if (level == 1) {
			parent.key[0] = 0;
		} else if (level <= 11) {
			const UINT32 FULL_32_BITS = 0xFFFFFFFF;
			const UINT32 LAST_BITS = (((UINT32) 1 << (2 + 3 * (11 - level))) - 1);
			parent.key[0] = parent.key[0] & (FULL_32_BITS - LAST_BITS);
			parent.key[1] = 0;
		} else if (level <= 22) {
			const UINT32 FULL_32_BITS = 0xFFFFFFFF;
			const UINT32 LAST_BITS = (((UINT32) 1 << (1 + 3 * (22 - level))) - 1);
			parent.key[1] = parent.key[1] & (FULL_32_BITS - LAST_BITS);
			parent.key[2] = 0;
		} else {
			const UINT32 FULL_32_BITS = 0xFFFFFFFF;
			const UINT32 LAST_BITS = (((UINT32) 1 << (0 + 3 * (33 - level))) - 1);
			parent.key[2] = parent.key[2] & (FULL_32_BITS - LAST_BITS);
		}

		return parent;
	}

	MortonCode createAncestor(unsigned char lvl) const
	{
		if (lvl >= level)
			throw std::runtime_error("Ancestor level should be smaller!");

		// Very similar to createParent()
		MortonCode ancestor = copy();
		ancestor.level = lvl;

		// (1 lvl) key[0]   (11 lvl)   key[1]    (22 lvl)  key[2]
		// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
		// |xyz..............xy|z.................x|yz..............xyz|
		// |  11-x  11-y  10-z |  11-x  10-y  11-z |  10-x  11-y  11-z |
		if (lvl == 0) {
			ancestor.key[0] = 0;
			ancestor.key[1] = 0;
			ancestor.key[2] = 0;
		} else if ((lvl + 1) <= 11) {
			const UINT32 FULL_32_BITS = 0xFFFFFFFF;
			const UINT32 LAST_BITS = (((UINT32) 1 << (2 + 3 * (11 - (lvl + 1)))) - 1);
			ancestor.key[0] = ancestor.key[0] & (FULL_32_BITS - LAST_BITS);
			ancestor.key[1] = 0;
			ancestor.key[2] = 0;
		} else if ((lvl + 1) <= 22) {
			const UINT32 FULL_32_BITS = 0xFFFFFFFF;
			const UINT32 LAST_BITS = (((UINT32) 1 << (1 + 3 * (22 - (lvl + 1)))) - 1);
			ancestor.key[1] = ancestor.key[1] & (FULL_32_BITS - LAST_BITS);
			ancestor.key[2] = 0;
		} else {
			const UINT32 FULL_32_BITS = 0xFFFFFFFF;
			const UINT32 LAST_BITS = (((UINT32) 1 << (0 + 3 * (33 - (lvl + 1)))) - 1);
			ancestor.key[2] = ancestor.key[2] & (FULL_32_BITS - LAST_BITS);
		}

		return ancestor;
	}

	void getSubindexFromParent(unsigned int &dx, unsigned int &dy, unsigned int &dz) const
	{
		if (level == 0)
			throw std::runtime_error("Root node at zero level has no parent!");

		// (1 lvl) key[0]   (11 lvl)   key[1]    (22 lvl)  key[2]
		// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
		// |xyz..............xy|z.................x|yz..............xyz|
		// |  11-x  11-y  10-z |  11-x  10-y  11-z |  10-x  11-y  11-z |
		if (level < 11) {
			unsigned int xyz = (key[0] >> (32 - 3 * level)) & 0x7;
			dx = (xyz & 0x4) > 0;
			dy = (xyz & 0x2) > 0;
			dz = (xyz & 0x1) > 0;
		} else if (level == 11) {
			const UINT32 FIRST_BIT = 0x80000000;
			dx = (key[0] & 0x2) > 0;
			dy = (key[0] & 0x1) > 0;
			dz = (key[1] & FIRST_BIT) > 0;
		} else if (level < 22) {
			unsigned int xyz = (key[1] >> (64 - 3 * level)) & 0x7;
			dx = (xyz & 0x4) > 0;
			dy = (xyz & 0x2) > 0;
			dz = (xyz & 0x1) > 0;
		} else if (level == 22) {
			const UINT32 FIRST_BIT = 0x80000000;
			const UINT32 SECND_BIT = 0x40000000;
			dx = (key[1] & 0x1) > 0;
			dy = (key[2] & FIRST_BIT) > 0;
			dz = (key[2] & SECND_BIT) > 0;
		} else {
			unsigned int xyz = (key[2] >> (96 - 3 * level)) & 0x7;
			dx = (xyz & 0x4) > 0;
			dy = (xyz & 0x2) > 0;
			dz = (xyz & 0x1) > 0;
		}
		assert(dx < 2 && dy < 2 && dz < 2);
	}

	MortonCode createChild(unsigned int dx, unsigned int dy, unsigned int dz) const
	{
		assert(dx < 2 && dy < 2 && dz < 2);

		MortonCode child = copy();
		child.level += 1;

		// (1 lvl) key[0]   (11 lvl)   key[1]    (22 lvl)  key[2]
		// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
		// |xyz..............xy|z.................x|yz..............xyz|
		// |  11-x  11-y  10-z |  11-x  10-y  11-z |  10-x  11-y  11-z |
		unsigned int xyz = dx * 4 + dy * 2 + dz;
		if (child.level < 11) {
			child.key[0] |= (xyz << (3 * (11 - child.level) - 1));
		} else if (child.level == 11) {
			child.key[0] |= (xyz >> 1);
			child.key[1] = (dz << 31);
		} else if (child.level < 22) {
			child.key[1] |= (xyz << (3 * (22 - child.level) - 2));
		} else if (child.level == 22) {
			child.key[1] |= (xyz >> 2);
			child.key[2] = (xyz << 30);
		} else {
			child.key[2] |= (xyz << (3 * (33 - child.level) - 3));
		}
		return child;
	}

	MortonCode createSibling(unsigned int dx, unsigned int dy, unsigned int dz) const
	{
		assert(dx < 2 && dy < 2 && dz < 2);
		assert(level != 0);

		MortonCode parent = createParent();
		return parent.createChild(dx, dy, dz);
	}

	MortonCode createNeighbour(unsigned int dx, unsigned int dy, unsigned int dz, bool &neighbour_out_of_cube) const
	{
		assert(dx < 3 && dy < 3 && dz < 3);

		if (dx == 1 && dy == 1 && dz == 1) {
			neighbour_out_of_cube = false;
			return copy();
		}

		if (level == 0) {
			neighbour_out_of_cube = true;
			return copy();
		}

		// TODO: speedup (re-implement without decodeKey+encodeKey)
		unsigned int x_index, y_index, z_index;
		decodeKey(key, level, x_index, y_index, z_index);

		neighbour_out_of_cube = false;

		UINT64 MAX_VALUE = ((UINT64) 1 << level) - 1;
		if (dx == 0 && x_index == 0) {
			neighbour_out_of_cube = true;
		}
		if (dx == 2 && x_index == MAX_VALUE) {
			neighbour_out_of_cube = true;
		}
		if (dy == 0 && y_index == 0) {
			neighbour_out_of_cube = true;
		}
		if (dy == 2 && y_index == MAX_VALUE) {
			neighbour_out_of_cube = true;
		}
		if (dz == 0 && z_index == 0) {
			neighbour_out_of_cube = true;
		}
		if (dz == 2 && z_index == MAX_VALUE) {
			neighbour_out_of_cube = true;
		}

		if (neighbour_out_of_cube) {
			return copy();
		} else {
			MortonCode neighbour(x_index + dx - 1, y_index + dy - 1, z_index + dz - 1, level);
			return neighbour;
		}
	}

	bool isAncestorFor(const MortonCode &child) const
	{
		if (level >= child.level)
			return false;

		MortonCode real_ancestor = child.createAncestor(level);
		return (*this) == real_ancestor;
	}

	class Comparator {
	public:
		bool operator()(const MortonCode &a, const MortonCode &b) const
		{
			assert(a.level <= MORTON_CODE_MAX_LEVEL);
			assert(b.level <= MORTON_CODE_MAX_LEVEL);
			for (int i = 0; i < MORTON_CODE_UINT32_N; ++i) {
				if (a.key[i] < b.key[i]) {
					return true;
				} else if (a.key[i] > b.key[i]) {
					return false;
				}
			}
			return a.level < b.level;
		}
	};

	class ComparatorCoarseToFine {
	public:
		bool operator()(const MortonCode &a, const MortonCode &b) const
		{
			assert(a.level <= MORTON_CODE_MAX_LEVEL);
			assert(b.level <= MORTON_CODE_MAX_LEVEL);
			if (a.level < b.level) {
				return true;
			} else if (b.level < a.level) {
				return false;
			} else {
				assert(a.level == b.level);
				for (int i = 0; i < MORTON_CODE_UINT32_N; ++i) {
					if (a.key[i] < b.key[i]) {
						return true;
					} else if (a.key[i] > b.key[i]) {
						return false;
					}
				}
				return false;
			}
		}
	};

	static unsigned char levelByRadius(float root_width, float radius)
	{
		unsigned char lvl = 0;
		float lvl_radius = root_width / 2;
		while (lvl + 1 <= MORTON_CODE_MAX_LEVEL && radius < lvl_radius) {
			lvl_radius /= 2.0f;
			++lvl;
		}
		assert(radius >= lvl_radius);
		assert(2.0f * lvl_radius > radius);
		return lvl;
	}

	static float radiusByLevel(float root_width, unsigned char lvl)
	{
		assert(lvl <= MORTON_CODE_MAX_LEVEL);
		float radius = root_width / 2.0f;
		float lvl_radius = radius / (1 << lvl);
		assert(levelByRadius(root_width, lvl_radius) == lvl);
		return lvl_radius;
	}

	static unsigned int maxIndex(unsigned char level)
	{
		assert(level <= MORTON_CODE_MAX_LEVEL);
		return (unsigned int) (((unsigned long long) 1 << level) - 1);
	}

	bool operator==(const MortonCode &that) const
	{
		return level == that.level
		&& key[2] == that.key[2]
		&& key[1] == that.key[1]
		&& key[0] == that.key[0];
	}
};

std::ostream &operator<<(std::ostream &os, MortonCode const &key) {
	os << "MortonCode {lvl=" << (int) key.getLevel() << " ";
	if (key.getLevel() == 0) {
		os << "root x=0 y=0 z=0 from [0; 1)}";
		return os;
	}
	MortonCode cur = key;
	std::vector<unsigned int> xyzs;
	while (true) {
		unsigned int dx, dy, dz;
		cur.getSubindexFromParent(dx, dy, dz);
		xyzs.push_back(dx);
		xyzs.push_back(dy);
		xyzs.push_back(dz);
		MortonCode par = cur.createParent();
		if (par.getLevel() == 0) {
			break;
		}
		cur = par;
	}
	assert(xyzs.size() == 3 * key.getLevel());
	unsigned long x = 0;
	unsigned long y = 0;
	unsigned long z = 0;
	for (int lvl = key.getLevel() - 1; lvl >= 0; --lvl) {
		x = 2 * x + xyzs[3*lvl+0];
		y = 2 * y + xyzs[3*lvl+1];
		z = 2 * z + xyzs[3*lvl+2];
	}
	unsigned long long index_limit = ((unsigned long long) 1 << key.getLevel());
	os << "x=" << x << " y=" << y << " z=" << z << " from [0; " << index_limit << ")";
	os << "\txyz=";
	for (int lvl = key.getLevel() - 1; lvl >= 0; --lvl) {
		x = 2 * x + xyzs[3*lvl+0];
		y = 2 * y + xyzs[3*lvl+1];
		z = 2 * z + xyzs[3*lvl+2];
		os << xyzs[3*lvl+0] << xyzs[3*lvl+1] << xyzs[3*lvl+2];
		if (lvl > 0)
			os << ".";
	}
	os << "}";
	return os;
}

#pragma pack(pop)

static_assert(sizeof(UINT8)  == 1,        "Unexpected uint8 size!");
static_assert(sizeof(UINT32) == 4,        "Unexpected uint32 size!");
static_assert(sizeof(UINT64) == 8,        "Unexpected uint64 size!");
static_assert(MORTON_CODE_UINT32_N == 3,  "Unexpected MortonCode key size!");
static_assert(sizeof(MortonCode)   == 13, "Unexpected MortonCode size!");

#undef UINT8
#undef UINT32
#undef UINT64
#undef UINT32_NBITS
#undef MORTON_CODE_UINT32_N