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

/*

	TripleMultiply    Perform triple matrix product a*b*c+d.


	Input:
		double*    a        first matrix
		int        nrowa    number of rows in a
		int        ncola    number of columns in a
		int        itrna    transpose flag for a
		double*    b        second matrix
		int        nrowb    number of rows in b
		int        ncolb    number of columns in b
		int        itrnb    transpose flag for b
		double*    c        third matrix
		int        nrowc    number of rows in c
		int        ncolc    number of columns in c
		int        itrnc    transpose flag for c
		double*    d        addition matrix
		int        iaddd    addition flag for d

	Output:
		double*    d        product matrix
		int                 function return value

*/
#undef __FUNCT__
#define __FUNCT__ "TripleMultiply"

#include "./matrix.h"
#include "../Exceptions/exceptions.h"
#include "../Alloc/alloc.h"
#include "../../include/macros.h"
#include "../Matlab/matlabshared.h"
#include <stdio.h>
#include <sys/types.h>
#include <math.h>     /*  fabs  */
#include <float.h>    /*  DBL_EPSILON  */

int TripleMultiply( double* a, int nrowa, int ncola, int itrna, double* b, int nrowb, int ncolb, int itrnb, double* c, int nrowc, int ncolc, int itrnc, double* d, int iaddd){
	
	int idima,idimb,idimc,idimd;
	double* dtemp;

/*  set up dimensions for triple product  */

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

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

	if (!itrnc) {
		if (nrowc != idimc) {
			throw ErrorException(__FUNCT__,"Matrix B and C inner vectors not equal size.");
		}
		idimd=ncolc;
	}
	else {
		if (ncolc != idimc) {
			throw ErrorException(__FUNCT__," 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 doubles, and a*(b*c)
	requires bd(a+c) multiplies and bd doubles (both are the same for
	a symmetric triple product)  */

/*  multiply (a*b)*c+d  */

	if (idima*idimc*(idimb+idimd) <= idimb*idimd*(idima+idimc)) {
		dtemp=(double *) xmalloc(idima*idimc*sizeof(double));

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

		xfree((void **)&dtemp);
	}

/*  multiply a*(b*c)+d  */

	else {
		dtemp=(double *) xmalloc(idimb*idimd*sizeof(double));

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

		xfree((void **)&dtemp);
	}
}





/*

	MatrixMultiply    Perform matrix multiplication a*b+c.


	Input:
		double*    a        first matrix
		int        nrowa    number of rows in a
		int        ncola    number of columns in a
		int        itrna    transpose flag for a
		double*    b        second matrix
		int        nrowb    number of rows in b
		int        ncolb    number of columns in b
		int        itrnb    transpose flag for b
		double*    c        addition matrix
		int        iaddc    addition flag for c

	Output:
		double*    c        product matrix
		int                 function return value

*/

int MatrixMultiply( double* a,
 				    int nrowa,
				    int ncola,
				    int itrna,
				    double* b,
				    int nrowb,
				    int ncolb,
				    int itrnb,
				    double* c,
				    int iaddc )
{
	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) {
		_printf_("%s -- Matrix A and B inner vectors not equal size.\n",
				 __func__);
	    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;
}


/*

	MatrixInverse    Perform matrix inversion and linear equation solution.


	Input:
		double*    a        coefficient matrix [ndim][ndim]
		int        ndim     row and column dimension of a
		int        nrow     number of rows in a
		double*    b        right-hand vectors [ndim][nvec] (may be nULL)
		int        nvec     number of vectors in b (0 for NULL b)

	Output:
		double*    a        inverted matrix [ndim][ndim]
		double*    b        solution vectors [ndim][nvec] (may be NULL)
		double*    pdet     determinant (may be NULL)
		int                 error flag

	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 MatrixInverse( double* a,
			       int ndim,
			       int nrow,
			       double* b,
			       int nvec,
			       double* pdet )
{
	int noerr=1;
	int i,j,k,ipt,jpt,irow,icol,ipiv,ncol;
	int (*pivrc)[2],*pindx;
	double pivot,det,dtemp;

	if (!b && nvec) {
		_printf_("%s -- No right-hand side for nvec=%d.\n",
				 __func__,nvec);
		noerr=0;
		return noerr;
	}

/*  initialize local variables and arrays  */

	ncol=nrow;
	det=1.;

	pivrc = (int (*)[2]) xmalloc((nrow*2)*sizeof(int));
	pindx = (int (*)   ) xcalloc( nrow   ,sizeof(int));

/*  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) {
			xfree((void **)&pivrc);
			xfree((void **)&pindx);
			_printf_("%s -- Pivot %f less than machine epsilon.\n",
					 __func__,pivot);
			noerr=0;
			return noerr;
		}

		pivrc[i][0]=irow;
		pivrc[i][1]=icol;

		ipiv=icol;
		pindx[ipiv]++;

//		_printf_("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_("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 (pivrc[j][0] != pivrc[j][1]) {
			irow=pivrc[j][0];
			icol=pivrc[j][1];

//			_printf_("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;

	xfree((void **)&pivrc);
	xfree((void **)&pindx);

	return noerr;
}
