#pragma once

#include <vector>
#include <limits>

#include "mars_linalg.h"
#include "mars_ptr.h"

namespace mars 
{
	class GuideEx : public std::exception {};

	template <typename V, typename T>
	class BasicGuide
	{
	public:
		typedef typename std::vector< V > PointList;
		typedef typename PointList::value_type Point;

		class SegmentType : public PointList::const_iterator
		{
		private:
			unsigned int _index;

		public:
			inline SegmentType () {}
			inline SegmentType (const SegmentType & copy)
				: PointList::const_iterator(copy), _index(copy._index) {}

			inline SegmentType & operator++()
			{
				PointList::const_iterator::operator++();
				++_index;
				return *this;
			}
			inline SegmentType operator++(int)
			{
				SegmentType old = *this;

				PointList::const_iterator::operator++();
				_index++;
				return old;
			}
			inline SegmentType & operator--()
			{
				PointList::const_iterator::operator--();
				--_index;
				return *this;
			}
			inline SegmentType operator--(int)
			{
				SegmentType old = *this;

				PointList::const_iterator::operator--();
				_index--;
				return old;
			}
			inline bool operator != (const SegmentType & other) const { return _index != other._index; }
			inline bool operator == (const SegmentType & other) const { return _index == other._index; }

			inline unsigned int index () const { return _index; }

		private:
			SegmentType (const typename PointList::const_iterator & i, unsigned int idx) : PointList::const_iterator(i), _index (idx) {}

			friend class BasicGuide;
		};

		class DistanceType
		{
		public:
			const SegmentType segment;
			const T distance;
			const V point;

		private:
			inline DistanceType (const V & p, const T dist, const SegmentType & seg)
				: segment(seg), distance(dist), point(p) {}

			friend class BasicGuide;

		public:

			inline float time () const
			{
				const T 
					a = MAGSQ(point - *segment), 
					b = MAGSQ(point - *(segment + 1));

				return a / (a + b);
			}
		};

	private:
		inline T dist2Segment (
			const mars::vector3D< T > & grSeg, 
			const mars::vector3D< T > & dp
			) const
		{
			return abs(MAGSQ(grSeg & dp) / MAGSQ(grSeg));	
		}
		inline T dist2Segment (
			const mars::vector2D< T > & grSeg, 
			const mars::vector2D< T > & dp
			) const
		{
			return abs(((grSeg & 1) * dp) / MAGSQ(grSeg));	
		}

	protected:
		PointList				_points;

	public:
		inline BasicGuide (const PointList & points)
			: _points (points) {}

		inline BasicGuide (const Point & a, const Point & b)
		{
			_points.push_back(a);
			_points.push_back(b);
		}

		inline const PointList & points () const { return _points; }
		inline const Point & front () const { return _points.front(); }
		inline const Point & back () const { return _points.back(); }
		inline SegmentType begin () const { return SegmentType(_points.begin(), 0); }
		inline SegmentType end () const { return SegmentType(_points.end(), _points.size()); }
		void offsetBy (const V & p)
		{
			for (typename PointList::iterator i = _points.begin(); i != _points.end(); ++i)
				*i += p;
		}

		inline DistanceType operator - (const V & p) const  { return distance(p, _points.begin()); }
		inline DistanceType distance (const V & p, const SegmentType & i) const
		{
			const SegmentType 
				inpt = nearestPoint(i, p),
				insg = nearestSegmentHead(inpt, p);

			return DistanceType 
			(
				p,
				dist2Segment(grad(insg), p - *insg),
				insg
			);
		}

		// Presuming that it was already determined that the specified point is closest to the point indicated by the specified iterator, this method
		// determines the segment that the point is closer to, either the segment following or preceding the point indicated by the specified iterator
		// NOTE: it is most sensible calling this method immediately after calling "nearestPoint" in order to determine the segment within this guide
		// that is closest to an arbitrary point.
		SegmentType nearestSegmentHead ( const SegmentType & from, const V & p) const
		{
			if (static_cast <typename PointList::const_iterator> (from) == _points.begin())
				return from;
			else 
			{
				SegmentType ia = from, ib = from;

				--ia;
				++ib;

				if (static_cast <typename PointList::const_iterator> (ib) == _points.end())
					return ia;
				else
				{
					const T 
						ma = MAGSQ(*(ia) - p),
						mb = MAGSQ(*(ib) - p);

					if (ma < mb)
						return ia;
					else
						return from;
				}
			}
		}

		// Iterates through all of the points in the guide and returns the iterator to the point that is closest to the specified point
		SegmentType nearestPoint( SegmentType from, const V & p ) const
		{
			T mag = -1;
			SegmentType iret, iend;

			iend = iret = SegmentType(_points.end(), _points.size());

			while (from != iend)
			{
				const T m = MAGSQ(*from - p);
				if (mag < 0 || m < mag)
				{
					mag = m;
					iret = from++;
				} else
					++from;
			}
			return iret;
		}

		inline V grad (const typename PointList::const_iterator & seg) const
		{
			return tail(seg) - *seg;
		}

		// Returns the tail-end of the specified segment
		inline V tail (typename PointList::const_iterator seg) const
		{
			typename PointList::const_iterator last = seg++;

			return seg == end() ? *last : *seg;
		}

		inline BasicGuide & operator = (const PointList & points)
		{
			_points = points;
			// XTODO: Implement QuadTree::clear()
			// _qtree.clear(); 
			return *this;
		}
	};

	template <class Derived, typename V, typename T>
	class TreeGuideNode;

	template <class Derived, typename V, typename T>
	class TreeGuideNode : public BasicGuide <V, T>
	{
	public:
		typedef std::vector < ptr< Derived > > KidList;

	private:
		const TreeGuideNode * _parent;
		KidList _kids;

	public:
		typedef typename std::vector< V > PointList;
  		typedef typename PointList::value_type Point;

		inline TreeGuideNode (const PointList & points)
			: BasicGuide<V, T> (points), _parent(NULL) {}

		inline TreeGuideNode (const PointList & points, const TreeGuideNode & parent)
			: BasicGuide<V, T>(points), _parent(&parent) {}

		inline TreeGuideNode (const Point & a, const Point & b)
			: BasicGuide<V, T>(a, b), _parent (NULL) {}

		const TreeGuideNode & top () const 
		{
			if (_parent == NULL)
				return *this;
			else
				return _parent->top();
		}

		inline const TreeGuideNode * parent () const { return _parent; }
		inline bool hasParent () const { return _parent != NULL; }

		inline void addChild (const ptr< Derived > & child) { _kids.push_back(child); }

		inline const typename KidList::const_iterator kidsBegin () const { return _kids.begin(); }
		inline const typename KidList::const_iterator kidsEnd () const { return _kids.end(); }
		inline const typename KidList::iterator kidsBegin () { return _kids.begin(); }
		inline const typename KidList::iterator kidsEnd () { return _kids.end(); }
		inline size_t kidsCount () const { return _kids.size(); }

		void offsetBy (const V & p)
		{
			BasicGuide<V, T>::offsetBy(p);

			for (typename KidList::iterator i = _kids.begin(); i != _kids.end(); ++i)
				(*i)->offsetBy(p);
		}
	};

}