/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
*
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source distribution.
*/

#ifndef __dgVector__
#define __dgVector__

#include "dgStdafx.h"
#include "dgDebug.h"
#include "dgMemory.h"
#include "dgSimd_Instrutions.h"

#define dgCheckVector(x) (dgCheckFloat(x[0]) && dgCheckFloat(x[1]) && dgCheckFloat(x[2]) && dgCheckFloat(x[3]))
//#define dgCheckVector(x) true

template<class T>
class dgTemplateVector {
public:
	constexpr dgTemplateVector() : m_x(0), m_y(0), m_z(0), m_w(0) {}
	dgTemplateVector(const T *ptr);
	constexpr dgTemplateVector(T m_x, T m_y, T m_z, T m_w);
	dgTemplateVector Scale(T s) const;
	dgTemplateVector Scale4(T s) const;

	T &operator[](dgInt32 i);
	const T &operator[](dgInt32 i) const;

	dgTemplateVector operator+ (const dgTemplateVector &A) const;
	dgTemplateVector operator- (const dgTemplateVector &A) const;
	dgTemplateVector &operator+= (const dgTemplateVector &A);
	dgTemplateVector &operator-= (const dgTemplateVector &A);

	// return dot product
	T operator% (const dgTemplateVector &A) const;

	// return cross product
	dgTemplateVector operator* (const dgTemplateVector &B) const;

	// return dot 4d dot product
	dgTemplateVector Add4(const dgTemplateVector &A) const;
	dgTemplateVector Sub4(const dgTemplateVector &A) const;
	T DotProduct4(const dgTemplateVector &A) const;
	dgTemplateVector CrossProduct4(const dgTemplateVector &A, const dgTemplateVector &B) const;

	// component wise multiplication
	dgTemplateVector CompProduct(const dgTemplateVector &A) const;

	// component wise multiplication
	dgTemplateVector CompProduct4(const dgTemplateVector &A) const;

	// check validity of floats
#ifdef _DEBUG
	void Trace() const {
		dgTrace(("%f %f %f %f\n", m_x, m_y, m_z, m_w));
	}
#endif

	DG_CLASS_ALLOCATOR(allocator)

	T m_x;
	T m_y;
	T m_z;
	T m_w;
};

class dgBigVector;

DG_MSC_VECTOR_ALIGMENT
class dgVector: public dgTemplateVector<dgFloat32> {
public:
	constexpr dgVector() = default;
#ifdef DG_BUILD_SIMD_CODE
	dgVector(const simd_type &val);
#endif
	dgVector(const dgTemplateVector<dgFloat32> &v);
	dgVector(const dgFloat32 *ptr);
	constexpr dgVector(dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w);
	dgVector(const dgBigVector &copy);

	dgFloat32 DotProductSimd(const dgVector &A) const;
	dgVector CrossProductSimd(const dgVector &A) const;
	dgVector CompProductSimd(const dgVector &A) const;

} DG_GCC_VECTOR_ALIGMENT;

DG_MSC_VECTOR_ALIGMENT
class dgBigVector: public dgTemplateVector<dgFloat64> {
public:
	dgBigVector();
	dgBigVector(const dgVector &v);
	dgBigVector(const dgTemplateVector<dgFloat64> &v);
	dgBigVector(const dgFloat32 *ptr);
#ifndef __USE_DOUBLE_PRECISION__
	dgBigVector(const dgFloat64 *ptr);
#endif
	dgBigVector(dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w);
} DG_GCC_VECTOR_ALIGMENT;


template<class T>
dgTemplateVector<T>::dgTemplateVector(const T *ptr)
	: m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(0.0f) {
//	NEWTON_ASSERT (dgCheckVector ((*this)));
}

template<class T>
constexpr dgTemplateVector<T>::dgTemplateVector(T x, T y, T z, T w)
	: m_x(x), m_y(y), m_z(z), m_w(w) {
}


template<class T>
T &dgTemplateVector<T>::operator[](dgInt32 i) {
	NEWTON_ASSERT(i < 4);
	NEWTON_ASSERT(i >= 0);
	return (&m_x)[i];
}

template<class T>
const T &dgTemplateVector<T>::operator[](dgInt32 i) const {
	NEWTON_ASSERT(i < 4);
	NEWTON_ASSERT(i >= 0);
	return (&m_x)[i];
}

template<class T>
dgTemplateVector<T> dgTemplateVector<T>::Scale(T scale) const {
	return dgTemplateVector<T> (m_x * scale, m_y * scale, m_z * scale, m_w);
}

template<class T>
dgTemplateVector<T> dgTemplateVector<T>::Scale4(T scale) const {
	return dgTemplateVector<T> (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
}


template<class T>
dgTemplateVector<T> dgTemplateVector<T>::operator+ (const dgTemplateVector<T> &B) const {
	return dgTemplateVector<T> (m_x + B.m_x, m_y + B.m_y, m_z + B.m_z, m_w);
}

template<class T>
dgTemplateVector<T> &dgTemplateVector<T>::operator+= (const dgTemplateVector<T> &A) {
	m_x += A.m_x;
	m_y += A.m_y;
	m_z += A.m_z;
//	NEWTON_ASSERT (dgCheckVector ((*this)));
	return *this;
}

template<class T>
dgTemplateVector<T> dgTemplateVector<T>::operator- (const dgTemplateVector<T> &A) const {
	return dgTemplateVector<T> (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w);
}

template<class T>
dgTemplateVector<T> &dgTemplateVector<T>::operator-= (const dgTemplateVector<T> &A) {
	m_x -= A.m_x;
	m_y -= A.m_y;
	m_z -= A.m_z;
	NEWTON_ASSERT(dgCheckVector((*this)));
	return *this;
}


template<class T>
T dgTemplateVector<T>::operator% (const dgTemplateVector<T> &A) const {
	return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z;
}


template<class T>
dgTemplateVector<T> dgTemplateVector<T>::operator* (const dgTemplateVector<T> &B) const {
	return dgTemplateVector<T> (m_y * B.m_z - m_z * B.m_y,
	                            m_z * B.m_x - m_x * B.m_z,
	                            m_x * B.m_y - m_y * B.m_x, m_w);
}

template<class T>
dgTemplateVector<T> dgTemplateVector<T>::Add4(const dgTemplateVector &A) const {
	return dgTemplateVector<T> (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
}

template<class T>
dgTemplateVector<T> dgTemplateVector<T>::Sub4(const dgTemplateVector &A) const {
	return dgTemplateVector<T> (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
}


// return dot 4d dot product
template<class T>
T dgTemplateVector<T>::DotProduct4(const dgTemplateVector &A) const {
	return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w;
}

template<class T>
dgTemplateVector<T> dgTemplateVector<T>::CrossProduct4(const dgTemplateVector &A, const dgTemplateVector &B) const {
	T cofactor[3][3];
	T array[4][4];

	const dgTemplateVector<T> &me = *this;
	for (dgInt32 i = 0; i < 4; i ++) {
		array[0][i] = me[i];
		array[1][i] = A[i];
		array[2][i] = B[i];
		array[3][i] = T(1.0f);
	}

	dgTemplateVector<T> normal;
	T sign = T(-1.0f);
	for (dgInt32 i = 0; i < 4; i ++)  {

		for (dgInt32 j = 0; j < 3; j ++) {
			dgInt32 k0 = 0;
			for (dgInt32 k = 0; k < 4; k ++) {
				if (k != i) {
					cofactor[j][k0] = array[j][k];
					k0 ++;
				}
			}
		}
		T x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
		T y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
		T z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
		T det = x + y + z;

		normal[i] = sign * det;
		sign *= T(-1.0f);
	}

	return normal;
}



template<class T>
dgTemplateVector<T> dgTemplateVector<T>::CompProduct(const dgTemplateVector<T> &A) const {
	return dgTemplateVector<T> (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w);
}

template<class T>
dgTemplateVector<T> dgTemplateVector<T>::CompProduct4(const dgTemplateVector<T> &A) const {
	return dgTemplateVector<T> (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
}



DG_INLINE dgVector::dgVector(const dgTemplateVector<dgFloat32> &v)
	: dgTemplateVector<dgFloat32>(v) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}

DG_INLINE dgVector::dgVector(const dgFloat32 *ptr)
	: dgTemplateVector<dgFloat32>(ptr) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}

DG_INLINE dgVector::dgVector(const dgBigVector &copy)
	: dgTemplateVector<dgFloat32>(dgFloat32(copy.m_x), dgFloat32(copy.m_y), dgFloat32(copy.m_z), dgFloat32(copy.m_w)) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}

#ifdef DG_BUILD_SIMD_CODE
DG_INLINE dgVector::dgVector(const simd_type &val) {
	NEWTON_ASSERT((dgUnsigned64(this) & 0x0f) == 0);
	(simd_type &) *this = val;
	NEWTON_ASSERT(dgCheckVector((*this)));
}
#endif

constexpr DG_INLINE dgVector::dgVector(dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w)
	: dgTemplateVector<dgFloat32>(x, y, z, w) {
}

DG_INLINE dgFloat32 dgVector::DotProductSimd(const dgVector &A) const {
#ifdef DG_BUILD_SIMD_CODE
//	simd_type r0;
//	dgFloat32 dot;
	dgVector tmp;
	(simd_type &) tmp = simd_mul_v((simd_type &) * this, (simd_type &)A);
//	r0 = simd_add_v(r0, simd_move_hl_v (r0, r0));
//	simd_store_s(simd_add_s(r0, simd_permut_v (r0, r0, PURMUT_MASK(3, 3, 3, 1))), &dot);
//	return dot;
	return tmp.m_x + tmp.m_y + tmp.m_z;
#else
	return dgFloat32(0.0f);
#endif
}

DG_INLINE dgVector dgVector::CrossProductSimd(const dgVector &e10) const {
#ifdef DG_BUILD_SIMD_CODE
	const dgVector &e21 = *this;
	return dgVector(simd_mul_sub_v(simd_mul_v(simd_permut_v((simd_type &)e21, (simd_type &)e21, PURMUT_MASK(3, 0, 2, 1)), simd_permut_v((simd_type &)e10, (simd_type &)e10, PURMUT_MASK(3, 1, 0, 2))),
	                               simd_permut_v((simd_type &)e21, (simd_type &)e21, PURMUT_MASK(3, 1, 0, 2)), simd_permut_v((simd_type &)e10, (simd_type &)e10, PURMUT_MASK(3, 0, 2, 1))));
#else
	return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
#endif

}


DG_INLINE dgVector dgVector::CompProductSimd(const dgVector &A) const {
#ifdef DG_BUILD_SIMD_CODE
	return dgVector(simd_mul_v((simd_type &) * this, (simd_type &)A));
#else
	return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
#endif
}

DG_INLINE dgBigVector::dgBigVector()
	: dgTemplateVector<dgFloat64>() {
}

DG_INLINE dgBigVector::dgBigVector(const dgVector &v)
	: dgTemplateVector<dgFloat64>(v.m_x, v.m_y, v.m_z, v.m_w) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}

DG_INLINE dgBigVector::dgBigVector(const dgTemplateVector<dgFloat64> &v)
	: dgTemplateVector<dgFloat64>(v) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}

DG_INLINE dgBigVector::dgBigVector(const dgFloat32 *ptr)
	: dgTemplateVector<dgFloat64>(ptr[0], ptr[1], ptr[2], dgFloat64(0.0f)) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}

#ifndef __USE_DOUBLE_PRECISION__
DG_INLINE dgBigVector::dgBigVector(const dgFloat64 *ptr)
	: dgTemplateVector<dgFloat64>(ptr) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}
#endif

DG_INLINE dgBigVector::dgBigVector(dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w)
	: dgTemplateVector<dgFloat64>(x, y, z, w) {
	NEWTON_ASSERT(dgCheckVector((*this)));
}


#endif