#pragma once

#include <exception>
#include <limits>
#include <assert.h>
#include <string.h>

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

#define STRINGIFY(a) #a

#define PARALLEL_FOR(i)
#ifdef __pragma
	__pragma(omp parallel for private(i))
#elif defined(_Pragma)
	_Pragma(STRINGIFY( omp parallel for private(i) ))
#endif

#define PARALLEL_FOR2(i,j)
#ifdef __pragma
	__pragma(omp parallel for private(i,j))
#elif defined(_Pragma)
	_Pragma(STRINGIFY( omp parallel for private(i,j) ))
#endif

namespace mars
{
	template< typename T >
	struct BaseNumerics
	{
		static inline T zero () { return static_cast< T > (0); }
		static inline T max () { return std::numeric_limits< T >::max(); }
	};

	template< typename T >
	struct IntegralNumerics : public BaseNumerics< T >
	{
		static inline T step() { return static_cast< T > (1); }
		static inline T min () { return std::numeric_limits< T >::min(); }
	};

	template< typename T >
	struct RationalNumerics : public BaseNumerics< T >
	{
		static inline T step () { return std::numeric_limits< T >::epsilon(); }
		static inline T min () { return std::numeric_limits< T >::lowest(); }
	};

	template< typename T >
	struct Numerics;

	template <>
	struct Numerics< float > : public RationalNumerics< float >
	{ 
		typedef double UP; 
		typedef float POS;
	};
	template <>
	struct Numerics< double > : public RationalNumerics< double >
	{ 
		typedef long double UP; 
		typedef double POS;
	};
	template <>
	struct Numerics< long double > : public RationalNumerics< long double >
	{ 
		typedef long double UP; 
		typedef long double POS;
	};

	template <>
	struct Numerics< signed char > : public IntegralNumerics< signed char >
	{ 
		typedef signed short UP; 
		typedef unsigned char POS;
	};
	template <>
	struct Numerics< unsigned char > : public IntegralNumerics< unsigned char >
	{ 
		typedef unsigned short UP; 
		typedef unsigned char POS;
	};

	template <>
	struct Numerics< signed short > : public IntegralNumerics< signed short >
	{ 
		typedef signed int UP; 
		typedef unsigned short POS;
	};
	template <>
	struct Numerics< unsigned short > : public IntegralNumerics< unsigned short >
	{ 
		typedef unsigned int UP; 
		typedef unsigned short POS;
	};

	template <>
	struct Numerics< signed int > : public IntegralNumerics< signed int >
	{ 
		typedef signed long long UP; 
		typedef unsigned int POS;
	};
	template <>
	struct Numerics< unsigned int > : public IntegralNumerics< unsigned int >
	{ 
		typedef unsigned long long UP; 
		typedef unsigned int POS;
	};

	template <>
	struct Numerics< signed long > : public IntegralNumerics< signed long >
	{
		typedef signed long long UP;
		typedef unsigned long POS;
	};
	template <>
	struct Numerics< unsigned long > : public IntegralNumerics< unsigned long >
	{
		typedef unsigned long long UP;
		typedef unsigned long POS;
	};

	template <>
	struct Numerics< signed long long > : public IntegralNumerics< signed long long >
	{
		typedef signed long long UP;
		typedef unsigned long long POS;
	};
	template <>
	struct Numerics< unsigned long long > : public IntegralNumerics< unsigned long long >
	{
		typedef unsigned long long UP;
		typedef unsigned long long POS;
	};

	class RingBufferBoundsEx : public std::exception 
	{
	public:
		const unsigned int index, count;

		RingBufferBoundsEx (const unsigned int i, const unsigned int count)
			: index (i), count(count) {}
	};

	template <typename T>
	class RingBuffer
	{
	private:
		T * _v;
		unsigned int _i;

		inline unsigned int index (const unsigned int i) const 
		{
#ifdef _DEBUG
			if (i >= count)
				throw RingBufferBoundsEx (i, count);
#endif
			return (_i + count - i) % count;
		}

	public:
		const unsigned int count;

		inline RingBuffer (const unsigned short nSize)
			: _v(new T[nSize]), _i(0), count(nSize) 
		{
			memset(_v, 0, sizeof(_v[0]) * count);
		}

		inline RingBuffer (const RingBuffer <T> & copy)
			: _v(new T[copy.count]), count(copy.count)
		{
			memcpy (_v, copy._v, count * sizeof(_v[0]));
		}

		inline T & operator [] (unsigned int i) { return _v[index(i)]; }
		inline const T & operator [] (unsigned int i) const { return _v[index(i)]; }
		inline T & last () { return _v[_i]; }
		inline const T & last () const { return _v[_i]; }
		inline void operator ++ () { _i = (_i + 1) % count; }

		inline void add (const T & val)
		{
			_v[_i = (_i + 1) % count] = val;
		}
		void clear ()
		{
			for (unsigned int i = 0; i < count; ++i)
				_v[i] = T();
		}

		inline ~RingBuffer () { delete _v; }
	};

	template <typename T, typename ST = Numerics< T > >
	class Range
	{
	public:
		typedef T Precision;

		T minimum, maximum;
		
		Range()
			: minimum(ST::zero()), maximum(ST::zero()) {}
		Range (const T minimum, const T maximum)
			: minimum(minimum), maximum(maximum)
			{ assert (minimum <= maximum); }

		inline virtual void update ()
			{ assert(minimum <= maximum);	}

		inline bool contains (const T val) const
			{ return val >= minimum && val <= maximum; }

		inline T clamp (const T value) const
			{ return std::min(maximum, std::max(minimum, value)); }

		inline Range & operator |= (const Range & other)
		{
			minimum = std::min(minimum, other.minimum);
			maximum = std::max(maximum, other.maximum);
			return *this;
		}
	};

	template <typename T >
	class RangeX : public Range< T >
	{
	public:
		typedef T Precision;
		typedef typename Numerics< typename Numerics< T >::POS >::UP Delta;
		typedef typename Numerics< T >::UP SuperPrecision;

		T step;
		Delta delta;

		RangeX()
			: Range<T>(), step(Numerics< T >::step()), delta(static_cast< Delta > (Numerics< Delta >::zero())) {}
		RangeX (const T minimum, const T maximum)
			: Range<T>(minimum, maximum), delta(static_cast< SuperPrecision > (maximum) - minimum + Numerics< T >::step()), step(Numerics< T >::step())
			{ assert (delta > Numerics< Delta >::zero()); }
		RangeX (const T minimum, const T maximum, const T step)
			: Range<T>(minimum, maximum), delta(static_cast< SuperPrecision > (maximum) - minimum + Numerics< T >::step()), step(step)
			{ assert (delta > Numerics< Delta >::zero() && step > Numerics< T >::zero()); }
		RangeX (const Range<T> & rng)
			: Range<T>(rng), delta(static_cast< SuperPrecision > (rng.maximum) - rng.minimum + Numerics< T >::step()), step(Numerics< T >::step())
			{ assert (delta > Numerics< Delta >::zero() && step > Numerics< T >::zero()); }
		RangeX (const RangeX<T> & copy)
			: Range<T>(copy), delta(copy.delta), step(copy.step) {}

		inline virtual void update ()
		{ 
			Range<T>::update();
			assert (step > Numerics< T >::zero());
			delta = static_cast< SuperPrecision > (this->maximum) - this->minimum + Numerics< T >::step(); 
		}

		inline T next() const 
		{ 
			assert (this->minimum <= this->maximum && step > Numerics< T >::zero());
			return next1<T> (identity<T>()); 
		}

		inline T median () const 
		{ 
			assert (this->minimum <= this->maximum && step > Numerics< T >::zero());
			return static_cast <T> (this->minimum + delta / 2); 
		}
		inline Range<T> operator / (const T n) const 
		{
			assert (this->minimum <= this->maximum && step > Numerics< T >::zero());
			const T min2 = this->minimum / n;
			return Range<T> (min2, min2 + delta / n);
		}

		RangeX & operator = (const Range< T > & rval)
		{
			this->minimum = rval.minimum;
			this->maximum = rval.maximum;
			update();
			return *this;
		}

	private:
		template <typename J>
		inline J nextf () const { return static_cast< J > (static_cast <J> (rand()) / static_cast <J> (RAND_MAX) * delta) + this->minimum; }

		template <typename J> inline J next1 (identity<J>) const { return static_cast <J> (fmod(rand(), delta)) + this->minimum; }
		inline double next1 (identity<double>) const { return nextf<double>(); }
		inline float next1 (identity<float>) const { return nextf<float>(); }
	};

	template <typename T>
	class BBox
	{
	private:
		inline void assertions () const
		{
			// Must not be empty for operations
			assert(!empty());

			// Must be a signed type
			assert(static_cast <T> (0) - 1 < 0);

			// Assert bounds
			assert(left <= right && top <= bottom);
		}

	public:
		T left, top, right, bottom;

		inline BBox ()
			: left(1), top(1), right(0), bottom(0) {}

		template <typename J>
		inline BBox (const BBox< J > & copy)
			: 
			left(static_cast< T > (copy.left)),
			top(static_cast< T > (copy.top)),
			right(static_cast< T > (copy.right)),
			bottom(static_cast< T > (copy.bottom))
			{}

		inline BBox (const T left, const T top, const T right, const T bottom)
			: left(left), top(top), right(right), bottom(bottom) 
		{ assertions(); }

		inline BBox (const vector2D< T > & vMin, const vector2D< T > &vMax)
			: left(vMin.x), top(vMin.y), right(vMax.x), bottom(vMax.y)
		{ assertions(); }

		inline T getWidth() const { return right - left + 1; }
		inline T getHeight() const { return bottom - top + 1; }

		inline bool empty () const { return getWidth() == 0 && getHeight() == 0; }

		inline bool inside (const vector2D< T > & v, const T pad = 0) const 
			{ return inside(v.x, v.y, pad); }
		inline bool inside (const T x, const T y, const T pad = 0) const 
		{ 
			assertions();
			return 
				x >= left + pad && x <= right - pad && 
				y >= top + pad && y <= bottom - pad; 
		}
		inline bool intersects (const BBox< T > & other) const
		{
			assertions();
			return 
				right >= other.left &&
				bottom >= other.top &&
				left <= other.right &&
				top <= other.bottom;
		}
		inline void add (const vector2D< T > & p)
		{
			assertions();
			if (p.x < left)
				left = p.x;
			if (p.x > right)
				right = p.x;
			if (p.y < top)
				top = p.y;
			if (p.y > bottom)
				bottom = p.y;
		}
		inline void add (const BBox< T > & bbox)
		{
			assertions();
			if (bbox.left < left)
				left = bbox.left;
			if (bbox.right > right)
				right = bbox.right;
			if (bbox.top < top)
				top = bbox.top;
			if (bbox.bottom > bottom)
				bottom = bbox.bottom;
		}
		inline vector2D< T > getMinimum () const { return vector2D< T > (left, top); }
		inline vector2D< T > getMaximum () const { return vector2D< T > (right, bottom); }
		inline bool operator () (const T x, const T y) const { return inside(x, y); }
		inline const Range< T > horizontal(const T step = 1) const { return mars::Range< T > (left, right, step); }
		inline const Range< T > vertical(const T step = 1) const { return mars::Range< T > (top, bottom, step); }

		template< typename F >
		inline BBox< F > operator << (const unsigned short n) const
		{
			return BBox< F > (
				static_cast< F > (left) << n,
				static_cast< F > (top) << n,
				static_cast< F > (right) << n,
				static_cast< F > (bottom) << n
			);
		}
		template< typename F >
		inline BBox< F > operator >> (const unsigned short n) const
		{
			return BBox< F > (
				static_cast< F > (left >> n),
				static_cast< F > (top >> n),
				static_cast< F > (right >> n),
				static_cast< F > (bottom >> n)
			);
		}

		template< typename F >
		inline operator BBox< F > () const
		{
			assertions();
			return BBox< F > (
				static_cast< F > (left),
				static_cast< F > (top),
				static_cast< F > (right),
				static_cast< F > (bottom)
			);
		}

		template< typename F >
		inline vector2D< F > clamp (const vector2D< F > & v) const
		{
			assertions();
			return 
				vector2D< F > (
					std::min(static_cast< F > (right), std::max(static_cast< F > (left), v.x)),
					std::min(static_cast< F > (top), std::max(static_cast< F > (bottom), v.y))
				);
		}
		template< typename F >
		inline vector3D< F > clampXY (const vector3D< F > & v) const
		{
			assertions();
			return 
				vector3D< F > (
					std::min(static_cast< F > (right), std::max(static_cast< F > (left), v.x)),
					std::min(static_cast< F > (bottom), std::max(static_cast< F > (top), v.y)),
					v.z
				);
		}

		inline bool operator == (const BBox< T > & other) const
		{
			return 
				left == other.left && 
				top == other.top && 
				right == other.right && 
				bottom == other.bottom;
		}

		template <typename J>
		inline const BBox & operator = (const BBox< J > & bbox)
		{
			left	= static_cast <T> (bbox.left);
			right	= static_cast <T> (bbox.right);
			top		= static_cast <T> (bbox.top);
			bottom	= static_cast <T> (bbox.bottom);

			return *this;
		}

		inline BBox operator - (const vector2D< T > & v) const
		{
			assertions();
			return BBox (
				left - v.x,
				top - v.y,
				right - v.x,
				bottom - v.y
			);
		}
		inline BBox operator + (const vector2D< T > & v) const
		{
			assertions();
			return BBox (
				left + v.x,
				top + v.y,
				right + v.x,
				bottom + v.y
			);
		}

		inline BBox & operator -= (const vector2D< T > & v)
		{
			assertions();
			left -= v.x;
			top -= v.y;
			right -= v.x;
			bottom -= v.y;
			return *this;
		}
		inline BBox & operator += (const vector2D< T > & v)
		{
			assertions();
			left += v.x;
			top += v.y;
			right += v.x;
			bottom += v.y;
			return *this;
		}
	};

	template <typename T>
	class Magnitudinal
	{
	private:
		T _val, _sq;

	public:
		enum ValueType
		{
			Normal,
			Squared
		};

		Magnitudinal () 
			: _val(0), _sq(0) {}

		Magnitudinal (const Magnitudinal & copy)
			: _val(copy._val), _sq(copy._sq) {}

		Magnitudinal (const T val, const ValueType envt = Normal) 
			: _val(envt == Normal ? val : sqrt(val)), _sq(envt == Squared ? val : mars::SQ(val)) {}

		static Magnitudinal hypotenuseOf (const T x, const T y)
			{ return Magnitudinal(mars::SQ(x) + mars::SQ(y), Squared); }

		Magnitudinal & operator = (const Magnitudinal & rval)
		{
			_sq = rval._sq;
			_val = rval._val;
			return *this;
		}

		Magnitudinal & operator += (const Magnitudinal & rval)
		{
			_sq += rval._sq;
			_val += rval._val;
		}

		inline operator const T () const { return _val; }
		inline operator T & () { return _val; }
		inline T SQ() const { return _sq; }

		inline T compareTo (const float x, const float y) const
			{ return (mars::SQ(x) + mars::SQ(y)) - _sq; }
	};

	class BitSet
	{
	private:
		static const unsigned char UINTBITS = sizeof(unsigned int) * 8;

		unsigned int * _bits;

		inline void init () { memset(_bits, 0, size / sizeof(unsigned int)); }

	public:
		class Bit
		{
		private:
			unsigned int 
				* _pointer, 
				_mask;

		public:
			inline Bit (const unsigned char bit, unsigned int * pointer)
				: _mask(1 << bit), _pointer(pointer) {}

			inline bool operator = (const bool b)
			{
				*_pointer = (*_pointer & ~_mask) | ((b ? ~0 : 0) & _mask);
				return b;
			}
			inline operator bool () const { return (*_pointer & _mask) != 0; }
		};

		const unsigned int size;

		inline BitSet (const BitSet & copy)
			: _bits(new unsigned int[copy.size / UINTBITS]), size(copy.size) 
			{ memcpy (_bits, copy._bits, size / 8); }
		inline BitSet ()
			: _bits(NULL), size(0) { init(); }
		inline BitSet (const unsigned int nSize)
			: _bits(new unsigned int[nSize / UINTBITS]), size(nSize) { init (); }

		inline const Bit operator [] (const unsigned int i) const 
			{ return Bit(i % UINTBITS, &_bits[i / UINTBITS]); }

		inline void set(const bool bFlag, const Range< unsigned int > & range)
		{
			assert(bFlag == 1 || bFlag == 0);

			const int 
				n0 = static_cast< int > (range.minimum / UINTBITS),
				n1 = static_cast< int > (range.maximum / UINTBITS);

			const unsigned int 
				b0 = range.minimum % UINTBITS,
				b1 = range.maximum % UINTBITS;
			const unsigned int 
				nBoolMask = (bFlag ? ~0 : 0);
				
			if (n1 - 1 >= n0 + 1)
				memset(&_bits[n0], nBoolMask, n1 - n0 + 1);

			if (n1 > n0)
			{
				_bits[n0 - 1] = (_bits[n0 - 1] & ((1 << (UINTBITS - b0)) - 1)) | (nBoolMask & ~((1 << (UINTBITS - b0)) - 1));
				_bits[n1 + 1] = (_bits[n1 + 1] & ~((1 << b1) - 1)) | (nBoolMask & (1 << b1) - 1);
			} else 
			{
				const unsigned int nMask =
					~0 & ~((1 << (UINTBITS - b0)) - 1) & ((1 << b1) - 1);

				_bits[range.minimum / UINTBITS] = (_bits[range.minimum / UINTBITS] & ~nMask) | (nBoolMask & nMask);
			}
		}

		inline ~BitSet()
			{ delete [] _bits; }
	};
}