// © Copyright 2008 Владимир Фролов
#define MGML_MATH_SIMD_GUARDIAN

#include <xmmintrin.h>
#include <emmintrin.h>

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


#define SIMD_SHUFFLE(srch,srcl,desth,destl) (((srch) << 6) | ((srcl) << 4) | ((desth) << 2) | ((destl)))

#define rotatelps(ps)		_mm_shuffle_ps((ps),(ps), 0x39)	// a,b,c,d -> b,c,d,a


#ifdef __CUDACC__

#endif

namespace MGML_MATH
{

	////////////////////////////////// самое важное  ////////////////////////////////////////////////
	inline float _mm_dot3(const __m128& ss1,const __m128& ss2)
	{
		register __m128 xmm0 = _mm_mul_ps(ss1,ss2);
		register __m128 xmm1 = _mm_add_ps(
							   _mm_shuffle_ps(xmm0,xmm0,SIMD_SHUFFLE(0x01,0x00,0x03,0x02)),xmm0);

		xmm0 = _mm_shuffle_ps(xmm0, xmm1, SIMD_SHUFFLE(0x02,0x03,0x00,0x01));
		return _mm_add_ps(xmm0,xmm1).m128_f32[0];
	}

	inline __m128 _mm_cross3(const __m128& ss1,const __m128& ss2)
	{
	 register __m128 xmm0 = 
		   _mm_sub_ps(
		   _mm_mul_ps(ss1, _mm_shuffle_ps(ss2, ss2, SIMD_SHUFFLE(0x03,0x00,0x02,0x01))),
		   _mm_mul_ps(ss2, _mm_shuffle_ps(ss1, ss1, SIMD_SHUFFLE(0x03,0x00,0x02,0x01))));

	 return _mm_shuffle_ps(xmm0,xmm0,SIMD_SHUFFLE(0x03,0x00,0x02,0x01));
	}

	//////////////////////////////////////////////// вектор ////////////////////////////////////////////////

	template<>
	inline VECTOR<4,float>& VECTOR<4,float>::operator=(const VECTOR<4,float>& rhs)
	 {
		////ss = _mm_load_ps(rhs.M);
		ss = rhs.ss;
		return *this;
	 }

	template<>
	inline VECTOR<4,float> VECTOR<4,float>::operator-(const VECTOR<4,float>& rhs) const
	 {  return _mm_sub_ps(*this,rhs); }

	template<>
	inline VECTOR<4,float> VECTOR<4,float>::operator+(const VECTOR<4,float>& rhs) const
	 {  return _mm_add_ps(*this,rhs); }

	template<>
	inline universal_call VECTOR<4,float> VECTOR<4,float>::operator->*(const VECTOR<4,float>& v) const
	 { return _mm_cross3(*this,v); }

	template<>
	inline universal_call float VECTOR<4,float>::operator*(const VECTOR<4,float>& rhs) const 
	{ return _mm_dot3(*this,rhs); }


	// константы
	template<>
	inline VECTOR<4,float> VECTOR<4,float>::operator*(const float rhs) const
	 { return _mm_mul_ps(*this,VECTOR<4,float>(rhs,rhs,rhs,rhs)); }


	template<>
	inline VECTOR<4,float> VECTOR<4,float>::operator/(const float rhs) const
	 { return _mm_div_ps(*this,VECTOR<4,float>(rhs,rhs,rhs,rhs)); }

	template<>
	inline VECTOR<4,float>& VECTOR<4,float>::operator/=(const float rhs)
	{  *this = *this/rhs; return *this; }

		
	template<>
	inline VECTOR<4,float> operator*(const MATRIX<4,4,float>& m,const VECTOR<4,float>& v)
	{
		VECTOR<4,float> res;
		matrix4x4f_mult_vector4f_fast(m.L,v.M,res.M);
	    return res;
	}

	//template<>
	//inline void MATRIX<4,4,float>::Transpose()
	//{
	//	
	//}

	template<>
	inline void MATRIX<4,4,float>::Zero()
	{
		ss[0] = _mm_xor_ps(ss[0],ss[0]);
		ss[1] = _mm_xor_ps(ss[1],ss[1]);
		ss[2] = _mm_xor_ps(ss[2],ss[2]);
		ss[3] = _mm_xor_ps(ss[3],ss[3]);
	}

	template<>
	inline MATRIX<4,4,float>& MATRIX<4,4,float>::operator+=(const MATRIX<4,4,float>& rhs)
	{
		ss[0] = _mm_add_ps(ss[0],rhs.ss[0]);
		ss[1] = _mm_add_ps(ss[1],rhs.ss[1]);
		ss[2] = _mm_add_ps(ss[2],rhs.ss[2]);
		ss[3] = _mm_add_ps(ss[3],rhs.ss[3]);
		return *this;
	}

//	// matrix
//	template<>
//	inline MATRIX<4,4,float>& MATRIX<4,4,float>::operator=(const MATRIX<4,4,float>& rhs)
//	{
//	  this->ss[0] = rhs.ss[0];
//	  this->ss[1] = rhs.ss[1];
//	  this->ss[2] = rhs.ss[2];
//	  this->ss[3] = rhs.ss[3];
//	  return *this;
//	}

	//////////////////////////////////////////////// сегмент ////////////////////////////////////////////////
	template<>
	inline bool SEGMENT<4,float>::contain(const VECTOR<4,float>& p) const
	{
		VECTOR<4,float> AB  = pos - dir;
		VECTOR<4,float> Av1 = p   - pos;
		VECTOR<4,float> v1B = dir - p;

		float lAB  = _mm_dot3(AB,AB);
		float lAv1 = _mm_dot3(Av1,Av1);
		float lv1B = _mm_dot3(v1B,v1B);

		return (lAv1 + lv1B) < (lAB);
	}

	//////////////////////////////////////////////// плоскость ////////////////////////////////////////////////
	template<>
	VECTOR<4,float> PLANE<4,float>::project(const VECTOR<4,float>& p0)
	{
		float t = (-1)*(_mm_dot3(norm(),p0) + D())/_mm_dot3(norm(),norm());
		return p0 + norm()*t;
	}

	template<>
	PLANE<4,float>::PLANE(const VECTOR<4,float>& p1,const VECTOR<4,float>& p2,const VECTOR<4,float>& p3)
	{
		VECTOR<4,float> v1 = p2 - p1;
		VECTOR<4,float> v2 = p3 - p1;
		VECTOR<4,float> n = _mm_cross3(v1,v2);
		n.Normalize();
		setNorm(n);
		D() = (-1)*_mm_dot3(norm(),p1);
	}

	////////////////////////////////// функции общего назначения  ////////////////////////////////////////////////
	//template<> // B - точка AC - отрезок
	//inline VECTOR<4,float> height_from_point_to_segment(const VECTOR<4,float>& B,
	//													const VECTOR<4,float>& A,
	//													const VECTOR<4,float>& C)
	//{
	//	VECTOR<4,float> D   = _mm_sub_ps(A,C);
	//	VECTOR<4,float> E   = _mm_sub_ps(B,A);
	//	VECTOR<4,float> dir = _mm_sub_ps(C,A);
	//	float t = _mm_dot3(D,E)/_mm_dot3(dir,D);
	//	return A + t*dir;
	//}



};

