#ifndef __SPHEREPACK_H__
#define __SPHEREPACK_H__

#include <list>
#include <assert.h>

#include "mars_lookup.h"
#include "mars_linalg.h"
#include "mars_util.h"
#include "mars_calc.h"
#include "mars_ptr.h"

namespace geoworld
{
	template <typename T>
	class RandomSpherePack;

	template <typename T>
	class RandomSpherePack
	{
	public:
		class Sphere;
		typedef typename mars::TreeTag3D< T >::VECTOR VECTOR;
		typedef typename mars::TreeTag3D< T >::SCALAR SCALAR;
		typedef std::list <VECTOR> PointList;
		typedef std::list< mars::ptr< Sphere > > SphereList;
		typedef typename SphereList::iterator SphereListIterator;
		typedef typename SphereList::const_iterator SphereListConstIterator;

		struct IteratorTraits
		{
			typedef RandomSpherePack SpherePack;
			using SphereType = Sphere;
			typedef typename SphereList::iterator SphereListIterator;
		};
		struct ConstIteratorTraits
		{
			typedef const RandomSpherePack SpherePack;
			using SphereType = const Sphere;
			typedef typename SphereList::const_iterator SphereListIterator;
		};

		class Sphere
		{
		private:
			SphereList _subnodes;

		public:
			const RandomSpherePack< T > * owner;

			VECTOR p;
			SCALAR r;

			inline Sphere (const RandomSpherePack< T > * pOwner)
				: owner(pOwner), r(0) {}

			inline Sphere (const Sphere & copy)
				: owner(copy.owner), p(copy.p), r(copy.r), _subnodes(copy._subnodes) {}

			inline Sphere (const RandomSpherePack< T > * pOwner, const VECTOR & p, const T & r)
				: owner(pOwner), p(p), r(r) {}

			template< typename Tag >
			inline operator mars::RadialRegion< Tag > () const { return mars::RadialRegion< Tag > (p, r); }

			inline operator mars::CylindricalRegion< T > () const { return mars::CylindricalRegion< T > (p, r); }

			inline static float computeCull (const float azimuth, const float zenith)
			{
				using namespace mars;

				static const float PIxPI = SQ(static_cast<float> (PI));
				// Calculates dot product of the displacement vector with the cull plane normal
				// Spherical analog of Pythagoras: cos(c) = cos(a)cos(b)
				return (PIxPI - SQ(acos(cos(azimuth)*cos(zenith)))) / PIxPI;
			}

			inline SphereListConstIterator begin() const { return _subnodes.begin(); }
			inline SphereListConstIterator end() const { return _subnodes.end(); }
			inline SphereListIterator begin() { return _subnodes.begin(); }
			inline SphereListIterator end() { return _subnodes.end(); }

			inline bool operator == (const Sphere & b) const
			{
				return p == b.p && r == b.r && owner == b.owner;
			}

			inline size_t count() { return _subnodes.size(); }

			// Connect an existing sphere to this sphere as a child
			void attach (const mars::ptr< Sphere > & pSphere)
			{
				assert (mars::MAG(pSphere->p) > 1);
				_subnodes.push_back(pSphere);
			}
			Sphere * attachCulled( const mars::RangeX<float> &rads, const mars::RangeX<float> &rrng, const mars::SphericalCoords<T> &spc0 ) 
			{
				using namespace mars;

				const T r = static_cast <T> (rads.next());
				SphericalCoords <T> spc1(
					r, 
					rrng.next(), 
					rrng.next()
				);

				// The closer a child is to the intersection of the two spheres (parent and current) the smaller it will be
				if (rrng.delta >= owner->_minimum)
					spc1.r = static_cast< T > (static_cast< float > (spc1.r) * computeCull(spc1.azimuth, spc1.zenith));

				// Ignore any and all children that are too small, just discard the information
				if (spc1.r >= owner->_minimum)
				{
					// Apply new randomly generated angles to the angles between the two spheres and the corresponding radius
					return & attach(
						spc1.zenith + spc0.zenith, 
						spc1.azimuth + spc0.azimuth, 
						spc1.r / 2,
						spc1.r
					);
				} else
					return NULL;
			}
			// Create and connect a new sphere with the specified radius at the specified spherical coordinates relative to this sphere
			Sphere & attach (const float zenith, const float azimuth, const T dr, const T rr)
			{
				using namespace mars;

				// Sphere is attached at the radius of the parent sphere plus the specified amount and at the spherical angles specified
				_subnodes.push_back(mars::ptr< Sphere > (new Sphere(owner, SphericalCoords<T>(r + dr, zenith, azimuth) + p, rr)));
				assert (mars::MAG(_subnodes.back()->p) > 1);
				return *_subnodes.back();
			}
			template< typename PTR, typename R, typename G >
			void store (mars::LookupTree< typename PTR::Type, R, G > & store, const PTR & pElem) const
			{
				store.add(pElem, *this);
				for (typename SphereList::const_iterator it = _subnodes.begin(); it != _subnodes.end(); ++it)
					(*it)->store(store, pElem);
			}
		};

	private:
		SphereList _toptier;

		const float _divisor;
		const mars::RangeX <float> _sizeflux;
		const mars::RangeX <unsigned short> _kidflux;
		const T _minimum, _maxtier;

		T _limit;	// Non-const for subdivision

		inline float intersectAngle (const Sphere & sphere0, const Sphere & sphere) const
		{
			using namespace mars;

			assert(mars::MAG(sphere.p - sphere0.p) < sphere.r + sphere0.r);
			return acos(
				static_cast< float > (MAGSQ(sphere.p - sphere0.p) + SQ(sphere0.r) - SQ(sphere.r)) / static_cast< float > (2 * MAG(sphere.p - sphere0.p) * sphere0.r)
			);
		}

		inline T distance (const T r1, const T r2) const
		{
			if (r1 > r2)
				return r1 + r2 / 2;
			else
				return r2 + r1 / 2;
		}

		mars::ptr< Sphere > spawnBetween (const Sphere & sphere0, const Sphere & sphere1)
		{
			using namespace mars;
			using namespace std;

			// The vector between the two precursor spheres
			const vector3D< T > dp = sphere1.p - sphere0.p;		

			// The scalar and the square of the distance between the two precursor spheres
			const T dsq = MAGSQ(dp), d = sqrt(dsq);

			// New sphere radius must be large enough to sufficiently overlap both the parent and child spheres 
			// but yet not exceed the parent sphere's radius
			const RangeX <T> rrng(
				sphere0.r / 2 + (d - sphere0.r - sphere1.r) / 2, 
				std::max(sphere0.r, sphere1.r)
			);
			const T r = rrng.next();

			const T 
				// Scalar and square distances between the sphere centers of the parent sphere and the newly spliced sphere
				br = distance(sphere0.r, r), brsq = SQ(br),

				// Distance between the sphere centers of the newly spliced sphere and the child sphere
				ar = distance(r, sphere1.r),

				// Projection of the vector between the sphere centers of the parent sphere and the newly spliced sphere onto the vector between the parent sphere and the child sphere
				ch = (brsq + dsq - SQ(ar)) / (2*d);

			assert (br + ar >= d);

			// Setup the rotation about the axis defined by the aforementioned vector
			const mars::Quaternion< T > 
				q = mars::Quaternion< T >::rotation(RANDf(PI * 2), mars::U(dp));

			const vector3D< T >
				// First-half: Make an arbitrarily perpendicular vector to the aforementioned axis
				// Second-half: Use the law of cosines to determine the height of the triangle 
				// (this would be the "opposite" shared by two imaginary right-triangles that compose the triangle)
				vb = mars::U(vector3D< T > (dp.y, -(dp.x + dp.z), dp.y)) * sqrt(brsq - SQ(ch)),

				// Apply the rotation
				vbr = q & vb,

				// Form the baseline vector
				vch = mars::U(dp) * ch,

				// Calculate the hypotenuse vector
				// - hypotenuse: distance between sphere centers of parent sphere and newly spliced sphere
				// - opposite: rotated vector
				pp = vch + vbr;

			assert(SQ(MAG(pp) - br) < 0.0001);
			assert(MAG(sphere1.p - (sphere0.p + pp)) < sphere1.r + r);
			assert(d - (MAG(pp) + r + sphere1.r) < 0);

			return mars::ptr< Sphere > (new Sphere (*this, sphere0.p + pp, r));
		}

		void spawn (const Sphere & sphere0, Sphere & sphere, const T nTier)
		{
			using namespace mars;

			static const float pi = static_cast <float> (PI);

			// Next number of breadth kids to generate for this sphere
			const unsigned short amt = _kidflux.next();

			// Allowed radii for children
			const RangeX <float> rads (
				_sizeflux.minimum * static_cast <float> (sphere.r), 
				_sizeflux.maximum * static_cast <float> (sphere.r)
			);

			// Spherical coordinate representation of vector between parent sphere and this current sphere
			const SphericalCoords<T> spc0 (sphere0.p - sphere.p);
			RangeX<float> rrng = naturalArcRange(sphere0, sphere, pi);

			for (unsigned int c = sphere.count(); c < amt; ++c)
			{
				Sphere * neww = sphere.attachCulled(rads, rrng, spc0);

				// Do not recurse if we are as deep as we are allowed to go 
				// or if the size of the sphere is small enough and doesn't need more descendants in this branch
				if (neww != NULL && neww->r > _limit && nTier < _maxtier)
					spawn (sphere, *neww, nTier + 1);
			}
		}

		void subdivide (const Sphere & sphere0, Sphere & sphere)
		{
			using namespace mars;
			const T r = sphere.r;
			const size_t len = sphere.count();
			static const float pi = static_cast <float> (PI);

			// Do not sub-divide this sphere if it is already too small
			if (sphere.r / _divisor >= _limit)
			{
				sphere.r /= _divisor;

				for (typename SphereList::iterator it = sphere.begin(); it != sphere.end(); ++it)
				{
					mars::ptr< Sphere > pSphTail = *it;
					// First divide the current sphere and all successive spheres before adding any new spheres
					subdivide(sphere, *pSphTail);

					// Splice-in a randomly-placed sphere between this current sphere and the iterator current sphere
					mars::ptr< Sphere > pSphere1 = spawnBetween(sphere, *pSphTail);

					spawnImmediates(*pSphere1, sphere, *pSphTail);

					pSphere1->attach(pSphTail);
					*it = pSphere1;

					// Generate allowed radii for additional children of the parent of the spliced sphere
					// Must come after sphere.r /= 2 because it depends on the new radius
					const RangeX <T> rads (
						_sizeflux.minimum * static_cast <float> (pSphTail->r), 
						_sizeflux.maximum * static_cast <float> (pSphTail->r)
					);

					// Spherical coordinate representation of vector between parent sphere and this current sphere
					const SphericalCoords<T> spc0 (pSphTail->p - pSphere1->p);
					RangeX<float> rrng = naturalArcRange(*pSphTail, *pSphere1, pi);

					// Add only as many new children as the original conditions permit for a new random quantity
					for (size_t n = _kidflux.next(); n > len; --n)
					{
						pSphTail->attachCulled(rads, rrng, spc0);
					}
				}
			}
		}
		void spawnImmediates( Sphere & sphere, Sphere & sphere0, Sphere & sphere1) 
		{
			using namespace mars;
			const short kfn = _kidflux.next();

			// Add new children if and only if the new random amount is within the bounds and accounts for the spliced one's descendant
			if (kfn > _kidflux.minimum)
			{
				// Accounts for the spliced one's descendant
				const unsigned short amt = kfn - 1;

				// Allowed radii for children
				const RangeX <T> rads (
					_sizeflux.minimum * static_cast <float> (sphere.r), 
					_sizeflux.maximum * static_cast <float> (sphere.r)
				);

				const SphericalCoords<T> spc (sphere0.p - sphere.p);

				// Allowed range where neither the descendant nor ancestor intersect with this current sphere
				const RangeX<float> rrng (
					intersectAngle(sphere, sphere1),
					PI - intersectAngle(sphere0, sphere),
					0.02f / 2.0f
				);

				// Adds children to the spliced sphere
				for (unsigned int c = 0; c < amt; ++c)
				{
					const T r = static_cast <T> (rads.next());
					SphericalCoords <T> spc1(
						r, 
						rrng.next() * 2.0f, 
						rrng.next() * 2.0f
						);

					// Since the two allowed ranges are disjoint, we compensate for the non-linear issue
					if (spc1.azimuth > rrng.maximum)
						spc1.azimuth = rrng.maximum - spc1.azimuth;
					if (spc1.zenith > rrng.maximum)
						spc1.zenith = rrng.maximum - spc1.zenith;

					// The closer to the intersections the child is, the smaller it becomes
					const float f = Sphere::computeCull(spc1.azimuth, spc1.zenith);

					spc1.r *= (f + (1.0f - f)) / 2.0f;

					// Attach the child
					if (spc1.r >= _minimum)
					{
						sphere.attach(
							spc.zenith + spc1.zenith, 
							spc.azimuth + spc1.azimuth, 
							spc1.r / 2,
							spc1.r
						);
					}
				}
			}
		}

		inline mars::RangeX<float> naturalArcRange( const Sphere & sphere0, Sphere & sphere, const float pi ) const
		{
			// Depending on radii of the two spheres (parent and current), determine the angle of the arc spanning the intersection of the two spheres
			const float deg = (sphere0 == sphere ? 0 : intersectAngle(sphere0, sphere));

			// Allowed angular range dispositions for children
			return mars::RangeX<float> (-pi + deg, +pi - deg, 0.02f);
		}

		template <typename I>
		struct StackInfo
		{
			I i, end;

			inline StackInfo(const I & begin, const I & end)
				: i(begin), end(end) {}
		};

	public:
		template< typename TRAITS = ConstIteratorTraits >
		class ConstIterator : public std::iterator< std::input_iterator_tag, Sphere >
		{
        protected:
   			typename TRAITS::SphereType * _spCurr;

		private:
			typedef typename TRAITS::SphereListIterator SphereListIterator;

			typename TRAITS::SpherePack * const _pack;
			SphereListIterator _it;
			StackInfo< SphereListIterator > * _stCurr;
			std::stack< StackInfo< SphereListIterator > > _stack;

			CR_CONTEXT;

			void process()
			{
				CR_START();

				_stack.push(StackInfo< SphereListIterator > (_pack->_toptier.begin(), _pack->_toptier.end()));
				while (!_stack.empty())
				{
					_stCurr = & _stack.top();

					while (_stCurr->i != _stCurr->end)
					{
						_spCurr = & **_stCurr->i++;

						CR_RETURN_VOID;

						if (_spCurr->begin() != _spCurr->end())
						{
							_stack.push(StackInfo< SphereListIterator > (_spCurr->begin(), _spCurr->end()));
							_stCurr = & _stack.top();
							continue;
						}
					}

					_stack.pop();
				}

				_spCurr = NULL;

				CR_END();
			}

		public:
			ConstIterator (typename TRAITS::SpherePack * const pack)
				: _pack(pack)
			{
				CR_INIT();
				process();
			}
			inline operator bool () { return _spCurr != NULL; }
			inline ConstIterator & operator++()
			{
				process();
				return *this;
			}
			inline ConstIterator operator++(int)
			{
				ConstIterator old = *this;
				process();
				return old;
			}
			inline const Sphere * operator -> () const { return &(*_spCurr); }
			inline const Sphere & operator * () const { return *_spCurr; }
		};

		class Iterator : public ConstIterator< IteratorTraits >
		{
		public:
			Iterator (typename IteratorTraits::SpherePack * const pack)
				: ConstIterator< IteratorTraits > (pack) {}
			
			inline Sphere * operator -> () { return &(*this->_spCurr); }
			inline Sphere & operator * () { return *this->_spCurr; }
		};

		typedef Iterator iterator;
		typedef ConstIterator< ConstIteratorTraits > const_iterator;

	public:
		inline RandomSpherePack (
			const T limit,		// Size below-which spheres are no-longer generated, essentially characterizes the degree of refined detail
			const T minimum,	// The minimum size of a sphere (usually just 1.0)
			const T maxtier,	// The maximum number depth-iterations (as opposed to breadth), this also affects the level of detail
			const mars::RangeX<float> & sizeflux, // Flux of size of spheres at starting tier
			const mars::RangeX<unsigned short> & kidflux	// Flux of number of breadth-iterations (as opposed to depth),
		)
			: _sizeflux(sizeflux), _kidflux(kidflux), _limit(limit), _minimum(minimum), _maxtier(maxtier),
			// Numeric stability will result if (iff?) divisor is not less than 2 because the gap between sub-divided spheres would be too great
			_divisor(4.0f / 3.0f) 
		{}

		inline Sphere createEmptySphere () const
			{ return Sphere(this); }

		inline const Sphere & spawn (const VECTOR & pt, const T size)
		{
			assert (size >= _minimum);

			_toptier.push_back(
				mars::ptr <Sphere> (
					new Sphere(
						this,
						pt, 
						static_cast <T> (static_cast <float> (size / _sizeflux.minimum) * _sizeflux.next())
					)
				)
			);

			return * _toptier.back();
		}

		inline const Sphere & spawn (const float zenith, const float azimuth, const T size)
		{
			assert (size >= _minimum);

			const T r = static_cast <T> (static_cast <float> (size / _sizeflux.minimum) * _sizeflux.next());
			const Sphere & neighbor = *_toptier.back();

			_toptier.push_back(
				mars::ptr <Sphere> (
					new Sphere(
						this,
						neighbor.p + mars::SphericalCoords< T > (distance (neighbor.r, r), zenith, azimuth), 
						r
					)
				)
			);

			return * _toptier.back();
		}

		inline void generateDescendants ()
		{
			for (typename SphereList::iterator it = _toptier.begin(); it != _toptier.end(); ++it)
				spawn(**it, **it, 1);
		}
		inline void subdivide ()
		{
			if (_limit / _divisor >= _minimum)
				_limit /= _divisor;
			for (typename SphereList::iterator it = _toptier.begin(); it != _toptier.end(); ++it)
				subdivide(**it, **it);

			typename SphereList::iterator itTail, itHead;

			itHead = _toptier.begin();
			itTail = itHead++;

			while (itHead != _toptier.end())
			{
				mars::ptr< Sphere > pSphereA = spawnBetween(**itTail, **itHead);

				spawnImmediates(*pSphereA, **itTail, **itHead);
				_toptier.insert (itHead, pSphereA);

				itTail = itHead++;
			}
		}
		inline iterator iterate () { return iterator(this); }
		inline const_iterator iterate () const { return const_iterator(this); }
		inline unsigned int count () const
		{
			std::stack< StackInfo< SphereListConstIterator > > stack;
			unsigned int c = 1;

			stack.push(StackInfo< SphereListConstIterator > (_toptier.begin(), _toptier.end()));

			while (!stack.empty())
			{
				StackInfo< SphereListConstIterator > & curr = stack.top();

				while (curr.i != curr.end)
				{
					const Sphere & sphere = **curr.i;

					++c;
					++curr.i;
					if (sphere.begin() != sphere.end())
					{
						stack.push(curr);
						curr = StackInfo< SphereListConstIterator >(sphere.begin(), sphere.end());
						continue;
					}
				}

				stack.pop();
			}
			return c;
		}

		template< typename PTR, typename R, typename G >
		void store (mars::LookupTree< typename PTR::Type, R, G > & store, const PTR & pElem) const
		{
			for (typename SphereList::const_iterator it = _toptier.begin(); it != _toptier.end(); ++it)
				(*it)->store(store, pElem);
		}

		// TODO: Find a purpose for this method, may not be necessary, was primordially conceived
		template< typename E, typename R, typename G, typename QR >
		float probability (mars::LookupTree< E, R, G > & store, const QR & search, E * = NULL) const
		{
			using namespace mars;
			float f = 0.0f;

			for (typename mars::LookupTree< E, R, G >::EntityRadialIterator it = store.contents(search); it.hasMore(); ++it)
			{
				const R & region = it.region();
				const float ff = GaussianFn<float>::f(MAGSQ(region.p - region.p), 1.0, SQ(region.r), 0, 1.0f, 0);

				if (ff > f)
					f = ff;
			}
			return f;
		}
	};
}

#endif