/*
 *      Mat.hpp
 *      
 *      Copyright 2009 Victor Theoktisto <vtheok@urras>
 *      
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 3 of the License, or
 *      (at your option) any later version.
 *      
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *      
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 *      MA 02110-1301, USA.
 */

#ifndef MAT_H
#define MAT_H
/*

Mat.h
Class for a constant-length matrix

Supports the following operations:
	mat m1;				// Initialized to ( (0,0,0); (0,0,0); (0,0,0) )
	mat3x3 m2(1,2,3);	// Initializes the 3 components
	mat3x3 m3(m2);		// Copy constructor
	float farray[3][3];
	mat3x3 m4 = mat3x3(farray);	// Explicit: "m4 = farray" won't work
	Mat<3,3,double> md;	// The "mat" used above is Mat<3,3,float>

	m3 = m1 + m2;		// Also -, *, /  (all componentwise)
	m3 = 3.5f * m1;		// Also vec * scalar, vec / scalar
				// NOTE: scalar has to be the same type:
				// it won't work to do double * vec<float>

	float f = m1(0,0);	// Subscript using parentheses
	float *fp = m1;		// Implicit conversion to float *

	f = len(v1);		// Length (also len2 == squared length)
	f = dist(p1, p2);	// Distance (also dist2 == squared distance)
	normalize(v1);		// Normalize (i.e., make it unit length)
						// normalize(vec(0,0,0)) => vec(1,0,0)

	cout << v1 << endl;	// iostream output in the form (1,2,3)
	cin >> v2;			// iostream input using the same syntax

Also defines the utility functions sqr, cube, sgn, swap,
fract, clamp, mix, step, smoothstep, and trinorm
*/


// Windows defines these as macros, which prevents us from using the
// type-safe versions from std::, as well as interfering with method defns

#include <LibTrimesh2/include/Vec.h>
#include <LibGeom/include/triplets.hpp>

// Boost-like compile-time assertion checking
template <bool X> struct MAT_STATIC_ASSERTION_FAILURE;
template <> struct MAT_STATIC_ASSERTION_FAILURE<true>
	{ void operator () () {} };
#define MAT_STATIC_CHECK(expr) MAT_STATIC_ASSERTION_FAILURE<bool(expr)>()

#define MAT_EPSILON 1.0E-06

// DR = Row Dimension, DC = Column Dimension, T = base type
template <int DR = 1, int DC = DR, typename T = float>
class Mat {
	
protected:

	T m[DR*DC];  // The matrix is stored as a linear vector of dimension DR*DC !!!
	//Vec<DR*DC,T> m;  // The matrix is stored as a linear vector of dimension DR*DC !!!

	inline unsigned int CELL (unsigned int row, unsigned int col) const {
		return (row*DC + col);  // Row Major
		//return (row + col*DR);  // Column Major
	}

public:
	// Some partial compatibility with std::vector
	// We get a name reference to the base type.
	typedef T value_type;
	typedef T *pointer;
	typedef const T *const_pointer;
	typedef T *iterator;
	typedef const T *const_iterator;
	typedef T &reference;
	typedef const T &const_reference;
	typedef size_t size_type;
	typedef ptrdiff_t difference_type;

	
	// The Dimensions.
	size_t rows() const
		{ return DR; }

	size_t columns() const
		{ return DC; }

	size_t size() const
		{ return DR*DC; }

	size_t dimension() const
		{ return DR*DC; }

	// Constructor for one argument.  Everything initialized to value.
	Mat(T value = T(0) ) {    // Default constructor
		for (int k = 0; k < DR*DC; k++)
//#pragma omp atomic
			m[k] = T(value);
	}

	// Constructor for empty argument MAT_UNINITIALIZED.  Nothing initialized.
	// Uninitialized constructor - meant mostly for internal use
#define MAT_UNINITIALIZED ((void *) 0)
	Mat(void *) {}

	Mat(const Mat<DR,DC,T>& P) {
    	for (unsigned int k = 0; k < DR*DC; k++)
//#pragma omp atomic
			m[k] = P[k];
	}

	Mat(const T orig[DR*DC]) {
		for (unsigned int k = 0; k < DR*DC; k++)
//#pragma omp atomic
			m[k] = orig[k];
	}

	// Constructors for 2-4 vector arguments
	Mat( Vec<DR,T>& v0) {
		MAT_STATIC_CHECK(DC == 1);
		for (unsigned int k = 0; k < DR; k++)
			m[CELL(k,0)] = v0[k];
	}
	Mat( Vec<DR,T>& v0,  Vec<DR,T>& v1) {
		MAT_STATIC_CHECK(DC == 2);
		for (unsigned int k = 0; k < DR; k++) {
			m[CELL(k,0)] = v0[k];
			m[CELL(k,1)] = v1[k];
		}
	}
	Mat( Vec<DR,T>& v0, Vec<DR,T>& v1, Vec<DR,T>& v2) {
		MAT_STATIC_CHECK(DC == 3);
		for (unsigned int k = 0; k < DR; k++) {
			m[CELL(k,0)] = v0[k];
			m[CELL(k,1)] = v1[k];
			m[CELL(k,2)] = v2[k];
		}
	}
	Mat( Vec<DR,T>& v0,  Vec<DR,T>& v1,  Vec<DR,T>& v2,  Vec<DR,T>& v3) {
		MAT_STATIC_CHECK(DC == 4);
		for (unsigned int k = 0; k < DR; k++) {
			m[CELL(k,0)] = v0[k];
			m[CELL(k,1)] = v1[k];
			m[CELL(k,2)] = v2[k];
			m[CELL(k,3)] = v3[k];
		}
	}

	// Array reference and conversion to pointer - no bounds checking
	const T &operator [] (int k) const   // for linear access
		{ return m[k]; }
	
	T &operator [] (int k)
		{ return m[k]; }

	const T &operator () (int i, int j) const
		{ return m[CELL(i,j)]; }

	T &operator () (int i, int j)
		{ return m[CELL(i,j)]; }

	operator const T * () const
		{ return m; }

	operator const T * ()
		{ return m; }

	operator T * ()
		{ return m; }

	static inline const Mat<DR,DC,T> Identity() {
		// MAT_STATIC_CHECK(DR == DC);   // only for square matrices
		Mat<DR,DC,T> temp(MAT_UNINITIALIZED);  // calls default constructor, no fill
		for (unsigned int i = 0; i < DR; i++)
			for (unsigned int j = 0; j < DC; j++)
				temp(i,j) = T(i==j);  // fills diagonal with ones
		return temp;
	}

	static inline const Mat<DR,DC,T> Zeroes() {
		Mat<DR,DC,T> temp;  // calls default constructor, fills with zeros
		return temp;
	}

	// Constructor from anything that can be accessed using []
	// This one's pretty aggressive, so marked explicit
	template <typename S> explicit Mat(const S &x)
		{ for (unsigned int i = 0; i < DR*DC; i++) m[i] = T(x[i]); }

	// No destructor or assignment operator needed
	
	// Some partial compatibility with valarrays and vectors

	T *begin() { return &(m[0]);          }
	T *end()   { return begin() + size(); }

	const T *begin() const { return &(m[0]);          }
	const T *end()   const { return begin() + size(); }

	// Obtain column vector - no bounds checking
	Vec<DR,T> column_vector(int j) const {
		Vec<DR,T> v(VEC_UNINITIALIZED);
		for (unsigned int i = 0; i < DR; i++)
//#pragma omp atomic
			v[i] = m[CELL(i,j)];
		return v;
	}
	
	// Obtain row vector - no bounds checking
	Vec<DC,T> row_vector(int i) const {
		Vec<DC,T> v(VEC_UNINITIALIZED);
		for (unsigned int j = 0; j < DC; j++)
//#pragma omp atomic
			v[j] = m[CELL(i,j)];
		return v;
	}

	// Member operators
	Mat<DR,DC,T> &operator += (const Mat<DR,DC,T> &Q) {
		for (unsigned int i = 0; i < size(); i++)
#pragma omp atomic
			m[i] += Q[i];
		return *this;
	}
	Mat<DR,DC,T> &operator -= (const Mat<DR,DC,T> &Q) {
		for (unsigned int i = 0; i < size(); i++)
#pragma omp atomic
			m[i] -= Q[i];
		return *this;
	}
	Mat<DR,DC,T> &operator *= (const T &r) {
//	std::cout << "mul0 ";
		for (unsigned int i = 0; i < size(); i++)
#pragma omp atomic
			m[i] *= r;
		return *this;
	}
	Mat<DR,DC,T> &operator /= (const T &r) {
//	std::cout << "div0 ";
		for (unsigned int i = 0; i < size(); i++)
#pragma omp atomic
			m[i] /= r;
		return *this;
	}

	Mat<DR,DC,T> &operator *= (const Mat<DR,DC,T> & Q) {
		MAT_STATIC_CHECK(DR == DC);  // only for square matrices, for now
		Mat<DR,DC,T> R(MAT_UNINITIALIZED);
		for (unsigned int i = 0; i < DR; i++)
			for (unsigned int j = 0; j < DR ; j++) {
				R(i,j) = (*this)(i,0) * Q(0,j);
				for (unsigned int k = 1; k < DR; k++)
#pragma omp atomic
					R(i,j) += (*this)(i,k) * Q(k,j);
			}
		return *this = R;
	}

	// Nonmember operators that take two Mats
	friend inline const Mat<DR,DC,T> operator+ (const Mat<DR,DC,T>& P, const Mat<DR,DC,T>& Q) {
		Mat<DR,DC,T> R(MAT_UNINITIALIZED);  // so we don't initialize twice
		for (unsigned int k = 0; k < DR*DC; k++)
// #pragma omp atomic
			R[k] = P[k] + Q[k];
		return R;
	}

	friend inline const Mat<DR,DC,T> operator- (const Mat<DR,DC,T>& P, const Mat<DR,DC,T>& Q) {
		Mat<DR,DC,T> R(MAT_UNINITIALIZED);  // so we don't initialize twice
		for (unsigned int k = 0; k < DR*DC; k++)
// #pragma omp atomic
			R[k] = P[k] - Q[k];
		return R;
	}

	friend inline const Mat<DR,DC,T> operator/ (const Mat<DR,DC,T>& P, const Mat<DR,DC,T>& Q) {
		MAT_STATIC_CHECK(DR == DC);  // only for square matrices, for now
		Mat<DR,DC,T> R(MAT_UNINITIALIZED);  // so we don't initialize twice
		return R;
	}

	T Det ( const T a1) const { 
		return a1;  //  [ m_00 ]  [ a1 ]
	}

	T Det ( const T a1, const T b1,
			const T a2, const T b2) const { 
		return 	                // [ m_00  m_01 ]  [ a1  a2 ]
				a1 * b2 - a2 * b1;  // [ m_10  m_11 ]  [ b1  b2 ]
	}

	T Det ( const T a1, const T b1, const T c1,
			const T a2, const T b2, const T c2,
			const T a3, const T b3, const T c3) const {
//		std::cout << "?" << std::flush;
		return
				a1 * Det (b2,c2, b3,c3) -  // [ m_00  m_01  m_02 ]  [ a1  a2  a3 ]
				b1 * Det (a2,c2, a3,c3) +  // [ m_10  m_11  m_12 ]  [ b1  b2  b3 ]
				c1 * Det (a2,b2, a3,b3);   // [ m_20  m_21  m_22 ]  [ c1  c2  c3 ]
	}

	T Det ( const T a1, const T b1, const T c1, const T d1,
			const T a2, const T b2, const T c2, const T d2,
			const T a3, const T b3, const T c3, const T d3,
			const T a4, const T b4, const T c4, const T d4) const {
		return
				a1 * Det (b2,c2,d2, b3,c3,d3, b4,c4,d4) -  // [ m_00  m_01  m_02  m_03 ]  [ a1  a2  a3  a4 ]
				b1 * Det (a2,c2,d2, a3,c3,d3, a4,c4,d4) +  // [ m_10  m_11  m_12  m_13 ]  [ b1  b2  b3  b4 ]
				c1 * Det (a2,b2,d2, a3,b3,d3, a4,b4,d4) -  // [ m_20  m_21  m_22  m_23 ]  [ c1  c2  c3  c4 ]
				d1 * Det (a2,b2,c2, a3,b3,c3, a4,b4,c4);   // [ m_30  m_31  m_32  m_33 ]  [ d1  d2  d3  d4 ]
	}

	// Outside of class: + - * / % ^ << >>

	void transpose(void) {
		MAT_STATIC_CHECK(DR == DC);   // only for square matrices
		for (unsigned int i = 0; i < DR; i++)
			for (unsigned int j = 0; j < i; j++)
//#pragma omp atomic
				std::swap (m[CELL(i,j)], m[CELL(j,i)]);
	}

	T min() const {
		T v = m[0];
		for (const T  *it = begin() + 1; it < end(); it++)
			if (*it < v)  v = *it;
		return v;
	}

	T max() const {
		T v = m[0];
		for (const T *it = begin() + 1; it < end(); it++)
			if (*it > v)  v = *it;
		return v;
	}

	T sum() const {
		T total = m[0];
		for (const T *it = begin() + 1; it < end(); it++)
			total += *it;
		return total;
	}

	T avg() const {
		return sum() / size();
	}

	void clear() {
		for (T *it = begin(); it < end(); it++)
			*it = T(0);
	}

	bool empty() const {
		for (const T *it = begin(); it < end(); it++)
			if ( *it ) return false;
		return true;
	}

	Mat<DC,DR,T> apply(T func(T)) const {
		Mat<DC,DR,T> R(MAT_UNINITIALIZED);
		for (unsigned int i = 0; i < DR*DC; i++)
			R[i] = func(m[i]);
//			R.m.apply(func);
		return R;
	}
		  
	Mat<DC,DR,T> apply(T func(const T&)) const {
		Mat<DC,DR,T> R(MAT_UNINITIALIZED);
		for (unsigned int i = 0; i < DR*DC; i++)
			R[i] = func(m[i]);
//			R.m.apply(func);
		return R;
	}
};

typedef Mat<3>   mat;  // default col dim is row dim, default type is float

typedef Mat<2>   mat2;
typedef Mat<2>   mat2x2;
typedef Mat<2,3> mat2x3;
typedef Mat<2,4> mat2x4;

typedef Mat<3>   mat3;
typedef Mat<3>   mat3x3;
typedef Mat<3,2> mat3x2;
typedef Mat<3,4> mat3x4;

typedef Mat<4>   mat4;
typedef Mat<4>   mat4x4;
typedef Mat<4,2> mat4x2;
typedef Mat<4,3> mat4x3;

typedef Mat<3,3,double> dmat;

typedef Mat<2,2,double> dmat2;
typedef Mat<2,2,double> dmat2x2;
typedef Mat<2,3,double> dmat2x3;
typedef Mat<2,4,double> dmat2x4;

typedef Mat<3,3,double> dmat3;
typedef Mat<3,2,double> dmat3x2;
typedef Mat<3,3,double> dmat3x3;
typedef Mat<3,4,double> dmat3x4;

typedef Mat<4,4,double> dmat4;
typedef Mat<4,2,double> dmat4x2;
typedef Mat<4,3,double> dmat4x3;
typedef Mat<4,4,double> dmat4x4;


// Nonmember operators that take two Mats
template <int DR, int DC, typename T>
static inline const Mat<DR,DC,T> operator + (const Mat<DR,DC,T> &M1, const Mat<DR,DC,T> &M2) {
	return Mat<DR,DC,T>(M1) += M2;
}

template <int DR, int DC, typename T>
static inline const Mat<DR,DC,T> operator - (const Mat<DR,DC,T> &M1, const Mat<DR,DC,T> &M2) {
	return Mat<DR,DC,T>(M1) -= M2;
}

// Unary operators
template <int DR, int DC, typename T>
static inline const Mat<DR,DC,T> & operator + (const Mat<DR,DC,T> &Q) {
	return Q;
}

template <int DR, int DC, typename T>
static inline const Mat<DR,DC,T> operator - (const Mat<DR,DC,T> &Q) {
	Mat<DR,DC,T> R(MAT_UNINITIALIZED);
	for (unsigned int i = 0; i < DR; i++)
		for (unsigned int j = 0; j < DC; j++)
			R(i,j) = -Q(i,j);
	return R;
}

template <int DR, int DC, typename T>
inline const Mat<DC,DR,T> operator ~ (const Mat<DR,DC,T> &Q) {  //Transpose operator
	return transpose(Q);
}

template <int DR, int DC, typename T>
inline const Mat<DC,DR,T> transpose(const Mat<DR,DC,T> &Q) {   //Transpose function
	Mat<DC,DR,T> R(MAT_UNINITIALIZED);
	// MAT_STATIC_CHECK(DR == DC);   // only for square matrices
	for (unsigned int i = 0; i < DR; i++)
		for (unsigned int j = 0; j < DC; j++)
			R(j,i) = Q(i,j);
	return R;
}

template <int DR, int DC, typename T>
static inline bool operator ! (const Mat<DR,DC,T> &v) {
	return v.empty();
}

// Mat/scalar operators
template <int DR, int DC, typename T>
static inline const  Mat<DR,DC,T> & operator * (const T &r, const  Mat<DR,DC,T> &Q) {
	Mat<DR,DC,T> P(MAT_UNINITIALIZED);
//	std::cout << "mul1 ";
	for (unsigned int i = 0; i < DR; i++)
		for (unsigned int j = 0; j < DC; j++)
			P(i,j) = r * Q(i,j);
	return P;
}

template <int DR, int DC, typename T>
static inline const Mat<DR,DC,T> & operator * (const Mat<DR,DC,T> &Q, const T &r) {
//	std::cout << "mul2 ";
	return Mat<DR,DC,T>(Q) *= r;
}

template <int DR, int DC, typename T>
static inline const Mat<DR,DC,T> operator/ (const Mat<DR,DC,T> &Q, const T &r) {
//	std::cout << "div2 ";
	return Mat<DR,DC,T>(Q) /= r;
}

// iostream operators
template <int DR, int DC, typename T>
static inline std::ostream &operator << (std::ostream &os, const Mat<DR,DC,T> &item) {
	os << std::endl << "[";
	for (unsigned int i = 0; i < DR; i++) {
		if (i==0) os << " "; else os << std::endl << "  ";
		for (unsigned int j = 0; j < DC-1; j++)
			os << item(i,j) << ", ";
		os << item(i,DC-1);
	}
	return os << " ]" << std::endl;
}

/*
template <int D, typename T>
static inline std::istream &operator >> (std::istream &is, Mat<D,T> &v)
{
	char c1 = 0, c2 = 0;

	is >> c1;
	if (c1 == '(' || c1 == '[') {
		is >> v[0] >> std::ws >> c2;
		for (int i = 1; i < D; i++) {
			if (c2 == ',')
				is >> v[i] >> std::ws >> c2;
			else
				is.setstate(std::ios::failbit);
		}
	}

	if (c1 == '(' && c2 != ')')
		is.setstate(std::ios::failbit);
	else if (c1 == '[' && c2 != ']')
		is.setstate(std::ios::failbit);

	return is;
}
*/


template <int DR, int DC, typename T>
static inline void swap(const Mat<DR,DC,T> &P, const Mat<DR,DC,T> &Q) {
	for (unsigned int i = 0; i < DR*DC; i++)
		swap(P[i], Q[i]);
}

template <int DR, int DC, typename T>
static inline Mat<DR,DC,T> abs(const Mat<DR,DC,T> &Q) {
	Mat<DR,DC,T> R(MAT_UNINITIALIZED);
	for (unsigned int i = 0; i < DR*DC; i++)
		R[i] = ( (Q[i] >= T(0)) ? Q[i] : -Q[i] );
	return R;
}

template <int DR, int DC, typename T>
inline T determinant( const Mat<DR,DC,T>& Q) { 
	MAT_STATIC_CHECK(DR == DC);  // only for square matrices, for now
	return Q.Det(Q[ 0]);
}

template <typename T>
inline T determinant ( const Mat<4,4,T>& Q)  { 
	return Q.Det(Q[ 0], Q[ 4], Q[ 8], Q[12],
				  Q[ 1], Q[ 5], Q[ 9], Q[13],
				  Q[ 2], Q[ 6], Q[10], Q[14],
				  Q[ 3], Q[ 7], Q[11], Q[15]);
}

template <typename T>
inline T determinant ( const Mat<3,3,T>& Q)  { 
//	std::cout << "@" << std::flush;
	return Q.Det(Q[ 0], Q[ 3], Q[ 6],
				  Q[ 1], Q[ 4], Q[ 7],
				  Q[ 2], Q[ 5], Q[ 8]);
}

template <typename T>
inline T determinant ( const Mat<2,2,T>& Q)  { 
	return Q.Det(Q[ 0], Q[ 2],
				  Q[ 1], Q[ 3]);
}

// Computes the adjoint Matrix
template <int DR, int DC, typename T>
inline Mat<DC,DR,T> adjoint( const Mat<DR,DC,T>& Q) { 
	MAT_STATIC_CHECK(DR == DC);  // only for square matrices, for now
	Mat<DC,DR,T> R(Q);
	return R;
}


template <typename T>
inline Mat<4,4,T> adjoint(const Mat<4,4,T>& Q)  {
	Mat<4,4,T> R(MAT_UNINITIALIZED);
	R(0,0) =   Q.Det(Q(1,1), Q(2,1), Q(3,1),  Q(1,2), Q(2,2), Q(3,2),  Q(1,3), Q(2,3), Q(3,3));
	R(1,0) = - Q.Det(Q(1,0), Q(2,0), Q(3,0),  Q(1,2), Q(2,2), Q(3,2),  Q(1,3), Q(2,3), Q(3,3));
	R(2,0) =   Q.Det(Q(1,0), Q(2,0), Q(3,0),  Q(1,1), Q(2,1), Q(3,1),  Q(1,3), Q(2,3), Q(3,3));
	R(3,0) = - Q.Det(Q(1,0), Q(2,0), Q(3,0),  Q(1,1), Q(2,1), Q(3,1),  Q(1,2), Q(2,2), Q(3,2));

	R(0,1) = - Q.Det(Q(0,1), Q(2,1), Q(3,1),  Q(0,2), Q(2,2), Q(3,2),  Q(0,3), Q(2,3), Q(3,3));
	R(1,1) =   Q.Det(Q(0,0), Q(2,0), Q(3,0),  Q(0,2), Q(2,2), Q(3,2),  Q(0,3), Q(2,3), Q(3,3));
	R(2,1) = - Q.Det(Q(0,0), Q(2,0), Q(3,0),  Q(0,1), Q(2,1), Q(3,1),  Q(0,3), Q(2,3), Q(3,3));
	R(3,1) =   Q.Det(Q(0,0), Q(2,0), Q(3,0),  Q(0,1), Q(2,1), Q(3,1),  Q(0,2), Q(2,2), Q(3,2));

	R(0,2) =   Q.Det(Q(0,1), Q(1,1), Q(3,1),  Q(0,2), Q(1,2), Q(3,2),  Q(0,3), Q(1,3), Q(3,3));
	R(1,2) = - Q.Det(Q(0,0), Q(1,0), Q(3,0),  Q(0,2), Q(1,2), Q(3,2),  Q(0,3), Q(1,3), Q(3,3));
	R(2,2) =   Q.Det(Q(0,0), Q(1,0), Q(3,0),  Q(0,1), Q(1,1), Q(3,1),  Q(0,3), Q(1,3), Q(3,3));
	R(3,2) = - Q.Det(Q(0,0), Q(1,0), Q(3,0),  Q(0,1), Q(1,1), Q(3,1),  Q(0,2), Q(1,2), Q(3,2));

	R(0,3) = - Q.Det(Q(0,1), Q(1,1), Q(2,1),  Q(0,2), Q(1,2), Q(2,2),  Q(0,3), Q(1,3), Q(2,3));
	R(1,3) =   Q.Det(Q(0,0), Q(1,0), Q(2,0),  Q(0,2), Q(1,2), Q(2,2),  Q(0,3), Q(1,3), Q(2,3));
	R(2,3) = - Q.Det(Q(0,0), Q(1,0), Q(2,0),  Q(0,1), Q(1,1), Q(2,1),  Q(0,3), Q(1,3), Q(2,3));
	R(3,3) =   Q.Det(Q(0,0), Q(1,0), Q(2,0),  Q(0,1), Q(1,1), Q(2,1),  Q(0,2), Q(1,2), Q(2,2));
	return R;
}

template <typename T>
inline Mat<3,3,T> adjoint(const Mat<3,3,T>& Q)  {
	Mat<3,3,T> R(MAT_UNINITIALIZED);
//	std::cout << "$" << std::flush;
	R(0,0) =   Q.Det(Q(1,1), Q(2,1),  Q(1,2), Q(2,2));
	R(1,0) = - Q.Det(Q(1,0), Q(2,0),  Q(1,2), Q(2,2));
	R(2,0) =   Q.Det(Q(1,0), Q(2,0),  Q(1,1), Q(2,1));

	R(0,1) = - Q.Det(Q(0,1), Q(2,1),  Q(0,2), Q(2,2));
	R(1,1) =   Q.Det(Q(0,0), Q(2,0),  Q(0,2), Q(2,2));
	R(2,1) = - Q.Det(Q(0,0), Q(2,0),  Q(0,1), Q(2,1));

	R(0,2) =   Q.Det(Q(0,1), Q(1,1),  Q(0,2), Q(1,2));
	R(1,2) = - Q.Det(Q(0,0), Q(1,0),  Q(0,2), Q(1,2));
	R(2,2) =   Q.Det(Q(0,0), Q(1,0),  Q(0,1), Q(1,1));
	return R;
}

template <typename T>
inline Mat<2,2,T> adjoint(const Mat<2,2,T>& Q)  {
	Mat<2,2,T> R(MAT_UNINITIALIZED);
	R(0,0) =   Q.Det(Q(1,1));
	R(1,0) = - Q.Det(Q(1,0));

	R(0,1) = - Q.Det(Q(0,1));
	R(1,1) =   Q.Det(Q(0,0));
	return R;
}

template <typename T>
inline Mat<1,1,T> adjoint(const Mat<1,1,T>& Q)  {
	Mat<1,1,T> R(MAT_UNINITIALIZED);
	R(0,0) =  Q.Det(Q(0,0));
	return R;
}

template <int DR, int DC, typename T>
inline Mat<DR,DC,T> inverse( const Mat<DR,DC,T>& Q) { 
	MAT_STATIC_CHECK(DR == DC);  // only for square matrices, for now
//	std::cout << "&" << std::flush;
	Mat<DR,DC,T> R(adjoint(Q));
	Vec<DR,T> v(R.row_vector(0)); 
	Vec<DR,T> w(Q.column_vector(0));
	T det = v DOT w;
	T adet = (det < 0 ? -det : det);
	
	_assert(adet > T(MAT_EPSILON));
	if (adet < T(MAT_EPSILON)) {  // this is zero
		std::cout << Q <<"\nDeterminant is 0 computing matrix inverse: " << __FILE__ 
			 	  << " at line " << __LINE__ << std::endl;
		return R;          // we return the adjoint, since det == 0
	} else
		return R/det;      // we divide R by det  for a correct inverse
}

// Component-wise equality and inequality (#include the usual caveats
// about comparing floats for equality...)
template <int DR, int DC, typename T>
static inline bool operator == (const Mat<DR,DC,T> &P, const Mat<DR,DC,T> &Q) {
	for (unsigned int k = 0; k < DR*DC; k++)
		if (P[k] != Q[k])
			return false;
	return true;
}

template <int DR, int DC, typename T>
static inline bool operator != (const Mat<DR,DC,T> &P, const Mat<DR,DC,T> &Q) {
	for (unsigned int k = 0; k < DR*DC; k++)
		if (P[k] != Q[k])
			return true;
	return false;
}

//Multiply a matrix and a vector 
template <int DR, int DC, typename T>
static inline Vec<DC,T> operator* (const Mat<DR,DC,T>& Q, const Vec<DC,T>& t) {
	Vec<DC,T> v(VEC_UNINITIALIZED);
	for (unsigned int i = 0; i < DR; i++) {
		v(i) = Q(i,0) * t(0);
		for (unsigned int j = 1; j < DC ; j++) 
#pragma omp atomic
			v(i) += Q(i,j) * t(j);
	}
	return v;
}

//Multiply two congruent matrices 
template <int DR, int DM, int DC, typename T>
static const Mat<DR,DC,T> operator* (const Mat<DR,DM,T>& P, const Mat<DM,DC,T>& Q) {
	//MAT_STATIC_CHECK(DR == DC);  // only for square matrices, for now
	Mat<DR,DC,T> R(MAT_UNINITIALIZED);  // so we don't initialize twice
	for (unsigned int i = 0; i < DR; i++)
		for (unsigned int j = 0; j < DC ; j++) {
			R(i,j) = P(i,0) * Q(0,j);
			for (unsigned int k = 1; k < DM; k++)
#pragma omp atomic
				R(i,j) += P(i,k) * Q(k,j);
		}
	return R;
}

//Solve system A X = B, => X = inv(A)*B
template <int DR, typename T>
static const Vec<DR,T> solve (const Mat<DR,DR,T>& A, const Vec<DR,T> & B) {
	//MAT_STATIC_CHECK(DR == DC);  // only for square matrices, for now
	Mat<DR,DR,T> R(inverse(A));  // so we don't initialize twice
	return R*B;
}

//Interpolate points 
template <int DR, typename T, typename S>
static inline Vec<DR,T> interpolate(const Vec<DR,T> &tail, const Vec<DR,T> &head, const S t) {
	//return ( tail + (t) * (head - tail) );
	return ( (t) * head + (S(1) - t) * tail );
}

#endif