/*!\file: MatrixUtils
 * \brief triple multiply
 */ 

/*Headers*/
/*{{{*/
#include <stdio.h>
#include <sys/types.h>
#include <math.h>
#include <float.h>    /*  DBL_EPSILON  */

#include "./matrix.h"
#include "../Exceptions/exceptions.h"
#include "../MemOps/MemOps.h"
/*}}}*/

/*FUNCTION TripleMultiply {{{*/
int TripleMultiply(IssmDouble* a, int nrowa, int ncola, int itrna, IssmDouble* b, int nrowb, int ncolb, int itrnb, IssmDouble* c, int nrowc, int ncolc, int itrnc, IssmDouble* d, int iaddd){
	/*TripleMultiply    Perform triple matrix product a*b*c+d.*/

	int         idima,idimb,idimc,idimd;
	IssmDouble *dtemp;
	_assert_(a && b && c && d);

	/*  set up dimensions for triple product  */
	if (!itrna){
		idima=nrowa;
		idimb=ncola;
	}
	else{
		idima=ncola;
		idimb=nrowa;
	}

	if (!itrnb){
		if (nrowb != idimb) _error_("Matrix A and B inner vectors not equal size.");
		idimc=ncolb;
	}
	else{
		if (ncolb != idimb) _error_("Matrix A and B inner vectors not equal size.");
		idimc=nrowb;
	}

	if (!itrnc) {
		if (nrowc != idimc) _error_("Matrix B and C inner vectors not equal size.");
		idimd=ncolc;
	}
	else{
		if (ncolc != idimc) _error_("Matrix B and C inner vectors not equal size.");
		idimd=nrowc;
	}

	/*  perform the matrix triple product in the order that minimizes the
		 number of multiplies and the temporary space used, noting that
		 (a*b)*c requires ac(b+d) multiplies and ac IssmDoubles, and a*(b*c)
		 requires bd(a+c) multiplies and bd IssmDoubles (both are the same for
		 a symmetric triple product)  */

	/*  multiply (a*b)*c+d  */
	if (idima*idimc*(idimb+idimd) <= idimb*idimd*(idima+idimc)) {
		dtemp=xNew<IssmDouble>(idima*idimc);

		MatrixMultiply(a,nrowa,ncola,itrna,b,nrowb,ncolb,itrnb,dtemp,0);
		MatrixMultiply(dtemp,idima,idimc,0,c,nrowc,ncolc,itrnc,d,iaddd);
		xDelete<IssmDouble>(dtemp);
	}

	/*  multiply a*(b*c)+d  */
	else{
		dtemp=xNew<IssmDouble>(idimb*idimd);

		MatrixMultiply(b,nrowb,ncolb,itrnb,c,nrowc,ncolc,itrnc,dtemp,0);
		MatrixMultiply(a,nrowa,ncola,itrna,dtemp,idimb,idimd,0,d,iaddd);
		xDelete<IssmDouble>(dtemp);
	}

	return 1;
}/*}}}*/
/*FUNCTION MatrixMuliply {{{*/
int MatrixMultiply(IssmDouble* a, int nrowa, int ncola, int itrna, IssmDouble* b, int nrowb, int ncolb, int itrnb, IssmDouble* c, int iaddc ){
	/*MatrixMultiply    Perform matrix multiplication a*b+c.*/
	int noerr=1;
	int i,j,k,ipta,iptb,iptc;
	int nrowc,ncolc,iinca,jinca,iincb,jincb,ntrma,ntrmb,nterm;

	_assert_(a && b && c);

	/*  set up dimensions and increments for matrix a  */
	if (!itrna) {
		nrowc=nrowa;
		ntrma=ncola;
		iinca=ncola;
		jinca=1;
	}
	else {
		nrowc=ncola;
		ntrma=nrowa;
		iinca=1;
		jinca=ncola;
	}

	/*  set up dimensions and increments for matrix b  */
	if (!itrnb) {
		ncolc=ncolb;
		ntrmb=nrowb;
		iincb=ncolb;
		jincb=1;
	}
	else {
		ncolc=nrowb;
		ntrmb=ncolb;
		iincb=1;
		jincb=ncolb;
	}

	if (ntrma != ntrmb) _error_("Matrix A and B inner vectors not equal size");

	nterm=ntrma;

	/*  zero matrix c, if not being added to product  */
	if (!iaddc) for (i=0;i<nrowc*ncolc;i++) c[i]=0.;

	/*  perform the matrix multiplication  */
	iptc=0;
	for (i=0; i<nrowc; i++){
		for (j=0; j<ncolc; j++){
			ipta=i*iinca;
			iptb=j*jincb;

			for (k=0; k<nterm; k++){
				c[iptc]+=a[ipta]*b[iptb];
				ipta+=jinca;
				iptb+=iincb;
			}
			iptc++;
		}
	}

	return noerr;
}/*}}}*/
/*FUNCTION MatrixInverse {{{*/
int MatrixInverse( IssmDouble* a, int ndim, int nrow, IssmDouble* b, int nvec, IssmDouble* pdet ){
	/* MatrixInverse    Perform matrix inversion and linear equation solution.

		This function uses Gaussian elimination on the original matrix
		augmented by an identity matrix of the same size to calculate
		the inverse (see for example, "Modern Methods of Engineering
		Computation", Sec. 6.4).  By noting how the matrices are
		unpopulated and repopulated, the calculation may be done in place.

		Gaussian elimination is inherently inefficient, and so this is
		intended for small matrices.  */
	int noerr=1;
	int i,j,k,ipt,jpt,irow,icol,ipiv,ncol;
	int *pivrc1,*pivrc2,*pindx;
	IssmDouble pivot,det,dtemp;

	if (!b && nvec) {
		_error_("No right-hand side for nvec=" << nvec << ".");
		noerr=0;
		return noerr;
	}

	/*In debugging mode, check that we are not dealing with simple matrices*/
	_assert_(!(ndim==2 && nrow==2));
	_assert_(!(ndim==3 && nrow==3));

	/*  initialize local variables and arrays  */

	ncol=nrow;
	det=1.;
	pivrc1 =xNew<int>(nrow);
	pivrc2 =xNew<int>(nrow);
	pindx =xNew<int>(nrow);

	/*  loop over the rows/columns of the matrix  */

	for (i=0; i<nrow; i++) {

		/*  search for pivot, finding the term with the greatest magnitude
			 in the rows/columns not yet used  */

		pivot=0.;
		for (j=0; j<nrow; j++)
		 if (!pindx[j])
		  for (k=0; k<ncol; k++)
			if (!pindx[k])
			 if (fabs(a[j*ndim+k]) > fabs(pivot)) {
				 irow=j;
				 icol=k;
				 pivot=a[j*ndim+k];
			 }

		if (fabs(pivot) < DBL_EPSILON) {
			xDelete<int>(pivrc1);
			xDelete<int>(pivrc2);
			xDelete<int>(pindx);
			_error_("Pivot " << pivot << " less than machine epsilon");
			noerr=0;
			return noerr;
		}

		pivrc1[i]=irow;
		pivrc2[i]=icol;

		ipiv=icol;
		pindx[ipiv]++;

		//		_printf_("pivot for i=" << i << ": irow=" << irow << ", icol=" << icol  << ", pindx[" << ipiv << "]=" << pindx[ipiv] << "\n\n");

		/*  switch rows to put pivot element on diagonal, noting that the
			 column stays the same and the determinant changes sign  */

		if (irow != icol) {
			//			_printf_("row switch for i=" << i << ": irow=" << irow << ", icol=" << icol << "\n\n");

			ipt=irow*ndim;
			jpt=icol*ndim;
			for (k=0; k<ncol; k++) {
				dtemp   =a[ipt+k];
				a[ipt+k]=a[jpt+k];
				a[jpt+k]=dtemp;
			}

			ipt=irow*nvec;
			jpt=icol*nvec;
			for (k=0; k<nvec; k++) {
				dtemp   =b[ipt+k];
				b[ipt+k]=b[jpt+k];
				b[jpt+k]=dtemp;
			}

			det=-det;
		}

		/*  divide pivot row by pivot element, noting that the original
			 matrix will have 1 on the diagonal, which will be discarded,
			 and the augmented matrix will start with 1 from the identity
			 matrix and then have 1/pivot, which is part of the inverse.  */

		a[ipiv*ndim+ipiv]=1.;

		ipt=ipiv*ndim;
		for (k=0; k<ncol; k++)
		 a[ipt+k]/=pivot;

		ipt=ipiv*nvec;
		for (k=0; k<nvec; k++)
		 b[ipt+k]/=pivot;

		/*  reduce non-pivot rows such that they will have 0 in the pivot
			 column, which will be discarded, and the augmented matrix will
			 start with 0 from the identity matrix and then have non-zero
			 in the corresponding column, which is part of the inverse.
			 only one column of the augmented matrix is populated at a time,
			 which corresponds to the only column of the original matrix
			 being zeroed, so that the inverse may be done in place.  */

		for (j=0; j<nrow; j++) {
			if (j == ipiv) continue;

			dtemp=a[j*ndim+ipiv];
			a[j*ndim+ipiv]=0.;

			if (fabs(dtemp) > DBL_EPSILON) {
				ipt=j   *ndim;
				jpt=ipiv*ndim;
				for (k=0; k<ncol; k++)
				 a[ipt+k]-=dtemp*a[jpt+k];

				ipt=j   *nvec;
				jpt=ipiv*nvec;
				for (k=0; k<nvec; k++)
				 b[ipt+k]-=dtemp*b[jpt+k];
			}
		}

		/*  for a diagonal matrix, the determinant is the product of the
			 diagonal terms, and so it may be accumulated from the pivots,
			 noting that switching rows changes the sign as above  */

		det*=pivot;
	}

	/*  switch columns back in reverse order, noting that a row switch
		 in the original matrix corresponds to a column switch in the
		 inverse matrix  */

	for (i=0; i<nrow; i++) {
		j=(nrow-1)-i;

		if (pivrc1[j] != pivrc2[j]) {
			irow=pivrc1[j];
			icol=pivrc2[j];

			//			_printf_("column switch back for j=" << j << ": irow=" << irow << ", icol=" << icol << "\n\n");

			ipt=0;
			for (k=0; k<nrow; k++) {
				dtemp      =a[ipt+irow];
				a[ipt+irow]=a[ipt+icol];
				a[ipt+icol]=dtemp;
				ipt+=ndim;
			}
		}
	}

	if (pdet) *pdet=det;
	xDelete<int>(pivrc1);
	xDelete<int>(pivrc2);
	xDelete<int>(pindx);
	return noerr;
}/*}}}*/
/*FUNCTION Matrix2x2Determinant(IssmDouble* Adet,IssmDouble* A) {{{*/
void Matrix2x2Determinant(IssmDouble* Adet,IssmDouble* A){
	/*Compute determinant of a 2x2 matrix*/

	/*det = a*d - c*b*/
	*Adet= A[0]*A[3]-A[2]*A[1];
}
/*}}}*/
/*FUNCTION Matrix2x2Invert(IssmDouble* Ainv,IssmDouble* A) {{{*/
void Matrix2x2Invert(IssmDouble* Ainv,IssmDouble* A){

	/*Intermediaries*/
	IssmDouble det;

	/*Compute determinant*/
	Matrix2x2Determinant(&det,A);
	if (fabs(det) < DBL_EPSILON) _error_("Determinant smaller than machine epsilon");

	/*Compute invert*/
	Ainv[0]=   A[3]/det; /* =  d/det */
	Ainv[1]= - A[1]/det; /* = -b/det */
	Ainv[2]= - A[2]/det; /* = -c/det */
	Ainv[3]=   A[0]/det; /* =  a/det */

}/*}}}*/
/*FUNCTION Matrix3x3Determinant(IssmDouble* Adet,IssmDouble* A) {{{*/
void Matrix3x3Determinant(IssmDouble* Adet,IssmDouble* A){
	/*Compute determinant of a 3x3 matrix*/

	/*det = a*(e*i-f*h)-b*(d*i-f*g)+c*(d*h-e*g)*/
	*Adet= A[0]*A[4]*A[8]-A[0]*A[5]*A[7]-A[3]*A[1]*A[8]+A[3]*A[2]*A[7]+A[6]*A[1]*A[5]-A[6]*A[2]*A[4];
}
/*}}}*/
/*FUNCTION Matrix3x3Invert(IssmDouble* Ainv,IssmDouble* A) {{{*/
void Matrix3x3Invert(IssmDouble* Ainv,IssmDouble* A){

	/*Intermediaries*/
	IssmDouble det;

	/*Compute determinant*/
	Matrix3x3Determinant(&det,A);
	if (fabs(det) < DBL_EPSILON) _error_("Determinant smaller than machine epsilon");

	/*Compute invert*/
	Ainv[0]=(A[4]*A[8]-A[5]*A[7])/det; /* = (e*i-f*h)/det */
	Ainv[1]=(A[2]*A[7]-A[1]*A[8])/det; /* = (c*h-b*i)/det */
	Ainv[2]=(A[1]*A[5]-A[2]*A[4])/det; /* = (b*f-c*e)/det */
	Ainv[3]=(A[5]*A[6]-A[3]*A[8])/det; /* = (f*g-d*i)/det */
	Ainv[4]=(A[0]*A[8]-A[2]*A[6])/det; /* = (a*i-c*g)/det */
	Ainv[5]=(A[2]*A[3]-A[0]*A[5])/det; /* = (c*d-a*f)/det */
	Ainv[6]=(A[3]*A[7]-A[4]*A[6])/det; /* = (d*h-e*g)/det */
	Ainv[7]=(A[1]*A[6]-A[0]*A[7])/det; /* = (b*g-a*h)/det */
	Ainv[8]=(A[0]*A[4]-A[1]*A[3])/det; /* = (a*e-b*d)/det */

}/*}}}*/
/*FUNCTION MatrixTranspose(IssmDouble* Adet,IssmDouble* A) {{{*/
void MatrixTranspose(IssmDouble* tA,IssmDouble* A, int nrows, int ncols){
	/*Transpose a n*m matrix*/

	int i,j;

	/*Check the the matrix has been allocated*/
	_assert_(tA);

	/*Transpose*/
	for(i=0;i<ncols;i++){
		for(j=0;j<nrows;j++){
			tA[i*nrows+j]=A[j*ncols+i];
		}
	}

}
/*}}}*/
