#ifndef __MARSLOOKUP_H__
#define __MARSLOOKUP_H__

#include <list>
#include <iterator>
#include <stack>
#include <set>

#include "coroutines.h"
#include "mars_linalg.h"
#include "mars_calc.h"
#include "mars_ptr.h"

/**
 * Purpose for this library:
 * This library is used (instead of for example Boost uBLAS) because it is optimized for 3D and 2D operations (since operations in any 
 * other Euclidian dimensions are unnecessary for this video game application).  3D and 2D classes and operations are static and computed
 * at compile-time rather than runtime as in the case of every other linear algebra library (i.e. this library doesn't use for-loops EVER).
 * An obvious disadvantage of this is that there is a lot of redundant code that would normally lead to a much larger code maintenance
 * burden, but basic linear algebra does not evolve, so this disadvantage is permissible.
 * 
 * TODO: Implement expression classes to optimize machine-code generation for vector expressions and reduce/eliminate temporaries
 */

namespace mars
{
	template <typename Tag> class RectRegion;
	template <typename Tag> class RadialRegion;
	template <typename Tag> class PointRegion;
	template <typename E, typename R, typename Tag> class TreeDegree;
	template <typename E, typename R> class Entity;
	template <typename Tag> class DegreeBounds;
	template< typename E, typename R, typename Tag > struct TreeIteratorTraits;
	template <typename E, typename R, typename QR, typename Tag, typename TIT = TreeIteratorTraits< E, R, Tag > > class DegreeNicheIterator;

	template <typename E, typename R>
	class Entity
	{
	public:
		ptr< E > pData;
		const R region;

		inline Entity (const ptr< E > & pData, const R & r)
			: pData(pData), region(r) {}
	};

	template <typename T>
	inline T lkmax (T a, T b, T c, T d)
	{
		if (a < b)
			if (b < c)
				if (c < d)
					return d;
				else
					return c;
			else 
				if (b < d)
					return d;
				else
					return b;
		else
			if (a < c)
				if (c < d)
					return d;
				else
					return c;
			else
				if (a < d)
					return d;
				else
					return a;
	}

	template <typename T>
	inline T lkmin (T a, T b, T c, T d)
	{
		if (a > b)
			if (b > c)
				if (c > d)
					return d;
				else
					return c;
			else 
				if (b > d)
					return d;
				else
					return b;
		else
			if (a > c)
				if (c > d)
					return d;
				else
					return c;
			else
				if (a > d)
					return d;
				else
					return a;
	}

	template <typename T, typename V>
	class DegreeRect
	{
	public:
		V p;
		T m;
	};

	template <typename T>
	struct TreeTag3D
	{
		static const unsigned int DEGREES = 8;
		typedef vector3D <T> VECTOR;
		typedef T SCALAR;
	};

	template <typename T>
	struct TreeTag2D
	{
		static const unsigned int DEGREES = 4;
		typedef vector2D <T> VECTOR;
		typedef T SCALAR;
	};

	/* General contract of regions:
	 * - x & y: Tests that y is contained completely within x, that is the intersection of x with y is equivalent to y
	 * - x ^ y: Tests that x and y at least overlap, that is the intersection of x with y is non-empty
	 */

	class AmbientRegion {};

	template <typename Tag>
	class RadialRegion
	{
	public:
		typedef typename Tag::VECTOR VECTOR;
		typedef typename Tag::SCALAR SCALAR;

		VECTOR p;
		SCALAR r;

		inline RadialRegion () {}

		inline RadialRegion (const VECTOR & p, const SCALAR & r)
			: p(p), r(r) {}

		inline bool operator ^ (const VECTOR & v) const
		{
			return MAG(v - p) <= r;
		}

		inline bool operator ^ (const RadialRegion< Tag > & rval) const
		{
			return MAG(p - rval.p) + rval.r <= r;
		}

		inline bool operator ^ (const PointRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return MAGSQ(p - rval.v) < SQ(r);
		}

		inline bool operator ^ (const PointRegion< TreeTag2D< SCALAR > > & rval) const
		{
			return MAGSQ(static_cast< typename TreeTag2D< SCALAR >::VECTOR > (p) - rval.v) < SQ(r);
		}

		inline bool operator ^ (const AmbientRegion &) const 
		{ 
			return true; 
		}

#define CODE_RadialRegion_Amp_RectRegion \
		{ \
			const VECTOR d2 = (rval.v2 - rval.v1) / 2; \
			const VECTOR dist = (p - (rval.v1 + d2)); \
\
			if (radialContainment(d2, dist)) \
				return true; \
\
			return MAGSQ(dist - d2) <= SQ(r); \
		}

		bool operator & (const RectRegion< TreeTag2D< SCALAR > > & rval) const CODE_RadialRegion_Amp_RectRegion	;
		bool operator & (const RectRegion< TreeTag3D< SCALAR > > & rval) const CODE_RadialRegion_Amp_RectRegion	;

		inline bool operator & (const RadialRegion< Tag > & rval) const
		{
			return rval.r < r && MAGSQ(p - rval.p) < SQ(r - rval.r);	// TODO: Verify, I think I fixed a bug, used to be "MAGSQ(p - rval.p) < SQ(r + rval.r)"
		}
		inline bool operator & (const PointRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return r <= static_cast< SCALAR > (1) && 
				p.x == rval.p.x && 
				p.y == rval.p.y && 
				p.z == rval.p.z;
		}
		inline bool operator & (const PointRegion< TreeTag2D< SCALAR > > & rval) const
		{
			return r <= static_cast< SCALAR > (1) && 
				p.x == rval.p.x && 
				p.y == rval.p.y;
		}
		inline bool operator & (const AmbientRegion &) const
		{
			return true;
		}

		inline bool operator == (const RadialRegion< Tag > & b) const
		{
			return p == b.p && r == b.r;
		}

		inline bool isNiche (const DegreeBounds <Tag> & b) const
		{
			// Comparing with diameter, not radius
			return r * 2 > static_cast <SCALAR> (b.m2) && r * 2 < static_cast <SCALAR> (b.m2) * 2;
		}

	protected:
		inline bool radialContainment (const vector3D< SCALAR > & d2, const vector3D< SCALAR > & dist) const
		{
			return (dist.x <= (d2.x + r) && dist.y <= (d2.y + r) && dist.z <= (d2.z + r) && 
				(dist.x <= d2.x || dist.y <= d2.y || dist.z <= d2.z));
		}

		inline bool radialContainment (const vector2D< SCALAR > & d2, const vector2D< SCALAR > & dist) const
		{
			return dist.x <= (d2.x + r) && dist.y <= (d2.y + r) && 
				(dist.x <= d2.x || dist.y <= d2.y);
		}
	};

	template <typename T>
	class CylindricalRegion : public RadialRegion< TreeTag2D< T > >
	{
	private:
		typedef RadialRegion< TreeTag2D< T > > Base;
		
	public:
		using typename Base::VECTOR;
		using typename Base::SCALAR;
        using Vec3D = typename TreeTag3D< T >::VECTOR;
		
		SCALAR z;
		
		inline CylindricalRegion () {}

		inline CylindricalRegion (const RadialRegion< TreeTag3D< T > > & rr)
			: Base(static_cast < typename TreeTag2D< T >::VECTOR > (rr.p), rr.r), z(rr.p.z) {}
		inline CylindricalRegion (const Vec3D & p, const T & r)
			: Base(static_cast < typename TreeTag2D< T >::VECTOR > (p), r), z(p.z) {}

		inline bool operator ^ (const VECTOR & v) const
		{
			return MAG(v - p3()) <= this->r;
		}

		inline bool operator ^ (const RadialRegion< TreeTag3D< T > > & rval) const
		{
			return MAG(p3() - rval.p) + rval.r <= this->r;
		}

		inline bool operator ^ (const PointRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return MAGSQ(p3() - rval.v) < SQ(this->r);
		}

		inline bool operator ^ (const AmbientRegion &) const
		{
			return true;
		}

		inline bool operator & (const PointRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return this->r <= static_cast< SCALAR > (1) && 
				this->p.x == rval.p.x && 
				this->p.y == rval.p.y && 
				z == rval.p.z;
		}

		bool operator & (const RectRegion< TreeTag3D< SCALAR > > & rval) const
		{
			const VECTOR d2 = (rval.v2 - rval.v1) / 2;
			const VECTOR dist = (p3() - (rval.v1 + d2));

			if (radialContainment(d2, dist))
				return true;

			return MAGSQ(dist - d2) <= SQ(this->r);
		}
		inline bool operator & (const RectRegion< TreeTag2D< SCALAR > > & rval) const
			{ return RadialRegion< TreeTag2D< T > > ::operator & (rval); }

		inline bool operator & (const RadialRegion< TreeTag3D< T > > & rval) const
		{
			return MAGSQ(p3() - rval.p) < SQ(this->r + rval.r);
		}

		inline bool operator & (const AmbientRegion &) const
		{
			return true;
		}

		inline Vec3D p3() const { return Vec3D(this->p.x, this->p.y, z); }
		inline operator RadialRegion< TreeTag3D< T > > () const
			{ return RadialRegion< TreeTag3D< T > > (p3(), this->r); }
	};

	template <typename Tag>
	class RectRegion
	{
	public:
		typedef typename Tag::VECTOR VECTOR;
		typedef typename Tag::SCALAR SCALAR;

		VECTOR v1, v2;

		inline RectRegion (const VECTOR & v1, const VECTOR & v2) 
			: v1(v1), v2(v2) 
		{ assertDimensions(v1, v2); }

		inline bool operator ^ (const vector3D <SCALAR> & v) const
		{
			return 
				v.x >= this->v1.x && v.x <= this->v2.x &&
				v.y >= this->v1.y && v.y <= this->v2.y &&
				v.z >= this->v1.z && v.z <= this->v2.z;
		}
		inline bool operator ^ (const vector2D <SCALAR> & v) const
		{
			return 
				v.x >= this->v1.x && v.x <= this->v2.x &&
				v.y >= this->v1.y && v.y <= this->v2.y;
		}
		inline bool operator ^ (const PointRegion < TreeTag3D< SCALAR > > & rval) const
		{
			return 
				v1.x <= rval.p.x && v2.x >= rval.p.x &&
				v1.y <= rval.p.y && v2.y >= rval.p.y &&
				v1.z <= rval.p.z && v2.z >= rval.p.z;
		}
		inline bool operator ^ (const PointRegion < TreeTag2D< SCALAR > > & rval) const
		{
			return 
				v1.x <= rval.p.x && v2.x >= rval.p.x &&
				v1.y <= rval.p.y && v2.y >= rval.p.y;
		}
		inline bool operator ^ (const AmbientRegion &) const
		{
			return true;
		}

		inline bool operator ^ (const RectRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return 
				this->v1.x <= rval.v1.x &&
				this->v1.y <= rval.v1.y &&
				this->v1.z <= rval.v1.z &&

				this->v2.x >= rval.v2.x &&
				this->v2.y >= rval.v2.y &&
				this->v2.z >= rval.v2.z;
		}
		inline bool operator ^ (const RectRegion< TreeTag2D< SCALAR > > & rval) const
		{
			return 
				this->v1.x <= rval.v1.x &&
				this->v1.y <= rval.v1.y &&

				this->v2.x >= rval.v2.x &&
				this->v2.y >= rval.v2.y;
		}

		inline bool operator & (const RectRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return 
				this->v2.x >= rval.v1.x &&
				this->v2.y >= rval.v1.y &&
				this->v2.z >= rval.v1.z &&

				this->v1.x <= rval.v2.x &&
				this->v1.y <= rval.v2.y &&
				this->v1.z <= rval.v2.z;
		}
		inline bool operator & (const RectRegion< TreeTag2D< SCALAR > > & rval) const
		{
			return 
				this->v2.x >= rval.v1.x &&
				this->v2.y >= rval.v1.y &&

				this->v1.x <= rval.v2.x &&
				this->v1.y <= rval.v2.y;
		}
		inline bool operator & (const PointRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return v1 == rval.v2 && 
				v1.x == rval.p.x &&
				v1.y == rval.p.y &&
				v1.z == rval.p.z;  
		}
		inline bool operator & (const PointRegion< TreeTag2D< SCALAR > > & rval) const
		{
			return v1 == rval.v2 && 
				v1.x == rval.p.x &&
				v1.y == rval.p.y;  
		}

		inline bool operator & (const RadialRegion <Tag> & rval) const
		{
			return rval & *this;
		}

		inline bool operator & (const AmbientRegion &) const
		{
			return true;
		}

		inline bool isNiche (const DegreeBounds <Tag> & b) const
		{
			const SCALAR 
				mrsq = MAGSQ(v2 - v1),
				m2sq = SQ(b.m2);

			return mrsq > m2sq && mrsq < m2sq * 2;
		}

	private:
		static inline void assertDimensions (const vector3D <SCALAR> & v1, const vector3D <SCALAR> & v2)
			{ assert(v1.x <= v2.x && v1.y <= v2.y && v1.z <= v2.z); }
		static inline void assertDimensions (const vector2D <SCALAR> & v1, const vector2D <SCALAR> & v2)
			{ assert(v1.x <= v2.x && v1.y <= v2.y); }
	};

	template <typename Tag>
	class PointRegion
	{
	public:
		typedef typename Tag::VECTOR VECTOR;
		typedef typename Tag::SCALAR SCALAR;

		VECTOR p;

		inline PointRegion (const VECTOR & v) 
			: p(v)
			{}

		inline bool operator ^ (const vector3D <SCALAR> & v) const
		{
			return
				v.x == this->p.x &&
				v.y == this->p.y &&
				v.z == this->p.z;
		}
		inline bool operator ^ (const vector2D <SCALAR> & v) const
		{
			return 
				v.x == this->p.x &&
				v.y == this->p.y;
		}

		inline bool operator ^ (const RectRegion< TreeTag3D< SCALAR > > & rval) const
		{ 
			return rval.v1 == rval.v2 && 
				rval.v1.x == p.x &&
				rval.v1.y == p.y &&
				rval.v1.z == p.z; 
		}
		inline bool operator ^ (const RectRegion< TreeTag2D< SCALAR > > & rval) const
		{ 
			return rval.v1 == rval.v2 && 
				rval.v1.x == p.x &&
				rval.v1.y == p.y; 
		} 

		inline bool operator & (const RectRegion< TreeTag3D< SCALAR > > & rval) const
		{
			return 
				this->p.x >= rval.v1.x &&
				this->p.y >= rval.v1.y &&
				this->p.z >= rval.v1.z &&

				this->p.x <= rval.v2.x &&
				this->p.y <= rval.v2.y &&
				this->p.z <= rval.v2.z;
		}
		inline bool operator & (const RectRegion< TreeTag2D< SCALAR > > & rval) const
		{
			return 
				this->p.x >= rval.v1.x &&
				this->p.y >= rval.v1.y &&

				this->p.x <= rval.v2.x &&
				this->p.y <= rval.v2.y;
		}
		inline bool operator & (const RadialRegion <Tag> & rval) const
		{
			return MAGSQ(rval.p - p) < SQ(rval.r);
		}
		inline bool isNiche (const DegreeBounds <Tag> & b) const
			{ return false; }
	};

	template <typename Tag>
	class DegreeBounds
	{
	public:
		typedef typename Tag::VECTOR VECTOR;
		typedef typename Tag::SCALAR SCALAR;
		typedef unsigned int W;

	private:
		static inline unsigned int ao2qbs2D (const unsigned int ao)
		{
			const unsigned int iao = ~ao;

			return	
				(iao & ((1 << 0) | (1 << 2)) ? 0 : 1 << 0) |
				(iao & ((1 << 1) | (1 << 2)) ? 0 : 1 << 1) |
				(iao & ((1 << 0) | (1 << 3)) ? 0 : 1 << 2) |
				(iao & ((1 << 1) | (1 << 3)) ? 0 : 1 << 3);
		}
		static inline unsigned int ao2qbs3D (const unsigned int ao)
		{
			const unsigned int iao = ~ao;

			return	
				(iao & ((1 << 0) | (1 << 2) | (1 << 4)) ? 0 : 1 << 0) |
				(iao & ((1 << 1) | (1 << 2) | (1 << 4)) ? 0 : 1 << 1) |
				(iao & ((1 << 0) | (1 << 3) | (1 << 4)) ? 0 : 1 << 2) |
				(iao & ((1 << 1) | (1 << 3) | (1 << 4)) ? 0 : 1 << 3) |
				(iao & ((1 << 0) | (1 << 2) | (1 << 5)) ? 0 : 1 << 4) |
				(iao & ((1 << 1) | (1 << 2) | (1 << 5)) ? 0 : 1 << 5) |
				(iao & ((1 << 0) | (1 << 3) | (1 << 5)) ? 0 : 1 << 6) |
				(iao & ((1 << 1) | (1 << 3) | (1 << 5)) ? 0 : 1 << 7);
		}

		static inline unsigned int quad (const DegreeBounds <Tag> & b, const SCALAR x, const SCALAR y)
		{
			const vector2D <SCALAR> c = b.center();
			return 
				(y >= c.y ? 1 << 1 : 0 << 1) | 
				(x >= c.x ? 1 << 0 : 0 << 0);
		}
		static inline unsigned int quad (const DegreeBounds <TreeTag3D<SCALAR>> & b, const SCALAR x, const SCALAR y, const SCALAR z)
		{
			const vector3D <SCALAR> c = b.center();
			return 
				(z >= c.z ? 1 << 2 : 0 << 2) | 
				(y >= c.y ? 1 << 1 : 0 << 1) | 
				(x >= c.x ? 1 << 0 : 0 << 0);
		}

		template< typename TagLVal >
		static inline unsigned int intersects (const DegreeBounds< TagLVal > & b, const AmbientRegion & ar, TagLVal * = NULL)
			{ return true; }

		template< typename TagLVal, typename TagRVal >
		static inline unsigned int intersects (const DegreeBounds< TagLVal > & b, const PointRegion< TagRVal > & pr, TagLVal * = NULL, TagRVal * = NULL)
		{
			const vector2D <SCALAR> c = b.center();
			return ao2qbs2D (
				(pr.p.x < c.x ? 1 << 0 : 0) |
				(pr.p.x >= c.x ? 1 << 1 : 0) |
				(pr.p.y < c.y ? 1 << 2 : 0) |
				(pr.p.y >= c.y ? 1 << 3 : 0)
			);
		}
		static inline unsigned int intersects (const DegreeBounds <TreeTag3D< SCALAR >> & b, const PointRegion <TreeTag3D< SCALAR > > & pr)
		{
			const vector3D <SCALAR> c = b.center();
			return ao2qbs3D (
				(pr.p.x < c.x ? 1 << 0 : 0) |
				(pr.p.x >= c.x ? 1 << 1 : 0) |
				(pr.p.y < c.y ? 1 << 2 : 0) |
				(pr.p.y >= c.y ? 1 << 3 : 0) |
				(pr.p.z < c.z ? 1 << 4 : 0) |
				(pr.p.z >= c.z ? 1 << 5 : 0)
			);
		}

		template< typename TagLVal, typename TagRVal >
		static inline unsigned int intersects (const DegreeBounds< TagLVal > & b, const RectRegion< TagRVal > & rr, TagLVal * = NULL, TagRVal * = NULL)
		{
			const vector2D <SCALAR> c = b.center();
			return ao2qbs2D (
					(rr.v1.x < c.x ? 1 << 0 : 0) |
					(rr.v2.x >= c.x ? 1 << 1 : 0) |
					(rr.v1.y < c.y ? 1 << 2 : 0) |
					(rr.v2.y >= c.y ? 1 << 3 : 0)
				);
		}
		static inline unsigned int intersects (const DegreeBounds <TreeTag3D< SCALAR >> & b, const RectRegion <TreeTag3D< SCALAR > > & rr)
		{
			const vector3D <SCALAR> c = b.center();
			return ao2qbs3D (
					(rr.v1.x < c.x ? 1 << 0 : 0) |
					(rr.v2.x >= c.x ? 1 << 1 : 0) |
					(rr.v1.y < c.y ? 1 << 2 : 0) |
					(rr.v2.y >= c.y ? 1 << 3 : 0) |
					(rr.v1.z < c.z ? 1 << 4 : 0) |
					(rr.v2.z >= c.z ? 1 << 5 : 0)
				);
		}

		template< typename TagLVal, typename TagRVal >
		static inline unsigned int intersects (const DegreeBounds< TagLVal > & b, const RadialRegion< TagRVal > & rr, TagLVal * = NULL, TagRVal * = NULL)
		{
			using namespace mars;

			const vector2D <SCALAR> c = b.center();
			unsigned int j = 
				SQ(rr.p.x - c.x) + SQ(rr.p.y - c.y) >= SQ(rr.r) ? 
					1 << quad (b, c.x + (b.m2 - rr.p.x), c.y + (b.m2 - rr.p.y)) : 
					0;

			return ao2qbs2D (
					(rr.p.x < (c.x + rr.r) ? 1 << 0 : 0) |
					(rr.p.x > (c.x - rr.r) ? 1 << 1 : 0) |
					(rr.p.y < (c.y + rr.r) ? 1 << 2 : 0) |
					(rr.p.y > (c.y - rr.r) ? 1 << 3 : 0)
				) & ~j;
		}
		static inline unsigned int intersects (const DegreeBounds <TreeTag3D <SCALAR> > & b, const RadialRegion < TreeTag3D <SCALAR> > & rr)
		{
			using namespace mars;

			const vector3D <SCALAR> c = b.center();
			unsigned int 
				j = SQ(rr.p.x - c.x) + SQ(rr.p.y - c.y) >= SQ(rr.r) ?
					1 << quad (b, c.x + (b.m2 - rr.p.x), c.y + (b.m2 - rr.p.y), rr.p.z) :
					0,
				k = SQ(rr.p.x - c.x) + SQ(rr.p.z - c.z) >= SQ(rr.r) ?
					1 << quad (b, c.x + (b.m2 - rr.p.x),	c.y + (b.m2 - rr.p.y), c.z + (b.m2 - rr.p.z)) :
					0;

			return ao2qbs3D (
					(rr.p.x < (c.x + rr.r) ? 1 << 0 : 0) |
					(rr.p.x > (c.x - rr.r) ? 1 << 1 : 0) |
					(rr.p.y < (c.y + rr.r) ? 1 << 2 : 0) |
					(rr.p.y > (c.y - rr.r) ? 1 << 3 : 0) |
					(rr.p.z < (c.z + rr.r) ? 1 << 4 : 0) |
					(rr.p.z > (c.z - rr.r) ? 1 << 5 : 0)
				) & ~(j | k);
		}
		static inline void slide (DegreeBounds <TreeTag2D <SCALAR> > & b, const unsigned int q0, const unsigned int q)
		{
			const SCALAR m = b.m2 * static_cast <SCALAR> (2);

			b.loc.x += m * static_cast <SCALAR> ((q & (1 << 0)) - (q0 & (1 << 0)));
			b.loc.y += m * static_cast <SCALAR> ((q & (1 << 1)) - (q0 & (1 << 1)));
		}
		static inline void slide (DegreeBounds <TreeTag3D <SCALAR> > & b, const unsigned int q0, const unsigned int q)
		{
			const SCALAR m = b.m2 * static_cast <SCALAR> (2);

			b.loc.x += m * static_cast <SCALAR> ((q & (1 << 0)) - (q0 & (1 << 0)));
			b.loc.y += m * static_cast <SCALAR> ((q & (1 << 1)) - (q0 & (1 << 1)));
			b.loc.z += m * static_cast <SCALAR> ((q & (1 << 2)) - (q0 & (1 << 2)));
		}

	public:
		VECTOR loc;
		SCALAR m2;

		inline DegreeBounds (const VECTOR & loc, const SCALAR m2)
			: loc(loc), m2(m2) {}

		inline VECTOR center() const { return loc + m2; }

		template< typename Tag2 >
		inline unsigned int intersects (const PointRegion < Tag2 > & pr) const { return intersects(*this, pr); }

		template< typename Tag2 >
 		inline unsigned int intersects (const RadialRegion < Tag2 > & rr) const { return intersects(*this, rr);  }
		
		template< typename Tag2 >
		inline unsigned int intersects (const RectRegion < Tag2 > & rr) const { return intersects(*this, rr); }

		inline unsigned int intersects (const AmbientRegion & ar) const { return intersects(*this, ar); }

		inline void slide (const unsigned int q0, const unsigned int q) { slide (*this, q0, q); }
		inline void descend () { m2 /= 2; }
	};

	template <typename E, typename R, typename Tag>
	class TreeDegree
	{
	public:
		typedef typename Tag::SCALAR SCALAR;
		typedef typename Tag::VECTOR VECTOR;
		typedef std::list < Entity <E, R> >	ListType;

	private:
		TreeDegree <E, R, Tag> *								_degrees [Tag::DEGREES];
		ListType *												_list;

		inline void push (const ptr< E > & pData, const R & rr)
		{
			if (_list == NULL)
				_list = new ListType ();

			_list->push_back(Entity <E, R> (pData, rr));
		}

		template <typename QR>
		void add2 (const QR & rad, const ptr< E > & pData, const DegreeBounds <Tag> &b)
		{
			for (DegreeNicheIterator <E, R, QR, Tag> it (this, rad, b); it; ++it)
				it->push(pData, rad);
		}

	public:
		inline bool hasEntities () const { return _list != NULL && !_list->empty(); }
		inline typename ListType::iterator begin () { return _list->begin(); }
		inline typename ListType::iterator end() { return _list->end(); }
		inline typename ListType::const_iterator begin () const { return _list->begin(); }
		inline typename ListType::const_iterator end() const { return _list->end(); }

		TreeDegree (unsigned short ndegs, SCALAR m)
			: _list(NULL)
		{
			if (--ndegs)
			{
				SCALAR m2 = m / static_cast <SCALAR> (2);
				for (unsigned int c = 0; c < Tag::DEGREES; c++)
					_degrees[c] = new TreeDegree <E, R, Tag> (ndegs, m2);
			} else
			{
				for (unsigned int c = 0; c < Tag::DEGREES; c++)
					_degrees[c] = NULL;
			}
		}

		inline void add (const CylindricalRegion <SCALAR> & cyl, const ptr< E > & pData, const DegreeBounds <Tag> & b)
			{ add2(cyl, pData, b); }
		inline void add (const RadialRegion <Tag> & rad, const ptr< E > & pData, const DegreeBounds <Tag> & b)
			{ add2(rad, pData, b); }
		inline void add (const RectRegion <Tag> & rect, const ptr< E > & pData, const DegreeBounds <Tag> & b)
			{ add2(rect, pData, b); }

		inline bool bottom () const { return _degrees[0] == NULL; }
		inline const TreeDegree <E, R, Tag> * operator [] (int i) const { return _degrees[i]; }
		inline TreeDegree <E, R, Tag> * operator [] (int i) { return _degrees[i]; }

		~TreeDegree ()
		{
			for (unsigned int i = 0; i < Tag::DEGREES; ++i)
				if (_degrees[i])
					delete _degrees[i];

			if (_list != NULL)
				delete _list;
		}
	};

	template< typename E, typename R, typename Tag >
	struct TreeIteratorTraits
	{
		typedef TreeDegree< E, R, Tag > MyTreeDegree;
		typedef ptr< E > PtrE;
	};
	template< typename E, typename R, typename Tag >
	struct ConstTreeIteratorTraits
	{
		typedef const TreeDegree< E, R, Tag > MyTreeDegree;
		typedef const ptr< E > PtrE;
	};

	template <typename E, typename R, typename QR, typename Tag, typename TIT = TreeIteratorTraits< E, R, Tag >>
	class DegreeIteratorBase
	{
	public:
		typedef typename TIT::MyTreeDegree TreeDegree;

	protected:
		class State
		{
		public:
			TreeDegree * window;
			DegreeBounds <Tag> subbounds;
			unsigned int intersection, idxlast;
			int idx;

			inline State (TreeDegree * const & window, const DegreeBounds <Tag> & bounds)
				: subbounds(bounds), window(window), intersection(0), idxlast(0), idx(-1) {}

		} _state;

		std::stack <State> _stack;
		const QR _q;
		TreeDegree * _degcurr;

		inline bool undrill ()
		{
			if (_stack.empty())
				return false;
			else
			{
				_state = _stack.top();
				_stack.pop();
				return true;
			}
		}
		inline void initDegree() 
		{
			// Some weirdness here, for optimization reasons, we're going to reuse the "subbounds" property and treat it as the
			// present window's "bounds" in order to compute the sub-division intersection bitset.  This is the only time we need
			// "bounds" for the window, so we can throw it away from here-on in and convert it to "subbounds" for the current 
			// window proper.
			_state.intersection = _state.subbounds.intersects(_q);
			_state.subbounds.descend();

			_state.idx = -1;
			_state.idxlast = 0;
		}
		inline void drill(TreeDegree * const & deg) 
		{
			_stack.push(_state);
			_state.window = deg;
			// Do not call subbounds.descend() yet, we need its present state somewhere else for a bit yet
		}
		inline void slideTo(const int i) 
		{
			_degcurr = (*_state.window)[i];
			_state.subbounds.slide(_state.idxlast, i);
			_state.idxlast = i;
		}

		inline DegreeIteratorBase (TreeDegree * top, const DegreeBounds <Tag> & bounds, const QR & q)
			: _state(top, bounds), _q(q), _degcurr(NULL) {}
	};

	template <typename E, typename R, typename QR, typename Tag, typename TIT >
	class DegreeNicheIterator : public DegreeIteratorBase< E, R, QR, Tag, TIT >, public std::iterator <std::input_iterator_tag, TreeDegree <E, R, Tag> >
	{
	private:
		typedef DegreeIteratorBase< E, R, QR, Tag, TIT > Base;
		using typename Base::TreeDegree;
	
		CR_CONTEXT;

		void process()
		{
			CR_START();
			if (this->_q.isNiche(this->_state.subbounds))
			{
				this->_degcurr = this->_state.window;
				CR_RETURN_VOID;
			} else
			{
				this->initDegree();

				do
				{
					while (++this->_state.idx < Tag::DEGREES)
					{
						if (this->_state.intersection & (1 << this->_state.idx))
						{
							this->slideTo(this->_state.idx);

							if (this->_q.isNiche(this->_state.subbounds) || this->_degcurr->bottom())
							{
								CR_RETURN_VOID;
							} else
							{	// Recurse downwards
								this->drill(this->_degcurr);
								this->initDegree();
								continue;
							}
						}
					}
				} while (this->undrill());
			}

			this->_degcurr = NULL;
			CR_END();
		}

	public:	
		inline DegreeNicheIterator (TreeDegree * top, const QR & q, const DegreeBounds <Tag> & bounds)
			: Base(top, bounds, q)
		{
			CR_INIT();
			process();
		}
		inline operator bool () { return this->_degcurr != NULL; }
		inline DegreeNicheIterator & operator++()
		{
			process();
			return *this;
		}
		inline DegreeNicheIterator operator++(int)
		{
			DegreeNicheIterator old = *this;
			process();
			return old;
		}
		inline const TreeDegree * operator -> () const { return &(*this->_degcurr); }
		inline const TreeDegree & operator * () const { return *this->_degcurr; }
		inline TreeDegree * operator -> () { return &(*this->_degcurr); }
		inline TreeDegree & operator * () { return *this->_degcurr; }
	};

	template <typename E, typename R, typename QR, typename Tag, typename TIT = TreeIteratorTraits< E, R, Tag >>
	class DegreeIterator : public DegreeIteratorBase <E, R, QR, Tag, TIT >, public std::iterator <std::input_iterator_tag, TreeDegree <E, R, Tag> >
	{
	private:
		typedef DegreeIteratorBase <E, R, QR, Tag, TIT > Base;
		using typename Base::TreeDegree;
		
		CR_CONTEXT;

		void process()
		{
			CR_START();

			this->initDegree();
			do
			{
				while (++this->_state.idx < Tag::DEGREES)
				{
					if (this->_state.intersection & (1 << this->_state.idx))
					{
						this->slideTo(this->_state.idx);
						CR_RETURN_VOID;

						// Recurse downwards
						if (!this->_degcurr->bottom())
						{
							this->drill (this->_degcurr);
							this->initDegree();
							continue;
						}
					}
				}
			} while (this->undrill());

			this->_degcurr = NULL;
			CR_END();
		}
	public:
		inline DegreeIterator (TreeDegree * top, const QR & q, const DegreeBounds <Tag> & bounds)
			: Base(top, bounds, q)
		{
			CR_INIT();
			process();
		}
		inline operator bool () { return this->_degcurr != NULL; }
		inline DegreeIterator & operator++()
		{
			process();
			return *this;
		}
		inline DegreeIterator operator++(int)
		{
			DegreeIterator old = *this;
			process();
			return old;
		}
		inline const TreeDegree * operator -> () const { return &(*this->_degcurr); }
		inline const TreeDegree & operator * () const { return *this->_degcurr; }
		inline TreeDegree * operator -> () { return &(*this->_degcurr); }
		inline TreeDegree & operator * () { return *this->_degcurr; }
	};

	template <typename E, typename R, typename QR, typename Tag, typename TIT = ConstTreeIteratorTraits< E, R, Tag > >
	class ConstEntityIterator : public std::iterator <std::input_iterator_tag, E>
	{
	public:
		typedef typename Tag::SCALAR SCALAR;
		typedef typename Tag::VECTOR VECTOR;
		typedef typename TIT::PtrE ElemPtr;

	private:
		typedef typename TIT::MyTreeDegree::ListType::const_iterator ListIterator;

		const QR _q;
		DegreeIterator <E, R, QR, Tag, TIT> _itD;
		ListIterator _itE;
		std::set < const E * > _set;
		bool _more;

		CR_CONTEXT;

	protected:
		void process ()
		{
			CR_START();
			
			_more = true;

			while (_itD)
			{
				if (_itD->hasEntities())
				{
					for (_itE = _itD->begin(); _itE != _itD->end(); ++_itE)
					{
						if (_itE->region & _q)
						{
							if (_set.find(_itE->pData.pointer()) == _set.end())
							{
								_set.insert(_itE->pData.pointer());
								CR_RETURN_VOID;
							}
						}
					}
				}
				++_itD;
			}

			_more = false;

			CR_END();
		}

	public:
		inline ConstEntityIterator (typename TIT::MyTreeDegree & top, const QR & query, const VECTOR & ptOffset, const SCALAR magnitude)
			: _itD (&top, query, DegreeBounds <Tag> (ptOffset, magnitude / 2)), _q(query), _more(false) 
		{
			CR_INIT();
			process();
		}

		inline operator bool () const { return _more; }
		inline bool hasMore () const { return _more; }
		inline const ConstEntityIterator & operator++()
		{
			process();
			return *this;
		}
		inline const ConstEntityIterator operator++(int)
		{
			const ConstEntityIterator old = *this;
			process();
			return old;
		}
		inline const R & region () const { return _itE->region; }
		inline const ptr< E > & operator -> () const { return _itE->pData; }
		inline const E & operator * () const { return *_itE->pData; }
		inline operator ElemPtr () const { return _itE->pData; }
	};

	template <typename E, typename R, typename QR, typename Tag >
	class EntityIterator : public ConstEntityIterator< E, R, QR, Tag, TreeIteratorTraits< E, R, Tag > >
	{
	private:
		typedef ConstEntityIterator< E, R, QR, Tag, TreeIteratorTraits< E, R, Tag > > Base;
		using typename Base::VECTOR;
		using typename Base::SCALAR;
		
	public:
		inline EntityIterator (typename TreeIteratorTraits< E, R, Tag >::MyTreeDegree & top, const QR & query, const VECTOR & ptOffset, const SCALAR magnitude)
			: Base(top, query, ptOffset, magnitude) {}

		inline ptr< E > & operator -> () { return this->_itE->pData; }
		inline E & operator * () { return *this->_itE->pData; }
	};

	template <typename E, typename R, typename Tag>
	class LookupTree
	{
	public:
		template< typename Tag2 >
		class Explicit
		{
		public:
			typedef typename Tag2::VECTOR VECTOR;
			typedef typename Tag2::SCALAR SCALAR;
			typedef mars::RectRegion <Tag2> RectRegion;
			typedef mars::RadialRegion <Tag2> RadialRegion;
			typedef mars::CylindricalRegion <SCALAR> CylindricalRegion;
			typedef mars::PointRegion <Tag2> PointRegion;
			typedef EntityIterator <E, R, PointRegion, Tag> EntityPointIterator;
			typedef EntityIterator <E, R, RectRegion, Tag> EntityRectIterator;
			typedef EntityIterator <E, R, RadialRegion, Tag> EntityRadialIterator;
			typedef ConstEntityIterator <E, R, PointRegion, Tag > const_EntityPointIterator;
			typedef ConstEntityIterator <E, R, RectRegion, Tag > const_EntityRectIterator;
			typedef ConstEntityIterator <E, R, RadialRegion, Tag > const_EntityRadialIterator;
		};

		typedef typename Tag::VECTOR VECTOR;
		typedef typename Tag::SCALAR SCALAR;
		typedef TreeTag3D< SCALAR > Tag3D;
		typedef TreeTag2D< SCALAR > Tag2D;
		typedef Entity <E, R> MyEntity;
		typedef RectRegion <Tag> MyRectRegion;
		typedef RadialRegion <Tag> MyRadialRegion;
		typedef CylindricalRegion <SCALAR> MyCylindricalRegion;
		typedef PointRegion <Tag> MyPointRegion;
		typedef R Region;
		typedef EntityIterator <E, R, MyPointRegion, Tag> EntityPointIterator;
		typedef EntityIterator <E, R, MyRectRegion, Tag> EntityRectIterator;
		typedef EntityIterator <E, R, MyRadialRegion, Tag> EntityRadialIterator;
		typedef EntityIterator <E, R, AmbientRegion, Tag> EntityAllIterator;
		typedef ConstEntityIterator <E, R, MyPointRegion, Tag > const_EntityPointIterator;
		typedef ConstEntityIterator <E, R, MyRectRegion, Tag > const_EntityRectIterator;
		typedef ConstEntityIterator <E, R, MyRadialRegion, Tag > const_EntityRadialIterator;
		typedef ConstEntityIterator <E, R, AmbientRegion, Tag > const_EntityAllIterator;
		typedef TreeDegree <E, R, Tag> MyTreeDegree;

	private:
		MyTreeDegree				_top;
		
		static inline unsigned short computeSuitableOrder (const SCALAR m)
		{
			return static_cast <unsigned short> (floor (log(static_cast<float> (m)) / log(4.0f)));
		}

	public:
		const SCALAR magnitude;
		const VECTOR offset;

		inline LookupTree(const SCALAR m, const VECTOR & offset = VECTOR()) : _top (computeSuitableOrder(m), m), magnitude(m), offset(offset) {}
		inline LookupTree(const SCALAR m, const unsigned short o, const VECTOR & offset = VECTOR()) : _top (o, m), magnitude(m), offset(offset) {}

		inline EntityAllIterator contents () { return EntityAllIterator (_top, AmbientRegion(), offset, magnitude); }
		inline const_EntityAllIterator contents () const { return const_EntityAllIterator (_top, AmbientRegion(), offset, magnitude); }

		template <typename QR>
		inline ConstEntityIterator <E, R, QR, Tag> contents (const QR & region) const
			{ return ConstEntityIterator <E, R, QR, Tag> (_top, region, offset, magnitude); }

		template <typename QR>
		inline EntityIterator <E, R, QR, Tag> contents (const QR & region)
			{ return EntityIterator <E, R, QR, Tag> (_top, region, offset, magnitude); }
		
		inline void add (const ptr< E > & pData, const R & r)
		{
			_top.add (r, pData, DegreeBounds <Tag> (offset, magnitude / static_cast <SCALAR> (2)));
		}
	};

	template <typename E, typename T>
	class QuadTree_CircleIndexed : public LookupTree <E, RadialRegion < TreeTag2D <T> >, TreeTag2D <T> > 
	{
	private:
		typedef LookupTree <E, RadialRegion < TreeTag2D <T> >, TreeTag2D <T> > Base;
		
	public:
		using typename Base::VECTOR;
		using typename Base::SCALAR;

        QuadTree_CircleIndexed (const SCALAR m, const VECTOR & offset = VECTOR()) : Base (m, offset) {}
		QuadTree_CircleIndexed (const SCALAR m, const unsigned short o, const VECTOR & offset = VECTOR()) : Base (m, o, offset) {}
	};

	template <typename E, typename T>
	class OctTree_SphereIndexed : public LookupTree <E, RadialRegion < TreeTag3D <T> >, TreeTag3D <T> > 
	{
	private:
		typedef LookupTree <E, RadialRegion < TreeTag3D <T> >, TreeTag3D <T> > Base;

	public:
		using typename Base::VECTOR;
		using typename Base::SCALAR;

        OctTree_SphereIndexed (const SCALAR m, const VECTOR & offset = VECTOR()) : Base (m, offset) {}
		OctTree_SphereIndexed (const SCALAR m, const unsigned short o, const VECTOR & offset = VECTOR()) : Base (m, o, offset) {}
	};

	template <typename E, typename T>
	class QuadTree_BBoxIndexed : public LookupTree <E, RectRegion < TreeTag2D <T> >, TreeTag2D <T> > 
	{
	private:
		typedef LookupTree <E, RectRegion < TreeTag2D <T> >, TreeTag2D <T> > Base;
		
	public:
		using typename Base::VECTOR;
		using typename Base::SCALAR;

        QuadTree_BBoxIndexed (const SCALAR m, const VECTOR & offset = VECTOR()) : Base (m, offset) {}
		QuadTree_BBoxIndexed (const SCALAR m, const unsigned short o, const VECTOR & offset = VECTOR()) : Base (m, o, offset) {}
	};

	template <typename E, typename T>
	class QuadTree_SphereIndexed : public LookupTree <E, CylindricalRegion <T>, TreeTag2D <T> > 
	{
	private:
		typedef LookupTree <E, CylindricalRegion <T>, TreeTag2D <T> > Base;

	public:
		using typename Base::VECTOR;
		using typename Base::SCALAR;

        QuadTree_SphereIndexed (const SCALAR m, const VECTOR & offset = VECTOR()) : Base (m, offset) {}
		QuadTree_SphereIndexed (const SCALAR m, const unsigned short o, const VECTOR & offset = VECTOR()) : Base (m, o, offset) {}
	};
}


#endif