#pragma once

#include <math.h>
#include <stdlib.h>
#include <exception>
#include <assert.h>

#include "mars_meta.h"

namespace mars
{
	const double PI = 3.14159265358979323846264338327950;

	template <typename T>
	inline T SQ(const T & a) { return a * a; }
	template <typename T>
	inline T CUBE(const T & a) { return a * a * a; }
	inline int RAND(const int n) { return (rand() * (n - 1) + (RAND_MAX / 2)) / RAND_MAX; }
	inline double toRadians(int deg) { return static_cast <double> (deg) / 180.0 * PI; }
	inline short toDegrees(double rad) { return static_cast <short> (rad / PI * 180.0); }
	template< typename T >
	inline T RANDf(const T a = 1.0f) { return static_cast <T> (rand()) / static_cast <T> (RAND_MAX) * a; }
	inline int RNDi(double f) { return static_cast <int> (f < 0 ? f - 0.5f : f + 0.5f); }	// TODO: Optimize tertiary operator
	template < typename T >
	inline T ALIGN(const T x, const T unit) 
	{ 
		assert(unit > 0);
		return x - (x % unit) - (x < 0 ? unit : 0); 
	}
	template <typename T>
	inline T AVG(const T a, const T b) { return (a + b) / static_cast< T > (2); }
	template< typename T, typename UT >
	inline UT WRAP(const T x, const UT w) { return static_cast< UT > ((x % static_cast< T > (w) + static_cast< T > (w)) % w); }

	static const double ABSCISSAE_5[5][2] = {
		-0.90617984593866399280,		0.23692688505618908751,
		-0.53846931010568309104,		0.47862867049936646804,
		0.0,							0.56888888888888888889,
		+0.53846931010568309104,		0.47862867049936646804,
		+0.90617984593866399280,		0.23692688505618908751
	};

	static const char LOG2_TABLE_256[256] = 
	{
	#define LT(n) n, n, n, n, n, n, n, n, n, n, n, n, n, n, n, n
		-1, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3,
		LT(4), LT(5), LT(5), LT(6), LT(6), LT(6), LT(6),
		LT(7), LT(7), LT(7), LT(7), LT(7), LT(7), LT(7), LT(7)
	};

	inline unsigned int LOG2(const unsigned int v)
	{
		unsigned int t, tt;

		if (tt = v >> 16)
			return (t = tt >> 8) ? 24 + LOG2_TABLE_256[t] : 16 + LOG2_TABLE_256[tt];
		else 
			return (t = v >> 8) ? 8 + LOG2_TABLE_256[t] : LOG2_TABLE_256[v];
	}

	class CalcEx : public std::exception {};
	
	template <typename T, typename Derived, unsigned NN>
	class GaussLegendreInt
	{
	private:
		template<unsigned N>
		T abscissa (const unsigned int i, const unsigned int j, quantity<N>) const
		{
			throw CalcEx ();
		}
		T abscissa (const unsigned int i, const unsigned int j, quantity<5>) const
		{
			return static_cast <T> (ABSCISSAE_5[i][j]);
		}
	
		template <unsigned N> 
		T abscissa (const unsigned int i, const unsigned int j) const
		{
			return abscissa(i, j, quantity<N>());
		}

		template <unsigned N> 
		T summate (const T val, const T a, const T b, quantity<N>) const
		{
            const Derived * derived = static_cast <const Derived *> (this);
			return val + summate <N - 1> (
				abscissa <NN> (N - 1, 1) * 
				derived->f(
					(b + a) / 2 + 
					((b - a) / 2) * abscissa <NN> (N - 1, 0)
				),
				a, b
			);
		}
		T summate (const T val, const T a, const T b, quantity<0>) const { return val; }

		template <unsigned N> 
		T summate (const T val, const T a, const T b) const
		{
			return summate(val, a, b, quantity<N>());
		}
		
	protected:
		inline T compute (const T a, const T b) const
		{
			return ((b - a) / 2) * summate <NN> (0, a, b);
		}
	};

	template <typename T, class EqLeft, class EqRight>
	class SolutionEquation
	{
	private:
		const EqLeft & _left;
		const EqRight & _right;

	public:
		inline SolutionEquation (const EqLeft & left, const EqRight & right)
			: _left(left), _right(right) {}

		inline const T f(const T x) const { return _left.f(x) - _right.f(x); }
		inline const T df(const T x) const { return _left.df(x) - _right.df(x); }
		inline const T f2(const T x) const { return _left.f2(x) - _right.f2(x); }
	};

	template <typename T, class Eq, unsigned NN = 5>
	class NewtonsMethod
	{
	private:
		const Eq & _eq;

		template <unsigned N> T iterate (const T x0, quantity<N>) const
		{
			return iterate <N-1> (x0 - _eq.f(x0) / _eq.df(x0));
		}
		T iterate (const T x0, quantity<0>) const { return x0; }
		
		template <unsigned N> T iterate (const T x0) const
		{
			return iterate(x0, quantity<N>());
		}

		inline NewtonsMethod (const Eq & eq)
			: _eq(eq) {}

		inline T solve (const T x0) const { return iterate <NN> (x0); }

	public:
		// TODO: Cache the result!
		static inline T solve (const Eq & eq, const T x0) { return NewtonsMethod (eq).solve(x0); }
		static inline T solve (const Eq & eq) { return NewtonsMethod (eq).solve(0); }
	};

	template <typename T, class Eq, unsigned NN = 5>
	class BisectMethod
	{
	private:
		const Eq & _eq;

		template <unsigned N> T iterate (const T l, const T r, quantity<N>) const
		{
			T m = (l + r) / 2;

			if (_eq.f(l) * _eq.f(m) > 0)
				return iterate <N-1> (m, r);
			else 
				return iterate <N-1> (l, m);
		}
		T iterate (const T l, const T r, quantity<0>) const
		{
			return (r + l) / 2;
		}
		
		template <unsigned N> T iterate (const T l, const T r) const
		{
			return iterate(l, r, quantity<N>());
		}

		inline BisectMethod (const Eq & eq)
			: _eq(eq) {}

	public:
		static inline T solve (const Eq & eq, const T l, const T r) { return BisectMethod(eq).iterate <NN> (l, r); }
	};

	template <typename T, int DIR = +1>
	class ParabolicFn
	{
	public:
		T scale, broadness, depth;

		ParabolicFn () : scale(0), broadness(0), depth(0) {}
		ParabolicFn (const T scale, const T broadness, const T depth)
			: scale(scale), broadness(broadness), depth(depth) {}

		inline const T f(const T x) const { return DIR * (mars::SQ(x / scale) / broadness + depth) * scale; }
		inline const T f(const T x, const T y) const 
		{ 
			return DIR * ((mars::SQ(x / scale) + mars::SQ(y / scale)) / broadness + depth) * scale; 
		}
		inline const T invf (const T y) const { return sqrt((y / scale * DIR - depth) * broadness) * scale; }
		inline const T df(const T x) const { return DIR * (2 * x / mars::SQ(scale)) / broadness * scale; }
		inline const T f2(const T x) const { return DIR * (mars::CUBE(x) / 3 / mars::SQ(scale) / broadness + x * depth) * scale; }
	};

	template <typename T, unsigned NN = 5>
	class ErrorFn
	{
	private:
		T _sqrtPI;

		template <unsigned L> static T computeEpsilon () { return computeEpsilon(quantity<L>()); }
		template <unsigned M> static T s (const T j) {
			return s(j, quantity<M>());
		}
		template <unsigned L> static T computeEpsilon (quantity<L>) { return s<L>(0); }
		inline static T computeEpsilon (quantity<0>) { return 1; }
		template <unsigned L, unsigned M> static T s (const T j, quantity<L>, quantity<M>)
			{ return j + s<M - 1> (computeEpsilon<L - 1 - (M-1)>() * computeEpsilon<M-1>() / (((M-1) + 1)*(2 * (M-1) + 1))); }
		template <unsigned M > static T s (const T j, quantity<0>, quantity<M>) { return j; }
		
		template <unsigned N> T prod (const T j, const T z, quantity<N>) const
		{
			const int k = (NN - N + 1);

			return j * prod <N - 1> (-(2 * k - 1) * z * z / (k * (2 * k + 1)), z);
		}
		inline T prod (const T j, const T z, quantity<0>) const { return j; }
		template <unsigned N> T prod (const T j, const T z) const { return prod(j, z, quantity<N>()); }

		template <unsigned N> T taylor (const T j, const T z, quantity<N>) const
			{ return j + taylor<N - 1> (prod <N>(1, z), z); }
		inline T taylor (const T j, const T z, quantity<0>) const { return j; }
		template <unsigned N> T taylor (const T j, const T z) const { return taylor(j, z, quantity<N>()); }

		template <unsigned N> T invfsum (const T j, const T z, quantity<N>) const
			{ return j + invfsum <N - 1> (computeEpsilon<N-1>() / (2*(N-1) + 1) * pow(static_cast <T> (z * _sqrtPI/2), static_cast <int> (2*(N-1) + 1)), z); }
		inline T invfsum (const T j, const T z, quantity<0>) const { return j; }
		template <unsigned N> T invfsum (const T j, const T z) const { return invfsum(j, z, quantity<N>()); }

		static T createRootPI () { return static_cast< T > (sqrt(mars::PI)); }

	public:
		T scale, depth, shift;

		ErrorFn ()
			: scale(0), depth(0), shift(0), _sqrtPI(createRootPI()) {}
		ErrorFn (const T frScale, const T fDepth, const T fShift)
			: scale(frScale), depth (fDepth), shift (fShift), _sqrtPI(createRootPI()) {}

		inline ErrorFn & operator = (const ErrorFn & copy)
		{
			const_cast< T & > (scale) = copy.scale;
			const_cast< T & > (depth) = copy.depth;
			const_cast< T & > (shift) = copy.shift;
			return *this;
		}

		inline const T f (const T x) const { return (static_cast <T> (2.0) / _sqrtPI * taylor <NN> (0, (x - shift) / scale) + depth) * scale; }	// TODO: Redundant factors for readability
		inline const T invf (const T y) const { return invfsum <NN + 1> (0, y / scale - depth) * scale + shift; }
		inline const T df (const T x) const { return static_cast <T> (2.0) / _sqrtPI * exp(-SQ((x - shift) / scale)) * scale; }
		inline const T f2 (const T x) const { return (exp(-SQ((x - shift) / scale)) / _sqrtPI + depth * ((x - shift) / scale)) * scale; }
	};

	template <typename T, int DIR = +1, unsigned NN = 5>
	class GaussianFn
	{
	private:
		ErrorFn <T, NN> _errorfn;

	protected:
		inline const T f_base (const T x) const
		{
			return f_base(x, broadness, amplitude);
		}

		inline static const T f_base(const T x, const T broadness, const T amplitude)
		{
			return amplitude * exp(
				-mars::SQ(x) / 
				(2.0f * mars::SQ(broadness))
			);
		}
		inline static const T f_base(const T x, const T y, const T bx, const T by, const T amplitude)
		{
			return amplitude * exp(
				-(
					mars::SQ(x) / (2.0f * mars::SQ(bx)) + 
					mars::SQ(y) / (2.0f * mars::SQ(by))
				)
			);
		}

	public:
		const T scale, broadness, depth, amplitude;

		GaussianFn ()
			: scale(0), amplitude(0), broadness(0), depth(0) {}
		GaussianFn (const T scale, const T amplitude = 1, const T broadness = 1, const T depth = 0)
			: scale(scale), amplitude(amplitude), broadness(broadness), depth(depth),
			_errorfn (scale, depth, 0) {}

		inline GaussianFn & operator = (const GaussianFn & copy)
		{
			_errorfn = copy._errorfn;
			const_cast< T & > (scale) = copy.scale;
			const_cast< T & > (broadness) = copy.broadness;
			const_cast< T & > (depth) = copy.depth;
			const_cast< T & > (amplitude) = copy.amplitude;
			return *this;
		}

		inline const T f(const T x) const 
		{ 
			return DIR * (scale * (f_base(x / scale, broadness, amplitude) + depth)); 
		}
		inline static const T f (const T x, const T scale, const T broadness, const T amplitude, const T depth)
		{
			return DIR * scale * (f_base(x / scale, broadness, amplitude) + depth);
		}

		inline const T f(const T x, const T y) const 
		{ 
			return DIR * scale * (f_base(x / scale, y / scale, broadness, broadness, amplitude) + depth);
		}

		// Inverse function, warning this is computationally intensive
		// Valid for all y > 0
		inline const T invf (const T y) const 
		{
			return sqrt(log(pow((y / scale * DIR - depth) / amplitude, -2*mars::SQ(broadness)))) * scale;
		}

		// The integral of the Gaussian is the error-function, but we will represent it using the iterative Taylor series,
		// which is the product of a sequence.  We will implement this using a recursive template algorithm.
		inline const T f2(const T x) const
		{
			return _errorfn.f(x);
		}

		// First-derivative of the Gaussian is the Gaussian multiplied by the first Hermite polynomial (or x)
		inline const T df (const T x) const
		{
			return DIR * (scale * (f_base(x / scale) * (x / scale)));
		}
	};

	template <typename T, int DIR>
	class InvertedSquareFn
	{
	public:
		T scale, broadness, depth;

		inline InvertedSquareFn (const T scale, const T broadness, const T depth)
			: scale(scale), broadness (broadness), depth(depth) {}

		inline const T f(const T x) const { return DIR * ((1.0f / mars::SQ(x / scale)) * broadness + depth) * scale; }
		inline const T f(const T x, const T y) const { return DIR * ((1.0f / (mars::SQ(x / scale) + mars::SQ(y / scale))) * broadness + depth) * scale; }
		inline const T invf(const T y) const { return sqrt(broadness / ((DIR * y / scale) - depth)) * scale; }
		inline const T df(const T x) const { return DIR * (-2.0f / (mars::CUBE(x) / mars::SQ(scale))) * broadness * scale; }
		inline const T f2(const T x) const { return DIR * (-1.0f / (x / scale) * broadness + (x / scale) * depth) * scale; }
	};

	template <typename T, int DIR>
	class CosineFn
	{
	public:
		T scale, period, amplitude, depth;

		inline CosineFn (const T scale, const T period, const T amplitude, const T depth)
			: scale(scale), period (period), amplitude(amplitude), depth(depth) {}

		inline const T f(const T x) const { return DIR * (cos(x * period / scale) / amplitude + depth) * scale; }
		inline const T invf(const T y) const { return acos(((DIR * y / scale) - depth) * amplitude) * scale / period; }
		inline const T df(const T x) const { return DIR * (-sin(x * period / scale) * period / scale) / amplitude * scale; }
		inline const T f2(const T x) const { return DIR * (sin(x * period / scale) / amplitude / (period / scale) + (x / scale)) * scale; }
	};

	namespace FnVar
	{
		enum Type
		{
			Normal = 0,
			Derivative = 1,
			Inverse = 2,
			Integral = 3
		};

		enum Flag
		{
			Flag_Normal = 1 << Normal,
			Flag_Derivative = 1 << Derivative,
			Flag_Inverse = 1 << Inverse,
			Flag_Integral = 1 << Integral
		};
	};

	template <typename T, typename Fn, FnVar::Flag FLAGS = FnVar::Flag_Normal>
	class CacheFnProxy
	{
	public:
		CacheFnProxy (Fn fn, const T fMin, const T fMax, const unsigned short nResolution)
			: _fn(fn), _fMin(fMin), _fMax(fMax),
			_fStep ((fMax - fMin) / static_cast< T > (nResolution)),
			_nResolution(nResolution + 1)
		{
			assert(fMax > fMin);
			assert(_fStep > 0);

			_ffTable[FnVar::Normal] = 
			_ffTable[FnVar::Derivative] = 
			_ffTable[FnVar::Inverse] =
			_ffTable[FnVar::Integral] = NULL;

			processFlag< FLAGS & FnVar::Flag_Normal > ();
			processFlag< FLAGS & FnVar::Flag_Derivative > ();
			processFlag< FLAGS & FnVar::Flag_Inverse > ();
			processFlag< FLAGS & FnVar::Flag_Integral > ();
		}

		inline const unsigned short getResolution () const { return _nResolution - 1; }

		inline const T f(const T x) const { return interpolate<FnVar::Normal>(x); }
		inline const T invf(const T y) const { return interpolate<FnVar::Inverse>(y); }
		inline const T df(const T x) const { return interpolate<FnVar::Derivative>(x); }
		inline const T f2(const T x) const { return interpolate<FnVar::Integral>(x); }

		~CacheFnProxy() 
		{
			using namespace FnVar;

			delete [] _ffTable[FnVar::Normal];
			delete [] _ffTable[FnVar::Derivative];
			delete [] _ffTable[FnVar::Inverse];
			delete [] _ffTable[FnVar::Integral];
		}

	private:
		Fn _fn;
        
		T _fMin, _fMax, _fStep;
		unsigned short _nResolution;

		T * _ffTable[4];

		template< int I >
		inline const T interpolate(const T x) const 
		{
			assert(_ffTable[I] != NULL);

			const T 
				mx = (x - _fMin) / _fStep,
				x0 = floor(mx);
			const unsigned int 
				nx0 = static_cast< unsigned int > (mx);

			assert(nx0 >= 0 && nx0 < _nResolution);

			return 
				_ffTable[I][nx0] + (_ffTable[I][nx0 + 1] - _ffTable[I][nx0]) * (mx - x0);
		}

		template< int FLAG > void processFlag () { processFlag(quantity<FLAG>()); }
		void processFlag (quantity<0>) {}
		
		void processFlag (quantity<FnVar::Flag_Normal>)
			{ computeTable< &Fn::f, FnVar::Normal > ();	}
		void processFlag (quantity<FnVar::Flag_Derivative>)
			{ computeTable< &Fn::df, FnVar::Derivative > ();	}
		void processFlag (quantity<FnVar::Flag_Integral>)
			{ computeTable< &Fn::f2, FnVar::Integral > ();	}
		void processFlag (quantity<FnVar::Flag_Inverse>)
			{ computeTable< &Fn::invf, FnVar::Inverse > ();	}

		template< const T (Fn::* FnFn)(const T) const, int I >
		void computeTable ()
		{
			if (FLAGS & (1 << I))
			{
				_ffTable[I] = new T[_nResolution];
				for (unsigned int i = 0; i < _nResolution; ++i)
					_ffTable[I][i] = (_fn.*FnFn)(_fMin + _fStep * static_cast< T > (i));
			} else
				_ffTable[I] = NULL;
		}
	};

	template< typename T >
	class ParabolicScale : protected ParabolicFn< float >
	{
	private:
		const size_t _nCount;
		const float _fScale;
		float * _fIndex;

	public:
		ParabolicScale(const size_t nCount, const float fFactor = 0.5f, const float fScale = 1.0f)
			: _nCount (nCount), _fScale(fScale), _fIndex(new float[nCount]), ParabolicFn(static_cast< float > (nCount - 1), 1.0f / fFactor, 0.0f)
		{
			const float fTotalArea = f2(static_cast< float > (nCount));
			for (size_t c = 0; c < nCount; ++c)
				_fIndex[c] = f2(static_cast< float > (c)) / fTotalArea * fScale;
		}
		~ParabolicScale()
			{ delete [] _fIndex; }

		inline T operator () (const float f) const
		{
			assert (f >= 0.0f && f <= _fScale);
			for (size_t c = 0; c < _nCount; ++c)
				if (f < _fIndex[c])
					return static_cast< T > (c);

			return static_cast< T > (_nCount - 1);
		}
		inline float operator [] (const T enps) const
			{ return _fIndex[enps]; }
	};

	template< typename T, T COUNT >
	class StaticParabolicScale : public ParabolicScale< T >
	{
	public:
		StaticParabolicScale (const float fFactor = 0.5f, const float fScale = 1.0f)
			: ParabolicScale<T>(COUNT, fFactor, fScale) {}
	};
}