#ifndef MATRIX_H
#define MATRIX_H

#include "./math/Mathematics.h"
#include "./math/MyMath.h"
#include <map>
#include <string>
using namespace std;

namespace MATRIX
{

enum MatrixType
{
	UNDEFINED,
	ZERO,
	IDENTITY,
	TRANSLATION,
	ROTATION,
	SCALE
};


/*
*	Matrix class,
*	Matrix layout is column dominant :

		a0   a4   a8   a12     
        a1   a5   a9   a13      
        a2   a6   a10  a14      
        a3   a7   a11  a15      

		a0   a4   a8   offsetX     
        a1   a5   a9   offsetY  
        a2   a6   a10  offsetZ      
        a3   a7   a11  a15     

*/

///Matrix class with 4 columns and 4 rows, matrix layout is column dominant.
template <typename T>
class Matrix4X4
{
private:
	T matrix[16];
public:
	Matrix4X4(void);
	Matrix4X4(	float e0, float e1, float e2, float e3,
				float e4, float e5, float e6, float e7,
				float e8, float e9, float e10, float e11,
				float e12, float e13, float e14, float e15);

	Matrix4X4(const T * src_array);
	Matrix4X4(const Matrix4X4 & matrix);

	void setEntry(unsigned int pos_x, unsigned int pos_y, const T& value); ///set matrix value on given position
	T getEntry(unsigned int pos_x, unsigned int pos_y);	///get matrix value on given position

	T* getRow(int row, T* dst_row);	///returns selected matrix row
	T* getColumn(int column, T* dst_column); ///returns selected matrix column

	void loadIdentity(void);	///load identity matrix
	void loadZero(void);	///load zero matrix
	void loadRotationMatrix(  T angle, T nAxisX, T nAxisY, T nAxisZ );	///load rotation matrix  
	void loadScaleMatrix( T factorX, T factorY, T factorZ );	///load scale matrix
	void loadTranslationMatrix( T offsetX, T offsetY, T offsetZ );	///load translation matrix

	void set(const T* srcPtr);	///set matrix values from array

	//binary operators
	Matrix4X4 operator+(const Matrix4X4 & matrix) const; ///sum 2 matrices
	Matrix4X4 operator-(const Matrix4X4 & matrix) const;	///subtract 2 matrices
	Matrix4X4 operator*(const Matrix4X4 & matrix) const;	///multiply 2 matrices
	friend Matrix4X4<T> operator*(float scaleFactor, const Matrix4X4<T> & matrix);	///multiply matrix by scalar value

	Matrix4X4& operator+=(const Matrix4X4 & matrix);	///sum 2 matrices
	Matrix4X4& operator-=(const Matrix4X4 & matrix);	///subtract 2 matrices

	void mulRowVec ( T* vec ) const;	/// multiply matrix with row vector ~ vec * Matrix
	void mulColVec( T* vec ) const;		/// multiply matrix with column vector ~ Matrix * vec

	
	const T& operator[](unsigned int index) const; ///acces to matrix value on given index
	T& operator[](unsigned int index);				///acces to matrix value on given index
	

	T* getPtr(void);	///returns ptr to matrix array

	void rotateX(T angle);	///rotates matrix on axis X
	void rotateY(T angle);	///rotates matrix on axis Y
	void rotateZ(T angle);	///rotates matrix on axis Z
	void rotate(T angleX, T angleY, T angleZ); ///rotates matrix
	void rotateAxis( T angle, T nAxisX, T nAxisY, T nAxisZ ); ///rotates matrix around given axis (nAxisX, nAxisY, nAxisZ );

	void shearX( T factorY, T factorZ);	///shears matrix on axis X
	void shearY( T factorX, T factorZ); ///shears matrix on axis Y
	void shearZ( T factorX, T factorY); ///shears matrix on axis Z

	void scaleX(T factor); ///scales matrix on axis X
	void scaleY(T factor); ///scales matrix on axis Y
	void scaleZ(T factor); ///scales matrix on axis Z
	void scale(T factorX, T factorY, T factorZ); ///scales matrix 

	void translateX(T offset); ///translates matrix on axis X
	void translateY(T offset); ///translates matrix on axis Y
	void translateZ(T offset); ///translates matrix on axis Z
	void translate(T offsetX, T offsetY, T offsetZ); ///translates matrix on axices XYZ
	
	void transpose(void); ///transpotes matrix
};


template <typename T> Matrix4X4<T>::Matrix4X4(void)
{
	this->loadIdentity();
}
template <typename T> Matrix4X4<T>::Matrix4X4(const T * src_array)
{
	for (int i=0; i<16; ++i)
	{
		this->matrix[i]=src_array[i];
	}
}
template <typename T> Matrix4X4<T>::Matrix4X4(	float e0, float e1, float e2, float e3,
											float e4, float e5, float e6, float e7,
											float e8, float e9, float e10, float e11,
											float e12, float e13, float e14, float e15)
{
	this->matrix[0]=e0;		this->matrix[1]=e1;		this->matrix[2]=e2;		this->matrix[3]=e3;
	this->matrix[4]=e4;		this->matrix[5]=e5;		this->matrix[6]=e6;		this->matrix[7]=e7;
	this->matrix[8]=e8;		this->matrix[9]=e9;		this->matrix[10]=e10;	this->matrix[11]=e11;
	this->matrix[12]=e12;	this->matrix[13]=e13;	this->matrix[14]=e14;	this->matrix[15]=e15;
}


template <typename T> Matrix4X4<T>::Matrix4X4(const Matrix4X4 & matrix)
{
	for (int i=0; i<16; ++i)
	{
		this->matrix[i]=matrix.matrix[i];
	}
}


template <typename T> void Matrix4X4<T>::setEntry(unsigned int pos_x, unsigned int pos_y, const T& value)
{
	assert(pos_x<=3 && pos_y<=3 && "Matrix position is out of range");

	this->matrix[pos_y*4+pos_x]=value;
}


template <typename T> T Matrix4X4<T>::getEntry(unsigned int pos_x, unsigned int pos_y)
{
	assert(pos_x<=3 && pos_y<=3 && "Matrix position is out of range");

	return this->matrix[pos_y*4+pos_x];
}

template <typename T> void Matrix4X4<T>::loadIdentity(void)
{
	this->matrix[0]=1;		this->matrix[1]=0;		this->matrix[2]=0;		this->matrix[3]=0;
	this->matrix[4]=0;		this->matrix[5]=1;		this->matrix[6]=0;		this->matrix[7]=0;
	this->matrix[8]=0;		this->matrix[9]=0;		this->matrix[10]=1;		this->matrix[11]=0;
	this->matrix[12]=0;		this->matrix[13]=0;		this->matrix[14]=0;		this->matrix[15]=1;
}

template <typename T> void Matrix4X4<T>::loadZero(void)
{
	for (int i=0; i<16; ++i)
	{
		this->matrix[i]=0;
	}
}

template <typename T> void Matrix4X4<T>::mulRowVec ( T* vec ) const
{
	T vecCopy[4] = { vec[0], vec[1], vec[2], vec[3] };

	vec[0] = this->matrix[0]	* vecCopy[0] + this->matrix[1]	* vecCopy[1] + this->matrix[2]	* vecCopy[2] + this->matrix[3]	* vecCopy[3];
	vec[1] = this->matrix[4]	* vecCopy[0] + this->matrix[5]	* vecCopy[1] + this->matrix[6]	* vecCopy[2] + this->matrix[7]	* vecCopy[3];
	vec[2] = this->matrix[8]	* vecCopy[0] + this->matrix[9]	* vecCopy[1] + this->matrix[10] * vecCopy[2] + this->matrix[11] * vecCopy[3];
	vec[3] = this->matrix[12]	* vecCopy[0] + this->matrix[13] * vecCopy[1] + this->matrix[14] * vecCopy[2] + this->matrix[15] * vecCopy[3];
}

template <typename T> void Matrix4X4<T>::mulColVec( T* vec ) const
{
	T vecCopy[4] = { vec[0], vec[1], vec[2], vec[3] };

	vec[0] = this->matrix[0]	* vecCopy[0] + this->matrix[4]	* vecCopy[1] + this->matrix[8]	* vecCopy[2] + this->matrix[12]	* vecCopy[3];
	vec[1] = this->matrix[1]	* vecCopy[0] + this->matrix[5]	* vecCopy[1] + this->matrix[9]	* vecCopy[2] + this->matrix[13]	* vecCopy[3];
	vec[2] = this->matrix[2]	* vecCopy[0] + this->matrix[6]	* vecCopy[1] + this->matrix[10] * vecCopy[2] + this->matrix[14] * vecCopy[3];
	vec[3] = this->matrix[3]	* vecCopy[0] + this->matrix[7]	* vecCopy[1] + this->matrix[11] * vecCopy[2] + this->matrix[15] * vecCopy[3];
}

template <typename T> void Matrix4X4<T>::loadRotationMatrix(  T angle, T nAxisX, T nAxisY, T nAxisZ )
{
	//taken from http://en.wikipedia.org/wiki/Rotation_matrix ( part: Rotation matrix given an axis and an angle )


	T cosAngle = cos( degToRad( -angle) );
	T oneMinusCosAngle = 1.0 - cosAngle;
	T sinAngle = sin( degToRad( -angle) );


	this->matrix[0] = cosAngle + nAxisX * nAxisX * oneMinusCosAngle;
	this->matrix[1] = nAxisX * nAxisY * oneMinusCosAngle - nAxisZ * sinAngle;
	this->matrix[2] = nAxisX * nAxisZ * oneMinusCosAngle + nAxisY * sinAngle;
	this->matrix[3] = 0;

	this->matrix[4] = nAxisY * nAxisX * oneMinusCosAngle + nAxisZ * sinAngle;
	this->matrix[5] = cosAngle + nAxisY * nAxisY * oneMinusCosAngle;
	this->matrix[6] = nAxisY * nAxisZ * oneMinusCosAngle - nAxisX * sinAngle;
	this->matrix[7] = 0;

	this->matrix[8] = nAxisZ * nAxisX * oneMinusCosAngle - nAxisY * sinAngle;
	this->matrix[9] = nAxisZ * nAxisY * oneMinusCosAngle + nAxisX * sinAngle;
	this->matrix[10] = cosAngle + nAxisZ * nAxisZ * oneMinusCosAngle;
	this->matrix[11] = 0;

	this->matrix[12]=0;		
	this->matrix[13]=0;		
	this->matrix[14]=0;		
	this->matrix[15]=1;
}

template <typename T> void Matrix4X4<T>::loadScaleMatrix( T factorX, T factorY, T factorZ )
{
	this->matrix[0] = factorX;
	this->matrix[1] = 0;
	this->matrix[2] = 0;
	this->matrix[3] = 0;

	this->matrix[4] = 0;
	this->matrix[5] = factorY;
	this->matrix[6] = 0;
	this->matrix[7] = 0;

	this->matrix[8] = 0;
	this->matrix[9] = 0;
	this->matrix[10] = factorZ;
	this->matrix[11] = 0;

	this->matrix[12] = 0;
	this->matrix[13] = 0;
	this->matrix[14] = 0;
	this->matrix[15] = 1;
}
template <typename T> void Matrix4X4<T>::loadTranslationMatrix( T offsetX, T offsetY, T offsetZ )
{
	this->matrix[0] = 1;
	this->matrix[1] = 0;
	this->matrix[2] = 0;
	this->matrix[3] = 0;

	this->matrix[4] = 0;
	this->matrix[5] = 1;
	this->matrix[6] = 0;
	this->matrix[7] = 0;

	this->matrix[8] = 0;
	this->matrix[9] = 0;
	this->matrix[10] = 1;
	this->matrix[11] = 0;

	this->matrix[12] = offsetX;
	this->matrix[13] = offsetY;
	this->matrix[14] = offsetZ;
	this->matrix[15] = 1;
}

template <typename T> void Matrix4X4<T>::set(const T* srcPtr)
{
	for (int i=0; i<16; ++i)
	{
		this->matrix[i]=*(srcPtr+i);
	}
}
template <typename T> Matrix4X4<T> Matrix4X4<T>::operator+(const Matrix4X4 & matrix) const
{
	return Matrix4X4<T>(	this->matrix[0]+matrix.matrix[0], this->matrix[1]+matrix.matrix[1], this->matrix[2]+matrix.matrix[2], this->matrix[3]+matrix.matrix[3],
							this->matrix[4]+matrix.matrix[4], this->matrix[5]+matrix.matrix[5], this->matrix[6]+matrix.matrix[6], this->matrix[7]+matrix.matrix[7],
							this->matrix[8]+matrix.matrix[8], this->matrix[9]+matrix.matrix[9], this->matrix[10]+matrix.matrix[10], this->matrix[11]+matrix.matrix[11],
							this->matrix[12]+matrix.matrix[12], this->matrix[13]+matrix.matrix[13], this->matrix[14]+matrix.matrix[14], this->matrix[15]+matrix.matrix[15] )
}
template <typename T> Matrix4X4<T>& Matrix4X4<T>::operator+=(const Matrix4X4 & matrix)
{
	for ( int i=0; i< 15; ++i)
	{
		this->matrix[i]+=matrix.matrix[i];
	}

	return *this;
}	

template <typename T> Matrix4X4<T> Matrix4X4<T>::operator-(const Matrix4X4 & matrix) const
{
	return Matrix4X4<T>(	this->matrix[0]-matrix.matrix[0], this->matrix[1]-matrix.matrix[1], this->matrix[2]-matrix.matrix[2], this->matrix[3]-matrix.matrix[3],
							this->matrix[4]-matrix.matrix[4], this->matrix[5]-matrix.matrix[5], this->matrix[6]-matrix.matrix[6], this->matrix[7]-matrix.matrix[7],
							this->matrix[8]-matrix.matrix[8], this->matrix[9]-matrix.matrix[9], this->matrix[10]-matrix.matrix[10], this->matrix[11]-matrix.matrix[11],
							this->matrix[12]-matrix.matrix[12], this->matrix[13]-matrix.matrix[13], this->matrix[14]-matrix.matrix[14], this->matrix[15]-matrix.matrix[15] )
}

template <typename T> Matrix4X4<T>& Matrix4X4<T>::operator-=(const Matrix4X4 & matrix)
{
	for ( int i=0; i< 15; ++i)
	{
		this->matrix[i]-=matrix.matrix[i];
	}

	return *this;
}

template <typename T> Matrix4X4<T> Matrix4X4<T>::operator*(const Matrix4X4 & matrix) const
{
	
	return Matrix4X4<T>(	this->matrix[0]*matrix.matrix[0]	+ this->matrix[4]*matrix.matrix[1]	+ this->matrix[8]*matrix.matrix[2]	+ this->matrix[12]*matrix.matrix[3],
							this->matrix[1]*matrix.matrix[0]	+ this->matrix[5]*matrix.matrix[1]	+ this->matrix[9]*matrix.matrix[2]	+ this->matrix[13]*matrix.matrix[3],
							this->matrix[2]*matrix.matrix[0]	+ this->matrix[6]*matrix.matrix[1]	+ this->matrix[10]*matrix.matrix[2]	+ this->matrix[14]*matrix.matrix[3],
							this->matrix[3]*matrix.matrix[0]	+ this->matrix[7]*matrix.matrix[1]	+ this->matrix[11]*matrix.matrix[2]	+ this->matrix[15]*matrix.matrix[3],

							this->matrix[0]*matrix.matrix[4]	+ this->matrix[4]*matrix.matrix[5]	+ this->matrix[8]*matrix.matrix[6]	+ this->matrix[12]*matrix.matrix[7],
							this->matrix[1]*matrix.matrix[4]	+ this->matrix[5]*matrix.matrix[5]	+ this->matrix[9]*matrix.matrix[6]	+ this->matrix[13]*matrix.matrix[7],
							this->matrix[2]*matrix.matrix[4]	+ this->matrix[6]*matrix.matrix[5]	+ this->matrix[10]*matrix.matrix[6]		+ this->matrix[14]*matrix.matrix[7],
							this->matrix[3]*matrix.matrix[4]	+ this->matrix[7]*matrix.matrix[5]	+ this->matrix[11]*matrix.matrix[6]		+ this->matrix[15]*matrix.matrix[7],

							this->matrix[0]*matrix.matrix[8]	+ this->matrix[4]*matrix.matrix[9]	+ this->matrix[8]*matrix.matrix[10]	+ this->matrix[12]*matrix.matrix[11],
							this->matrix[1]*matrix.matrix[8]	+ this->matrix[5]*matrix.matrix[9]	+ this->matrix[9]*matrix.matrix[10]	+ this->matrix[13]*matrix.matrix[11],
							this->matrix[2]*matrix.matrix[8]	+ this->matrix[6]*matrix.matrix[9]	+ this->matrix[10]*matrix.matrix[10]	+ this->matrix[14]*matrix.matrix[11],
							this->matrix[3]*matrix.matrix[8]	+ this->matrix[7]*matrix.matrix[9]	+ this->matrix[11]*matrix.matrix[10]	+ this->matrix[15]*matrix.matrix[11],


							this->matrix[0]*matrix.matrix[12]	+ this->matrix[4]*matrix.matrix[13]	+ this->matrix[8]*matrix.matrix[14]	+ this->matrix[12]*matrix.matrix[15],
							this->matrix[1]*matrix.matrix[12]	+ this->matrix[5]*matrix.matrix[13]	+ this->matrix[9]*matrix.matrix[14]	+ this->matrix[13]*matrix.matrix[15],
							this->matrix[2]*matrix.matrix[12]	+ this->matrix[6]*matrix.matrix[13]	+ this->matrix[10]*matrix.matrix[14]	+ this->matrix[14]*matrix.matrix[15],
							this->matrix[3]*matrix.matrix[12]	+ this->matrix[7]*matrix.matrix[13]	+ this->matrix[11]*matrix.matrix[14]	+ this->matrix[15]*matrix.matrix[15] );
}

template <typename T> Matrix4X4<T> operator*(float scaleFactor, const Matrix4X4<T> & matrix)
{
	return Matrix4X4<T>(	scaleFactor * matrix.matrix[0], scaleFactor * matrix.matrix[1], scaleFactor * matrix.matrix[2], scaleFactor * matrix.matrix[3],
							scaleFactor * matrix.matrix[4], scaleFactor * matrix.matrix[5], scaleFactor * matrix.matrix[6], scaleFactor * matrix.matrix[7],
							scaleFactor * matrix.matrix[8], scaleFactor * matrix.matrix[9], scaleFactor * matrix.matrix[10], scaleFactor * matrix.matrix[11],
							scaleFactor * matrix.matrix[12], scaleFactor * matrix.matrix[13], scaleFactor * matrix.matrix[14], scaleFactor * matrix.matrix[15] );
}

template <typename T> T* Matrix4X4<T>::getRow(int row, T* dst_row)
{
	assert(row<=3 && "Matrix position is out of range");

	dst_row[0]	=this->matrix[row];
	dst_row[1]	=this->matrix[row+4];
	dst_row[2]	=this->matrix[row+8];
	dst_row[3]	=this->matrix[row+12];

	return dst_row;
}

template <typename T> T* Matrix4X4<T>::getColumn(int column, T* dst_column)
{
	assert(column<=3 && "Matrix position is out of range");

	dst_column[0]	=this->matrix[column*4];
	dst_column[1]	=this->matrix[column*4+1];
	dst_column[2]	=this->matrix[column*4+2];
	dst_column[3]	=this->matrix[column*4+3];

	return dst_column
}



template <typename T> const T& Matrix4X4<T>::operator[](unsigned int index) const
{
	assert(index<=15 && "Matrix position is out of range");

	return this->matrix[index];
}

template <typename T> T& Matrix4X4<T>::operator[](unsigned int index)
{
	assert(index<=15 && "Matrix position is out of range");

	return this->matrix[index];
}

template <typename T> T* Matrix4X4<T>::getPtr(void)
{
	return this->matrix;
}

//copied and modified from NEHE CZ
template <typename T>  void Matrix4X4<T>::rotateX(T angle)	// Rotace na ose x
{
	T p;
	T sin_ = sin( degToRad( -angle) );
	T cos_ = cos( degToRad( -angle) );

// Osa X na X
	p = matrix[4];
	matrix[4] = p * cos_ - matrix[8] * sin_;
	matrix[8] = p * sin_ + matrix[8] * cos_;

// Osa X na Y
	p = matrix[5];
	matrix[5] = p * cos_ - matrix[9] * sin_;
	matrix[9] = p * sin_ + matrix[9] * cos_;

// Osa X na Z
	p = matrix[6];
	matrix[6] = p * cos_ - matrix[10] * sin_;
	matrix[10] = p * sin_ + matrix[10] * cos_;
}

template <typename T>  void Matrix4X4<T>::rotateY(T angle)	// Rotace na ose y
{
	T p;
	T sin_ = sinf( degToRad( -angle) );
	T cos_ = cosf( degToRad( -angle) );

// Osa Y na X
	p = matrix[0];
	matrix[0] = p * cos_ - matrix[8] * sin_;
	matrix[8] = p * sin_ + matrix[8] * cos_;

// Osa Y na Y
	p = matrix[1];
	matrix[1] = p * cos_ - matrix[9] * sin_;
	matrix[9] = p * sin_ + matrix[9] * cos_;

// Osa Y na Z
	p = matrix[2];
	matrix[2] = p * cos_ - matrix[10] * sin_;
	matrix[10] = p * sin_ + matrix[10] * cos_;
}

template <typename T>  void Matrix4X4<T>::rotateZ(T angle)	// Rotace na ose z
{
	T p;
	T sin_ = sin( degToRad( -angle) );
	T cos_ = cos( degToRad( -angle) );

// Osa Z na X
	p = matrix[0];
	matrix[0] = p * cos_ - matrix[4] * sin_;
	matrix[4] = p * sin_ + matrix[4] * cos_;

// Osa Z na X
	p = matrix[1];
	matrix[1] = p * cos_ - matrix[5] * sin_;
	matrix[5] = p * sin_ + matrix[5] * cos_;

// Osa Z na X
	p = matrix[2];
	matrix[2] = p * cos_ - matrix[6] * sin_;
	matrix[6] = p * sin_ + matrix[6] * cos_;

}

template <typename T>  void Matrix4X4<T>::rotate(T angleX, T angleY, T angleZ)
{
	if(angleX)
	{
		rotateX(angleX);
	}
	if(angleY)
	{
		rotateY(angleY);
	}
	if(angleZ)
	{
		rotateZ(angleZ);
	}

}

template <typename T>  void Matrix4X4<T>::rotateAxis( T angle, T nAxisX, T nAxisY, T nAxisZ )
{
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> rotationMatrix;
	rotationMatrix.loadRotationMatrix( angle, nAxisX, nAxisY, nAxisZ );

	*this = copyMatrix * rotationMatrix;
}

template <typename T>  void Matrix4X4<T>::shearX( T factorY, T factorZ)
{
	this->matrix[4] += factorY;
	this->matrix[8] += factorZ;
}
template <typename T>  void Matrix4X4<T>::shearY( T factorX, T factorZ)
{
	this->matrix[1] += factorX;
	this->matrix[9] += factorZ;
}
template <typename T>  void Matrix4X4<T>::shearZ( T factorX, T factorY)
{
	this->matrix[2] += factorX;
	this->matrix[6] += factorY;
}

template <typename T>  void Matrix4X4<T>::scaleX(T factor)	// scale na ose X
{
	//Note: could be optimalized: only 1 row will be changed!!!
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> scaleMatrix;
	scaleMatrix.loadScaleMatrix( factor, 0, 0 );

	*this = copyMatrix * scaleMatrix;
}
template <typename T>  void Matrix4X4<T>::scaleY(T factor)	// scale na ose Y
{
	//Note: could be optimalized: only 1 row will be changed!!!
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> scaleMatrix;
	scaleMatrix.loadScaleMatrix( 0, factor, 0 );

	*this = copyMatrix * scaleMatrix;
}
template <typename T>  void Matrix4X4<T>::scaleZ(T factor)	// scale na ose Y
{
	//Note: could be optimalized: only 1 row will be changed!!!
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> scaleMatrix;
	scaleMatrix.loadScaleMatrix( 0, 0, factor );

	*this = copyMatrix * scaleMatrix;
}
template <typename T>  void Matrix4X4<T>::scale(T factorX, T factorY, T factorZ)
{
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> scaleMatrix;
	scaleMatrix.loadScaleMatrix( factorX, factorY, factorZ );

	*this = copyMatrix * scaleMatrix;
}

template <typename T>  void Matrix4X4<T>::translateX(T offset)
{
	//Note: could be optimalized: only 1 row will be changed!!!
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> translationMatrix;
	translationMatrix.loadTranslationMatrix( offset,0,0 );

	*this = copyMatrix * translationMatrix;
}
template <typename T>  void Matrix4X4<T>::translateY(T offset)
{
	//Note: could be optimalized: only 1 row will be changed!!!
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> translationMatrix;
	translationMatrix.loadTranslationMatrix( 0,offset,0 );

	*this = copyMatrix * translationMatrix;
}
template <typename T>  void Matrix4X4<T>::translateZ(T offset)
{
	//Note: could be optimalized: only 1 row will be changed!!!
	Matrix4X4<T> copyMatrix ( *this );

	Matrix4X4<T> translationMatrix;
	translationMatrix.loadTranslationMatrix( 0,0,offset );

	*this = copyMatrix * translationMatrix;
}

template <typename T>  void Matrix4X4<T>::translate(T offsetX, T offsetY, T offsetZ)
{
	translateX(offsetX);
	translateY(offsetY);
	translateZ(offsetZ);
}


template <typename T>  void Matrix4X4<T>::transpose(void)
{
	Matrix4X4<T> tmpMat( *this );

	matrix[0] = tmpMat.matrix[0];
	matrix[1] = tmpMat.matrix[4];
	matrix[2] = tmpMat.matrix[8];
	matrix[3] = tmpMat.matrix[12];

	matrix[4] = tmpMat.matrix[1];
	matrix[5] = tmpMat.matrix[5];
	matrix[6] = tmpMat.matrix[9];
	matrix[7] = tmpMat.matrix[13];

	matrix[8] = tmpMat.matrix[2];
	matrix[9] = tmpMat.matrix[6];
	matrix[10] = tmpMat.matrix[10];
	matrix[11] = tmpMat.matrix[14];

	matrix[12] = tmpMat.matrix[3];
	matrix[13] = tmpMat.matrix[7];
	matrix[14] = tmpMat.matrix[11];
	matrix[15] = tmpMat.matrix[15];
}

//##################################################################################3
//	Matrix Manager

//TODO, FIXME remove metoda chybi!!!

///Manages matrices.
class MatrixManager
{
	map<string, Matrix4X4<float>* > matrices;

public:
	
	typedef map<string, Matrix4X4<float>* >::iterator iterator;
	typedef map<string, Matrix4X4<float>* >::const_iterator const_iterator;

	iterator begin(void);
	const_iterator begin(void) const;

	iterator end(void);
	const_iterator end(void) const;

	MatrixManager(void);
	MatrixManager( const MatrixManager &matrixManager );
	MatrixManager& operator=( const MatrixManager &matrixManager );	

	~MatrixManager(void);
	Matrix4X4<float>* getMatrix( const string &matrixName);
	Matrix4X4<float>* insertMatrix( const string& matrixName, Matrix4X4<float>* matrixPtr);  /**< Insert new matrix to database. WARNING: The MatrixManager adapt the responsibility for dealocation. */
};

}

#endif