#pragma once

#include <limits>

#include "linalgadapt.h"

#include <mars_calc.h>
#include <mars_splines.h>
#include <mars_geometry.h>
#include <mars_util.h>
#include <mars_ptr.h>
#include <mars_streams.h>

namespace fldyn
{
	template <class Derived, typename DP, typename HMPrec>
	class IQueryBridge
	{
	public:
		typedef DP DensityPrecision;

		inline DensityPrecision density (const unsigned int x, const unsigned int y) const
			{ return static_cast <const Derived *> (this) -> density(x, y, 0); }

		inline DensityPrecision density (const unsigned int x, const unsigned int y, const unsigned int z) const
			{ return static_cast <const Derived *> (this) -> density(x, y, z); }

		inline HMPrec elevation (const unsigned int x, const unsigned int y) const
			{ return static_cast <const Derived *> (this) -> elevation(x, y); }
	};

	class Weights
	{
	public:
		float 
			fwStraightness,
			fwElevationInfluence,
			fwDensityInfluence,
			fwChaosInfluence,
			fWeightSum;

		inline Weights () : fwStraightness(0), fwElevationInfluence(0), fwDensityInfluence(0), fwChaosInfluence(0), fWeightSum(0) {}
		inline Weights (
				const float fwStraightness, const float fwElevationInfluence, 
				const float fwDensityInfluence, const float fwChaosInfluence
		) : fwStraightness(fwStraightness), fwElevationInfluence(fwElevationInfluence),
			fwDensityInfluence(fwDensityInfluence), fwChaosInfluence(fwChaosInfluence),
			fWeightSum(fwStraightness + fwElevationInfluence + fwDensityInfluence + fwChaosInfluence)
		{}
	};
	static inline mars::ObjectStream & operator >> (mars::ObjectStream & ins, Weights & weights)
	{
		return ins >> weights.fwStraightness >> weights.fwElevationInfluence >> weights.fwDensityInfluence >> weights.fwChaosInfluence >> weights.fWeightSum;
	}
	static inline mars::ObjectStream & operator << (mars::ObjectStream & outs, const Weights & weights)
	{
		return outs << weights.fwStraightness << weights.fwElevationInfluence << weights.fwDensityInfluence << weights.fwChaosInfluence << weights.fWeightSum;
	}

	class FluiDynEx : public std::exception
	{
	};

	template <class QB, typename T, typename H>
	class SplineCPCandidate;

	template <class QB, typename T, typename H>
	class GuidedSplineCPCandidate;

	template <class QB, typename T, typename H>
	class FlowLine;

	template <typename QB, typename T, typename H>	// QB, Control point type, spline precision type
	struct FluiDynTraits
	{
		typedef QB QueryBridgeType;						// Bridge pattern for querying the world
		typedef typename QB::DensityPrecision QBDensityPrec;		// Precision used for sampling density
		typedef SplineCPCandidate<QB, T, H> BasicCP;													// Control-point type
		typedef GuidedSplineCPCandidate<QB, T, H> GuidedCP;
		typedef typename VectorTag< T >::V2 Vector2;
		typedef typename VectorTag< T >::V3 Vector3;
		typedef typename VectorTag< T >::PC PolarCoords;
		typedef typename VectorTag< T >::CC CylindricalCoords;
		typedef T Precision;										// Precision of the n-D vector type previously mentioned
		typedef H HMPrec;
		typedef mars::Spline< mars::CubicBSplineBase <Vector2, Precision> > SplineType;	// BBox computation depends on the Cubic B-Spline algorithm
		typedef mars::TreeGuideNode< FlowLine <QB, T, H>, Vector3, T > GuideType;		// Guide type used as the base
		typedef FlowLine<QB, T, H> FlowLineType;

        using SphericalCoords = typename mars::SphericalCoords< T >;
    };

	template< typename T >
	struct StartPoint
	{
		typename VectorTag< T >::V3 position;
		T direction;

		StartPoint () 
			: direction(0) {}
		StartPoint (typename VectorTag< T >::V3 position, T direction)
			: position(position), direction(direction) {}
	};
	template< typename T >
	mars::ObjectStream & operator >> (mars::ObjectStream & ins, StartPoint< T > & sp)
	{
		return ins >> sp.position >> sp.direction;
	}

	template< typename T >
	mars::ObjectStream & operator << (mars::ObjectStream & outs, const StartPoint< T > & sp)
	{
		return outs << sp.position << sp.direction;
	}

	template <class QB, typename T, typename H>		// QB, Algebraic vector type
	class SplineCPCandidate : public FluiDynTraits<QB, T, H>::Vector3, public FluiDynTraits<QB, T, H>
	{
	private:

		template <typename J>
		inline J nonzero (const J n) const
		{
			if (n == 0)
				return std::numeric_limits< J >::epsilon();
			else
				return n;
		}

	protected:
        using typename FluiDynTraits<QB, T, H>::QBDensityPrec;
        using typename FluiDynTraits<QB, T, H>::HMPrec;
        using typename FluiDynTraits<QB, T, H>::Vector2;
        using typename FluiDynTraits<QB, T, H>::Vector3;
        using typename FluiDynTraits<QB, T, H>::CylindricalCoords;
        using typename FluiDynTraits<QB, T, H>::GuideType;
        using typename FluiDynTraits<QB, T, H>::FlowLineType;
        using typename FluiDynTraits<QB, T, H>::BasicCP;
        using typename FluiDynTraits<QB, T, H>::GuidedCP;
        using typename FluiDynTraits<QB, T, H>::PolarCoords;
        
		QBDensityPrec _maxDens;
		HMPrec _maxElev, _minElev;
		float _fDot;

	public:
		inline explicit SplineCPCandidate (const Vector3 & p) 
			: Vector3(p), _maxDens(0), _minElev(0), _maxElev(0), _fDot(0) {}

		inline SplineCPCandidate (
			const Vector3 & p1, const Vector3 & p0, const Vector3 & dp0,
			const mars::ptr< QB > & query
			) : Vector3(p1)				
		{
			const Vector3 & 
				dp = p1 - p0,
				pM = dp / 2 + p0;

			// TODO: Check for negative/signed values of p1 and pM
			_maxDens = std::max(
				query->density(static_cast <unsigned int> (p1.x), static_cast <unsigned int> (p1.y)),
				query->density(static_cast <unsigned int> (pM.x), static_cast <unsigned int> (pM.y))
			);

#ifdef _DEBUG
			if (_maxDens == 0)
				throw FluiDynEx();
#endif

			const unsigned short 
				nElevTerm = query->elevation(static_cast <unsigned int> (p1.x), static_cast <unsigned int> (p1.y)),
				nElevMid = query->elevation(static_cast <unsigned int> (pM.x), static_cast <unsigned int> (pM.y));

			if (nElevTerm > nElevMid)
			{
				_maxElev = nonzero(nElevTerm);
				_minElev = nElevMid;
			} else
			{
				_maxElev = nonzero(nElevMid);
				_minElev = nElevTerm;
			}

			_fDot = dp0 * dp;
#ifdef _DEBUG
			if (_fDot == 0)
				throw FluiDynEx();
#endif
		}

		float compareTo (const SplineCPCandidate & other, const float fStuckFactor, const Weights & weights) const
		{
			return
				static_cast <float> (_maxElev) / static_cast <float> (other._maxElev) * weights.fwElevationInfluence +
				static_cast <float> (_maxDens / other._maxDens * weights.fwDensityInfluence) +
				other._fDot / _fDot * weights.fwStraightness;					
		}

		inline const QBDensityPrec & getDensity () const { return _maxDens; }
		inline const HMPrec & getMaxElevation () const { return _maxElev; }
		inline const HMPrec & getMinElevation () const { return _minElev; }
		inline const float & getStraightness () const { return _fDot; }
	};

	template <class QB, typename T, typename H>		// QB, Guide parent type, Algebraic vector type
	class GuidedSplineCPCandidate : public SplineCPCandidate <QB, T, H>
	{
	protected:
        using typename FluiDynTraits<QB, T, H>::QBDensityPrec;
        using typename FluiDynTraits<QB, T, H>::HMPrec;
        using typename FluiDynTraits<QB, T, H>::Vector2;
        using typename FluiDynTraits<QB, T, H>::Vector3;
        using typename FluiDynTraits<QB, T, H>::CylindricalCoords;
        using typename FluiDynTraits<QB, T, H>::GuideType;
        using typename FluiDynTraits<QB, T, H>::FlowLineType;
        using typename FluiDynTraits<QB, T, H>::BasicCP;
        using typename FluiDynTraits<QB, T, H>::GuidedCP;
        using typename FluiDynTraits<QB, T, H>::PolarCoords;

        typename GuideType::DistanceType _gdist;

	public:			
		inline GuidedSplineCPCandidate (
			const Vector3 & p1, const Vector3 & p0, const Vector3 & dp0, const typename GuideType::DistanceType & gdist,
			const mars::ptr< QB > & query, const GuideType & guide
			) : SplineCPCandidate<QB, T, H> (p1, p0, dp0, query), _gdist(gdist) {}

		float compareTo (const GuidedSplineCPCandidate & other, const float fStuckFactor, const Weights & weights) const
		{
			return
				SplineCPCandidate<QB, T, H>::compareTo(other, fStuckFactor, weights) +
				_gdist.distance / other._gdist.distance * weights.fwChaosInfluence * (fStuckFactor + 1); // XTODO: Boosts the anti-chaos influence by a straight factor of how many times the same guide segment was successively selected, this may be overly simplistic, see line below as well
		}

		const typename FlowLineType::SegmentType & getNearestGuideSegment () const { return _gdist.segment; }
		const float & getGuideDistance () const { return _gdist; }
	};

	template <class QB, typename T, typename H>	// QB, List of algebraic vector type, spline precision type
	class FlowLine : public FluiDynTraits<QB, T, H>::GuideType, public FluiDynTraits<QB, T, H>
	{
    protected:
        using typename FluiDynTraits<QB, T, H>::QBDensityPrec;
        using typename FluiDynTraits<QB, T, H>::HMPrec;
        using typename FluiDynTraits<QB, T, H>::Vector2;
        using typename FluiDynTraits<QB, T, H>::Vector3;
        using typename FluiDynTraits<QB, T, H>::CylindricalCoords;
        using typename FluiDynTraits<QB, T, H>::GuideType;
        using typename FluiDynTraits<QB, T, H>::FlowLineType;
        using typename FluiDynTraits<QB, T, H>::BasicCP;
        using typename FluiDynTraits<QB, T, H>::GuidedCP;
        using typename FluiDynTraits<QB, T, H>::PolarCoords;
        using typename FluiDynTraits<QB, T, H>::SplineType;
        using typename FluiDynTraits<QB, T, H>::Precision;
        using typename GuideType::DistanceType;

	public:
		template <typename R, typename E1, typename E2>
		class Iterator : public mars::SplineZoneIterator<
			SplineType,
			E2, E1,
			R,												// The raster precision
			typename VectorTag<R>::V2,								// The raster version of the output vector type
			typename VectorTag<Precision>::V3						// Used for "unormal"
		>
		{
		private:
			const FlowLineType & _parent;

			inline Iterator (const FlowLineType & parent, const E1 & fnEaseIn, const E2 & fnEaseOut)
				: mars::SplineZoneIterator<SplineType, E2, E1, R, typename VectorTag<R>::V2, typename VectorTag<Precision>::V3 > (parent.getSpline(), parent.radius, fnEaseOut, fnEaseIn, parent.unormal), _parent (parent) {}

			friend FlowLineType;
		public:

			inline typename SplineType::VectorType stress () const
				{ return (this->laplace() - this->gradient()) / this->seglen(); }
		};

	public:
		const unsigned int radius;
		const typename VectorTag<Precision>::V3 unormal;

		FlowLine (const typename GuideType::PointList & points, const unsigned int radius, const typename VectorTag<Precision>::V3 & unormal)
			: GuideType (points), _spline (SplineType::template createFrom <typename GuideType::PointList> (points.begin(), points.end())), radius(radius), unormal(unormal) {}

		FlowLine (const GuideType & parent, const typename GuideType::PointList & points, const unsigned int radius, const typename VectorTag<Precision>::V3 & unormal)
			: GuideType (points, parent), _spline (SplineType::template createFrom <typename GuideType::PointList> (points.begin(), points.end())), radius(radius), unormal(unormal) {}

		template <typename R, typename E1, typename E2>
		inline Iterator< R, E1, E2 > iterate (const E1 & fnEaseIn = E1(), const E2 & fnEaseOut = E2()) const
		{
			return Iterator< R, E1, E2 > (*this, fnEaseIn, fnEaseOut);
		}

		// Computes the distance between this flow-line and an arbitrary point
		inline float sdist (const Vector2 & p) const 
		{ 
			DistanceType dist = *this - p;
			return _spline.compute(dist.segment.index(), dist.time()); 
		}

		inline typename SplineType::real getLength () const { return _spline.len(); }
		inline const SplineType & getSpline () const { return _spline; }

		void offsetBy (const Vector3 & p)
		{
			GuideType::offsetBy(p);
			_spline += p;
		}

	private:
		SplineType _spline;
	};

	template <class QB, typename T, typename H>
	class PathFinder : public FluiDynTraits< QB, T, H >
	{
    public:
        using typename FluiDynTraits<QB, T, H>::FlowLineType;

	private:
        using typename FluiDynTraits<QB, T, H>::BasicCP;
        using typename FluiDynTraits<QB, T, H>::GuidedCP;
        using typename FluiDynTraits<QB, T, H>::Vector2;
        using typename FluiDynTraits<QB, T, H>::Vector3;
        using typename FluiDynTraits<QB, T, H>::GuideType;
        using typename FluiDynTraits<QB, T, H>::CylindricalCoords;
        using typename FluiDynTraits<QB, T, H>::Precision;
        using typename FluiDynTraits<QB, T, H>::SphericalCoords;

        typedef std::list <BasicCP> CandidatesList;
		typedef std::list <GuidedCP> GuidedCandidatesList;

		float 
			_frSnapToGuide,
			_fMaxSegLenSQ,
			_fGravitySlopeGrace;

		mars::RangeX< float >		
			_mmfSplineSeg,
			_mmfBendZenith;

		unsigned int			
			_nCandidateSampleAmt,
			_nBendZenithSteps;

		mars::BBox< T >
			_bbox;

		Vector3 _vun3;
		Weights _weights;

		mars::ptr< QB > _query;

		static Vector3 createUnitZ () { return Vector3(0, 0, 1); }

		inline BasicCP createSplineCPCandidate(
			const Vector3 & p1, const Vector3 & p0, const Vector3 & dp0
			) const { return BasicCP(p1, p0, dp0, _query); }

		inline GuidedCP createGuidedSplineCPCandidate (
			const Vector3 & p1, const Vector3 & p0, const Vector3 & dp0, const typename GuideType::DistanceType & gdist, const GuideType & guide
			) const { return GuidedCP(p1, p0, dp0, gdist, _query, guide); }

		template <typename CL> typename CL::const_iterator findBest (const CL & candidates, const unsigned int nStuckCount) const
		{
			typename CL::const_iterator io = candidates.begin();
			const float fStuckFactor = static_cast <float> (nStuckCount);

			for (typename CL::const_iterator i = candidates.begin(); i != candidates.end(); ++i)
			{
				if (i->compareTo(*io, fStuckFactor, _weights) / (_weights.fWeightSum + fStuckFactor) < 1.0)	// Factoring in the stuck factor
				{
					io = i;
				}
			}
			return io;
		}
		
		template <typename CaDList>
		inline void candidatesReset (CaDList & candidates, float & zenith) const
		{
			using namespace mars;

			candidates.clear();
			zenith = _mmfBendZenith.minimum;
		}
		inline void initCandidateSearch (
			Vector3 & pCurr,
			CylindricalCoords & ccDir, CylindricalCoords & ccDir0, CylindricalCoords & ccTest, 
			typename GuideType::PointList & points
		) const
		{
			using namespace mars;

			ccDir.z =
			ccDir0.z = 
			pCurr.z = _query->elevation(
				static_cast< unsigned int > (mars::RNDi (pCurr.x)), 
				static_cast< unsigned int > (mars::RNDi (pCurr.y))
			);

			ccDir0.azimuth = 
			ccTest.azimuth = ccDir.azimuth;

			points.push_back(BasicCP(pCurr));
		}

		inline bool computeCandidate (
			const float zenith, const float pstep, const Vector3 & pCurr,
			CylindricalCoords & ccTest, CylindricalCoords & ccDir0, Vector3 & pTest
		) const
		{
			using namespace mars;

			// Calculate a candidate position at some random point within an angular threshold of the preset flow direction
			ccTest.azimuth = ccDir0.azimuth + RANDf(zenith) - zenith / 2.0f;
			ccTest.p = pstep + _mmfSplineSeg.minimum;

			// Add the candidate point to the current point plus the density vector for a test
			// Actually what does this do anyway again?
				//+ _query->density(static_cast <HMPrec> (posTest.x), static_cast <HMPrec> (posTest.y));

			pTest = pCurr + ccTest;

			pTest.z = 
			ccTest.z = static_cast< float > (
				_query->elevation(
					mars::RNDi(pTest.x),	// TODO: Verify use of RNDi
					mars::RNDi(pTest.y)		// TODO: Verify use of RNDi
				)
			);

			return 
				static_cast< Precision > (pTest.z - pCurr.z)
				/ ccTest.p
				< _fGravitySlopeGrace;
		}

		inline bool isBBoxApproved (const T x, const T y, const T fRadius) const
			{ return _bbox.inside(x - fRadius, y - fRadius) && _bbox.inside(x + fRadius, y + fRadius); }

		void pathFind( const Vector2 & p, const float fDir, typename GuideType::PointList & points, const T fRadius ) const
		{
			using namespace mars;

			CandidatesList candidates;
			typename CandidatesList::const_iterator iCandidate;
			Vector3 posTest, pCurr;
			CylindricalCoords
				ccDir (1, fDir, 0),
				ccTest,	// TODO: Make dirLast a scalar cuz it's only for angle 
				ccDir0 = ccDir; // Assign a non-zero magnitude initializer to avoid division by zero problems later
			// Calculate maximum segment length
			const float pstep = static_cast< float > (_mmfSplineSeg.delta);

			pCurr.x = p.x;
			pCurr.y = p.y;
			initCandidateSearch(pCurr, ccDir, ccDir0, ccTest, points);

			float zenith;
			unsigned short iz;
			do 
			{
				candidatesReset(candidates, zenith);

				for (iz = 0; iz < _nBendZenithSteps && candidates.size() < _nCandidateSampleAmt; ++iz, zenith += _mmfBendZenith.step)
				{
					if (computeCandidate(zenith, pstep, pCurr, ccTest, ccDir0, posTest) && // Returns false when elevation slopes upwards
						isBBoxApproved(posTest.x, posTest.y, fRadius)/* && (posTest.x != pCurr.x || posTest.y != pCurr.y) */)	// ccTest.p > 0 assumption guarantees the previous check is unnecessary
					{
						candidates.push_back(createSplineCPCandidate(posTest, pCurr, ccDir0));
					}
				}

				iCandidate = findBest (candidates, 0);

				if (iCandidate != candidates.end())
				{
					const BasicCP & best = *iCandidate;
					points.push_back(best);
					ccDir0 = static_cast< Vector3 > (best - pCurr);
					pCurr = best;
				}
			} while (candidates.size() >= _nCandidateSampleAmt); // XTODO: Superficially tested: Tests that the distance between the current position and the end of the guide is still large enough
		}

		void pathFind( const FlowLineType & fl, typename GuideType::PointList & points, const T fRadius ) const
		{
			using namespace mars;
            using PolarCoords = typename VectorTag< T >::PC;
            
			GuidedCandidatesList candidates;
			typename GuidedCandidatesList::const_iterator iCandidate;
			Vector3 posTest, pCurr, pDir = fl.back() - fl.front();
			CylindricalCoords 
				ccDir = pDir,
				ccTest, // TODO: Make dirLast a scalar cuz it's only for angle
				ccDirPrev = ccDir; // Assign a non-zero magnitude initializer to avoid division by zero problems later
			typename FlowLineType::SegmentType igseg = fl.begin();	// Tracks the last accepted guide segment head
			unsigned int nStuckCount = 0;
			bool bExitCondDeviantChild = false;

			pCurr = _bbox.template clampXY< typename Vector3::Precision > (fl.front() + (mars::U(pDir) & _vun3 * RANDf(std::min(_mmfSplineSeg.maximum, MAG(fl.grad(fl.begin()))))));
			initCandidateSearch(pCurr, ccDir, ccDirPrev, ccTest, points);

			unsigned short iz;
			float zenith;
			do 
			{
				// Get the gradient for the current guide segment
				const PolarCoords ccGuideSegDir = static_cast<Vector2> (fl.grad(igseg));
				// Calculate maximum segment length relating to the current guide segment
				const float pstep = std::min(static_cast< float > (_mmfSplineSeg.delta), ccGuideSegDir.p - _mmfSplineSeg.minimum);
				const float pstepSQ = SQ(pstep);

				// TODO: Optimize angle calculation
				//ccGuideSegDir.azimuth = PolarCoords (guide.tail(igseg) - pCurr).azimuth;

				candidatesReset <GuidedCandidatesList> (candidates, zenith);
				
				for (iz = 0; iz < _nBendZenithSteps && candidates.size() < _nCandidateSampleAmt; ++iz, zenith += _mmfBendZenith.step)
				{
					if (computeCandidate(zenith, pstep, pCurr, ccTest, ccDirPrev, posTest) && // Returns false when elevation slopes upwards
						isBBoxApproved(posTest.x, posTest.y, fRadius)
						) /*&& (posTest.x != pCurr.x || posTest.y != pCurr.y) (See above)*/
					{
						// Calculate median point between candidate position and source position
						typename GuideType::DistanceType gdist = fl.distance (posTest, igseg);
						const Vector3 pTail = fl.tail(gdist.segment);

						if (MAGSQ(posTest - pTail) / pstepSQ < _frSnapToGuide)
							posTest = pTail;

						candidates.push_back(
							createGuidedSplineCPCandidate(posTest, pCurr, ccDirPrev, gdist, fl)
						);
					}
				}

				iCandidate = findBest (candidates, nStuckCount);

				if (iCandidate != candidates.end())
				{
					const GuidedCP & best = *iCandidate;
					points.push_back(best);
					ccDirPrev = static_cast< Vector3 > (best - pCurr);
					pCurr = best;

					// If the nearest guide segment was the same as last time, then increase the stuck count
					// otherwise the guide segment was incremented and we're not stuck
					if (igseg != best.getNearestGuideSegment())
					{
						igseg = best.getNearestGuideSegment();
						nStuckCount = 0;
					} else
					{
						++nStuckCount;
					}
				}
				bExitCondDeviantChild = MAGSQ(static_cast <Vector3> (pCurr - fl.back())) > _fMaxSegLenSQ;
			} while (bExitCondDeviantChild && candidates.size() >= _nCandidateSampleAmt); // XTODO: Superficially tested: Tests that the distance between the current position and the end of the guide is still large enough

			if (!bExitCondDeviantChild)
				points.push_back(fl.back()); // HACK: Just add the guide's last point as this last point in the path find
		}

	public:
		PathFinder () 
			: _frSnapToGuide(0), _fMaxSegLenSQ(0), _fGravitySlopeGrace(0), _nCandidateSampleAmt(0), _nBendZenithSteps(0), _vun3(createUnitZ())
		{}

		PathFinder (
			const mars::RangeX< float > & mmfBendZenith,
			const unsigned int nBendZenithSteps,
			const unsigned int nCandidateSampleAmt,
			const mars::RangeX< float > & mmfSplineSeg,
			const float fGravitySlopeGrace,
			const float frSnapToGuide,
			const float fwStraightness,
			const float fwElevationInfluence,
			const float fwDensityInfluence,
			const float fwChaosInfluence
		) : _mmfBendZenith(mars::RangeX< float > (mmfBendZenith.minimum, mmfBendZenith.maximum, static_cast< float > (mmfBendZenith.delta / static_cast <float> (nBendZenithSteps)))),
		_nBendZenithSteps(nBendZenithSteps), _nCandidateSampleAmt (nCandidateSampleAmt), _mmfSplineSeg(mmfSplineSeg), _fGravitySlopeGrace(fGravitySlopeGrace), 
		_frSnapToGuide(frSnapToGuide),
		_weights(fwStraightness, fwElevationInfluence, fwDensityInfluence, fwChaosInfluence),
		_fMaxSegLenSQ (mars::SQ(mmfSplineSeg.maximum)), _vun3(createUnitZ())
		{}

		inline const mars::RangeX< float > & getBendZenith () const { return _mmfBendZenith; }
		inline unsigned int getBendZenithSteps () const { return _nBendZenithSteps; }
		inline unsigned int getCandidateSampling () const { return _nCandidateSampleAmt; }
		inline const mars::RangeX< float > & getSplineSegment () const { return _mmfSplineSeg; }
		inline float getGravitySlopeGrace () const { return _fGravitySlopeGrace; }
		inline float getSnapToGuide () const { return _frSnapToGuide; }
		inline const Weights & getWeights () const { return _weights; }

		inline FlowLineType * find (const Vector2 & p, const float fDir, const unsigned int nRadius) const
		{
			typename GuideType::PointList points;
			
			pathFind(p, fDir, points, static_cast< T > (nRadius));
			if (points.size() < 2)
				return NULL;
			else
				return new FlowLineType(points, nRadius, _vun3);
		}

		inline FlowLineType * find (const FlowLineType & guide, const unsigned int nRadius) const
		{
			typename GuideType::PointList points;

			pathFind(guide, points, static_cast< T > (nRadius));
			if (points.size() < 2)
				return NULL;
			else
				return new FlowLineType(guide, points, nRadius, _vun3);
		}

		inline void setBBoxLimit (const mars::BBox< T > & bbox)
		{
			_bbox = bbox;
		}

		inline void setQueryCallback (const mars::ptr< QB > & pqb)
		{
			_query = pqb;
		}

		inline mars::ObjectStream & operator >> (mars::ObjectStream & outs) const
		{
			return outs << _mmfBendZenith << _nBendZenithSteps << _nCandidateSampleAmt << _mmfSplineSeg << _fGravitySlopeGrace << _frSnapToGuide << _weights;
		}
		inline mars::ObjectStream & operator << (mars::ObjectStream & ins)
		{
			ins >> _mmfBendZenith >> _nBendZenithSteps >> _nCandidateSampleAmt >> _mmfSplineSeg >> _fGravitySlopeGrace >> _frSnapToGuide >> _weights;
			_fMaxSegLenSQ = mars::SQ(_mmfSplineSeg.maximum);
			return ins;
		}
	};

	template <class QB, typename T, typename H, typename DS>	
	class ChannelSet : public FluiDynTraits< QB, T, H >
	{
	public:
        using typename FluiDynTraits<QB, T, H>::FlowLineType;
        using typename FluiDynTraits<QB, T, H>::Precision;
        using typename FluiDynTraits<QB, T, H>::GuideType;
        using typename FluiDynTraits<QB, T, H>::Vector2;
        using typename FluiDynTraits<QB, T, H>::Vector3;

		typedef std::list < mars::ptr< FlowLineType > > GuideList;
		typedef PathFinder< QB, T, H > MyPathFinder;
		typedef std::list < StartPoint< T > > StartingPoints;

	protected:
		virtual void applyFlowLine (const FlowLineType & fl, DS * pData) const = 0;
		virtual void finishedTier (const GuideList & guides, DS * pData) const {}

	private:
		mars::RangeX< float >
			_mmfFlowlineWidth;

		mars::RangeX< unsigned int>
			_mmnKidsPerLevel;

		StartingPoints _starts;
		MyPathFinder _finder;
		GuideList _guides;
		mars::BBox< Precision > _bbox;

		void computeBBox ()
		{
			using namespace mars;

			GuideList tier1, tier2, 
				* pTierSrc = &tier1, 
				* pTierKids = &tier2;

			*pTierSrc = _guides;
			_bbox = BBox< Precision >();
			while (!pTierSrc->empty())
			{
				pTierKids->clear();
				for (typename GuideList::iterator i = pTierSrc->begin(); i != pTierSrc->end(); ++i)
				{
					for (typename GuideType::SegmentType k = (*i)->begin(); k != (*i)->end(); ++k)
					{
						const Vector2 
							v1 = *k - static_cast< float > ((*i)->radius),
							v2 = *k + static_cast< float > ((*i)->radius);

						if (_bbox.empty())
							_bbox = BBox< Precision > (v1, v2);
						else
						{
							_bbox.add(v1);
							_bbox.add(v2);
						}
					}
					for (typename GuideType::KidList::iterator j = (*i)->kidsBegin(); j != (*i)->kidsEnd(); ++j)
						pTierKids->push_back(*j);
				}
				std::swap(pTierKids, pTierSrc);	// Flip the guide lists
			}
		}

		// Traverses up the ancestry of flow-lines starting at the specified one and returns the ancestral flowline
		// that is closest to the specified point based on an approximated spline calculation
		const FlowLineType * closestFlowLine (const Vector3 & p, const FlowLineType & flBegin)
		{
			const Precision fMin = std::numeric_limits<Precision>::max;
			const FlowLineType * pfl = &flBegin, *pflResult = NULL;

			do 
			{
				const Precision fDist = pfl->sdist(p);

				if (fDist < fMin)
				{
					fMin = fDist;
					pflResult = pfl;
				}
			} while ((pfl = pfl->parent()) != NULL);

			return pflResult;
		}
		
	public:
		ChannelSet() {}
		ChannelSet(
			const mars::RangeX< float > & mmfBendZenith,
			const unsigned int nBendZenithSteps,
			const unsigned int nCandidateSampleAmt,
			const mars::RangeX< float > & mmfSplineSeg,
			const float fGravitySlopeGrace,
			const float frSnapToGuide,
			const mars::RangeX< float > & mmfFlowlineWidth,
			const mars::RangeX< unsigned int > & mmnKidsPerLevel,
			const float fwStraightness,
			const float fwElevationInfluence,
			const float fwDensityInfluence,
			const float fwChaosInfluence
		)
			: _finder(mmfBendZenith, nBendZenithSteps, nCandidateSampleAmt, mmfSplineSeg, fGravitySlopeGrace, frSnapToGuide, fwStraightness, fwElevationInfluence, fwDensityInfluence, fwChaosInfluence),
			_mmfFlowlineWidth (mmfFlowlineWidth), _mmnKidsPerLevel(mmnKidsPerLevel) {}

		inline const mars::RangeX< float > & getWidthRange() const { return _mmfFlowlineWidth; }
		inline const mars::RangeX< unsigned int > & getKidsPerLevel () const { return _mmnKidsPerLevel; }
		inline const MyPathFinder & getPathFinder () const { return _finder; }

		inline void addStartPoint (const StartPoint< Precision > & spt)
			{ _starts.push_back(spt); }
		inline void addStartPoint (const Vector3 & pos, const Precision fDir)
			{ addStartPoint(StartPoint< Precision >(pos, fDir)); }

		// Query is not referenced beyond this call
		void init (const mars::ptr< QB > & query, const mars::BBox< Precision > & bbox, const unsigned int nLevels)
		{
			using namespace mars;

			_finder.setQueryCallback(query);
			_guides.clear();

			GuideList tier1, tier2, 
				* pTierSrc = &tier1, 
				* pTierKids = &tier2;

			_finder.setBBoxLimit(bbox);

			for (typename StartingPoints::const_iterator i = _starts.begin(); i != _starts.end(); ++i)
			{
				// HACK: No longer calculating radius based on configuration or level depth
				const unsigned int nRandRadius =	// HACK: Redundapendency in doLevel
					static_cast <unsigned int> (_mmfFlowlineWidth.next());
				//const unsigned int nRandRadius = mars::RAND(6) + 5;

				mars::ptr< FlowLineType > pfl = _finder.find(i->position, i->direction, nRandRadius);

				if (pfl != NULL)
				{
					pTierSrc->push_back (pfl);
					_guides.push_back(pfl);
				}
			}

			for (unsigned int l = nLevels; l > 0 && !pTierSrc->empty(); --l)
			{
				pTierKids->clear();
				for (typename GuideList::iterator i = pTierSrc->begin(); i != pTierSrc->end(); ++i)
				{
					const unsigned int nIterations = _mmnKidsPerLevel.next();

					// First step is to find paths for all the subsequent flow-lines at this order of magnitude
					for (unsigned int k = 0; k < nIterations; ++k)
					{
						ptr< FlowLineType > flChild = _finder.find(*(*i), static_cast< unsigned int > (_mmfFlowlineWidth.next()));

						if (flChild != NULL)
							(*i)->addChild(
								flChild
							);
					}

					// Second step is to add sub-sequent flows to the next level-down list
					pTierKids->insert(pTierKids->end(), (*i)->kidsBegin(), (*i)->kidsEnd());
				}
				std::swap(pTierKids, pTierSrc);	// Flip the guide lists
			}

			computeBBox();

			_finder.setQueryCallback(NULL);
		}

		void run (DS * pData) const
		{
			using namespace mars;

			GuideList tier1, tier2, 
				* pTierSrc = &tier1, 
				* pTierKids = &tier2;

			*pTierSrc = _guides;
			while (!pTierSrc->empty())
			{
				pTierKids->clear();
				for (typename GuideList::iterator i = pTierSrc->begin(); i != pTierSrc->end(); ++i)
				{
					applyFlowLine(*(*i), pData);
					for (typename GuideType::KidList::iterator j = (*i)->kidsBegin(); j != (*i)->kidsEnd(); ++j)
						pTierKids->push_back(*j);
				}
				finishedTier (*pTierSrc, pData);

				std::swap(pTierKids, pTierSrc);	// Flip the guide lists
			}
		}

		mars::BBox< Precision > getBBox () const { return _bbox; }

		inline typename GuideList::const_iterator begin() const { return _guides.begin(); }
		inline typename GuideList::const_iterator end() const { return _guides.end(); }
		inline typename GuideList::iterator begin() { return _guides.begin(); }
		inline typename GuideList::iterator end() { return _guides.end(); }
		inline size_t count () const { return _guides.size(); }

		mars::ObjectStream & operator >> (mars::ObjectStream & outs) const
		{
			_finder >> outs;
			outs << _mmfFlowlineWidth << _mmnKidsPerLevel << _starts << _bbox;

			GuideList tier1(_guides), tier2, 
				* pTierSrc = &tier1, 
				* pTierKids = &tier2;

			do
			{
				outs << static_cast< size_t > (pTierSrc->size());
				pTierKids->clear();
				
				for (typename GuideList::const_iterator i = pTierSrc->begin(); i != pTierSrc->end(); ++i)
				{
					outs << (*i)->radius << (*i)->unormal << (*i)->points() << (*i)->kidsCount();
					pTierKids->insert(pTierKids->end(), (*i)->kidsBegin(), (*i)->kidsEnd());
				}

				std::swap(pTierKids, pTierSrc);
			} while(!pTierSrc->empty());

			return outs;
		}

		mars::ObjectStream & operator << (mars::ObjectStream & ins)
		{
			using namespace mars;

			_finder << ins;
			ins >> _mmfFlowlineWidth >> _mmnKidsPerLevel >> _starts >> _bbox;

			struct GuideInfo
			{
				FlowLineType * parent, * line;
				size_t childcount;

				GuideInfo ()
					: parent(NULL), childcount(0) {}
				GuideInfo (FlowLineType * pParent)
					: parent(pParent), childcount(0) {}

				ObjectStream & operator << (ObjectStream & ins)
				{
					unsigned int nRadius;
					Vector3 unorm;
					typename GuideType::PointList points;

					ins >> nRadius >> unorm >> points >> childcount;
					if (parent == NULL)
					{
						line = new FlowLineType(points, nRadius, unorm);
					}
					else
					{
						line = new FlowLineType(*parent, points, nRadius, unorm);
						parent->addChild(line);
					}
					return ins;
				}
			};

			typedef std::vector< GuideInfo > GuideInfoList;

			GuideInfoList
				tier1, tier2,
				* pSrcParentGuides = &tier1,
				* pKidGuides = &tier2;

			size_t nCount;
			GuideInfo info;
			size_t j;

			ins >> nCount;
			_guides.clear();
			for (size_t i = 0; i < nCount; ++i)
			{
				info << ins;
				pSrcParentGuides->push_back(info);
				_guides.push_back(info.line);
			}

			do 
			{
				pKidGuides->clear();
				for (typename GuideInfoList::iterator i = pSrcParentGuides->begin(); i != pSrcParentGuides->end(); ++i)
				{
					GuideInfo info (i->line);

					for (j = 0; j < i->childcount; ++j)
					{
						info << ins;
						pKidGuides->push_back(info);
					}
				}

				std::swap(pSrcParentGuides, pKidGuides);

			} while (!pSrcParentGuides->empty());

			return ins;
		}
	};
}