//**************************************************************************/
// Copyright (c) 2008 Autodesk, Inc.
// All rights reserved.
//
// Use of this software is subject to the terms of the Autodesk license
// agreement provided at the time of installation or download, or which
// otherwise accompanies this software in either electronic or hard copy form.
//
//**************************************************************************/
// DESCRIPTION:
// CREATED: October 2008
//**************************************************************************/

#ifndef __MUDBOXSDK_SSE_H__
#define __MUDBOXSDK_SSE_H__

#if defined(JAMBUILD)
#include <Mudbox/mudbox.h>
#else
#include "mudbox.h"
#endif

#if defined(__GNUC__)

#ifndef __SSE3__
#error This file was intended to compiled with SSE3 instruction set enabled.
#endif

//
// If you are using GCC instead of the Intel C Compiler, don't forget 
// to specify -I/usr/lib/gcc/i686-apple-darwin9/4.0.1/include when compiling
// a file that use this header.
//
#include <xmmintrin.h>
#include <pmmintrin.h>
#endif

#if defined(WIN32) || defined(WIN64)
#include <xmmintrin.h>
#include <intrin.h>
#endif

#ifndef MB_SSE_ALIGN16_VAR

#if defined(_MSC_VER)
#define MB_SSE_ALIGN16_VAR(v) __declspec(align(16)) v
#define MB_SSE_ALIGN16_CLASS  __declspec(align(16))
#elif defined(__GNUC__)
#define MB_SSE_ALIGN16_VAR(v) v __attribute__ ((aligned(16)))
#define MB_SSE_ALIGN16_CLASS  __attribute__ ((aligned(16)))
#else
#error The MB_SSE_ALIGN16_VAR needs to be ported for this compiler.
#endif

#endif
/// This class represents a four dimensional vector stored in the SSE registers

struct MBDLL_DECL HWVector
{
	inline HWVector( void ) {};

    inline void setZero( void ) 
    {
        v = _mm_setzero_ps();
    }

    inline HWVector( float f )
    {
        v = _mm_set_ps1( f );
    }

	inline HWVector( const mudbox::Vector &h ) 
	{ 
		v = _mm_set_ps( h.x, h.y, h.z, 0); 
	};
	inline HWVector( float x, float y, float z, float w = 0 ) 
	{ 
		v = _mm_set_ps( x,y,z,w ); 
	};
	void Fill( const mudbox::Vector &vVector );
	inline HWVector( const float a[4] )
	{
		v = _mm_load_ps( a );
	};
	inline HWVector ShiftLeft( void ) const    { HWVector r; r.v = _mm_shuffle_ps(v,v,_MM_SHUFFLE(2,1,3,0)); return r; };
	inline void ShiftLeft(HWVector &result)    { result.v = _mm_shuffle_ps(v,v,_MM_SHUFFLE(2,1,3,0)); }
	inline void ShiftLeftInPlace()             { v = _mm_shuffle_ps(v,v,_MM_SHUFFLE(2,1,3,0)); }
	inline HWVector ShiftRight( void ) const   { HWVector r; r.v = _mm_shuffle_ps(v,v,_MM_SHUFFLE(1,3,2,0)); return r; };
	inline void ShiftRight( HWVector &result ) { result.v = _mm_shuffle_ps(v,v,_MM_SHUFFLE(1,3,2,0)); };
	inline HWVector operator &(  HWVector &o )
	{
		//HWVector al = (*this).ShiftLeft(), bl = o.ShiftLeft(), ar = (*this).ShiftRight(), br = o.ShiftRight();
		HWVector al, bl, ar, br;
		ShiftLeft(al);
		o.ShiftLeft(bl);
		ShiftRight(ar);
		o.ShiftRight(br);

		return al*br-ar*bl;
	};

	inline HWVector operator |( const HWVector &o ) const
	{
		HWVector r;
		r.v = _mm_mul_ps( v, o.v );
		r.v = _mm_hadd_ps( r.v, r.v );
		r.v = _mm_hadd_ps( r.v, r.v );
		return r;
	};

	inline HWVector Length( void ) const
	{
		HWVector r = operator |( *this );
		r.v = _mm_sqrt_ss( r.v );
		return r;
	};

	inline HWVector LengthSquared( void ) const
	{
		HWVector r = operator |( *this );
		return r;
	};

	inline void Normalize( void )
	{
		HWVector f = operator |( *this );
		f.v = _mm_rsqrt_ps( f.v );
		v = _mm_mul_ps( f.v, v );
	};

    inline HWVector Floor( void )
    {
        static unsigned int a = 1<<23;
        static float twoTo23AsFloat = (float)a;
        static const __m128 twoTo23 = _mm_set_ps( twoTo23AsFloat,twoTo23AsFloat,twoTo23AsFloat,twoTo23AsFloat );
        // b = fabs(v)
        __m128 b = _mm_castsi128_ps(_mm_srli_epi32( _mm_slli_epi32( _mm_castps_si128(v),1 ), 1 ));
        // The essence of the floor routine
        __m128 d = _mm_sub_ps( _mm_add_ps( _mm_add_ps( _mm_sub_ps( v,twoTo23 ), twoTo23 ),twoTo23 ), twoTo23 );
        // –1 if v >= 2**23
        __m128 largeMaskE = _mm_cmpgt_ps( b, twoTo23 );
        // Check for possible off by one error
        __m128 g = _mm_cmplt_ps( v, d );
        // Convert positive check result to -1.0, negative to 0.0
        __m128 h = _mm_cvtepi32_ps( _mm_castps_si128(g) );
        // Add in the error if there is one
        __m128 t = _mm_add_ps( d, h );
        //Select between output result and input value based on v >= 2**23
        __m128 w = _mm_and_ps( v, largeMaskE );
        t = _mm_andnot_ps( largeMaskE, t );
        HWVector vResult;
        vResult.v = _mm_or_ps( t, w );
        return vResult;
    };

	inline HWVector Minimum( const HWVector &o ) const    { HWVector r; r.v = _mm_min_ps( v, o.v ); return r; };
	inline HWVector Maximum( const HWVector &o ) const    { HWVector r; r.v = _mm_max_ps( v, o.v ); return r; };
	inline HWVector operator +( const HWVector &o ) const { HWVector r; r.v = _mm_add_ps( v, o.v ); return r; };
	inline HWVector operator -( const HWVector &o ) const { HWVector r; r.v = _mm_sub_ps( v, o.v ); return r; };
	inline void operator +=( const HWVector &o ) { v = _mm_add_ps( v, o.v ); };
	inline void operator -=( const HWVector &o ) { v = _mm_sub_ps( v, o.v ); };
	inline void operator *=( const HWVector &o ) { v = _mm_mul_ps( v, o.v ); };
	inline void operator /=( const HWVector &o ) { v = _mm_div_ps( v, o.v ); };
	inline HWVector operator *( const HWVector &o ) const { HWVector r; r.v = _mm_mul_ps( v, o.v ); return r; };
	inline HWVector operator *( float f ) const { HWVector r; r.v = _mm_mul_ps( v, _mm_set1_ps( f ) ); return r; };
	inline HWVector operator /( const HWVector &o ) const { HWVector r; r.v = _mm_div_ps( v, o.v ); return r; };
	inline void operator *=( float f ) { v = _mm_mul_ps( v, _mm_set1_ps(f ) ); };
	inline void Store( float *p ) { _mm_store_ps( p, v ); };
    inline void Load( float f ) { v = _mm_set_ps1( f ); };
	inline void StoreNormalAsInt( int *pBuffer ) const
	{
		//static __declspec(align(16)) float c[4] = { 32766.0f, 32766.0f, 32766.0f, 32766.0f };
		// instead of the correct value, we use a little bit smaller number, because after 
        // normalization a component can be a littlebit bigger than 1.0. in that case storing it in
		// a 16 bit integer would overflow, and artifacts on the surface would appear.
		static const MB_SSE_ALIGN16_VAR(float c[4]) = { 32740.0f, 32740.0f, 32740.0f, 32740.0f };

		__m128 f = _mm_load_ps( c );
		f = _mm_mul_ps( f, v );
		__m128i i = _mm_cvtps_epi32( f );
		_mm_store_si128( (__m128i *)pBuffer, i );
	};
	inline void StoreAsInt( int *pBuffer ) const
	{
		__m128i i = _mm_cvtps_epi32( v );
		_mm_store_si128( (__m128i *)pBuffer, i );
	};
	inline operator float( void ) const
	{
		MB_SSE_ALIGN16_VAR(float f);
		_mm_store_ss( &f, v );
		return f;
	};
	inline operator mudbox::Vector( void ) const
	{
		mudbox::Vector r;
		__m128 t = _mm_shuffle_ps( v, v, _MM_SHUFFLE(2,1,0,3) );
		_mm_store_ss( &r.x, t );
		t = _mm_shuffle_ps( t, t, _MM_SHUFFLE(2,1,0,3) );
		_mm_store_ss( &r.y, t );
		t = _mm_shuffle_ps( t, t, _MM_SHUFFLE(2,1,0,3) );
		_mm_store_ss( &r.z, t );
		return r;
	};
	inline operator mudbox::Vector4( void ) const
	{
		mudbox::Vector4 r;
		__m128 t = _mm_shuffle_ps( v, v, _MM_SHUFFLE(2,1,0,3) );
		_mm_store_ss( &r.x, t );
		t = _mm_shuffle_ps( t, t, _MM_SHUFFLE(2,1,0,3) );
		_mm_store_ss( &r.y, t );
		t = _mm_shuffle_ps( t, t, _MM_SHUFFLE(2,1,0,3) );
		_mm_store_ss( &r.z, t );
		t = _mm_shuffle_ps( t, t, _MM_SHUFFLE(2,1,0,3) );
		_mm_store_ss( &r.w, t );
		return r;
	};

	MB_SSE_ALIGN16_VAR(__m128 v);
};

inline mudbox::Vector &operator <<( mudbox::Vector &v, const HWVector &r )
{
	__m128 t = _mm_shuffle_ps( r.v, r.v, _MM_SHUFFLE(2,1,0,3) );
	_mm_store_ss( &v.x, t );
	t = _mm_shuffle_ps( t, t, _MM_SHUFFLE(2,1,0,3) );
	_mm_store_ss( &v.y, t );
	t = _mm_shuffle_ps( t, t, _MM_SHUFFLE(2,1,0,3) );
	_mm_store_ss( &v.z, t );
	return v;

	//// this is a hacked and unsafe way
	//__m128 t = _mm_shuffle_ps( r.v, r.v, _MM_SHUFFLE(0,1,2,3) );
	//int a = ((int *)&v)[3];
	//_mm_storeu_ps( &v.x, t );
	//((int *)&v)[3] = a;
	//return v;
};

/// This class represents a four by four matrix stored in the SSE registers

struct HWMatrix
{
	inline HWMatrix( void ) {};
	inline HWMatrix( const mudbox::Matrix &m )
	{
		r0 = _mm_loadu_ps( &m._11 );
		r1 = _mm_loadu_ps( &m._21 );
		r2 = _mm_loadu_ps( &m._31 );
		r3 = _mm_loadu_ps( &m._41 );
	};
	void MirrorX( void )
	{
		r0 = _mm_shuffle_ps( r0, r0, _MM_SHUFFLE(0, 1, 2, 3) );
		r1 = _mm_shuffle_ps( r1, r1, _MM_SHUFFLE(0, 1, 2, 3) );
		r2 = _mm_shuffle_ps( r2, r2, _MM_SHUFFLE(0, 1, 2, 3) );
		r3 = _mm_shuffle_ps( r3, r3, _MM_SHUFFLE(0, 1, 2, 3) );
	};
	inline HWVector Transform( const HWVector &v ) const
	{
		static MB_SSE_ALIGN16_VAR(float c[4]) = { 1, 1, 1, 1 };

		__m128 v0 = _mm_shuffle_ps( v.v, v.v, _MM_SHUFFLE(3,3,3,3) );
		__m128 v1 = _mm_shuffle_ps( v.v, v.v, _MM_SHUFFLE(2,2,2,2) );
		__m128 v2 = _mm_shuffle_ps( v.v, v.v, _MM_SHUFFLE(1,1,1,1) );
		__m128 v3 = _mm_load_ps( c );

		__m128 a0 = _mm_mul_ps( v0, r0 );
		__m128 a1 = _mm_mul_ps( v1, r1 );
		__m128 a2 = _mm_mul_ps( v2, r2 );
		__m128 a3 = _mm_mul_ps( v3, r3 );

		__m128 r = _mm_add_ps( a0, _mm_add_ps( a1, _mm_add_ps( a2, a3 ) ) );

		HWVector z;
		z.v = r;
		return z;
	};
	inline HWVector ProjectedTransform( const HWVector &v ) const
	{
		static MB_SSE_ALIGN16_VAR(float c[4]) = { 1, 1, 1, 1 };

		__m128 v0 = _mm_shuffle_ps( v.v, v.v, _MM_SHUFFLE(3,3,3,3) );
		__m128 v1 = _mm_shuffle_ps( v.v, v.v, _MM_SHUFFLE(2,2,2,2) );
		__m128 v2 = _mm_shuffle_ps( v.v, v.v, _MM_SHUFFLE(1,1,1,1) );
		__m128 v3 = _mm_load_ps( c );

		__m128 a0 = _mm_mul_ps( v0, r0 );
		__m128 a1 = _mm_mul_ps( v1, r1 );
		__m128 a2 = _mm_mul_ps( v2, r2 );
		__m128 a3 = _mm_mul_ps( v3, r3 );

		__m128 r = _mm_add_ps( a0, _mm_add_ps( a1, _mm_add_ps( a2, a3 ) ) );
		__m128 d = _mm_shuffle_ps( r, r, _MM_SHUFFLE(3,3,3,3) );

		HWVector z;
		z.v = _mm_div_ps( r, d );
		return z;
	};
	
	MB_SSE_ALIGN16_VAR(__m128 r0);
	MB_SSE_ALIGN16_VAR(__m128 r1);
	MB_SSE_ALIGN16_VAR(__m128 r2);
	MB_SSE_ALIGN16_VAR(__m128 r3);
};

#endif
