#include "Matrice4.h"

const Matrice4 MAT4_NULL(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
const Matrice4 MAT4_ID(1.0, 0.0, 0.0, 0.0,
		       0.0, 1.0, 0.0, 0.0,
		       0.0, 0.0, 1.0, 0.0,
		       0.0, 0.0, 0.0, 1.0);

void produitVectoriel(float & x, float & y, float & z,float x1, float y1, float z1, float x2, float y2, float z2){
	x = (y1*z2 - y2*z1);
	y = (x2*z1 - x1*z2);
	z = (x1*y2 - x2*y1);
}

void normaliseVecteur(float & x, float & y, float & z){
	float norme(sqrt(x*x + y*y + z*z));
	if(norme != 0.0 && norme != 1.0){
		x /= norme;
		y /= norme;
		z /= norme;
	}
}

///////////////////////////////////////////////////
//                                               //
//   Fonctions publiques de la classe Matrice4   //
//                                               //
///////////////////////////////////////////////////

Matrice4::Matrice4(){
	p_mat = NULL;
	initialisationMatrice4(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}

Matrice4::Matrice4(const Vecteur4 & v1, const Vecteur4 & v2, const Vecteur4 & v3, const Vecteur4 & v4){
	p_mat = NULL;
	this->initialisationMatrice4(v1, v2, v3, v4);
}

Matrice4::Matrice4(float x1, float y1, float z1, float t1,
	float x2, float y2, float z2, float t2,
	float x3, float y3, float z3, float t3,
	float x4, float y4, float z4, float t4)
{
	p_mat = NULL;
	initialisationMatrice4(x1, y1, z1, t1, x2, y2, z2, t2, x3, y3, z3, t3, x4, y4, z4, t4);
}

Matrice4::Matrice4(const Matrice4 & other){
	p_mat = NULL;
	this->copyMatrice4(other);
}

Matrice4::~Matrice4(){
	depiler();
	if(p_mat != NULL) delete [] p_mat;
}

void Matrice4::setMatrice4(const Vecteur4 & v1, const Vecteur4 & v2, const Vecteur4 & v3, const Vecteur4 & v4){
	this->initialisationMatrice4(v1, v2, v3, v4);
}

void Matrice4::setMatrice4(float x1, float y1, float z1, float t1,
			float x2, float y2, float z2, float t2,
			float x3, float y3, float z3, float t3,
			float x4, float y4, float z4, float t4)
{
	initialisationMatrice4(x1, y1, z1, t1, x2, y2, z2, t2, x3, y3, z3, t3, x4, y4, z4, t4);
}

void Matrice4::loadIdentity(){
	depiler();
	this->loadNull();
	p_mat[0] = 1;
	p_mat[5] = 1;
	p_mat[10] = 1;
	p_mat[15] = 1;
}

void Matrice4::loadNull(){
	for(unsigned int i(0); i < 16; i++) p_mat[i] = 0.0;
}

void Matrice4::translated(float x, float y, float z){
	Matrice4 translation;
	translation.loadIdentity();
	translation.p_mat[3] = x;
	translation.p_mat[7] = y;
	translation.p_mat[11] = z;
	*this *= translation;
}

void Matrice4::scale(float x, float y, float z){
	Matrice4 scale;
	scale.p_mat[0] = x;
	scale.p_mat[5] = y;
	scale.p_mat[10] = z;
	scale.p_mat[15] = 1.0;
	*this *= scale;
}

void Matrice4::rotateRad(float angleRad, float x, float y, float z){
	Matrice4 rotation;
	// Normalisation du vecteur axe
	float norme(sqrt(x*x + y*y + z*z));
	if(norme != 0.0){
		x /= norme;
		y /= norme;
		z /= norme;
	}
	
	rotation.p_mat[0] = x*x + (1 - x*x)*cos(angleRad);
	rotation.p_mat[4] = x*y*(1 - cos(angleRad)) + z*sin(angleRad);
	rotation.p_mat[8] =x*z*(1 - cos(angleRad)) - y*sin(angleRad);
	
	rotation.p_mat[1] = x*y*(1 - cos(angleRad)) - z*sin(angleRad);
	rotation.p_mat[5] = y*y + (1 - y*y)*cos(angleRad);
	rotation.p_mat[9] = y*z*(1- cos(angleRad)) + x*sin(angleRad);
	
	rotation.p_mat[2] = x*z*(1 - cos(angleRad)) + y*sin(angleRad);
	rotation.p_mat[6] = y*z*(1 - cos(angleRad)) - x*sin(angleRad);
	rotation.p_mat[10] = z*z + (1 - z*z)*cos(angleRad);

	rotation.p_mat[15] = 1.0;
	*this *= rotation;
}

void Matrice4::rotateDeg(float angle, float x, float y, float z){
	rotateRad((angle*M_PI)/180.0, x, y, z);
}

void Matrice4::perspectiveRad(float angle, float ratio, float near, float far){
	float f(1/tan((angle/2.0)));
	Matrice4 projection;
	projection.p_mat[0] = f/ratio;
	projection.p_mat[5] = f;
	projection.p_mat[10] = (near + far)/(near - far);
	projection.p_mat[11] = (2.0*near*far)/(near - far);
	projection.p_mat[14] = -1.0;
	
	*this *= projection;
}

void Matrice4::perspectiveDeg(float angle, float ratio, float near, float far){
	perspectiveRad((angle*M_PI)/180.0, ratio, near, far);
}

void Matrice4::lookAt(float eyeX, float eyeY, float eyeZ, float centerX, float centerY, float centerZ, float upX, float upY, float upZ){
	float regardX(centerX - eyeX), regardY(centerY - eyeY), regardZ(centerZ - eyeZ);
	float normaleX, normaleY, normaleZ, newAxeX,newAxeY, newAxeZ;
	
	produitVectoriel(normaleX, normaleY, normaleZ, regardX, regardY, regardZ, upX, upY, upZ);
	produitVectoriel(newAxeX,newAxeY, newAxeZ, normaleX, normaleY, normaleZ, regardX, regardY, regardZ);
	
	normaliseVecteur(normaleX, normaleY, normaleZ);
	normaliseVecteur(newAxeX,newAxeY, newAxeZ);
	normaliseVecteur(regardX, regardY, regardZ);
	Matrice4 matrice;
	matrice.p_mat[0] = normaleX;
	matrice.p_mat[1] = normaleY;
	matrice.p_mat[2] = normaleZ;
	matrice.p_mat[3] = 0.0;
	
	matrice.p_mat[4] = newAxeX;
	matrice.p_mat[5] = newAxeY;
	matrice.p_mat[6] = newAxeZ;
	matrice.p_mat[7] = 0.0;

	matrice.p_mat[8] = -regardX;
	matrice.p_mat[9] = -regardY;
	matrice.p_mat[10] = -regardZ;
	matrice.p_mat[11] = 0.0;

	matrice.p_mat[12] = 0.0;
	matrice.p_mat[13] = 0.0;
	matrice.p_mat[14] = 0.0;
	matrice.p_mat[15] = 1.0;

	
	// Multiplication des matrices, puis translation de la matrice modelview
	*this = *this * matrice;
	translated(-eyeX, -eyeY, -eyeZ);
}

bool Matrice4::push(){
	Matrice4 *newCase = new Matrice4;
	// Si l'allocation réussit
	if(newCase != 0){
		// Copie des valeurs dans la nouvelle case
		*newCase = *this;
		// On pointe sur la sauvegarde précédente
		newCase->p_sauvegardePrecedente = p_sauvegardePrecedente;
		// Redéfinition du sommet de la pile
		p_sauvegardePrecedente = newCase;
		return true;
	}else return false;
}

bool Matrice4::pop(){
	Matrice4 *matriceTmp = p_sauvegardePrecedente;
	// Si la pile existe toujours
	if(matriceTmp != 0){
		// Copie des valeurs depuis la sauvegarde
		*this = *matriceTmp;
		// Redéfinition du sommet de la pile
		p_sauvegardePrecedente = matriceTmp->p_sauvegardePrecedente;
		matriceTmp->p_sauvegardePrecedente = 0;
		delete matriceTmp;
		return true;
	}else return false;
}

void Matrice4::depiler(){
	while(pop() != false);
}

float * Matrice4::getValue() const{
	return p_mat;
}

///////////////////////////////////////////////
//                                           //
//   Les opérateurs de la classe Matrice4   //
//                                           //
///////////////////////////////////////////////

Matrice4 & Matrice4::operator = (const Matrice4 & other){
	this->copyMatrice4(other);
	return *this;
}

Matrice4 & Matrice4::operator - (){
	for(unsigned int i(0); i < 16; i++){
		p_mat[i] = -p_mat[i];
	}
	return *this;
}

Matrice4 & Matrice4::operator -= (const Matrice4 & other){
	for(unsigned int i(0); i < 16; i++){
		p_mat[i] -= other.p_mat[i];
	}
	return *this;
}

Matrice4 & Matrice4::operator += (const Matrice4 & other){
	for(unsigned int i(0); i < 16; i++){
		p_mat[i] += other.p_mat[i];
	}
	return *this;
}

Matrice4 & Matrice4::operator *= (const Matrice4 & other){
	Matrice4 matrice;
	float calcul(0);
	int i,j,k;
	for(j = 0; j < 4; j++){
		for(i = 0; i < 4; i++){
			calcul = 0;
			for(k = 0; k < 4; k++){
				calcul +=  p_mat[4*j + k]*other.p_mat[4*k + i];
			}
			matrice.p_mat[4*j + i] = calcul;
		}
	}
	this->copyMatrice4(matrice);
	return *this;
}

Matrice4 & Matrice4::operator *= (float nb){
	for(unsigned int i(0); i < 16; i++){
		p_mat[i] *= nb;
	}
	return *this;
}

Matrice4 & Matrice4::operator /= (float nb){
	for(unsigned int i(0); i < 16; i++){
		p_mat[i] /= nb;
	}
	return *this;
}

////////////////////////////////////////////////////
//                                                //
//   Les opérateurs amis de la classe Matrice4    //
//                                                //
////////////////////////////////////////////////////

bool operator == (const Matrice4 & other1, const Matrice4 & other2){
	unsigned int i(0);
	while(i < 16){
		if(other1.p_mat[i] != other2.p_mat[i]) return false;
		i++;
	}
	return true;
}

bool operator != (const Matrice4 & other1, const Matrice4 & other2){
	unsigned int i(0);
	while(i < 16){
		if(other1.p_mat[i] != other2.p_mat[i]) return true;
		i++;
	}
	return false;
}

Matrice4 operator + (const Matrice4 & other1, const Matrice4 & other2){
	Matrice4 matrice;
	for(unsigned int i(0); i < 16; i++){
		matrice.p_mat[i] = other1.p_mat[i] + other2.p_mat[i];
	}
	return matrice;
}

Matrice4 operator - (const Matrice4 & other1, const Matrice4 & other2){
	Matrice4 matrice;
	for(unsigned int i(0); i < 16; i++){
		matrice.p_mat[i] = other1.p_mat[i] - other2.p_mat[i];
	}
	return matrice;
}

Matrice4 operator * (float nb, const Matrice4 & other1){
	Matrice4 matrice;
	for(unsigned int i(0); i < 16; i++){
		matrice.p_mat[i] = other1.p_mat[i]*nb;
	}
	return matrice;
}

Matrice4 operator * (const Matrice4 & other1, float nb){
	Matrice4 matrice;
	for(unsigned int i(0); i < 16; i++){
		matrice.p_mat[i] = other1.p_mat[i]*nb;
	}
	return matrice;
}

Matrice4 operator * (const Matrice4 & other1, const Matrice4 & other2){
	Matrice4 matrice;
	float calcul(0);
	unsigned int i,j,k;
	for(j = 0; j < 4; j++){
		for(i = 0; i < 4; i++){
			calcul = 0;
			for(k = 0; k < 4; k++){
				calcul +=  other1.p_mat[4*j + k]*other2.p_mat[4*k + i];
			}
			matrice.p_mat[4*j + i] = calcul;
		}
	}
	return matrice;
}

Vecteur4 operator * (const Matrice4 & matrice, const Vecteur4 & vecteur){
	float x(vecteur.getX()), y(vecteur.getY()), z(vecteur.getZ()), t(vecteur.getT());
	Vecteur4 vect(matrice.p_mat[0]*x + matrice.p_mat[1]*y + matrice.p_mat[2]*z + matrice.p_mat[3]*t,
		     matrice.p_mat[4]*x + matrice.p_mat[5]*y + matrice.p_mat[6]*z + matrice.p_mat[7]*t,
		     matrice.p_mat[8]*x + matrice.p_mat[9]*y + matrice.p_mat[10]*z + matrice.p_mat[11]*t,
		      matrice.p_mat[12]*x + matrice.p_mat[13]*y + matrice.p_mat[14]*z + matrice.p_mat[15]*t);
	return vect;
}

Matrice4 operator / (const Matrice4 & other1, float nb){
	Matrice4 matrice;
	for(unsigned int i(0); i < 16; i++){
		matrice.p_mat[i] = other1.p_mat[i]/nb;
	}
	return matrice;
}

std::ostream & operator << (std::ostream & out, const Matrice4 & matrice){
	if(matrice.p_mat == NULL){
		out << "Pointeur NULL de matrice" << std::endl;
	}else{
		for(unsigned int k(0); k < 4; k++){
			out << "| ";
			for(unsigned int i(0); i < 4; i++){
				out << " " << matrice.p_mat[4*k + i] << " ";
			}
			out << "| " << std::endl;
		}
	}
	return out;
}

/////////////////////////////////////////////////
//                                             //
//   Fonctions private de la classe Matrice4   //
//                                             //
/////////////////////////////////////////////////

void Matrice4::initialisationMatrice4(const Vecteur4& v1, const Vecteur4& v2, const Vecteur4& v3, const Vecteur4& v4){
	createMatrice4();
	p_mat[0] = v1.getX(); p_mat[1] = v2.getX(); p_mat[2] = v3.getX(); p_mat[3] = v4.getX();
	p_mat[4] = v1.getY(); p_mat[5] = v2.getY(); p_mat[6] = v3.getY(); p_mat[7] = v4.getY();
	p_mat[8] = v1.getZ(); p_mat[9] = v2.getZ(); p_mat[10] = v3.getZ();p_mat[11] = v4.getZ();
	p_mat[12] = v1.getT();p_mat[13] = v2.getT();p_mat[14] = v3.getT();p_mat[15] = v4.getT();
	p_sauvegardePrecedente = NULL;
}

void Matrice4::initialisationMatrice4(float x1, float y1, float z1, float t1,
				float x2, float y2, float z2, float t2,
				float x3, float y3, float z3, float t3,
				float x4, float y4, float z4, float t4){
	createMatrice4();
	p_mat[0] = x1; p_mat[1] = x2; p_mat[2] = x3; p_mat[3] = x4;
	p_mat[4] = y1; p_mat[5] = y2; p_mat[6] = y3; p_mat[7] = y4;
	p_mat[8] = z1; p_mat[9] = z2; p_mat[10] = z3;p_mat[11] = z4;
	p_mat[12] = t1;p_mat[13] = t2;p_mat[14] = t3;p_mat[15] = t4;
	p_sauvegardePrecedente = NULL;
}

void Matrice4::createMatrice4(){
	if(p_mat == NULL) p_mat = new float[16];
}

void Matrice4::copyMatrice4(const Matrice4 & other){
	createMatrice4();
	for(unsigned int i(0); i < 16; i++){
		p_mat[i] = other.p_mat[i];
	}
}