/*
QtWagon: a project about 3D objects.
Science and technology promotion license applied. Third party license automatically cascaded.
Zhikai Wang/ www.heteroclinic.net 2013
You can do anything with this file or any file(s) published as part QtWagon project, given this header is kept.
*/
#ifndef __MATHMATRIX
#define __MATHMATRIX
#include "mathVector.h"
#include <iostream>

template <class T>
class mathMatrix {
protected:
	T data[16];
public:

	bool inverse()
	{
		T x=determinant();
		if (isAlmost<T>(x,0.0)) 
			return false;

		T i[16];

		i[0]= (-data[13]*data[10]*data[7] +data[9]*data[14]*data[7] +data[13]*data[6]*data[11]
		-data[5]*data[14]*data[11] -data[9]*data[6]*data[15] +data[5]*data[10]*data[15])/x;
		i[4]= ( data[12]*data[10]*data[7] -data[8]*data[14]*data[7] -data[12]*data[6]*data[11]
		+data[4]*data[14]*data[11] +data[8]*data[6]*data[15] -data[4]*data[10]*data[15])/x;
		i[8]= (-data[12]*data[9]* data[7] +data[8]*data[13]*data[7] +data[12]*data[5]*data[11]
		-data[4]*data[13]*data[11] -data[8]*data[5]*data[15] +data[4]*data[9]* data[15])/x;
		i[12]=( data[12]*data[9]* data[6] -data[8]*data[13]*data[6] -data[12]*data[5]*data[10]
		+data[4]*data[13]*data[10] +data[8]*data[5]*data[14] -data[4]*data[9]* data[14])/x;
		i[1]= ( data[13]*data[10]*data[3] -data[9]*data[14]*data[3] -data[13]*data[2]*data[11]
		+data[1]*data[14]*data[11] +data[9]*data[2]*data[15] -data[1]*data[10]*data[15])/x;
		i[5]= (-data[12]*data[10]*data[3] +data[8]*data[14]*data[3] +data[12]*data[2]*data[11]
		-data[0]*data[14]*data[11] -data[8]*data[2]*data[15] +data[0]*data[10]*data[15])/x;
		i[9]= ( data[12]*data[9]* data[3] -data[8]*data[13]*data[3] -data[12]*data[1]*data[11]
		+data[0]*data[13]*data[11] +data[8]*data[1]*data[15] -data[0]*data[9]* data[15])/x;
		i[13]=(-data[12]*data[9]* data[2] +data[8]*data[13]*data[2] +data[12]*data[1]*data[10]
		-data[0]*data[13]*data[10] -data[8]*data[1]*data[14] +data[0]*data[9]* data[14])/x;
		i[2]= (-data[13]*data[6]* data[3] +data[5]*data[14]*data[3] +data[13]*data[2]*data[7]
		-data[1]*data[14]*data[7] -data[5]*data[2]*data[15] +data[1]*data[6]* data[15])/x;
		i[6]= ( data[12]*data[6]* data[3] -data[4]*data[14]*data[3] -data[12]*data[2]*data[7]
		+data[0]*data[14]*data[7] +data[4]*data[2]*data[15] -data[0]*data[6]* data[15])/x;
		i[10]=(-data[12]*data[5]* data[3] +data[4]*data[13]*data[3] +data[12]*data[1]*data[7]
		-data[0]*data[13]*data[7] -data[4]*data[1]*data[15] +data[0]*data[5]* data[15])/x;
		i[14]=( data[12]*data[5]* data[2] -data[4]*data[13]*data[2] -data[12]*data[1]*data[6]
		+data[0]*data[13]*data[6] +data[4]*data[1]*data[14] -data[0]*data[5]* data[14])/x;
		i[3]= ( data[9]* data[6]* data[3] -data[5]*data[10]*data[3] -data[9]* data[2]*data[7]
		+data[1]*data[10]*data[7] +data[5]*data[2]*data[11] -data[1]*data[6]* data[11])/x;
		i[7]= (-data[8]* data[6]* data[3] +data[4]*data[10]*data[3] +data[8]* data[2]*data[7]
		-data[0]*data[10]*data[7] -data[4]*data[2]*data[11] +data[0]*data[6]* data[11])/x;
		i[11]=( data[8]* data[5]* data[3] -data[4]*data[9]* data[3] -data[8]* data[1]*data[7]
		+data[0]*data[9]* data[7] +data[4]*data[1]*data[11] -data[0]*data[5]* data[11])/x;
		i[15]=(-data[8]* data[5]* data[2] +data[4]*data[9]* data[2] +data[8]* data[1]*data[6]
		-data[0]*data[9]* data[6] -data[4]*data[1]*data[10] +data[0]*data[5]* data[10])/x;
		setData (i);
		return true;
	}

	T determinant()
	{
		return
			data[12]*data[9]*data[6]*data[3]-
			data[8]*data[13]*data[6]*data[3]-
			data[12]*data[5]*data[10]*data[3]+
			data[4]*data[13]*data[10]*data[3]+
			data[8]*data[5]*data[14]*data[3]-
			data[4]*data[9]*data[14]*data[3]-
			data[12]*data[9]*data[2]*data[7]+
			data[8]*data[13]*data[2]*data[7]+
			data[12]*data[1]*data[10]*data[7]-
			data[0]*data[13]*data[10]*data[7]-
			data[8]*data[1]*data[14]*data[7]+
			data[0]*data[9]*data[14]*data[7]+
			data[12]*data[5]*data[2]*data[11]-
			data[4]*data[13]*data[2]*data[11]-
			data[12]*data[1]*data[6]*data[11]+
			data[0]*data[13]*data[6]*data[11]+
			data[4]*data[1]*data[14]*data[11]-
			data[0]*data[5]*data[14]*data[11]-
			data[8]*data[5]*data[2]*data[15]+
			data[4]*data[9]*data[2]*data[15]+
			data[8]*data[1]*data[6]*data[15]-
			data[0]*data[9]*data[6]*data[15]-
			data[4]*data[1]*data[10]*data[15]+
			data[0]*data[5]*data[10]*data[15];
	}
	// construct a point/vector matrix
	mathMatrix(const mathVector<T> & nmv) {
		int row, col;
		for (row = 0; row < 4; row++)
			for (col = 0; col < 4 ; col++)
				data [row*4+col] = (T)0.0;

		data[0] = nmv.getx(); 
		data[1] = nmv.gety(); 
		data[2] = nmv.getz(); 
		data[3] = 1.0; 
	}
	void setVector ( const T  v[]) {
		int row, col;
		for (row = 0; row < 4; row++)
			for (col = 0; col < 4 ; col++)
				data [row*4+col] = (T)0.0;

		data[0] = v[0]; 
		data[1] = v[1]; 
		data[2] = v[2]; 
		data[3] = 1.0; 
	}
	const mathVector<T> returnVector() {
		return mathVector<T> (data[0],data[1],data[2]);
	}

	mathMatrix(const mathPoint<T> & nmp) {
		int row, col;
		for (row = 0; row < 4; row++)
			for (col = 0; col < 4 ; col++)
				data [row*4+col] = (T)0.0;

		data[0] = nmp.getx(); 
		data[1] = nmp.gety(); 
		data[2] = nmp.getz(); 
		data[3] = 1.0; 
	}


	T * getDataPointer() const{
		return (T *)data;
	}
	mathMatrix(int identity = 1) {
		int row, col;
		if (0 != identity) { 
			for (row = 0; row < 4; row++)
				for (col = 0; col < 4 ; col++)
					data [row*4+col] = ((T)(row == col));
		} else {
			for (row = 0; row < 4; row++)
				for (col = 0; col < 4 ; col++)
					data [row*4+col] = (T)0.0;
		}
	}
	//operators maybe (* vector),[];
	T operator [] (int i) {
		return data[i];

	}

	void setData (const T ndata[]) {// equivalent to void setData (const T*  ndatap) {
		int row, col;
		for (row = 0; row < 4; row++)
			for (col = 0; col < 4 ; col++)
				data [row*4+col] = ndata[row*4+col];
	}

	mathMatrix(const T ndata[]) {
		setData(ndata);
	}

	// consider to be same as mathMatrix(const T ndata[]);
	//mathMatrix(const T*  ndatap) {

	//// copy constructor
	// our assumption is copy/assign ONLY among same class
	mathMatrix(const mathMatrix & nmm) {
		setData(nmm.getDataPointer());
	}
	// assignment operator
	mathMatrix & operator=(const mathMatrix & nmm) {
		if (this == &nmm) return *this;  // time-saving self-test
		setData(nmm.getDataPointer());
		return *this;                    // for daisy-chaining
	}
	virtual ~ mathMatrix() {
	}

	////! not valid
	////T operator [] (int i,int j) {
	////	return data[i*4+j];
	////}

	template <class U> 
	friend std::ostream & operator << (std::ostream & os,const mathMatrix<U> & mm);
	template <class U> 
	friend std::istream & operator >> (std::istream & is, mathMatrix<U> & mm);
};
template <class U> 
std::ostream & operator << (std::ostream & os,const mathMatrix<U> & mm) {
	int row, col;
	for (row = 0; row < 4; row++)
		for (col = 0; col < 4 ; col++) {
			os << std::setw(OUTPUTWIDTH)<<std::fixed<<std::scientific  <<std::setprecision(FLOATING_PRECISION)<<mm.data[row*4+col];
			if (3 == col)
				os <<std::endl;
		}
		return os;
};
template <class U> 
std::istream & operator >> (std::istream & is, mathMatrix<U> & mm) {
	std::string line;
	int row, col;
	for (row = 0; row < 4; row++) {
		std::getline(is,line);
		std::stringstream ss;//you must re-init it here
		ss.str(line);
		for (col = 0; col < 4 ; col++) {
			ss>>mm.data[row*4+col];
		}
	}
	return is;
};

#endif