/* myMatrixCalc.cp */
#define INT8 char
#define UINT8 unsigned char
#define INT16 short
#define UINT16 unsigned short
#define INT32 int
#define UINT32 unsigned int
#define INT64 long long int
#define UINT64 unsigned long long int

#include "myMatrixCalc.h"
#include "matinv.h"
#include <stdlib.h>
#include <math.h>
extern double  zeroDouble;


static double dabs(double x)
{
	if (x<0)
		x = -x;
	
	return x;
	
}/* dabs */

// for a INT32, counts the number of bits after and including the leading 1 bit, but without the sign bit
static INT32 NumBits(INT32 x)
{
	static INT32		mask;
	static bool		initGood=false;
	INT32			theBits;
	
	if(!initGood)
	{
		mask = 1;
		mask = (mask<<31); // 0x80000000 (which we can't use directly since it's a negative INT32!
		initGood = true;
	}
	
	theBits = 31;
	
	x = abs(x);
	
	x = (x<<1); // gets rid of sign bit
	
	while(!(x & mask) && theBits)
	{
		x = (x<<1);
		theBits--;
	}
	
	return theBits;
	
}/* NumBits */

// z = x*y
static inline void Mul(double **z, double **x, double **y, long n)
{
	long	i, j, k;
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			z[i][j] = 0;
			for(k=0;k<n;k++)
				z[i][j] = z[i][j] + x[i][k]*y[k][j];
		}
    
    // we cannot have any extremely small but non-zero elements
    for(i=0;i<n;++i)
    {
        for(j=0;j<n;++j)
        {
            if(dabs(z[i][j])<zeroDouble)
            {
                z[i][j] = 0;
            }
        }
    }
			
}/* Mul */

// z = x
static inline void Equate(double **z, double **x, int n)
{
	long	i, j;
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			z[i][j] = x[i][j];

}/* Equate */


// z = z/k
static inline void DivD(double **z, int k, int n)
{
	long	i, j;
	
	if(k==0)
		return;
		
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			z[i][j] = z[i][j]/k;

}/* DivD */

// z = -z
static inline void Negate(double **z, int n)
{
	long	i, j;
		
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			z[i][j] = -z[i][j];

}/* Negate */

// z = x + y
static inline void Add(double **z, double **x, double **y, int n)
{
	long	i, j;
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			z[i][j] = x[i][j] +  y[i][j];
	
}/* Add */

// z = x - y
static inline void Sub(double **z, double **x, double **y, int n)
{
	long	i, j;
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			z[i][j] = x[i][j] -  y[i][j];
	
}/* Sub */


// does z==x?
static inline bool isEqual(double **z, double **x, int n)
{
	long		i, j;
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			if(z[i][j]!=x[i][j])
				return false;
		}
		
	return true;
	
}/* isEqual */

// z = x*x
void mySqr(double **z, double **x, int n)
{
	if(n<=0)
		return;
		
	if(n==1)
	{
		z[0][0] = x[0][0]*x[0][0];
		return;
	}
	
	Mul(z, x, x, n);
		
}/* mySqr */


// z = exp(x) x and z must be square of order n
void myExp(double **z, double **x, int n)
{
	long		i, j, k;
	double		**temp, **currentTerm, **zCurrent;
	double		maxEl;
	INT32		m;
	double		**xt;
	
	if(n<=0)
		return;
	
	if(n==1)
	{
		z[0][0] = exp(x[0][0]);
		return;
	}
	
	
	// find maximum element of x in absolute value
	maxEl = 0;
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			if(dabs(x[i][j])>maxEl)
				maxEl = dabs(x[i][j]);
	
	if(maxEl==0)
	{
		// z = unit matrix
		for(i=0;i<n;i++)
			for(j=0;j<n;j++)
				if(i==j)
					z[i][j] = 1.;
				else
					z[i][j] = 0;
		
		return;
	}
	
	xt = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		xt[i] = (double *)malloc(n*sizeof(double));
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			xt[i][j] = x[i][j];
	
	m = 1;
	if(maxEl<(double)0x7FFFFFFF)
	{
		m = maxEl;
		m++;
		if(m>1)
		{
			// divide matrix xt by m
			for(i=0;i<n;i++)
				for(j=0;j<n;j++)
					xt[i][j] = xt[i][j]/m;
		}
	}
	
	temp = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		temp[i] = (double *)malloc(n*sizeof(double));
	
	currentTerm = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		currentTerm[i] = (double *)malloc(n*sizeof(double));
	
	zCurrent = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		zCurrent[i] = (double *)malloc(n*sizeof(double));
	
	
	
	// make z, currentTerm, and zCurrent Unit matrices
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			if(i==j)
				z[i][j] = currentTerm[i][j] = zCurrent[i][j] = 1;
			else
				z[i][j] = currentTerm[i][j] = zCurrent[i][j] = 0;
	
	
	for(k=1; ;k++)
	{
		Mul(temp, xt, currentTerm, n);
		Equate(currentTerm, temp, n);
		DivD(currentTerm, k, n);
		
		Add(z, z, currentTerm, n);
		
		
		
		if(isEqual(z, zCurrent, n))
			break;
		
		Equate(zCurrent, z, n);
	}
	
	if(m>1)
	{
		myPower(z, zCurrent, m, n);
	}
	
	for(i=0;i<n;i++)
	{
		free(temp[i]);
		free(currentTerm[i]);
		free(zCurrent[i]);
		free(xt[i]);
	}
	
	free(temp);
	free(currentTerm);
	free(zCurrent);
	free(xt);
	
}/* myExp */
// z = sin(x) x and z must be square of order n
void mySin(double **z, double **x, INT32 n)
{
	long		i, k;
	double		**temp, **currentTerm, **zCurrent, **xSqr;
	
	if(n<=0)
		return;
		
	if(n==1)
	{
		z[0][0] = sin(x[0][0]);
		return;
	}
	
	temp = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		temp[i] = (double *)malloc(n*sizeof(double));
		
	currentTerm = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		currentTerm[i] = (double *)malloc(n*sizeof(double));
		
	zCurrent = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		zCurrent[i] = (double *)malloc(n*sizeof(double));
		
	xSqr = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		xSqr[i] = (double *)malloc(n*sizeof(double));
		
	//zRe = z.Re = x.Re = currentTerm.Re = xRe;
	//zIm = z.Im = x.Im = currentTerm.Im = xIm;
	
	// Equate z, currentTerm, zCurrent, and temp to x
	Equate(z, x, n);
	Equate(currentTerm, x, n);
	Equate(zCurrent, x, n);
	
	Mul(xSqr, x, x, n);
	
	for(k=2; ;k+=2)
	{
		Mul(temp, xSqr, currentTerm, n);
		Equate(currentTerm, temp, n);
		DivD(currentTerm, -k*(k+1), n);
		
		Add(z, z, currentTerm, n);
		
		if(isEqual(z, zCurrent, n))
			break;
		
		Equate(zCurrent, z, n);
	}
	
	for(i=0;i<n;i++)
	{
		free(temp[i]);
		free(currentTerm[i]);
		free(zCurrent[i]);
		free(xSqr[i]);
	}
	
	free(temp);
	free(currentTerm);
	free(zCurrent);
	free(xSqr);
	
}/* mySin */


// z = cos(x) x and z must be square of order n
void myCos(double **z, double **x, INT32 n)
{
	long		i, j, k;
	double		**temp, **currentTerm, **zCurrent, **xSqr;
	
	if(n<=0)
		return;
		
	if(n==1)
	{
		z[0][0] = cos(x[0][0]);
		return;
	}
	
	temp = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		temp[i] = (double *)malloc(n*sizeof(double));
		
	currentTerm = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		currentTerm[i] = (double *)malloc(n*sizeof(double));
		
	zCurrent = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		zCurrent[i] = (double *)malloc(n*sizeof(double));
		
	xSqr = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		xSqr[i] = (double *)malloc(n*sizeof(double));
		
	// make z, currentTerm, and zCurrent Unit matrices
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			if(i==j)
				z[i][j] = currentTerm[i][j] = zCurrent[i][j] = 1;
			else
				z[i][j] = currentTerm[i][j] = zCurrent[i][j] = 0;
	
	Mul(xSqr, x, x, n);
	
	for(k=1; ;k+=2)
	{
		Mul(temp, xSqr, currentTerm, n);
		Equate(currentTerm, temp, n);
		DivD(currentTerm, -k*(k+1), n);
		
		Add(z, z, currentTerm, n);
		
		if(isEqual(z, zCurrent, n))
			break;
		
		Equate(zCurrent, z, n);
	}
	
	for(i=0;i<n;i++)
	{
		free(temp[i]);
		free(currentTerm[i]);
		free(zCurrent[i]);
		free(xSqr[i]);
	}
	
	free(temp);
	free(currentTerm);
	free(zCurrent);
	free(xSqr);
	
}/* myCos */


// z = tan(x) x and z must be square of order n
// note from power series that [sin(x), cos(x)] = 0
// so tan(x) = Inverse(cos(x))*sin(x) = sin(x)*Inverse(cos(x))
void myTan(double **z, double **x, INT32 n)
{
	double		**theSin, **theCos;
	double		determinant;
	long		i;
	
	if(n<=0)
		return;
		
	if(n==1)
	{
		z[0][0] = tan(x[0][0]);
		return;
	}
	
	theSin = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		theSin[i] = (double *)malloc(n*sizeof(double));
		
	theCos = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		theCos[i] = (double *)malloc(n*sizeof(double));
		
	mySin(theSin, x, n);
	myCos(theCos, x, n);
	
	determinant = matinv(theCos, n); //theCos is now Inverted
	
	if(determinant!=0)
		Mul(z, theCos, theSin, n);
	
	for(i=0;i<n;i++)
	{
		free(theSin[i]);
		free(theCos[i]);
	}
	
	free(theSin);
	free(theCos);
	
}/* myTan */


void mySinh(double **z, double **x, INT32 n)
{
	double		**temp;
	long		i;
	
	if(n<=0)
		return;
		
	if(n==1)
	{
		z[0][0] = sinh(x[0][0]);
		return;
	}
	
	temp = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		temp[i] = (double *)malloc(n*sizeof(double));
		
	Equate(temp, x, n); // temp = x
	Negate(temp, n); // temp = -x
	myExp(z, temp, n); // z = exp(-x)
	Equate(temp, z, n); // temp = exp(-x)
	
	myExp(z, x, n); // z = exp(x)
	Sub(z, z, temp, n); // z = exp(x) - exp(-x)
	DivD(z, 2, n); // z = sinh(x)
		
	for(i=0;i<n;i++)
		free(temp[i]);
	
	free(temp);
	
}/* mySinh */


void myCosh(double **z, double **x, INT32 n)
{
	double		**temp;
	long		i;
	
	if(n<=0)
		return;
		
	if(n==1)
	{
		z[0][0] = cosh(x[0][0]);
		return;
	}
	
	temp = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		temp[i] = (double *)malloc(n*sizeof(double));
		
	Equate(temp, x, n); // temp = x
	Negate(temp, n); // temp = -x
	myExp(z, temp, n); // z = exp(-x)
	Equate(temp, z, n); // temp = exp(-x)
	
	myExp(z, x, n); // z = exp(x)
	Add(z, z, temp, n); // z = exp(x) + exp(-x)
	DivD(z, 2, n); // z = sinh(x)
		
	for(i=0;i<n;i++)
		free(temp[i]);
	
	free(temp);
	
}/* myCosh */


// z = tanh(x) x and z must be square of order n
// note from power series that [sinh(x), cosh(x)] = 0
// so tanh(x) = Inverse(cosh(x))*sinh(x) = sinh(x)*Inverse(cosh(x))
void myTanh(double **z, double **x, INT32 n)
{
	double		**theSinh, **theCosh;
	double		determinant;
	long		i;
	
	if(n<=0)
		return;
		
	if(n==1)
	{
		z[0][0] = tanh(x[0][0]);
		return;
	}
	
	theSinh = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		theSinh[i] = (double *)malloc(n*sizeof(double));
		
	theCosh = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		theCosh[i] = (double *)malloc(n*sizeof(double));
		
	mySinh(theSinh, x, n);
	myCosh(theCosh, x, n);
	
	determinant = matinv(theCosh, n); //theCosh is now Inverted
	
	if(determinant!=0)
		Mul(z, theCosh, theSinh, n);
	
	for(i=0;i<n;i++)
	{
		free(theSinh[i]);
		free(theCosh[i]);
	}
	
	free(theSinh);
	free(theCosh);
	
}/* myTanh */


// z = x*y - y*x
void myCommutator(double **z, double **x, double **y, int n)
{
	double		**temp;
	long		i;
	
	temp = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		temp[i] = (double *)malloc(n*sizeof(double));
		
	Mul(temp, y, x, n);  // temp = y*x
	Mul(z, x, y, n);  // z = x*y
	Sub(z, z, temp, n);  // z = x*y - y*x
		
	for(i=0;i<n;i++)
		free(temp[i]);
		
	free(temp);
	
}/* myCommutator */

// z=x^m, m an integer
// z<>x, plugin code makes z<>x
double myPower(double **z, double **x, int m, int n)
{
	long	i, j, ii, jj;
    int     nb;
	static unsigned INT32	mask; // 0x80000000
	bool	isNegative;
	double	det, **zt;
	static bool	initGood=false;
	
	if(!initGood)
	{
		mask = 1;
		mask = (mask<<31); // 0x80000000
		initGood = true;
	}
	
	det = 1.;
	
	if (m==0)  // z = unit matrix
	{
		for(i=0;i<n;i++)
			for(j=0;j<n;j++)
				if(i==j)
					z[i][j] = 1.;
				else
					z[i][j] = 0;
		
		return det;
	}
	
	if (m==1)  // z = x
	{
		for(i=0;i<n;i++)
			for(j=0;j<n;j++)
				z[i][j] = x[i][j];
		
		return det;
	}
	
	if (m==-1)  // z = 1/x
	{
		for(i=0;i<n;i++)
			for(j=0;j<n;j++)
				z[i][j] = x[i][j];
		
		det = matinv(z, n);
		
		return det;
	}
	
	if (m<0)
	{
		isNegative = true;
		m = -m;
	}
	else
	{
		isNegative = false;
	}
	
	// initiate zt
	zt = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		zt[i] = (double *)malloc(n*sizeof(double));
	
	// now m>0, make z a unit matrix
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			if(i==j)
				z[i][j] = 1.;
			else
				z[i][j] = 0;
	
	nb = NumBits(m);
	
	m = (m<<(32-nb)); // go to leading 1 bit
	
	for(i=0;i<nb;i++)
	{
		Mul(zt, z, z, n);
		for(ii=0;ii<n;ii++)
			for(jj=0;jj<n;jj++)
				z[ii][jj] = zt[ii][jj];
		if(m & mask)
		{
			Mul(zt, z, x, n);
			for(ii=0;ii<n;ii++)
				for(jj=0;jj<n;jj++)
					z[ii][jj] = zt[ii][jj];
		}
		
		m = (m<<1);
		
	}
	
	for(i=0;i<n;i++)
		free(zt[i]);
	
	free(zt);
	
	if(isNegative)
		det = matinv(z, n);
	
	return det;
	
}/* myPower */
