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

/*Headers*/
/*{{{*/
#include "./matrix.h"
#include "../Exceptions/exceptions.h"
#include "../Alloc/alloc.h"
#include <stdio.h>
#include <sys/types.h>
#include <math.h>
#include <float.h>    /*  DBL_EPSILON  */
#include "../../include/include.h"
#include "../../io/io.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;

/*  set up dimensions for triple product  */

	if (!itrna) {
		idima=nrowa;
		idimb=ncola;
	}
	else {
		idima=ncola;
		idimb=nrowa;
	}

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

	if (!itrnc) {
		if (nrowc != idimc) {
			_error2_("Matrix B and C inner vectors not equal size.");
		}
		idimd=ncolc;
	}
	else {
		if (ncolc != idimc) {
			_error2_("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;

/*  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) {
		_error2_("Matrix A and B inner vectors not equal size");
	    noerr=0;	
		return noerr;
	}
	else
		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) {
		_error2_("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);
			_error2_("Pivot " << pivot << " less than machine epsilon");
			noerr=0;
			return noerr;
		}

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

		ipiv=icol;
		pindx[ipiv]++;

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

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

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

			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_(true,"column switch back for j=%d: irow=%d, icol=%d\n",
//					 j,irow,icol);

			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) _error2_("Determinant smaller that 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) _error2_("Determinant smaller that 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];
		}
	}

}
/*}}}*/
