/*
	Auteur : Pierre Aubert
	Mail : aubertp7@gmail.com
	Licence : lgpl
*/
#include "matrice3f.h"

const Matrice3f MAT3F_NULL(VECT3F_NULL, VECT3F_NULL, VECT3F_NULL);
const Matrice3f MAT3F_ID(1.0,0.0,0.0 , 0.0,1.0,0.0 , 0.0,0.0,1.0);

///converti un angle en radian en degré
/**	@param rad : angle en radian
	@return angle en degré
*/
float radToDegF(float rad){return rad*180.0/M_PI;}

///converti un angle en degré en radian
/**	@param deg : angle en degré
	@return angle en radian
*/
float degToRadF(float deg){return deg*M_PI/180.0;}

using namespace std;

///constructeur de Matrice3f
/**	@param v1 : Vecteur3f de la premiere colonne
 * 	@param v2 : Vecteur3f de la deuxième colonne
 * 	@param v3 : Vecteur3f de la troisième colonne
*/
Matrice3f::Matrice3f(const Vecteur3f& v1, const Vecteur3f& v2, const Vecteur3f& v3){
	this->initialisation(v1, v2, v3);
}

///constructeur de Matrice3f
/**	@param x1 : coeficient en (0,0)
 * 	@param y1 : coeficient en (0,1)
 * 	@param z1 : coeficient en (0,2)
 * 	@param x2 : coeficient en (1,0)
 * 	@param y2 : coeficient en (1,1)
 * 	@param z2 : coeficient en (1,2)
 * 	@param x3 : coeficient en (2,0)
 * 	@param y3 : coeficient en (2,1)
 * 	@param z3 : coeficient en (2,2)
*/
Matrice3f::Matrice3f(float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3){
	this->initialisation(Vecteur3f(x1, y1, z1), Vecteur3f(x2, y2, z2), Vecteur3f(x3, y3, z3));
}

///constructeur de copie de Matrice3f
/**	@param matrice : Matrice3f à copier
*/
Matrice3f::Matrice3f(const Matrice3f & matrice){
	this->copyMatrice3f(matrice);
}

///destructeur de Matrice3f
Matrice3f::~Matrice3f(){
	delete [] this->p_mat;
}

///initialise les Vecteur3fs colonnes de la Matrice3f
/**	@param v1 : Vecteur3f de la premiere colonne
 * 	@param v2 : Vecteur3f de la deuxième colonne
 * 	@param v3 : Vecteur3f de la troisième colonne
*/
void Matrice3f::setVecteur3f(const Vecteur3f& v1, const Vecteur3f& v2, const Vecteur3f& v3){
	this->initialisation(v1, v2, v3);
}

///remplace toute les valeurs de la matrice par une seule
/** @param val : nouvelle valeur des "cases" de la Matrice3f
*/
void Matrice3f::fill(float val){
	for(unsigned int i = 0; i < 9; i++){
		this->p_mat[i] = val;
	} 
}

///transforme la matrice en matrice de rotation avec un axe de rotation et un angle angleRad en radians
/**	@param axe : axe de rotation (pas forcément unitaire)
 * 	@param angleRad : angle de rotation autour de l'axe Ox en radians
*/
void Matrice3f::setRotRad(const Vecteur3f& axe, float angleRad){
	Vecteur3f axeUnit(axe.getUnit());
	if(axeUnit == VECT3F_NULL) return;
	float x(axeUnit.getx());
	float y(axeUnit.gety());
	float z(axeUnit.getz());
	Vecteur3f colonne1(x*x + (1 - x*x)*cos(angleRad), x*y*(1 - cos(angleRad)) + z*sin(angleRad), x*z*(1 - cos(angleRad)) - y*sin(angleRad));
	Vecteur3f colonne2(x*y*(1 - cos(angleRad)) - z*sin(angleRad), y*y + (1 - y*y)*cos(angleRad), y*z*(1- cos(angleRad)) + x*sin(angleRad));
	Vecteur3f colonne3(x*z*(1 - cos(angleRad)) + y*sin(angleRad), y*z*(1 - cos(angleRad)) - x*sin(angleRad), z*z + (1 - z*z)*cos(angleRad));
	this->setVecteur3f(colonne1, colonne2, colonne3);
}

///transforme la matrice en matrice de rotation  avec des angles en radians
/**	@param rotX : rotation suivant Ox
 * 	@param rotY : rotation suivant Oy
 * 	@param rotZ : rotation suivant Oz
*/
void Matrice3f::setRotRad(float rotX, float rotY, float rotZ){
	Matrice3f matRotX;
	matRotX.setRotXRad(rotX);
	Matrice3f matRotY;
	matRotY.setRotYRad(rotY);
	Matrice3f matRotZ;
	matRotZ.setRotZRad(rotZ);
	this->copyMatrice3f(matRotX*matRotY*matRotZ);
}

///transforme la matrice en matrice de rotation d'axe Ox et d'angle angleRad en radians
/**	@param angleRad : angle de rotation autour de l'axe Ox en radians
*/
void Matrice3f::setRotXRad(float angleRad){
	Vecteur3f colonne1(1.0, 0.0, 0.0);
	Vecteur3f colonne2(0.0, cos(angleRad), sin(angleRad));
	Vecteur3f colonne3(0.0, -sin(angleRad), cos(angleRad));
	this->setVecteur3f(colonne1, colonne2, colonne3);
}

///transforme la matrice en matrice de rotation d'axe Oy et d'angle angleRad en radians
/**	@param angleRad : angle de rotation autour de l'axe Oy en radians
*/
void Matrice3f::setRotYRad(float angleRad){
	Vecteur3f colonne1(cos(angleRad), 0.0, -sin(angleRad));
	Vecteur3f colonne2(0.0, 1.0, 0.0);
	Vecteur3f colonne3(sin(angleRad), 0.0, cos(angleRad));
	this->setVecteur3f(colonne1, colonne2, colonne3);
}

///transforme la matrice en matrice de rotation d'axe Oz et d'angle angleRad en radians
/**	@param angleRad : angle de rotation autour de l'axe Oz en radians
*/
void Matrice3f::setRotZRad(float angleRad){
	Vecteur3f colonne1(cos(angleRad), sin(angleRad), 0.0);
	Vecteur3f colonne2(-sin(angleRad), cos(angleRad), 0.0);
	Vecteur3f colonne3(0.0, 0.0, 1.0);
	this->setVecteur3f(colonne1, colonne2, colonne3);
}

///transforme la matrice en matrice de rotation avec un axe de angleDeg et un angle angleRad en degrés
/**	@param axe : axe de rotation (pas forcément unitaire)
 * 	@param angleDeg : angle de rotation autour de l'axe Ox en degrés
*/
void Matrice3f::setRotDeg(const Vecteur3f& axe, float angleDeg){
	this->setRotRad(axe, degToRadF(angleDeg));
}

///transforme la matrice en matrice de rotation avec des angles en dégrés
/**	@param rotX : rotation suivant Ox
 * 	@param rotY : rotation suivant Oy
 * 	@param rotZ : rotation suivant Oz
*/
void Matrice3f::setRotDeg(float rotX, float rotY, float rotZ){
	setRotRad(degToRadF(rotX), degToRadF(rotY), degToRadF(rotZ));
}

///transforme la matrice en matrice de rotation d'axe Ox et d'angle angleDeg en degrés
/**	@param angleDeg : angle de rotation autour de l'axe Ox en degrés
*/
void Matrice3f::setRotXDeg(float angleDeg){
	setRotXRad(degToRadF(angleDeg));
}

///transforme la matrice en matrice de rotation d'axe Oy et d'angle angleDeg en degrés
/**	@param angleDeg : angle de rotation autour de l'axe Oy en degrés
*/
void Matrice3f::setRotYDeg(float angleDeg){
	setRotYRad(degToRadF(angleDeg));
}

///transforme la matrice en matrice de rotation d'axe Oz et d'angle angleDeg en degrés
/**	@param angleDeg : angle de rotation autour de l'axe Oz en degrés
*/
void Matrice3f::setRotZDeg(float angleDeg){
	setRotZRad(degToRadF(angleDeg));
}

///calcul de déterminant de la Matrice3f
/** @return déterminant de la Matrice3f
*/
float Matrice3f::determinant(){
	return (this->p_mat[0]*(this->p_mat[4]*this->p_mat[8] - this->p_mat[7]*this->p_mat[5])
		- this->p_mat[3]*(this->p_mat[1]*this->p_mat[8] - this->p_mat[7]*this->p_mat[2])
		+ this->p_mat[6]*(this->p_mat[1]*this->p_mat[5] - this->p_mat[4]*this->p_mat[2]));
}

///calcul et renvoie la matrice transposée
/**	@return matrice transposée
*/
Matrice3f Matrice3f::transposed(){
	Matrice3f matrice(*this);
	float tmp;
	tmp = matrice.p_mat[1];
	matrice.p_mat[1] = matrice.p_mat[3];
	matrice.p_mat[3] = tmp;
	tmp = matrice.p_mat[2];
	matrice.p_mat[2] = matrice.p_mat[6];
	matrice.p_mat[6] = tmp;
	tmp = matrice.p_mat[5];
	matrice.p_mat[5] = matrice.p_mat[7];
	matrice.p_mat[7] = tmp;
	return matrice;
}

///calcul et renvoie la comatrice
/**	@return comatrice
*/
Matrice3f Matrice3f::comatrice(){
	Matrice3f matrice;
	matrice.p_mat[0] = this->p_mat[4]*this->p_mat[8] - this->p_mat[7]*this->p_mat[5];
	matrice.p_mat[1] = this->p_mat[5]*this->p_mat[6] - this->p_mat[3]*this->p_mat[8];
	matrice.p_mat[2] = this->p_mat[4]*this->p_mat[7] - this->p_mat[5]*this->p_mat[6];
	matrice.p_mat[3] = this->p_mat[2]*this->p_mat[7] - this->p_mat[1]*this->p_mat[8];
	matrice.p_mat[4] = this->p_mat[0]*this->p_mat[8] - this->p_mat[2]*this->p_mat[6];
	matrice.p_mat[5] = this->p_mat[1]*this->p_mat[6] - this->p_mat[0]*this->p_mat[7];
	matrice.p_mat[6] = this->p_mat[1]*this->p_mat[5] - this->p_mat[2]*this->p_mat[4];
	matrice.p_mat[7] = this->p_mat[2]*this->p_mat[3] - this->p_mat[0]*this->p_mat[5];
	matrice.p_mat[8] = this->p_mat[0]*this->p_mat[4] - this->p_mat[1]*this->p_mat[3];
	return matrice;
}

///calcul et renvoie la comatrice transposée
/**	@return comatrice transposée
*/
Matrice3f Matrice3f::comatriceTranposed(){
	Matrice3f matrice;
	matrice.p_mat[0] = this->p_mat[4]*this->p_mat[8] - this->p_mat[7]*this->p_mat[5];
	matrice.p_mat[4] = this->p_mat[0]*this->p_mat[8] - this->p_mat[2]*this->p_mat[6];
	matrice.p_mat[8] = this->p_mat[0]*this->p_mat[4] - this->p_mat[1]*this->p_mat[3];
	
	matrice.p_mat[3] = this->p_mat[5]*this->p_mat[6] - this->p_mat[3]*this->p_mat[8];
	matrice.p_mat[6] = this->p_mat[4]*this->p_mat[7] - this->p_mat[5]*this->p_mat[6];
	matrice.p_mat[7] = this->p_mat[1]*this->p_mat[6] - this->p_mat[0]*this->p_mat[7];
	
	matrice.p_mat[1] = this->p_mat[2]*this->p_mat[7] - this->p_mat[1]*this->p_mat[8];
	matrice.p_mat[2] = this->p_mat[1]*this->p_mat[5] - this->p_mat[2]*this->p_mat[4];
	matrice.p_mat[5] = this->p_mat[2]*this->p_mat[3] - this->p_mat[0]*this->p_mat[5];
	
	return matrice;
}

///calcul et renvoie la matrice inverse de la Matrice3f
/**	@return matrice inverse de la Matrice3f, où matrice NULLE si elle n'existe pas (det = 0.0)
*/
Matrice3f Matrice3f::invert(){
	Matrice3f matrice = this->comatriceTranposed();
	float det = this->determinant();
	if(det != 0.0){
		matrice /= det;
	}else{
		matrice.fill(0.0);
	}
	return matrice;
}

///définition de l'operateur =
/**	@param matrice : Matrice3f à copier
 * 	@return Matrice3f copiée
*/
Matrice3f & Matrice3f::operator = (const Matrice3f & matrice){
	this->copyMatrice3f(matrice);
	return *this;
}

///définition de l'operateur +=
/**	@param matrice : Matrice3f à additioner
 * 	@return résultat de l'addition
*/
Matrice3f & Matrice3f::operator += (const Matrice3f & matrice){
	for(unsigned int i = 0; i < 9; i++){
		this->p_mat[i] += matrice.p_mat[i];
	} 
	return *this;
}

///définition de l'operateur *=
/**	@param matrice : Matrice3f à multiplier
 * 	@return résultat de la multiplication
*/
Matrice3f & Matrice3f::operator *= (const Matrice3f & matrice){
	Matrice3f buffer;
	for(unsigned int j = 0; j < 3; j++){
		for(unsigned int i = 0; i < 3; i++){
			float calcul = 0;
			for(unsigned int k = 0; k < 3; k++){
				calcul +=  this->p_mat[3*j + k]*matrice.p_mat[3*k + j];
			}
			buffer.p_mat[3*j + i] = calcul;
		}
	}
	*this = buffer;
	return *this;
}

///définition de l'operateur *=
/**	@param scal : scalaire multipliant la matrice
 * 	@return résultat de la multiplication
*/
Matrice3f & Matrice3f::operator *= (float scal){
	for(unsigned int i = 0; i < 9; i++){
		this->p_mat[i] *= scal;
	} 
	return *this;
}

///définition de l'operateur /=
/**	@param scal : scalaire divisant la matrice
 * 	@return résultat de la division
*/
Matrice3f & Matrice3f::operator /= (float scal){
	for(unsigned int i = 0; i < 9; i++){
		this->p_mat[i] /= scal;
	} 
	return *this;
}

///opérateur - sur une Matrice3f
/** @return opposé de la Matrice3f
*/
Matrice3f & Matrice3f::operator - (){
	for(unsigned int i = 0; i < 9; i++){
		this->p_mat[i] = -this->p_mat[i];
	} 
	return *this;
}

///renvoie un nombre de la Matrice
/**	@param i : position du nombre dans la Matrice3f
 * 	@return nombre à la position i dans la Matrice3f
*/
float Matrice3f::operator [] (unsigned int i) const{
	if(i < 9){return this->p_mat[i];}
	else {return 0.0;}
}

//////////////////////////////////////////////////////////////////////////
//                Fonctions amies de la classe Matrice3f                 //
//////////////////////////////////////////////////////////////////////////

///définition de l'opérateur +
/**	@param matrice1 : Matrice3f
 * 	@param matrice2 : Matrice3f
 * 	@return addition matrice1 + matrice2
*/
Matrice3f operator + (const Matrice3f & matrice1, const Matrice3f & matrice2){
	Matrice3f matrice;
	for(unsigned int i = 0; i < 9; i++){
		matrice.p_mat[i] = matrice1.p_mat[i] + matrice2.p_mat[i];
	}	
	return matrice;
}

///définition de l'opérateur -
/**	@param matrice1 : Matrice3f
 * 	@param matrice2 : Matrice3f
 * 	@return soustraction matrice1 + matrice2
*/
Matrice3f operator - (const Matrice3f & matrice1, const Matrice3f & matrice2){
	Matrice3f matrice;
	for(unsigned int i = 0; i < 9; i++){
		matrice.p_mat[i] = matrice1.p_mat[i] - matrice2.p_mat[i];
	}	
	return matrice;
}

///définition de l'opérateur -
/**	@param matrice1 : Matrice3f
 * 	@param scal : scalaire
 * 	@return division de matrice1 par scal
*/
Matrice3f operator / (const Matrice3f & matrice1, float scal){
	Matrice3f matrice;
	for(unsigned int i = 0; i < 9; i++){
		matrice.p_mat[i] = matrice1.p_mat[i]/scal;
	}
	return matrice;
}

///définition de l'opérateur * (multiplication à droite par un scalaire)
/**	@param matrice1 : Matrice3f
 * 	@param scal : scalaire
 * 	@return multiplication de matrice1 par scal
*/
Matrice3f operator * (const Matrice3f & matrice1, float scal){
	Matrice3f matrice;
	for(unsigned int i = 0; i < 9; i++){
		matrice.p_mat[i] = matrice1.p_mat[i]*scal;
	}
	return matrice;
}

///définition de l'opérateur * (multiplication à gauche par un scalaire)
/**	@param scal : scalaire
 * 	@param matrice2 : Matrice3f
 * 	@return multiplication de matrice1 par scal
*/
Matrice3f operator * (float scal, const Matrice3f & matrice2){
	Matrice3f matrice;
	for(unsigned int i = 0; i < 9; i++){
		matrice.p_mat[i] = matrice2.p_mat[i]*scal;
	}
	return matrice;
}

///définition de l'opérateur * 
/**	@param matrice1 : Matrice3f
 * 	@param matrice2 : Matrice3f
 * 	@return multiplication de matrice1 par matrice2
*/
Matrice3f operator * (const Matrice3f & matrice1, const Matrice3f & matrice2){
	Matrice3f matrice;
	for(unsigned int j = 0; j < 3; j++){
		for(unsigned int i = 0; i < 3; i++){
			float calcul = 0;
			for(unsigned int k = 0; k < 3; k++){
				calcul +=  matrice1.p_mat[3*j + k]*matrice2.p_mat[3*k + i];
			}
			matrice.p_mat[3*j + i] = calcul;
		}
	}
	return matrice;
}

///définition de l'opérateur * (multiplication à gauche par un Vecteur3f
/**	@param matrice1 : Matrice3f
 * 	@param vecteur : Vecteur3f
 * 	@return multiplication de matrice1 par matrice2
*/
Vecteur3f operator * (const Matrice3f & matrice1, const Vecteur3f & vecteur){
	Vecteur3f vect(matrice1.p_mat[0]*vecteur.getx() + matrice1.p_mat[1]*vecteur.gety() + matrice1.p_mat[2]*vecteur.getz(),
		     matrice1.p_mat[3]*vecteur.getx() + matrice1.p_mat[4]*vecteur.gety() + matrice1.p_mat[5]*vecteur.getz(),
		     matrice1.p_mat[6]*vecteur.getx() + matrice1.p_mat[7]*vecteur.gety() + matrice1.p_mat[8]*vecteur.getz());
	return vect;
}

///opérateur d'égalité
/**	@param matrice1 : Matrice3f
 * 	@param matrice2 : Matrice3f
 * 	@return true si matrice1 = matrice2, false sinon
*/
bool operator == (const Matrice3f & matrice1, const Matrice3f & matrice2){
	bool pareil = true;
	unsigned int i = 0;
	while(pareil && i < 9){
		pareil = pareil && (matrice1.p_mat[i] == matrice2.p_mat[i]);
		i++;
	}
	return pareil;
}

///opérateur d'inégalité
/**	@param matrice1 : Matrice3f
 * 	@param matrice2 : Matrice3f
 * 	@return false si matrice1 = matrice2, true sinon
*/
bool operator != (const Matrice3f & matrice1, const Matrice3f & matrice2){
	bool different = true;
	unsigned int i = 0;
	while(different && i < 9){
		different = different && (matrice1.p_mat[i] != matrice2.p_mat[i]);
		i++;
	}
	return different;
}

///opérateur de flux pour afficher la Matrice3f
/**	@param out : flux de sortie
 * 	@param matrice : matrice à afficher
 * 	@return flux de sortie comprenant la Matrice3f à afficher
*/
std::ostream & operator << (std::ostream & out, const Matrice3f & matrice){
	out << matrice.p_mat[0] << " " << matrice.p_mat[1] << " " << matrice.p_mat[2] << endl;
	out << matrice.p_mat[3] << " " << matrice.p_mat[4] << " " << matrice.p_mat[5] << endl;
	out << matrice.p_mat[6] << " " << matrice.p_mat[7] << " " << matrice.p_mat[8] << endl;
	return out;
}

///initialise la Matrice3f
/**	@param v1 : Vecteur3f de la premiere colonne
 * 	@param v2 : Vecteur3f de la deuxième colonne
 * 	@param v3 : Vecteur3f de la troisième colonne
*/
void Matrice3f::initialisation(const Vecteur3f& v1, const Vecteur3f& v2, const Vecteur3f& v3){
	this->p_mat = new float[9];
	this->p_mat[0] = v1.getx();
	this->p_mat[3] = v1.gety(); 
	this->p_mat[6] = v1.getz();
	this->p_mat[1] = v2.getx();
	this->p_mat[4] = v2.gety();
	this->p_mat[7] = v2.getz();
	this->p_mat[2] = v3.getx();
	this->p_mat[5] = v3.gety();
	this->p_mat[8] = v3.getz();
}

///fonction de copie de la Matrice3f
/**	@param matrice : Matrice3f à copier
*/
void Matrice3f::copyMatrice3f(const Matrice3f & matrice){
	this->p_mat = new float[9];
	for(unsigned int i = 0; i < 9; i++){
		this->p_mat[i] = matrice.p_mat[i];
	}
}

///fonction qui ne sert manifestement à rien (mais bon si elle est là c'est peut être pour une raison, mais la question est de savoir laquelle)
/**	@param i : entrée
 * 	@return déterminant de la matrice 2×2 qui reste (enfin je croix
*/
float Matrice3f::det2(unsigned int i){
	if(i < 4){
		return this->p_mat[i]*this->p_mat[i + 4] - this->p_mat[i + 1]*this->p_mat[i + 3];
	}
	return 0.0;
}