/*	cmatinv.cp */

/*
	a procedure to invert a matrix
	translated from FORTRAN code by
	Robert M. Delaney
	5/2/91
*/

/*
	array indices run from 0 to n-1 where n is the order of the square matrix
*/

/*
	we need to dynamically declare the matrix array and pass it to this procedure as per
	
	array = (double **)malloc(n*sizeof(double *));
	for (i=0;i<n;i++)
		array[i] = (double *)malloc(long)(n*sizeof(double));
*/

#include <stdlib.h>
#include <math.h>
#include "myTypedef.h"
#include "cmatinv.h"

static double dabs(Complex x)
{
	
	return sqrt(x.Re*x.Re + x.Im*x.Im);
	
} /* dabs */

static inline void complexMultiply(Complex& z, const Complex& x, const Complex& y)
{
	double			ac, bd;
	
	if(x.Re!=0 && x.Im!=0 && y.Re!=0 && y.Im!=0)
	{
		ac = x.Re*y.Re;
		bd = x.Im*y.Im;
		
		// the order here is important since z might be the same as x or y
		z.Im = (x.Re + x.Im)*(y.Re + y.Im) - ac - bd;
		z.Re = ac - bd;
		return;
	}
	
	if(x.Re==0)
	{
		ac = -x.Im*y.Im;
		z.Im = x.Im*y.Re;
		z.Re = ac;
		return;
	}
	
	if(x.Im==0)
	{
		ac = x.Re*y.Re;
		z.Im = x.Re*y.Im;
		z.Re = ac;
		return;
	}
	
	if(y.Re==0)
	{
		ac = -x.Im*y.Im;
		z.Im = x.Re*y.Im;
		z.Re = ac;
		return;
	}
	
	if(y.Im==0)
	{
		ac = x.Re*y.Re;
		z.Im = x.Im*y.Re;
		z.Re = ac;
		return;
	}
	
}/* complexMultiply */


static inline void complexDivide(Complex& z, const Complex& x, const Complex& y)
{
	double			ac, bd, den;
	
	// Re[(a + ib)/(c + id)] = (ac + bd)/(c*c + d*d)
	// Im[(a + ib)/(c + id)] = (bc - ad)/(c*c + d*d) = [(a + b)(c - d) - ac + bd]/(c*c + d*d)
	
	// a = x.Re, b = x.Im, c = y.Re, d = y.Im
	
	if(y.Re==0 && y.Im==0)
	{
		z.Re = NAN;
		z.Im = NAN;
		return;
	}
	
	if(x.Im==0 && y.Im==0)
	{
		z.Re = x.Re/y.Re;
		z.Im = 0;
		return;
	}
	
	if(x.Re==0 && y.Re==0)
	{
		z.Re = x.Im/y.Im;
		z.Im = 0;
		return;
	}
	
	if(x.Re==0 && y.Im==0)
	{
		z.Re = 0;
		z.Im = x.Im/y.Re;
		return;
	}
	
	if(x.Im==0 && y.Re==0)
	{
		z.Re = 0;
		z.Im = -x.Re/y.Im;
		return;
	}
	
	den = y.Re*y.Re + y.Im*y.Im;
	
	ac = x.Re*y.Re;
	bd = x.Im*y.Im;
	
	// the order here is important since z might be the same as x or y
	z.Im = ((x.Re + x.Im)*(y.Re - y.Im) - ac + bd)/den;
	z.Re = (ac + bd)/den;
	
}/* complexDivide */


//struct Complex cmatinv(Complex **array, long n)
Complex cmatinv(Complex **array, long n)
{
	long			i, j, k, L, *ik, *jk;
	Complex		amax, save, zero, one, determinant, temp;
	
	zero.Re = 0;
	zero.Im = 0;
	one.Re = 1;
	one.Im = 0;
	
	ik = (long *)malloc(n*sizeof(long));
	jk = (long *)malloc(n*sizeof(long));
	
	determinant = one;
	
	for(k=0; k<n; k++)
	{
		/* find largest element in abs value in rest of matrix */
		amax = zero;
		
		for(i=k; i<n; i++)
		{
			for(j=k; j<n; j++)
			{
				if(dabs(array[i][j]) > dabs(amax))
				{
					amax = array[i][j];
					ik[k] = i;
					jk[k] = j;
				}
			}
		}
		
		if(dabs(amax) == 0)
		{
			determinant = zero;
			return determinant;
		}
		
		/* interchange rows and columns to put amax in array(k,k) */
		i = ik[k];
		if(i>k)
		{
			for(j=0; j<n; j++)
			{
				save = array[k][j];
				array[k][j] = array[i][j];
				array[i][j].Re = -save.Re;
				array[i][j].Im = -save.Im;
			}
		}
		
		j = jk[k];
		if(j>k)
		{
			for(i=0; i<n; i++)
			{
				save = array[i][k];
				array[i][k] = array[i][j];
				array[i][j].Re = -save.Re;
				array[i][j].Im = -save.Im;
			}
		}
		
		/* accumulate elements of inverse matrix */
		for(i=0; i<n; i++)
		{
			if(i!=k)
			{
				complexDivide(temp, array[i][k], amax);
				array[i][k].Re = -temp.Re;
				array[i][k].Im = -temp.Im;
			}
		}
		
		for(i=0; i<n; i++)
		{
			for(j=0; j<n; j++)
			{
				if(i!=k && j!=k)
				{
					complexMultiply(temp, array[i][k], array[k][j]);
					array[i][j].Re = array[i][j].Re + temp.Re;
					array[i][j].Im = array[i][j].Im + temp.Im;
				}
			}
		}
		
		for(j=0; j<n; j++)
		{
			if(j!=k)
			{
				complexDivide(temp, array[k][j], amax);
				array[k][j].Re = temp.Re;
				array[k][j].Im = temp.Im;
			}
		}
		
		complexDivide(array[k][k], one, amax);
		complexMultiply(temp, determinant, amax);
		determinant = temp;
	}
		
	/* restore ordering of the matrix */
	for(L=0; L<n; L++)
	{
		k = n - L - 1;
		j = ik[k];
		
		if(j>k)
		{
			for(i=0; i<n; i++)
			{
				save = array[i][k];
				array[i][k].Re = -array[i][j].Re;
				array[i][k].Im = -array[i][j].Im;
				array[i][j] = save;
			}
		}
		
		i = jk[k];
		
		if(i>k)
		{
			for(j=0; j<n; j++)
			{
				save = array[k][j];
				array[k][j].Re = -array[i][j].Re;
				array[k][j].Im = -array[i][j].Im;
				array[i][j] = save;
			}
		}
	}
	
	free(ik);
	free(jk);
	
	return determinant;
	
} /* cmatinv */


