/**
 * This file, Mars Linag, is released under the MIT license
 */
#ifndef __MARSLINEARALGEBRA_H__
#define __MARSLINEARALGEBRA_H__

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

#ifdef _DEBUG
#include <assert.h>
#endif

#include "mars_meta.h"
#include "mars_calc.h"

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

	class MathOverrides
	{
	public:
		static inline int sqrt (const int x)
			{ return static_cast <int> (::sqrt (static_cast <double> (x))); }
		static inline short sqrt (const short x)
			{ return static_cast <short> (::sqrtf (static_cast <float> (x))); }
		static inline long sqrt (const long x)
			{ return static_cast <long> (::sqrt (static_cast <long> (x))); }
		static inline long double sqrt (const long double x)
			{ return static_cast <long double> (::sqrt (static_cast <long double> (x))); }
		static inline double sqrt (const double x)
			{ return static_cast <double> (::sqrt (static_cast <double> (x))); }
		static inline float sqrt (const float x)
			{ return static_cast <float> (::sqrtf (static_cast <float> (x))); }

/*
		static inline int atan2 (const int x, const int y)
			{ return static_cast <int> (::atan2 (static_cast <double> (x), static_cast <double> (y))); }
		static inline short atan2 (const short x, const short y)
			{ return static_cast <short> (::atan2f (static_cast <float> (x), static_cast <float> (y))); }
		static inline long double atan2 (const long double x, const long double y)
			{ return static_cast <long double> (::atan2 (static_cast <long double> (x), static_cast <long double> (y))); }
		static inline double atan2 (const double x, const double y)
			{ return static_cast <double> (::atan2 (static_cast <double> (x), static_cast <double> (y))); }
		static inline float atan2 (const float x, const float y)
			{ return static_cast <float> (::atan2f (static_cast <float> (x), static_cast <float> (y))); }

		static inline int acos (const int x)
			{ return static_cast <int> (::acos (static_cast <double> (x))); }
		static inline short acos (const short x)
			{ return static_cast <short> (::acosf (static_cast <float> (x))); }
		static inline long double acos (const long double x)
			{ return static_cast <long double> (::acos (static_cast <long double> (x))); }
		static inline double acos (const double x)
			{ return static_cast <double> (::acos (static_cast <double> (x))); }
		static inline float acos (const float x)
			{ return static_cast <float> (::acosf (static_cast <float> (x))); }*/
	};

	// Linear optimization technique, taken from: http://www.flipcode.com/archives/Faster_Vector_Math_Using_Templates.shtml
	// Operator controllers:

	template <typename T>
	struct VOpAdd
	{
		typedef T ResultType;

		template <int I, typename LVal, typename RVal> 
		static inline T evalOp (const LVal & lval, const RVal & rval)
			{ return lval.template eval<I>() + rval.template eval<I>(); }
	};
	template <typename T>
	struct VOpSub
	{
		typedef T ResultType;

		template <int I, typename LVal, typename RVal> 
		static inline T evalOp (const LVal & lval, const RVal & rval)
			{ return lval.template eval<I>() - rval.template eval<I>(); }
	};
	template <typename T, int D>
	struct VOpScale
	{
		typedef T ResultType;

		template <int I, typename LVal, typename RVal> 
		static inline T evalOp (const LVal & lval, const RVal & rval)
			{ return lval.template eval<I>() * rval.template eval<I>(); }
	};
	template <typename T, int D>
	struct VOpContract
	{
		typedef T ResultType;

		template <int I, typename LVal, typename RVal> 
		static inline T evalOp (const LVal & lval, const RVal & rval)
			{ return lval.template eval<I>() / rval.template eval<I>(); }
	};
	template <typename T>
	struct VOpNegate
	{
		typedef T ResultType;

		template <int I, typename RVal> 
		static inline T evalOp (const RVal & rval)
			{ return -rval.template eval<I>(); }
	};
	template <typename T, typename TNew>
	struct VOpCast
	{
		typedef TNew ResultType;

		template <int I, typename RVal>
		static inline TNew evalOp (const RVal & rval)
			{ return static_cast< TNew > (rval.template eval<I>()); }
	};

	template <typename T, int D>
	struct VExprTagDim
	{
		typedef T 						Precision;
		typedef VOpAdd< T >				OpAdd;
		typedef VOpSub< T >				OpSub;
		typedef VOpScale< T, D >		OpScale;
		typedef VOpContract< T, D >		OpContract;
		typedef VOpNegate< T >			OpNegate;

		enum { DIMENSION = D };
	};

	template <typename T, typename Derived, int D>
	class VBaseExpr : public VExprTagDim< T, D >
	{
	public:
		typedef Derived ExprType;

	private:
		template <typename Functor>
		struct Reduce
		{
			template <unsigned I>
			static inline const T go (const Functor & f, const Derived & expr) { return go(f, expr, quantity<I>()); }

			template <unsigned I>
			static inline const T go (const Derived & expr) { return go(expr, quantity<I>()); }

		private:
			template <unsigned I>
			static inline const T go (const Functor & f, const Derived & expr, quantity<I>)
			{
				return f.run<I-1>(expr.template eval<I-1>()) + go<I-1>(f, expr);
			}
			static inline const T go (const Functor &, const Derived &, quantity<0>)
				{ return static_cast <T> (0); }

			template <unsigned I>
			static inline const T go (const Derived & expr, quantity<I>)
			{
				return Functor::template run<I-1>(expr.template eval<I-1>()) + go<I-1>(expr);
			}
			static inline const T go (const Derived &, quantity<0>)
				{ return static_cast <T> (0); }
		};

		template <typename Expr>
		struct Equivalence
		{
			template <unsigned I>
			static inline bool test (const Derived & lval, const Expr & rval) { return test(lval, rval, quantity<I>()); }

		private:
			template <unsigned I>
			static inline bool test (const Derived & lval, const Expr & rval, quantity<I>)
			{
				return (lval.template eval<I-1>() == rval.template eval<I-1>()) && test<I-1>(lval, rval);
			}
			static inline bool test (const Derived &, const Expr &, quantity<0>) { return true; }
		};

	public:
		template <typename Functor> inline const T reduce () const
			{ return Reduce<Functor>::template go<D>(*static_cast <const Derived *> (this)); }

		template <typename Functor> inline const T reduce (const Functor & f) const
			{ return Reduce<Functor>::template go<D>(f, *static_cast <const Derived *> (this)); }

		template <typename Expr>
		inline bool	equals (const Expr & expr, const typename Expr::ExprType * = 0) const
			{ return Equivalence< Expr >::template test<D>(*static_cast <const Derived *> (this), expr); }

		template <typename V>
		inline bool	equals (const V & v, const typename V::VectorType * = 0) const
			{ return v == *static_cast <const Derived *> (this); }
	};

	struct VRedSqFn 
	{
		template <int I, typename T>
		static inline T run (const T & val) { return SQ(val); }
	};

	template <typename LVal>
	class VRedDotEFn 
	{
	private:
		const typename LVal::ExprType _lval;

	public:
		inline VRedDotEFn (const LVal & lval)
			: _lval(lval) {}

		template <int I, typename T>
		inline T run (const T & val) const { return val * _lval.template eval<I>(); }
	};

	template <typename V>
	class VRedDotVFn 
	{
	private:
		const typename V::VectorType _v;

	public:
		inline VRedDotVFn (const V & v)
			: _v(v) {}

		template <int I, typename T>
		inline T run (const T & val) const { return val * _v.template element<I>(); }
	};

	// Expression builders:
	// Argument
	template <typename T, int D>
	class VExpArg : public VBaseExpr<T, VExpArg< T, D >, D >
	{ 
	private:
		const T _arg;

	public:
		inline VExpArg (const T & argg)
			: _arg (argg) {}

		template <int I>
		inline const T eval () const
		{ return _arg; }
	};

	// Vector
	template <typename V, int D>
	class VExpArgVec : public VBaseExpr< typename V::Precision, VExpArgVec< typename V::VectorType, D >, D >
	{
	private:
		const V _v;

	public:
		inline VExpArgVec (const V & v)
			: _v (v) {}

		template <int I>
		inline const typename V::Precision eval () const
		{ return _v.template element<I>(); }
	};
		
	// Pairing
	template <class LVal, typename RVal, class Op, int D>
	class VExpr2 : public VBaseExpr<typename Op::ResultType, VExpr2< typename LVal::ExprType, typename RVal::ExprType, Op, D >, D >
	{ 
	private:
		const LVal _lval;
		const RVal _rval;

	public:
		typedef VExpr2< typename LVal::ExprType, typename RVal::ExprType, Op, D > ExprType;
		typedef VBaseExpr<typename Op::ResultType, VExpr2< typename LVal::ExprType, typename RVal::ExprType, Op, D >, D > Base;
		typedef typename Op::ResultType Precision;

        inline VExpr2 (const LVal & lval, const RVal & rval)
			: _lval(lval), _rval(rval) {}

		template <int I>
		inline const typename Op::ResultType eval () const
			{ return Op::template evalOp<I>(_lval, _rval); }

	public:
		// Scalar
		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D>
			operator * (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }

		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D>
			operator / (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }

		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D>
			operator + (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D> (*this, VExpArg< Precision, D > (n)); }

		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D>
			operator - (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D> (*this, VExpArg< Precision, D > (n)); }

		// Vector
		template <typename V>
		inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D>
			operator + (const V & v) const
				{ return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D> (*this, VExpArgVec< V, D > (v)); }

		template <typename V>
		inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D>
			operator - (const V & v) const
				{ return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D> (*this, VExpArgVec< V, D > (v)); }

		// Other expressions
		template <typename RVal2>
		inline const VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D>
			operator + (const RVal2 & rval) const
				{ return VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D> (*this, rval); }

		template <typename RVal2>
		inline const VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D>
			operator - (const RVal2 & rval) const
				{ return VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D> (*this, rval); }

		template <typename RVal2>
		inline bool operator == (const RVal2 & rval) const { return equals(rval); }
	};

	// Mono
	template <typename RVal, typename Op, int D>
	class VExpr1 : public VBaseExpr<typename Op::ResultType, VExpr1< typename RVal::ExprType, Op, D >, D >
	{ 
	private:
		const RVal _rval;

	public:
		typedef VExpr1< typename RVal::ExprType, Op, D > ExprType;
		typedef typename Op::ResultType Precision;

        inline VExpr1 (const typename RVal::ExprType & rval)
			: _rval(rval) {}

		template <int I>
		inline const typename Op::ResultType eval () const
			{ return Op::template evalOp<I>(_rval); }

	public:

		// Scalar
		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D>
			operator * (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpScale< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }

		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D>
			operator / (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpContract< Precision, D >, D> (*this, VExpArg< Precision, D > (n)); }

		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D>
			operator + (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpAdd< Precision >, D> (*this, VExpArg< Precision, D > (n)); }

		inline const VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D>
			operator - (const Precision & n) const
				{ return VExpr2< ExprType, VExpArg< Precision, D >, VOpSub< Precision >, D> (*this, VExpArg< Precision, D > (n)); }

		// Vector
		template <typename V>
		inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D>
			operator + (const V & v) const
				{ return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpAdd< Precision >, D> (*this, VExpArgVec< V, D > (v)); }

		template <typename V>
		inline const VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D>
			operator - (const V & v) const
				{ return VExpr2< ExprType, VExpArgVec< typename V::VectorType, D >, VOpSub< Precision >, D> (*this, VExpArgVec< V, D > (v)); }

		// Other expressions
		template <typename RVal2>
		inline const VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D>
			operator + (const RVal2 & rval) const
				{ return VExpr2< ExprType, typename RVal2::ExprType, VOpAdd< Precision >, D> (*this, rval); }

		template <typename RVal2>
		inline const VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D>
			operator - (const RVal2 & rval) const
				{ return VExpr2< ExprType, typename RVal2::ExprType, VOpSub< Precision >, D> (*this, rval); }

		template <typename RVal2>
		inline bool operator == (const RVal2 & rval) const { return equals(rval); }
	};

	template <typename T, typename V, typename Desc, int D >
	struct VectorDefsTag : public VExprTagDim< T, D >
	{
		typedef VectorDefsTag<T , V, Desc, D>		VectorTagType;
		typedef V									VectorType;
		typedef VExpArg< T, D >						Arg;
		typedef VExpArgVec< V, D >					ArgVec;

		template <int I>
		inline const T element () const
		{  
#ifdef _DEBUG
			int i = I;
			assert(I < D);
#endif
			return reinterpret_cast <const T *> (static_cast <const Desc *> (this)) [I]; 
		}
	};

	template <typename T, typename V, typename Desc, int D >
	class ArithmeticVector : public VectorDefsTag< T, V, Desc, D >
	{
	public:
		//typedef ArithmeticVector< T, V, Desc, D >			ArithmeticVectorType;
		typedef VectorDefsTag< T, V, Desc, D >				Base;
		typedef VExprTagDim< T, D >							ExprTag;
		
		using typename Base::Arg;
		using typename Base::ArgVec;
		using typename ExprTag::OpNegate;
		using typename ExprTag::OpScale;
		using typename ExprTag::OpContract;
		using typename ExprTag::OpAdd;
		using typename ExprTag::OpSub;

		typedef VExpr1< ArgVec, OpNegate, D >				ExpNeg;
		typedef VExpr2< ArgVec, Arg, OpScale, D >			ExpScale;
		typedef VExpr2< ArgVec, Arg, OpContract, D >		ExpContract;
		typedef VExpr2< ArgVec, Arg, OpAdd, D >				ExpAddScalar;
		typedef	VExpr2< ArgVec, ArgVec, OpAdd, D >			ExpAddVector;
		typedef VExpr2< ArgVec, Arg, OpSub, D >				ExpSubScalar;
		typedef VExpr2< ArgVec, ArgVec, OpSub, D >			ExpSubVector;
		
		template <typename Expr>
		struct ExpOpExp
		{
			typedef VExpr2< ArgVec, typename Expr::ExprType, OpAdd, D > Add;
			typedef VExpr2< ArgVec, typename Expr::ExprType, OpSub, D > Sub;
		};

		// Negation
		inline ExpNeg neg () const
			{ return ExpNeg (ArgVec(* static_cast <const V *> (this))); }

		// Scaling
		inline ExpScale scale (const T & n) const
			{ return ExpScale (ArgVec(* static_cast <const V *> (this)), Arg(n)); }

		// Contraction
		inline ExpContract contract (const T & n) const
			{ return ExpContract (ArgVec(* static_cast <const V *> (this)), Arg(n)); }

		// Add scalar
		inline ExpAddScalar add (const T & n) const
			{ return ExpAddScalar (ArgVec(* static_cast <const V *> (this)), Arg(n)); }

		// Add vector
		inline ExpAddVector add (const V & v) const
			{ return ExpAddVector (ArgVec(* static_cast <const V *> (this)), ArgVec(v)); }

		// Subtract scalar
		inline ExpSubScalar sub (const T & n) const
			{ return ExpSubScalar (ArgVec(* static_cast <const V *> (this)), Arg(n)); }

		// Subtract vector
		inline ExpSubVector sub (const V & v) const
			{ return ExpSubVector (ArgVec(* static_cast <const V *> (this)), ArgVec(v)); }
	};

	template <typename T, typename V>
	struct DescV2 : public ArithmeticVector< T, V, DescV2< T, V >, 2 >
	{
		typedef ArithmeticVector< T, V, DescV2< T, V >, 2 > 	Base;
		
		T x, y;

		inline DescV2 () 
			: x(0), y(0) {}
		inline DescV2 (const T x, const T y)
			: x(x), y(y) {}

		template <typename E, typename Vv>
		inline DescV2 (const DescV2 <E, Vv> & copy)
			: x(static_cast <T> (copy.x)), 
			  y(static_cast <T> (copy.y)) {}

		template <typename Exp>
		inline V & operator = (const Exp & expr) const
		{ 
			x = expr.template eval<0>();
			y = expr.template eval<1>();
			return * static_cast <const V *> (this);
		}
	};

	template <typename T, typename V>
	struct DescV3 : public ArithmeticVector< T, V, DescV3< T, V >, 3 >
	{
		typedef ArithmeticVector< T, V, DescV3< T, V >, 3 > 	Base;
		
		T x, y, z;

		inline DescV3 () 
			: x(0), y(0), z(0) {}
		inline DescV3 (const T x, const T y, const T z)
			: x(x), y(y), z(z) {}

		template <typename E, typename Vv>
		inline DescV3 (const DescV3 <E, Vv> & copy)
			: x(static_cast <T> (copy.x)), 
			  y(static_cast <T> (copy.y)),
			  z(static_cast <T> (copy.z)) {}

		template <typename Exp>
		inline V & operator = (const Exp & expr) const
		{ 
			x = expr.template eval<0>();
			y = expr.template eval<1>();
			z = expr.template eval<2>();
			return * static_cast <const V *> (this);
		}
	};

	template <typename T, typename V>
	struct DescQ : public VectorDefsTag< T, V, DescQ< T, V >, 3 >
	{
		typedef VectorDefsTag< T, V, DescQ< T, V >, 3 > Base;
		
		T x, y, z, w;

		inline DescQ () 
			: x(0), y(0), z(0), w(0) {}
		inline DescQ (const T x, const T y, const T z, const T w)
			: x(x), y(y), z(z), w(w) {}

		template <typename Vv>
		inline DescQ (const DescV3< T, Vv> & v)
			: x(v.x), y(v.y), z(v.z), w(0) {}

		template <typename Vv>
		inline DescQ (const T s, const DescV3< T, Vv> & v)
			: x(v.x), y(v.y), z(v.z), w(s) {}

		template <typename E>
		inline DescQ (const DescQ <E, V> & copy)
			: x(static_cast <T> (copy.x)), 
			  y(static_cast <T> (copy.y)),
			  z(static_cast <T> (copy.z)),
			  w(static_cast <T> (copy.w)) {}
	};

	template <typename T> class vector3D;
	template <typename T> class SphericalCoords;
	template <typename T> class PolarCoords;
	template <typename T> class CylindricalCoords;

	// *** Vector 2D class template ***	
	template <typename T>
	class vector2D : public DescV2< T, vector2D< T > >, private MathOverrides
	{
	private:
		typedef DescV2< T, vector2D< T > > Base;
		
	public:
		template <typename Exp>
		inline vector2D (const Exp & expr, const typename Exp::ExprType * dummy = 0)
			: DescV2< T, vector2D< T > >(expr.template eval<0>(), expr.template eval<1>()) {}

		inline vector2D ()
			: DescV2< T, vector2D< T > >() {}
		inline vector2D (const T x, const T y)
			: DescV2< T, vector2D< T > >(x, y) {}
		template <typename E>
		inline vector2D (const vector2D <E> & other)
			: DescV2< T, vector2D< T > >(other) {}

		// BEGIN ArithmeticVector INHERITANCE {
		// Binary expression mappings
		template <typename Expr>
		inline typename Base::template ExpOpExp<typename Expr::ExprType>::Add operator + (const Expr & rval) const			
			{ return typename Base::template ExpOpExp<typename Expr::ExprType>::Add (typename Base::ArgVec(*this), rval); }

		template <typename Expr>
		inline typename Base::template ExpOpExp<typename Expr::ExprType>::Sub operator - (const Expr & rval) const			
			{ return typename Base::template ExpOpExp<typename Expr::ExprType>::Sub (typename Base::ArgVec(*this), rval); }

		template <typename Expr>
		inline typename enable_ifb< has_def< typename Expr::ExprType >::value, bool >::Type operator == (const Expr & rval) const 
			{ return rval.equals(*this); }
			
		// Import dependent types
		using typename Base::ExpNeg;
		using typename Base::ExpScale;
		using typename Base::ExpContract;
		using typename Base::ExpAddScalar;
		using typename Base::ExpAddVector;
		using typename Base::ExpSubScalar;
		using typename Base::ExpSubVector;
		
		using typename Base::VectorType;

		// Basic binary mappings
		inline ExpNeg operator - () const								{ return this->neg(); }
		inline ExpScale operator * (const T & n) const					{ return this->scale(n); }
		inline ExpContract operator / (const T & n) const				{ return this->contract(n); }
		inline ExpAddScalar operator + (const T & n) const				{ return this->add(n); }
		inline ExpAddVector operator + (const VectorType & v) const		{ return this->add(v); }
		inline ExpSubScalar operator - (const T & n) const				{ return this->sub(n); }
		inline ExpSubVector operator - (const VectorType & v) const		{ return this->sub(v); }
		// } END ArithmeticVector INHERITANCE

		// Dot-product
		inline T operator * (const vector2D<T> & v) const
		{ return this->x * v.x + this->y * v.y; }

		// Maps to 2D-cross product below
		inline const vector2D <T> operator & (const vector3D <T> & v) const
		{ return operator & (v.z); }

		// 2D-cross product (perpendicular)
		inline const vector2D <T> operator & (const T s) const
		{ return vector2D<T> (this->y * s, -this->x * s); }

		// Accumulate scalar
		inline const vector2D <T> & operator += (const T & n)
		{
			this->x += n;
			this->y += n;
			return *this;
		}

		// Accumulate vector
		inline const vector2D <T> & operator += (const vector2D<T> & v)
		{
			this->x += v.x;
			this->y += v.y;

			return *this;
		}

		// Decrement vector
		inline const vector2D <T> & operator -= (const vector2D<T> & v)
		{
			this->x -= v.x;
			this->y -= v.y;

			return *this;
		}
		inline T magSQ () const { return static_cast <T> (SQ(this->x) + SQ(this->y)); }
		inline T magnitude () const { return static_cast <T> (sqrt(magSQ())); }
		inline vector2D <T> normalize () const
		{
			const T m = magnitude();

			if (m == 0)
				return vector2D <T> (0,0);
			else
				return vector2D<T> (
					this->x / m,
					this->y / m
				);
		}

		template <typename J>
		inline bool operator != (const vector2D <J> & other)
		{
			return this->x != other.x || this->y != other.y;
		}
		template <typename J>
		inline bool operator == (const vector2D <J> & other)
		{
			return this->x == other.x && this->y == other.y;
		}

		// Assignment
		template <typename J>
		inline vector2D <T> & operator = (const vector2D <J> & copy)
		{
			this->x = static_cast <T> (copy.x);
			this->y = static_cast <T> (copy.y);

			return *this;
		}

		// Scale vector
		inline vector2D <T> & operator *= (const T factor)
		{
			this->x *= factor;
			this->y *= factor;
			return *this;
		}

		// Contract vector
		inline vector3D <T> & operator /= (const T factor)
		{
			this->x /= factor;
			this->y /= factor;
			return *this;
		}

		// Length comparisons
		inline bool operator < (const T len) const
			{ return magSQ() < SQ(len); }
		inline bool operator > (const T len) const
			{ return magSQ() > SQ(len); }
		inline bool operator <= (const T len) const
			{ return magSQ() <= SQ(len); }
		inline bool operator >= (const T len) const
			{ return magSQ() >= SQ(len); }

		inline bool operator == (const vector2D< T > & b) const
			{ return this->x == b.x && this->y == b.y; }
	};

	// *** Vector 3D class template ***	
	template <typename T>
	class vector3D : public DescV3< T, vector3D< T > >, private MathOverrides
	{
	private:
		typedef DescV3< T, vector3D< T > > Base;
		
	public:
		template <typename Exp>
		inline vector3D (const Exp & expr, const typename Exp::ExprType * dummy = 0)
			: DescV3< T, vector3D< T > >(expr.template eval<0>(), expr.template eval<1>(), expr.template eval<2>()) {}

		template <typename E>
		inline vector3D (const SphericalCoords< E > & sphc)
			: DescV3< T, vector3D< T > >(
				static_cast <T> (sphc.r * sin(sphc.zenith) * cos(sphc.azimuth)),
				static_cast <T> (sphc.r * sin(sphc.zenith) * sin(sphc.azimuth)),
				static_cast <T> (sphc.r * cos(sphc.zenith))
			) {}

		inline vector3D ()
			: DescV3< T, vector3D< T > >() {}
		inline vector3D (const T x, const T y, const T z)
			: DescV3< T, vector3D< T > >(x, y, z) {}

		template <typename E>
		inline vector3D (const vector3D <E> & other)
			: DescV3< T, vector3D< T > > (other) {}

		// Spherical adapter
		inline vector3D operator = (const SphericalCoords <T> & sphc)
		{
			this->x = sphc.r * sin(sphc.zenith) * cos(sphc.azimuth);
			this->y = sphc.r * sin(sphc.zenith) * sin(sphc.azimuth);
			this->z = sphc.r * cos(sphc.zenith);
		}
		template <typename E>
		inline vector3D<E> & cast ()
		{
			return vector3D <E> (
				static_cast<T> (this->other.x),
				static_cast<T> (this->other.y),
				static_cast<T> (this->other.z)
			);
		}

		// BEGIN ArithmeticVector INHERITANCE {
		// Binary expression mappings
		template <typename Expr>
		inline typename Base::template ExpOpExp<typename Expr::ExprType>::Add operator + (const Expr & rval) const			
			{ return typename Base::template ExpOpExp<typename Expr::ExprType>::Add (typename Base::ArgVec(*this), rval); }

		template <typename Expr>
		inline typename Base::template ExpOpExp<typename Expr::ExprType>::Sub operator - (const Expr & rval) const			
			{ return typename Base::template ExpOpExp<typename Expr::ExprType>::Sub (typename Base::ArgVec(*this), rval); }

		template <typename Expr>
		inline typename enable_ifb< has_def< typename Expr::ExprType >::value, bool >::Type operator == (const Expr & rval) const 
			{ return rval.equals(*this); }

		// Import dependent types
		using typename Base::ExpNeg;
		using typename Base::ExpScale;
		using typename Base::ExpContract;
		using typename Base::ExpAddScalar;
		using typename Base::ExpAddVector;
		using typename Base::ExpSubScalar;
		using typename Base::ExpSubVector;
		
		using typename Base::VectorType;

		// Basic binary mappings
		inline ExpNeg operator - () const								{ return this->neg(); }
		inline ExpScale operator * (const T & n) const					{ return this->scale(n); }
		inline ExpContract operator / (const T & n) const				{ return this->contract(n); }
		inline ExpAddScalar operator + (const T & n) const				{ return this->add(n); }
		inline ExpAddVector operator + (const VectorType & v) const		{ return this->add(v); }
		inline ExpSubScalar operator - (const T & n) const				{ return this->sub(n); }
		inline ExpSubVector operator - (const VectorType & v) const		{ return this->sub(v); }
		// } END ArithmeticVector INHERITANCE

		// Shortcut mappings
		inline T operator * (const vector3D<T> & v) const { return dot(v); }
		inline bool operator || (const vector3D<T> & v) const { return orthogonal(v); }
		inline const vector3D <T> operator & (const vector3D <T> & v) const { return cross(v); }

		inline bool operator == (const vector3D< T > & b) const { return equals(b); }
		template <typename J> inline bool operator != (const vector3D <J> & other) const { return nequal(other); }
		template <typename J> inline bool operator == (const vector3D <J> & other) const { return equals(other); }

		inline bool operator < (const T len) const { return lesser(len); }
		inline bool operator > (const T len) const { return greater(len); }
		inline bool operator <= (const T len) const { return lessequal(len); }
		inline bool operator >= (const T len) const { return greatequal(len); }

		// Vector2 adapter
		inline operator vector2D <T> () const { return vector2D <T> (this->x, this->y); }

		// 3D-specific operations
		inline T dot (const vector3D<T> & v) const 
			{ return this->x * v.x + this->y * v.y + this->z * v.z; }
		inline bool orthogonal (const vector3D<T> & v) const
			{ return Matrix3D(vector3D<T>(1,1,1), *this, v) == 0; }
		inline const vector3D <T> cross (const vector3D <T> & v) const
		{
			return vector3D <T> (
				this->y*v.z - this->z*v.y,
				this->z*v.x - this->x*v.z,
				this->x*v.y - this->y*v.x
			);
		}

		// Length
		inline T magSQ () const { return static_cast <T> (SQ(this->x) + SQ(this->y) + SQ(this->z)); }
		inline T magnitude () const { return static_cast <T> (sqrt(magSQ())); }
		inline vector3D <T> normalize () const
		{
			const T m = magnitude();

			if (m == 0)
				return vector3D <T> (0,0,0);
			else
				return vector3D<T> (
					this->x / m,
					this->y / m,
					this->z / m
				);
		}

		// Equivalence
		inline bool equals (const vector3D< T > & b) const
			{ return this->x == b.x && this->y == b.y && this->z == b.z; }

		template <typename J>
		inline bool nequal (const vector3D <J> & other) const
			{ return this->x != other.x || this->y != other.y || this->z != other.z; }

		template <typename J>
		inline bool equals (const vector3D <J> & other) const
			{ return this->x == other.x && this->y == other.y && this->z == other.z; }

		// Length comparisons
		inline bool lesser (const T len) const
			{ return magSQ() < SQ(len); }
		inline bool greater (const T len) const
			{ return magSQ() > SQ(len); }
		inline bool lessequal (const T len) const
			{ return magSQ() <= SQ(len); }
		inline bool greatequal (const T len) const
			{ return magSQ() >= SQ(len); }

		inline vector2D <T> v2D() const { return vector2D <T> (this->x, this->y); }

		// Accumulate scalar
		inline const vector3D <T> & operator += (const T & n)
		{
			this->x += n;
			this->y += n;
			this->z += n;
		}

		// Accumulate vector
		inline const vector3D <T> & operator += (const vector3D<T> & v)
		{
			this->x += v.x;
			this->y += v.y;
			this->z += v.z;

			return *this;
		}

		// Decrement by vector
		inline const vector3D <T> & operator -= (const vector3D<T> & v)
		{
			this->x -= v.x;
			this->y -= v.y;
			this->z -= v.z;

			return *this;
		}

		// Assignment
		template <typename J>
		inline vector3D <T> & operator = (const vector3D <J> & copy)
		{
			this->x = static_cast <T> (copy.x);
			this->y = static_cast <T> (copy.y);
			this->z = static_cast <T> (copy.z);

			return *this;
		}

		// Scale vector
		inline vector3D <T> & operator *= (const T factor)
		{
			this->x *= factor;
			this->y *= factor;
			this->z *= factor;
			return *this;
		}

		// Contract vector
		inline vector3D <T> & operator /= (const T factor)
		{
			this->x /= factor;
			this->y /= factor;
			this->z /= factor;
			return *this;
		}
	};
	// Swap-operator mappings
	template <typename V>
	inline typename V::ExpScale operator * (const typename V::Precision f, const V & v) 
		{ return v.scale(f); }

	template <typename T, typename RVal>
	inline VExpr2< typename RVal::ExprType, VExpArg< T, RVal::DIMENSION >, VOpScale< T, RVal::DIMENSION >, RVal::DIMENSION > 
		operator * (const T f, const RVal & rval) 
			{ return VExpr2< typename RVal::ExprType, VExpArg< T, RVal::DIMENSION >, VOpScale< T, RVal::DIMENSION >, RVal::DIMENSION > (rval, VExpArg< T, RVal::DIMENSION > (f)); }

	// *** Generic mappings ***
	// ** Expressions **
	template <typename E> 
	inline typename E::ExprType::Precision
		MAG(const E & expr) { return MathOverrides::sqrt(expr.template reduce<VRedSqFn>()); }

	template <typename E> 
	inline typename E::ExprType::Precision
		MAGSQ(const E & expr) { return expr.template reduce<VRedSqFn>(); }

	template <typename V> 
	inline typename V::VectorType::Precision
		DOT(const V & a, const V & b) { return a * b; }

	template <typename V, typename Expr> 
	inline typename enable_ifb< andb< has_def< typename V::VectorType>::value, has_def< typename Expr::ExprType >::value >::value, typename V::Precision >::Type
		DOT(const V & a, const Expr & b) { return b.template reduce< VRedDotVFn< V > > (VRedDotVFn< V >(a)); }

	template <typename Expr, typename V> 
	inline typename enable_ifb< andb< has_def< typename Expr::ExprType >::value, has_def< typename V::VectorType >::value >::value, typename Expr::Precision >::Type
		DOT(const Expr & a, const V & b) { return a.template reduce< VRedDotVFn< V > > (VRedDotVFn< V >(b)); }

	template <typename LVal, typename RVal> 
	inline typename enable_ifb< andb< has_def< typename LVal::ExprType >::value, has_def< typename RVal::ExprType >::value >::value, typename LVal::Precision >::Type
		DOT(const LVal & a, const RVal & b) { return a.template reduce<VRedDotEFn< RVal > > (VRedDotEFn< RVal > (b)); }

	template <typename A, typename B>
	inline typename enable_ifb< andb< has_def<typename A::VExprTagType>::value, has_def<typename B::VExprTagType>::value >::value, typename A::Precision>::Type 
		ANGLE (const A & a, const B & b) { return acosf (DOT(a,b) / (MAG(a) * MAG(b))); }

	template <typename V> 
	inline typename V::VectorType::Precision
		MAG(const V & v) { return v.magnitude(); }

	template <typename V> 
	inline typename V::VectorType::Precision
		MAGSQ(const V & v) { return v.magSQ(); }

	template <typename V>
	inline typename V::VectorType
		U(const V & a) { return a.normalize(); }

	template <typename Expr, typename V>
	inline typename enable_ifb< andb< has_def< typename Expr::ExprType >::value, has_def< typename V::VectorType >::value >::value >::Type
		U(const Expr & a) { return V(a.normalize()); }

	template < typename TNew, typename E > 
	inline VExpr1< E, VOpCast< typename E::ExprType::Precision, TNew >, E::ExprType::DIMENSION >
		CAST(const E & expr) { return VExpr1< E, VOpCast< typename E::ExprType::Precision, TNew >, E::ExprType::DIMENSION > (expr); }

	typedef vector2D<float> vector2Df;
	typedef vector2D<double> vector2Dd;
	typedef vector3D<float> vector3Df;
	typedef vector3D<double> vector3Dd;

	// Spherical coordinates, definition taken from http://mathworld.wolfram.com/SphericalCoordinates.html
	template <typename T>
	class SphericalCoords : private MathOverrides
	{
	public:
		typedef T Precision;

		T r;
		float 
			zenith,		// The circle with a vertical bar through it, thus represents the polar angle
			azimuth;	// Looks like "theta", circle with horizontal bar through it, thus represents the angle in the x/y plane

		inline SphericalCoords () : r(0), zenith(0), azimuth(0) {}

		inline SphericalCoords (const vector3D <T> & v)
			:	r(static_cast< T > (sqrt(SQ(v.x) + SQ(v.y) + SQ(v.z)))), 
				zenith(atan2f(static_cast< float > (v.y), static_cast< float > (v.x))), 
				azimuth(0)
		{
			if (r != 0)
				azimuth = acos(static_cast< float > (v.z) / static_cast< float > (r));
		}
		inline SphericalCoords (const T r, const float zenith, const float azimuth)
			: r(r), zenith(zenith), azimuth(azimuth) {}

		inline SphericalCoords <T> & operator = (const vector3D <T> & v)
		{
			r = sqrt(SQ(v.x) + SQ(v.y) + SQ(v.z));

			azimuth = atan2(
				static_cast< float > (v.y), 
				static_cast< float > (v.x)
			);

			if (r != 0)
				zenith = acos(
					static_cast< float > (v.z) 
					/ 
					static_cast< float > (r)
				);

			return *this;
		}
		inline const vector3D <T> operator + (const vector3D <T> rval) const { return static_cast <const vector3D<T> > (*this) + rval; }
		inline operator const vector3D <T> () const
		{
			return vector3D <T> 
			(
				static_cast< T > (static_cast< float > (r) * sin(zenith) * cos(azimuth)),
				static_cast< T > (static_cast< float > (r) * sin(zenith) * sin(azimuth)),
				static_cast< T > (static_cast< float > (r) * cos(zenith))
			);
		}
		inline SphericalCoords <T> & operator = (const CylindricalCoords <T> & c)
		{
			r = static_cast< T > (sqrt(SQ(c.p) + SQ(c.z)));
			zenith = atan2(c.p, c.z);
			azimuth = c.azimuth;
			return *this;
		}
		inline operator const CylindricalCoords <T> () const
		{
			return CylindricalCoords <T> 
			(
				static_cast< T > (static_cast< float > (r) * sin(zenith)),
				azimuth,
				static_cast< T > (static_cast< float > (r) * cos(zenith))
			);
		}
	};

	template <typename T>
	class PolarCoords
	{
	private:
		inline PolarCoords <T> & assign (const T x, const T y)
		{
			p = sqrt(SQ(x) + SQ(y));
			azimuth = atan2(y, x);
			return *this;
		}

	public:
		T p;
		float azimuth;

		inline PolarCoords () : p(0), azimuth(0) {}
		inline PolarCoords (const vector2D <T> & v) : p(sqrt(SQ(v.x)) + sqrt(SQ(v.y))), azimuth(atan2(v.y, v.x)) {}

		inline PolarCoords (const T p, const float azimuth)
			: p(p), azimuth(azimuth) {}

		inline PolarCoords <T> & operator = (const vector2D <T> & v)
			{ return assign(v.x, v.y);	 }		
		inline PolarCoords <T> & operator = (const vector3D <T> & v)
			{ return assign(v.x, v.y); }
		
		template <typename J>
		inline operator const vector2D <J> () const
		{
			return vector2D <J> (
				static_cast <J> (static_cast <double> (p) * cos(azimuth)),
				static_cast <J> (static_cast <double> (p) * sin(azimuth))
			);
		}
		inline PolarCoords <T> & operator = (const SphericalCoords <T> & s)
		{
			p = s.r * sin(s.zenith);
			azimuth = s.azimuth;
			return *this;
		}
	};

	template <typename T>
	class CylindricalCoords : public PolarCoords <T>
	{
	public:
		T z;

		inline CylindricalCoords () : PolarCoords<T> (0, 0), z(0) {}
		inline CylindricalCoords (const vector3D <T> & v) : PolarCoords<T> (v), z(v.z) {}

		inline CylindricalCoords (const T p, const float azimuth, const T z)
			: PolarCoords<T>(p, azimuth), z(z) {}

		inline CylindricalCoords <T> & operator = (const vector3D <T> & v)
		{
			PolarCoords <T>::operator = (v);
			z = v.z;
			return *this;
		}
		inline CylindricalCoords <T> & operator = (const SphericalCoords <T> & s)
		{
			PolarCoords <T>::operator = (s);
			z = s.r * cos(s.zenith);
			return *this;
		}
		template <typename J>
		inline operator const vector3D <J> () const
		{
			return vector3D <J> (
				static_cast <J> (static_cast <double> (this->p) * cos(this->azimuth)),
				static_cast <J> (static_cast <double> (this->p) * sin(this->azimuth)),
				static_cast <J> (z)
			);
		}
		inline operator const SphericalCoords <T> () const
		{
			return SphericalCoords <T>
			(
				sqrt(SQ(this->p) + SQ(z)),
				atan2(this->p, z),
				this->azimuth
			);
		}
	};

	template <typename T>
	class Matrix3D
	{
	public:
		typedef T Precision;

		T values [3][3];

		inline Matrix3D (const T a1, const T b2, const T c3)
		{
			values[0][0] = a1; values[0][1] = 0;  values[0][2] = 0;
			values[1][0] = 0;  values[1][1] = b2; values[1][2] = 0;
			values[2][0] = 0;  values[2][1] = 0;  values[2][2] = c3; 
		}
		inline Matrix3D (
			const T a1, const T b1, const T c1, 
			const T a2, const T b2, const T c2,
			const T a3, const T b3, const T c3
		) 
		{
			values[0][0] = a1; values[0][1] = b1; values[0][2] = c1;
			values[1][0] = a2; values[1][1] = b2; values[1][2] = c2;
			values[2][0] = a3; values[2][1] = b3; values[2][2] = c3; 
		}
		inline Matrix3D (
			const vector3D<T> & a, vector3D<T> b, vector3D<T> c
		) 
		{
			values[0][0] = a.x; values[0][1] = a.y; values[0][2] = a.z;
			values[1][0] = b.x; values[1][1] = b.y; values[1][2] = b.z;
			values[2][0] = c.x; values[2][1] = c.y; values[2][2] = c.z;
		}

		inline operator T ()
		{
			return 
				values[0][0] * values[1][1] * values[2][2] -
				values[0][0] * values[1][2] * values[2][1] +
				values[0][1] * values[1][2] * values[2][0] -
				values[0][1] * values[1][0] * values[2][2] +
				values[0][2] * values[1][0] * values[2][1] -
				values[0][2] * values[1][1] * values[2][0];
		}
		inline mars::vector3D <T> operator * (const mars::vector3D <T> v) const
		{
			return vector3D<T> (
				v.x * values[0][0] + v.y * values[0][1] + v.z * values[0][2],
				v.x * values[1][0] + v.y * values[1][1] + v.z * values[1][2],
				v.x * values[2][0] + v.y * values[2][1] + v.z * values[2][2]
			);
		}
		inline mars::Matrix3D <T> transpose () const
		{
			return Matrix3D (
				values[0][0], values[1][0], values[2][0],
				values[0][1], values[1][1], values[2][1],
				values[0][2], values[1][2], values[2][2]
			);
		}
		static inline mars::Matrix3D <T> rotateZ (float theta)
		{
			return Matrix3D <T> (
				cos(theta), -sin(theta), 0,
				sin(theta), cos(theta), 0,
				0, 0, 1
			);
		}
	};

	template <typename T>
	class Quaternion : public DescQ< T, Quaternion< T > >
	{
	public:
		inline Quaternion (const T w, const T x, const T y, const T z)
			: DescQ< T, Quaternion< T > >(x, y, z, w) {}
		inline Quaternion (const vector3D <T> & vec)
			: DescQ< T, Quaternion< T > >(vec) {}
		inline Quaternion (const T s, const vector3D <T> & v)
			: DescQ< T, Quaternion< T > >(s, v) {}
		inline Quaternion ()
			: DescQ< T, Quaternion< T > >() {}

		static 
		inline Quaternion <T> 
		rotation (const T theta, const vector3D <T> & u)
		{
			return Quaternion <T> (
				cos(theta / 2),
				u * sin(theta / 2)
			);
		}

		inline const Quaternion <T> operator * (const Quaternion <T> & q) const
		{
			const T s1 = this->w, s2 = q.w;
			const vector3D <T> & v1 = this->v(), v2 = q.v();

			return Quaternion <T> (
				s1 * s2 - (v1 * v2),
				(s1 * v2) + (s2 * v1) + (v1 & v2)
			);
		}
		inline const Quaternion <T> operator - () const
		{
			return Quaternion <T> (this->w, -this->x, -this->y, -this->z);
		}
		inline const Quaternion <T> operator ! () const
		{
			const Quaternion <T> qq = operator - ();
			return qq / (operator * (qq));
		}
		inline const Quaternion <T> operator / (const Quaternion <T> & q) const
		{
			const T divisor = (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
			return Quaternion <T> (
				(q.x*this->x + q.y*this->y + q.z*this->z + q.w*this->w) / divisor,
				(this->x*q.w - this->y*q.z + this->z*q.y - this->w*q.x) / divisor,
				(this->x*q.z + this->y*q.w - this->z*q.x - this->w*q.y) / divisor,
				(this->y*q.x + this->z*q.w - this->x*q.y - this->w*q.z) / divisor
			);
		}
		inline const Quaternion <T> operator & (const Quaternion <T> & q) const
		{
			return operator * (q * operator ! ());
		}
		inline operator vector3D <T> () const
		{
			return this->v();
		}
	};
}

#endif