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

#pragma warning(disable:4996)


#ifndef __CUDACC__
	#include <xmmintrin.h>
	#include <emmintrin.h>
#endif

#include <math.h>

#ifndef __CUDACC__
	#ifndef MGML_ERROR_GUARDIAN
		#include "MGML_ERROR.h"
	#endif
#endif

#ifndef MGML_GLOBALS
	#include "MGML_GLOBALS.h"
#endif


#ifndef	 MPMG_GUARDIAN
	#include "MPMG.h"
#endif


#ifndef MGML_VECTOR_GUARDIAN
	#include "MGML_VECTOR.h"
#endif

#ifndef MGML_MQ_GUARDIAN 
	#include "MGML_MQ.h"
#endif



namespace MGML_MATH
{
 static const float PI = ((float)3.141592654f);
 // defines for small numbers
 static const float EPSILON_E3 = (float)(1E-3); 
 static const float EPSILON_E4 = (float)(1E-4); 
 static const float EPSILON_E5 = (float)(1E-5);
 static const float EPSILON_E6 = (float)(1E-6);
 static const float EPSILON_E7 = (float)(1E-7);
 static const float EPSILON_E8 = (float)(1E-8);
 static const float EPSILON_E9 = (float)(1E-9);
 static const float EPSILON_E10 = (float)(1E-10);
 static const float EPSILON_E11 = (float)(1E-11);
 static const float EPSILON_E12 = (float)(1E-12);
 static const float MAX_FLOAT_VALUE = 1e38;

 template<int n,class T>	   class VECTOR;
 template<int n,int m,class T> class MATRIX;
 template<int n,class T>       class LINE;
 template<int n,class T>       class RAY;
 template<int n,class T>       class SEGMENT;
 template<int n,class T>       class PLANE;
 template<int n,class T>       class SPHERE;
 template<int n,class T>       class AABB;
 template<int n,class T>       class BOX;
 template<int n,class T>       class TRIANGLE;



 // В STL эта ф-я почему-то не работает 
 template <class T>
 inline universal_call void SWAP(T& a,T& b)
 {
		T temp = a;
		a=b;b=temp;
 }

 template <class T> universal_call T MIN(T a, T b){ return a < b ? a : b; }
 template <class T> universal_call T MAX(T a, T b){ return a > b ? a : b; }

 template <class T>
 inline universal_call const T DEG_TO_RAD(const T ang)
  { return ang*PI/180.0f; }

 template <class T>
 inline universal_call const T RAD_TO_DEG(const T rads)
 { return rads*180.0f/PI; }

 template <class T> inline universal_call T MIN(T a,T b,T c)
  { return (a < b) ? ((a < c ) ? a:c) : (b < c) ? b:c; }

 template <class T> inline universal_call T MAX(T a,T b,T c)
  { return (a > b) ? ((a > c ) ? a:c) : (b > c) ? b:c; }

 template <class T>
 inline universal_call T SIGN(T num) 
 { 
	 if(num>0)
		 return 1;
	 else if(num < 0)
		 return -1;
	 else
		 return 0;
 } 

 template <int n,class T>
 inline universal_call const VECTOR<n,T> SPHERICAL_TO_ORTHOGONAL(const T r,const T phi,const T theta)
 {
   VECTOR<n,T> res;
   res.x() = r*sin(theta)*cos(phi);
   res.y() = r*sin(theta)*sin(phi);
   res.z() = r*cos(theta);
   return res;
 }

 template <int n,class T>
 inline universal_call const VECTOR<n,T> SPHERICAL_TO_ORTHOGONAL(const VECTOR<n,T>& v)
 { return SPHERICAL_TO_ORTHOGONAL<n,T>(v.x(),v.y(),v.z()); }
	
 // возвращает вектор (r,phi,theta)
 template <int n,class T>
 inline universal_call const VECTOR<n,T> ORTHOGONAL_TO_SPHERICAL(const VECTOR<n,T>& v)
 { return ORTHOGONAL_TO_SPHERICAL<n,T>(v.x(),v.y(),v.z()); }

 template <int n,class T>
 inline universal_call const VECTOR<n,T> ORTHOGONAL_TO_SPHERICAL(const T x,const T y,const T z)
 {
   VECTOR<n,T> res;
   res.M[0] = sqrt(x*x + y*y + z*z);
   res.M[2] = acos(z/res.M[0]);
   // оптимизация с помощью cmovCC
   if(x<0)
    res.M[1] = PI + atan(y/x);
   else if(y>=0)
	res.M[1] =  atan(y/x);
   else
    res.M[1] = 2*PI + atan(y/x);
  return res;
 }

 template <class T>
 inline const T rnd(T s,T e)
 {
  T t = (rand()%1000)*0.001; // !! _ftol
  return s + t*(e-s);
 }

 
 template<int n,class T>
 inline universal_call VECTOR<n,T> reflect(const VECTOR<n,T>& dir,const VECTOR<n,T>& normal)
 {
	VECTOR<n,T> temp =  normal * dot(dir,normal) * (-2);
	VECTOR<n,T> rVec = temp + dir;
	rVec.Normalize();				
	return rVec;
 }


template<int n,class T>
inline universal_call bool refract(RAY<n,T>* inout_ray, VECTOR<n,T> normal, T eta)
{
  T cos_theta = -dot(normal, inout_ray->dir);

  if(cos_theta < 0)
  {
    cos_theta *= -1.0f;
    normal *= (-1);
    eta = 1.0f/eta;
  }
 
	T k = 1.0f - eta*eta*(1.0-cos_theta*cos_theta);
  if(k > 0)
  {
	  inout_ray->dir = eta*inout_ray->dir + (eta*cos_theta - sqrt(k))*normal;
	  inout_ray->dir.Normalize();
  }
  else
  {
    inout_ray->pos.M[0] = INFINITY;
    inout_ray->pos.M[1] = INFINITY;
    inout_ray->pos.M[2] = INFINITY;

    inout_ray->dir.M[0] = 1;
    inout_ray->dir.M[0] = 0;
    inout_ray->dir.M[0] = 0;
  }

	return (k > 0);
}


template<int n,class T>
inline universal_call VECTOR<n,T> refract(const VECTOR<n,T>& dir,const VECTOR<n,T>& normal,T n1,T n2)
{
	T eta = n1/n2;
	T cos_theta = -dot(normal,dir);
	T k = 1.0f - eta*eta*(1.0-cos_theta*cos_theta);
	VECTOR<n,T> ret = eta*dir + (eta*cos_theta - sqrt(k))*normal;
	ret.Normalize();				// can be removed
	return ret;
}

 template<class X>
 inline universal_call X SAFE_INVERT(X e)
 {
   if(e >= 0 && e < EPSILON)
   {
		e = EPSILON;
   }
   else if( e < 0 && e > -EPSILON)
   {
		e = -EPSILON;
   }
 
   return ((X)1)/e;
 }


template<int n,class T>
class LINE: public Auto_Align<n,T>
{
public:
	enum {m_sz =(n==4?n:n+1)};
	 VECTOR<n,T> pos;
     VECTOR<n,T> dir;

#ifndef __CUDACC__
	 universal_call LINE(){}
	 universal_call LINE(const VECTOR<n,T>& p,const VECTOR<n,T>& d): pos(p),dir(d){}
#endif

	 inline universal_call void set(const VECTOR<n,T>& p,const VECTOR<n,T>& d)
	 {
		 pos = p;
		 dir = d;
	 }
	// LINE<n,T> operator*(const MATRIX<m_sz,m_sz,T>& m) const
	//  {return LINE(pos*m,dir*m);}
};


template<int n,class T>
class RAY: public LINE<n,float>
{
public:
	enum {m_sz =(n==4?n:n+1)};

#ifndef __CUDACC__
	universal_call RAY(){}
	universal_call RAY(const VECTOR<n,T>& p,const VECTOR<n,T>& d): LINE<n,T>(p,d){}
#endif

	inline universal_call RAY<n,T> operator*(const MATRIX<m_sz,m_sz,T>& m)
	{
		RAY<n,T> ray;
		this->pos.w = 1;
		this->dir.w = 0;
		ray.pos = this->pos*m;
		ray.dir = this->dir*m;
		return ray;
	}

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

};

template<int n,class T>
inline universal_call RAY<n,T> operator*(const MATRIX< RAY<n,T>::m_sz,RAY<n,T>::m_sz,T>& m,RAY<n,T>& ray_old)
{
		RAY<n,T> ray;

	//	ray_old.pos.w() = 1;
	//	ray_old.dir.w() = 0;
		ray.pos = m*ray_old.pos;
		ray.pos.x() += m.M[0][3];
		ray.pos.y() += m.M[1][3];
		ray.pos.z() += m.M[2][3];

		ray.dir = m*ray_old.dir;
		return ray;
}


template<int n,class T>
class SEGMENT: public LINE<n,float>
{
public:
	enum {m_sz =(n==4?n:n+1)};

#ifndef __CUDACC__
	universal_call SEGMENT(){}
	universal_call SEGMENT(const VECTOR<n,T>& p,const VECTOR<n,T>& d): LINE<n,T>(p,d){}


	universal_call VECTOR<n,T>& begin() {return pos;}
	universal_call const VECTOR<n,T>& begin() const {return pos;}

	universal_call VECTOR<n,T>& end() {return dir;}
	universal_call const VECTOR<n,T>& end() const {return dir;}
#endif


	universal_call SEGMENT<n,T> operator*(const MATRIX<m_sz,m_sz,T>& m) const
	  {return SEGMENT(this->pos*m,this->dir*m);}

	inline universal_call bool contain(const VECTOR<n,T>& p) const
	{
		VECTOR<n,T> AB  = this->pos - this->dir;
		VECTOR<n,T> Av1 = p   - this->pos;
		VECTOR<n,T> v1B = this->dir - p;

		float lAB  = AB.lenSquare();
		float lAv1 = Av1.lenSquare();
		float lv1B = v1B.lenSquare();

		return (lAv1 + lv1B) < (lAB + 1e-4);
	}

};
 


template <int _n,class T> 
class PLANE: public Auto_Align<_n,T>
{
public:

	union
	{
		struct
		{
		  T M[4];
		};
	};

#ifndef __CUDACC__
	PLANE() { M[0] = M[1] = 0; M[2] = 1; M[3] = 0;}
	PLANE(const VECTOR<_n,T>& p1, const VECTOR<_n,T>& p2, const VECTOR<_n,T>& p3)
	{
		VECTOR<_n,T> v1 = p2 - p1;
		VECTOR<_n,T> v2 = p3 - p1;
		n = v1 ->* v2;
		//n.Normalize();
		D() = (-1)*(n*p1);
	}

	~PLANE(){}
#endif

	inline universal_call T& A() {return M[0];}
	inline universal_call T& B() {return M[1];}
	inline universal_call T& C() {return M[2];}
	inline universal_call T& D() {return M[3];}

	inline universal_call const T& A() const {return M[0];}
	inline universal_call const T& B() const {return M[1];}
	inline universal_call const T& C() const {return M[2];}
	inline universal_call const T& D() const {return M[3];}

	inline universal_call const VECTOR<_n,T> norm() const 
	{	
		VECTOR<_n,T> n;
		n.M[0] = M[0];
		n.M[1] = M[1];
		n.M[2] = M[2];
		return n;
	}
	
	inline universal_call void setNorm(const VECTOR<_n,T>& in_n)
	{
		M[0] = in_n.M[0];
		M[1] = in_n.M[1];
		M[2] = in_n.M[2];
	}

	inline universal_call void compute(const VECTOR<_n,T>& p1, const VECTOR<_n,T>& p2, const VECTOR<_n,T>& p3)
	{
		VECTOR<_n,T> v1 = p2 - p1;
		VECTOR<_n,T> v2 = p3 - p1;
		VECTOR<_n,T> n = v1 ->* v2;
		n.Normalize();
		setNorm(n);
		D() = (-1)*(norm()*p1);
	}

	//PLANE(const VECTOR<_n,T>& p1,const VECTOR<_n,T>& p2,const VECTOR<_n,T>& p3)
	//{
	//	T x1 = p1.M[0], x2 = p2.M[0], x3 = p3.M[0];
	//	T y1 = p1.M[1], y2 = p2.M[1], y3 = p3.M[1];
	//	T z1 = p1.M[2], z2 = p2.M[2], z3 = p3.M[2];
	//
	//	A = y1*(z2 - z3) + y2*(z3 - z1) + y3*(z1 - z2);
	//	B = z1*(x2 - x3) + z2*(x3 - x1) + z3*(x1 - x2);
	//	C = x1*(y2 - y3) + x2*(y3 - y1) + x3*(y1 - y2);
	//	D = -( x1*(y2*z3 - y3*z2) + x2*(y3*z1 - y1*z3) + x3*(y1*z2 - y2*z1));
	//}


	inline universal_call VECTOR<_n,T> project(const VECTOR<_n,T>& p0)
	{
		float t = (-1)*substitute(p0)/(norm()*norm());
		return p0 + norm()*t;
	}

	inline universal_call T substitute(const VECTOR<_n,T>& p) const
	{
		return p.M[0]*A() + p.M[1]*B() + p.M[2]*C() + D();
	}

};


template<int n,class T>
class SPHERE: public Auto_Align<n,T>
{
public:
	VECTOR<n,T> pos;
	T r;
	T _rSquare;
	unsigned int material_id;

	inline universal_call void setRadius(float rhs)
	{
     r = rhs;
	 _rSquare = rhs*rhs;
	}

	inline universal_call T rSquare() const { return _rSquare; }

#ifndef __CUDACC__
	inline universal_call SPHERE(){}
	inline universal_call SPHERE(const VECTOR<n,T>& p,float _r)
	{
		pos = p;
		r = _r;
		_rSquare = r*r;
	}
	universal_call ~SPHERE(){}
#endif

};


template<class T>
struct PACKED_SPHERE3
{
#ifndef __CUDACC__
  inline PACKED_SPHERE3() {}
  inline PACKED_SPHERE3(const VECTOR<3,T>& p, T r)
	{
		m_pos.M[0] = p.M[0];
    m_pos.M[1] = p.M[1];
    m_pos.M[2] = p.M[2];
		m_r = r;
	}
#endif

	inline universal_call void setRadius(T rhs)
	{ m_r = rhs; }

	inline universal_call T r() const { return m_r; } 
	inline universal_call T rSquare() const {return m_r*m_r;} 

  inline universal_call void setPosition(const VECTOR<3,T>& in_pos)
  {
    m_pos.M[0] = in_pos.M[0];
    m_pos.M[1] = in_pos.M[1];
    m_pos.M[2] = in_pos.M[2];
  }

  inline universal_call VECTOR<3,T> getPosition() const
  {
    return m_pos;
  }

public:
	VECTOR<3,T> m_pos;
  T m_r;
};


// Axis Alined Bounding Box
template<int n,class T>
class AABB: public Auto_Align<n,T>
{
	typedef const VECTOR<n,T> CVEC;
public:


	enum {k = MGML_MATH::VECTOR<n,T>::k};

	VECTOR<n,T> vmin;
	VECTOR<n,T> vmax;

	universal_call AABB(){}
	universal_call AABB(CVEC& _one){vmin=_one;vmax=_one;}
	universal_call AABB(CVEC& _min,CVEC& _max){vmin=_min;vmax=_max;}
    
	enum {	FRONT = 0,
			    BACK  = 1,
			    LEFT  = 2,
			    RIGHT = 3,
			    TOP	  = 4,
			    BOTTOM= 5};
	// FRONT(+z),BACK(-z) - xy
	// LEFT(-x),RIGHT(+x) - yz
	// TOP(+y),BOTTOM(-y) - xz
	// ALERT ! NOT SAFE!
	inline universal_call const VECTOR<n,T>* CVPointer() const {return &vmin;}

	inline universal_call VECTOR<n,T> center() const
	 {return 0.5*(vmin + vmax);}

	template<int m>
	inline universal_call bool contain(const VECTOR<n,T>& point) const
	{
		for(int i=0;i<m;i++)
			if( point.M[i] < vmin.M[i] || point.M[i] > vmax.M[i])
				return false;
		return true;
    }

	inline universal_call void include(const VECTOR<n,T>& point)
    {
		for(int i=0;i<k;i++)
		{
			if( point.M[i] < vmin.M[i])
				vmin.M[i] = point.M[i];

			if( point.M[i] > vmax.M[i])
				vmax.M[i] = point.M[i];
		}
    }

	inline universal_call void include(const SPHERE<n,T>& sph)
	{
		for(int i=0;i<k;i++)
		{
			if(vmin.M[i]  > sph.pos.M[i] - sph.r)
				vmin.M[i] = sph.pos.M[i] - sph.r;
			
			if(vmax.M[i]  < sph.pos.M[i] + sph.r)
				vmax.M[i] = sph.pos.M[i] + sph.r;
		}
	}

  inline universal_call void include(const AABB<n,T>& in_box)
  { 
    for(int i=0;i<k;i++)
		{
      if(vmin.M[i] > in_box.vmin.M[i])
        vmin.M[i] = in_box.vmin.M[i];

      if(vmax.M[i] < in_box.vmax.M[i])
				vmax.M[i] = in_box.vmax.M[i];
    }
  }

  inline universal_call T surfaceArea() const
  {
     T a = vmax.x() - vmin.x();
     T b = vmax.y() - vmin.y();
     T c = vmax.z() - vmin.z();
     return 2*(a*(b+c) + b*c);
  }

  inline universal_call T volume() const
  {
     T a = vmax.x() - vmin.x();
     T b = vmax.y() - vmin.y();
     T c = vmax.z() - vmin.z();
     return a*b*c;
  }

	// получить двухмерные координаты точки в пространстве грани
	// в соответствие с точкой и номером грани
	inline universal_call  VECTOR<2,T> get_flat_coord(int num,const VECTOR<n,T>& p1) const
	{
		VECTOR<2,T> ret;
		switch(num)
		{
			case FRONT:
			case BACK:
				ret.x = p1.x - vmin.x;
				ret.y = p1.y - vmin.y;
			break;

			case LEFT:
			case RIGHT:
				ret.x = p1.z - vmin.z;
				ret.y = p1.y - vmin.y;
			//	ret.x = p1.y - vmin.y;
			//	ret.y = p1.z - vmin.z;
			break;

			case TOP:
			case BOTTOM:
				ret.x = p1.x - vmin.x;
				ret.y = p1.z - vmin.z;
			break;

		}
		return ret;
	}

};


// параллелепипед представлен как выровненный по осям параллелепипед + 
// матрица преобразования к его настоящему положению (и обратно)
// рекумендуется записывать только матрицу поворота, так как позиция известна 
// из центра
template<int n,class T>
class BOX: public AABB<n,T>
{
	enum {m_sz = ( n == 4 ? 4: n + 1)}; // размер матрицы
public:
	
	universal_call BOX(){}

	universal_call BOX(const VECTOR<n,T>& p): AABB<n,T>(p) {}

	universal_call BOX(const VECTOR<n,T>& p,const MATRIX<m_sz,m_sz,T>& m): AABB<n,T>(p)
	 {  setMatrix(m); }

	universal_call BOX(const VECTOR<n,T>& p1,const VECTOR<n,T>& p2): AABB<n,T>(p1,p2)
	 { }

	universal_call BOX(const VECTOR<n,T>& p1,const VECTOR<n,T>& p2,const MATRIX<m_sz,m_sz,T>& m): AABB<n,T>(p1,p2)
	 { setMatrix(m); }
	

	universal_call void setMatrix(const MATRIX<m_sz,m_sz,T>& A)
	{
		MATRIX<m_sz,m_sz,T> C;

		//C.setTranslate(this->center());	

		_matrix = C.getInverse()*A*C;
		_matrix_inverse = _matrix.getInverse();
	}

	universal_call void setMatrix(const MATRIX<m_sz,m_sz,T>& m,const MATRIX<m_sz,m_sz,T>& m_inv)
	{
		_matrix = m;
		_matrix_inverse = m_inv;

	}

	inline universal_call  MATRIX<m_sz,m_sz,T>& matrix() {return _matrix;}
	inline universal_call  MATRIX<m_sz,m_sz,T>& matrix_inverse() {return _matrix_inverse;}

	inline universal_call  const MATRIX<m_sz,m_sz,T>& matrix() const  {return _matrix;}
	inline universal_call  const MATRIX<m_sz,m_sz,T>& matrix_inverse() const  {return _matrix_inverse;}

	inline universal_call  const MATRIX<m_sz,m_sz,T>& matrix_transpose() const  {return _matrix.getTranspose();}
	inline universal_call  const MATRIX<m_sz,m_sz,T>& matrix_inverse_transpose() const  {return _matrix_inverse.getTranspose();}

protected:
	MATRIX<m_sz,m_sz,T> _matrix;        // матрица преобразования из AABB в box
	MATRIX<m_sz,m_sz,T> _matrix_inverse;// матрица преобразования из box в AABB

};



template<int n,class T>       
class TRIANGLE: Auto_Align<n,T>
{
	typedef const VECTOR<n,T> CV;

public:

#ifndef __CUDACC__
	TRIANGLE(){}
	TRIANGLE(CV& a,CV& b,CV& c)
	 { A=a;B=b;C=c; }
#endif

	VECTOR<n,T> A;
	VECTOR<n,T> B;
	VECTOR<n,T> C;
	
};



// some functions
void __fastcall matrix4x4f_mult_vector4f_fast(const float* m,const float* v,float* vres);
void __fastcall matrix4x4f_mult_matrix4x4f_fast(const float* m1,const float* m2,float* mres);



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

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

	    return res;
}

static universal_call void matrix4x4f_mult_ray3f(const float* in_M, 
										  const MGML_MATH::RAY<3,float>& in_ray,
										  MGML_MATH::RAY<3,float>* out_result)
{
	VECTOR<3,float> pos  = matrix4x4f_mult_vector3f(in_M, in_ray.pos);
	VECTOR<3,float> pos2 = matrix4x4f_mult_vector3f(in_M, in_ray.pos + in_ray.dir);

	out_result->pos = pos;
	out_result->dir = pos2-pos;
	out_result->dir.Normalize();
}

// dinamic types.
enum {TYPE_OF_TRIANGLE=0,
	  TYPE_OF_SPHERE=1,
	  TYPE_OF_BOX=2};

};
