// © Copyright 2008 Vladimir Frolov, Moscow State University Graphics & Media Lab
#define MGML_VECTOR_GUARDIAN

#ifndef MGML_MATH_GUARDIAN
	#include "MGML_MATH.h"
#endif


using namespace MPMG;

// это макрос включает использование 4D векторов как 3D
// если нужно использовать четвертое измерение - #define DISCARD_FOURTH_DIMENTION 0
#define DISCARD_FOURTH_DIMENTION 1

namespace MGML_MATH
{
template<int n,class T>		  class VECTOR;
template<int n,int m,class T> class MATRIX;


// обозначим внутреннею структуру векторов.
template<class T>
struct Tvec2
{
	
  union
  {
    struct
    {
      T	x,y;
    };
    T M[2];
  };


   inline universal_call void set(const T a,const T b)
   {
	   M[0]=a;
	   M[1]=b;
   }

   //universal_call inline T& x() {return M[0];}
   //universal_call inline T& y() {return M[1];}

   //universal_call T x() const {return M[0];}
   //universal_call T y() const {return M[1];}

};

template<class T>
struct Tvec3
{
	 
  union
  {
    struct
    {
      T	x,y,z;
    };
    T M[3];
  };
 
  inline universal_call void set(const T a,const T b,const T c)
  {
   M[0]=a;
   M[1]=b;
   M[2]=c;
  }

  //universal_call inline T& x() {return M[0];}
  //universal_call inline T& y() {return M[1];}
  //universal_call inline T& z() {return M[2];}

  //universal_call inline T x() const {return M[0];}
  //universal_call inline T y() const {return M[1];}
  //universal_call inline T z() const {return M[2];}

};


template<class T>
struct Tvec4
{
  union
  {
    struct
    {
      T	x,y,z,w;
    };
    T M[4];
  };

  inline universal_call void set(const T a,const T b,const T c,const T d)
  {
   M[0]=a;
   M[1]=b;
   M[2]=c;
   M[3]=d;
  }

  //universal_call inline T& x() {return M[0];}
  //universal_call inline T& y() {return M[1];}
  //universal_call inline T& z() {return M[2];}
  //universal_call inline T& w() {return M[3];}

  //universal_call inline T x() const {return M[0];}
  //universal_call inline T y() const {return M[1];}
  //universal_call inline T z() const {return M[2];}
  //universal_call inline T w() const {return M[3];}

};
// вектор для SIMD
template<>
struct SSE_ALIGNED Tvec4<float>
{
#ifndef __CUDACC__
  union
  {
    __m128 ss;
    SSE_ALIGNED float M[4];
    struct
    {
      float x,y,z,w;
    };
  };
	
  inline universal_call operator __m128() const {return ss;}
#else
		SSE_ALIGNED float M[4];
#endif

  //universal_call float& x() {return M[0];}
  //universal_call float& y() {return M[1];}
  //universal_call float& z() {return M[2];}
  //universal_call float& w() {return M[3];}

  //universal_call float x() const {return M[0];}
  //universal_call float y() const {return M[1];}
  //universal_call float z() const {return M[2];}
  //universal_call float w() const {return M[3];}

  inline universal_call void set(const float a,const float b,const float c,const float d)
    { M[0]=a; M[1]=b; M[2]=c; M[3]=d; }



#ifndef __CUDACC__


  // нужно переопределять new[] так как иначе не работают
  // напрямую команды sse с адресами векторов, расположенных в массиве
  static void* operator new[](size_t size)
  {
	  void* ptr = _aligned_malloc(size,16);
	  if(!ptr)
		  throw std::bad_alloc("_aligned_malloc(): unsufficient memory");
	  return ptr;
  }

  static void operator delete[](void* raw_memory)
  {
	  if(!raw_memory) return;
	  _aligned_free(raw_memory);
  }
  static void* operator new(size_t size)
  {
	  void* ptr = _aligned_malloc(size,16);
	  if(!ptr)
		  throw std::bad_alloc("_aligned_malloc(): unsufficient memory");
	  return ptr;
  }

  static void operator delete(void* raw_memory)
  {
	  if(!raw_memory) return;

	  _aligned_free(raw_memory);
  }

#endif

};

template<int n,class T>
struct Default_Vector 
{
	T M[n];

	inline void set(T* ptr,unsigned int N)
	  { for(int i=0;i<n;i++) M[i] = ptr[i]; }

};
//===================================================================================

// метапрограммирование позволяет немного повыпендриваться
template<int n,class T>
class VECTOR: public SWITCH<(n),
					 CASE<2,Tvec2<T>,
					 CASE<3,Tvec3<T>,
					 CASE<4,Tvec4<T>,
					 CASE<DEFAULT,Default_Vector<n,T> > > > >
					 >::RET
{
public:

	enum { k = (DISCARD_FOURTH_DIMENTION && n==4) ? 3:n};

#ifndef __CUDACC__
	// конструкторы
	inline universal_call VECTOR(){} 
    
	inline universal_call VECTOR(const T* ptr)
	 { MovVec<k,T>::exec(M,ptr); }


  inline universal_call VECTOR(const VECTOR<n,T>& rhs)
	 {*this = rhs;}
 
	inline universal_call VECTOR(const T a,const T b)
	 { set(a,b); }

	inline universal_call VECTOR(const T a,const T b,const T c)
	 { set(a,b,c); }

	inline universal_call VECTOR(const T a,const T b,const T c,const T d)
	 { set(a,b,c,d); }

	// маленькая особенность 4D векторов
	inline universal_call VECTOR(const __m128& rhs)
	 { ss = rhs; }

#endif
	
	// векторное произведение
	inline universal_call VECTOR<n,T> cross(const VECTOR<n,T>& v) const
	 {return *this->*v;}

	// квадрат длины
	inline universal_call T lenSquare() const 
	 { return *this*(*this); }

	// длина
	inline universal_call T len() const {return sqrt(lenSquare());}

	inline universal_call void Normalize() { *this/=len();}

	inline universal_call void Zero()
	{ for(register int i=0;i<n;i++) this->M[i]=0; }

	// операторы
	inline universal_call VECTOR<n,T>& operator=(const VECTOR<n,T>& rhs)
	{
		// we must use n instead of k here to copy dta correctly
    MovVec<n,T>::exec(this->M, rhs.M);
		return *this;
	}

	// #CHECK_THAT
	inline universal_call bool operator==(const VECTOR<n,T>& rhs)
	{
		bool res = true;
		for(int i=0;i<n;i++) // USE n, not k !!!
		{
			if(abs(this->M[i] - rhs.M[i]) > EPSILON) // overload abs - not integer
			{
				res=false;
				break;
			}
		}
		return res;
	}

	inline universal_call bool operator!=(const VECTOR<n,T>& rhs)
		{return !(*this==rhs);}

	inline universal_call T operator[](int i) const
		{ ASSERT(i<n); return this->M[i];}

	inline universal_call T& operator[](int i)
		{ ASSERT(i<n); return this->M[i];}


	// скалярное произведение
	inline universal_call T operator*(const VECTOR<n,T>& rhs) const 
	{ return DotMult<k,T>::exec(this->M,rhs.M); }

	inline universal_call T dot(const VECTOR<n,T>& rhs) const 
	{ return *this*rhs; }


	// векторное произведение (продублировано из соображений оптимизации)
	inline universal_call VECTOR<3,T> operator->*(const VECTOR<3,T>& v) const
	{
		VECTOR<3,T> res;
		res.set(this->y*v.z - v.y*this->z,
			      this->z*v.x - v.z*this->x,
			      this->x*v.y - v.x*this->y);
		return res;
	}
	inline universal_call VECTOR<4,T> operator->*(const VECTOR<4,T>& v) const
	{
		VECTOR<n,T> res;
		res.set(this->y*v.z - v.y*this->z,
			      this->z*v.x - v.z*this->x,
			      this->x*v.y - v.x*this->y,1);
		return res;
	}


  // операторы взаимодействия с векторами  
  inline universal_call VECTOR operator+(const VECTOR& rhs) const
	{ 
	  VECTOR v;
	  AddVec<n,T>::exec(v.M,this->M,rhs.M);
	  return v;
	}

	inline universal_call VECTOR operator-(const VECTOR& rhs) const
	{  
	  VECTOR v;
	  SubVec<n,T>::exec(v.M,this->M,rhs.M);
	  return v;
	}

	inline universal_call VECTOR& operator+=(const VECTOR& rhs)
	{
    AddVec<n,T>::exec(this->M,rhs.M);
	  return *this;
	}

	inline universal_call VECTOR& operator-=(const VECTOR& rhs)
	{
    SubVec<n,T>::exec(this->M,rhs.M);
	  return *this;
	}

	inline universal_call VECTOR& operator*=(const VECTOR& rhs)
	{
    MulVec<n,T>::exec(this->M,rhs.M);
	  return *this;
	}

	//операторы взаимодействия c константами

	inline universal_call VECTOR<n,T> operator+(const T rhs) const
	{ 
	  VECTOR v;
	  AddVec<n,T>::exec(v.M,this->M,rhs);
	  return v;
	}

	inline universal_call friend VECTOR<n,T> operator+(const T lhs,const VECTOR<n,T>& rhs)
	 {return rhs + lhs;}

	inline universal_call VECTOR<n,T> operator-(const T rhs) const
	{ 
	  VECTOR<n,T> v;
	  SubVec<n,T>::exec(v.M,this->M,rhs);
	  return v;
	}

	inline universal_call friend VECTOR<n,T> operator-(const T lhs,const VECTOR<n,T>& rhs)
	 {return (-1)*(rhs - lhs);}

	inline universal_call VECTOR<n,T> operator*(const T rhs) const
	{ 
	  VECTOR<n,T> v;
	  MulVec<n,T>::exec(v.M,this->M,rhs);
	  return v;
	}

	inline universal_call friend VECTOR<n,T> operator*(const T lhs,const VECTOR<n,T>& rhs)
	 {return rhs * lhs;}

	inline VECTOR operator/(const T rhs) const
	{ 
	  VECTOR v;
	  DivVec<n,T>::exec(v.M,this->M,rhs);
	  return v;
	}

	inline universal_call VECTOR<n,T>& operator+=(const T rhs)
	{
    AddVec<n,T>::exec(this->M,rhs);
	  return *this;
	}

	inline universal_call VECTOR<n,T>& operator-=(const T rhs)
	{
    SubVec<n,T>::exec(this->M,rhs);
	  return *this;
	}

	inline universal_call VECTOR<n,T>& operator*=(const T rhs)
	{
    MulVec<n,T>::exec(this->M,rhs);
	  return *this;
	}

	inline universal_call VECTOR<n,T>& operator/=(const T rhs)
	{
    DivVec<n,T>::exec(this->M,rhs);
	  return *this;
	}
    

	VECTOR universal_call operator*(const MATRIX<n,n,T>& m) const
	{
    VECTOR res; res.Zero();

	  for(register int i=0;i<n;i++)
		  for(register int j=0;j<n;j++)
			  res.M[i] += this->M[j]*m.M[j][i];
	
	  return res;
	}

	// #CHECK_THAT
	VECTOR<n,T> universal_call operator*(const MATRIX<n+1,n+1,T>& m) const
	{
      VECTOR<n,T> res; res.Zero();
	  for(register int i=0;i<n;i++)
		  for(register int j=0;j<n;j++)
			  res.M[i] += this->M[j]*m.M[j][i];

	  for(int i=0;i<n;i++) res.M[i] += m.M[i][n];
      
	  T w = 0;
	  for(int i=0;i<n;i++) w +=  this->M[i]*m.M[n][i]; 
	  
	  w += m.M[n][n];

	  w==0 ? w=EPSILON_E5M :0; w = (T)(1)/w;
      for(int i=0;i<n;i++) 
		  res.M[i]*=w;
	  return res;
	}

	inline universal_call VECTOR<n,T>& operator*=(const MATRIX<n,n,T>& m)
	 {*this = *this*m; return *this; }

    inline universal_call VECTOR<n,T>& operator*=(const MATRIX<n+1,n+1,T>& m)
	 {*this = *this*m; return *this; }

#ifndef __CUDACC__
	void print(std::ostream& out) const
	 { for(int i=0;i<n;i++) out << M[i] << " "; }

#endif
};

// просто умножение
template<int n,class T>
VECTOR<n,T> universal_call operator*(const MATRIX<n,n,T>& m,const VECTOR<n,T>& v)
{
		VECTOR<n,T> res; res.Zero();
		
		for(register int i=0;i<n;i++)
		  for(register int j=0;j<n;j++)
			  res.M[i] += v.M[j]*m.M[i][j];

	    return res;
}

// умножение с нормализацией по последней координате(однородные коорд)
template<int n,class T>
VECTOR<n,T> universal_call operator*(const MATRIX<n+1,n+1,T>& m,const VECTOR<n,T>& v)
{
	  VECTOR<n,T> res; res.Zero();
	  for(register int i=0;i<n;i++)
		  for(register int j=0;j<n;j++)
			  res.M[i] += m.M[i][j]*v.M[j];

	  // assume that fourth coord always = 1
	  for(int i=0;i<n;i++) 
		  res.M[i] += m.M[i][n];
      
	  return res;
}

// отдельно для 4D матриц и векторов - оптимизация
template<class T>
VECTOR<4,T> universal_call operator*(const MATRIX<4,4,T>& m,const VECTOR<4,T>& v)
{
		VECTOR<4,T> res; 
	
		// компилятор nvcc плохо оптимизирует вложенные циклы - не умеет разворачивать.
		res.M[0] = m.M[0][0]*v.M[0] + m.M[0][1]*v.M[1] + m.M[0][2]*v.M[2] + m.M[0][3]*v.M[3];
		res.M[1] = m.M[1][0]*v.M[0] + m.M[1][1]*v.M[1] + m.M[1][2]*v.M[2] + m.M[1][3]*v.M[3];
		res.M[2] = m.M[2][0]*v.M[0] + m.M[2][1]*v.M[1] + m.M[2][2]*v.M[2] + m.M[2][3]*v.M[3];
		res.M[3] = m.M[3][0]*v.M[0] + m.M[3][1]*v.M[1] + m.M[3][2]*v.M[2] + m.M[3][3]*v.M[3];

	   return res;
}

#ifdef __CUDACC__

// отдельно для 4D матриц и векторов - оптимизация
template<class T>
VECTOR<3,T> universal_call operator*(const MATRIX<4,4,T>& m,const VECTOR<3,T>& v)
{
		VECTOR<3,T> res; 
	
		// компилятор nvcc плохо оптимизирует вложенные циклы - не умеет разворачивать.
		res.M[0] = m.M[0][0]*v.M[0] + m.M[0][1]*v.M[1] + m.M[0][2]*v.M[2];
		res.M[1] = m.M[1][0]*v.M[0] + m.M[1][1]*v.M[1] + m.M[1][2]*v.M[2];
		res.M[2] = m.M[2][0]*v.M[0] + m.M[2][1]*v.M[1] + m.M[2][2]*v.M[2];

		res.M[0] += m.M[0][3];
		res.M[1] += m.M[1][3];
		res.M[2] += m.M[2][3];

	    return res;
}



#endif

// скалярное произведение

template<int n,class T>
inline universal_call T dot(const VECTOR<n,T>& rhs,const VECTOR<n,T>& lhs) {return rhs*lhs;}

#if DISCARD_FOURTH_DIMENTION
	template<class T>
	inline universal_call T dot3(const VECTOR<4,T>& rhs,const VECTOR<4,T>& lhs) {return rhs*lhs;}
#else
	template<int n,class T>
	inline universal_call T dot3(const VECTOR<n,T>& rhs,const VECTOR<n,T>& lhs) 
		{return rhs.x*lhs.x + rhs.y*lhs.y + rhs.z*lhs.z;}
#endif	


template<int n,class T>
inline universal_call VECTOR<n,T> cross3(const VECTOR<n,T>& rhs,const VECTOR<n,T>& lhs) {return rhs->*lhs;}

template<int n,class T>
inline universal_call VECTOR<n,T> cross(const VECTOR<n,T>& rhs,const VECTOR<n,T>& lhs) {return rhs->*lhs;}

template<int n,class T>
inline universal_call T length(const VECTOR<n,T>& rhs) { return rhs.len(); }

// Эта функция еще не проверена
template<int n,class T>
inline universal_call VECTOR<n,T> normalize(const VECTOR<n,T>& rhs)
{
  VECTOR<n,T> res;
  T len_inv = 1.0f/length(rhs);
  MulVec<n,T>::exec(res.M, len_inv);
  return res;
}

#ifndef __CUDACC__

	template<int n,class T>
	std::ostream& operator<<(std::ostream& out,const VECTOR<n,T>& m) 
	{m.print(out); return out; }

#endif

}

