/* HandleArrays.cp */

#include "myTypedef.h"
#include "HandleArrays.h"
#include "Complex.h"
#include "LaguerreOld.h"
#include "charPolyOld.h"
#include "ProjectionM.h"
#include <math.h>
#include <stdlib.h>
#include "ExtComplex.h"
#include "fpConv.h"
#include "fpMath.h"
#include "myFindRoots.h"

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


static void InverseC(Complex& z, Complex& x)
{
	double		den;
	
	if(x.Re==0 && x.Im==0)
		return;
	
	den = sqrt(x.Re*x.Re + x.Im*x.Im);
	z.Re = x.Re/den;
	z.Im = -x.Im/den;

}/* InverseC */


// Give this filled matrix M and dimension n. This will reserve memory for the rest
// of the arrays and fill some of them.
bool FillArrays(int n, Complex **M, bool& isSingular, ComplexPtr& root, longPtr& multiplicity,
				int& k, ComplexPtr3& P, ComplexPtr3& Omega, ComplexPtr2& R, ComplexPtr2& tempM,
				ComplexPtr2& tempM1, ComplexPtr2& sum, ComplexPtr2& current)
{
	ExtComplex			*c, *rootE;
	Complex				*root1, saveRoot;
	int				*badRoot, badSaveRoot;
	static ExtComplex	x;
	fp					dxAbs;
	int				i, j, m, ii, jj;
	Complex				*p;
	Polynomial			*projP;
	Complex				tempC;
	static bool			arrayGood=false;
	
	if(!arrayGood)
	{
		init(x);
		init(dxAbs);
		arrayGood = true;
	}
	
	// reserve memory for passed arrays, except for M
	c = (ExtComplex *)malloc((n+1)*sizeof(ExtComplex));
	for(i=0;i<=n;i++)
		init(c[i]);
	
	rootE = (ExtComplex *)malloc(n*sizeof(ExtComplex));
	for(i=0;i<n;i++)
		init(rootE[i]);
	
	root = (Complex *)malloc(n*sizeof(Complex));
	
	multiplicity = (INT32 *)malloc(n*sizeof(INT32));
	
	P = (Complex***)malloc(n*sizeof(Complex**)); // projection matrices, use n not k
	for(i=0;i<n;i++)
	{
		P[i] = (Complex**)malloc(n*sizeof(Complex*));
		for(j=0;j<n;j++)
			P[i][j] = (Complex*)malloc(n*sizeof(Complex));
	}
	
	Omega = (Complex***)malloc(n*sizeof(Complex**));
	for(i=0;i<n;i++)
	{
		Omega[i] = (Complex**)malloc(n*sizeof(Complex*));
		for(j=0;j<n;j++)
			Omega[i][j] = (Complex*)malloc(n*sizeof(Complex));
	}
	
	R = (Complex**)malloc(n*sizeof(Complex*));
	for(i=0;i<n;i++)
		R[i] = (Complex*)malloc(n*sizeof(Complex));
		
	tempM = (Complex**)malloc(n*sizeof(Complex*));
	for(i=0;i<n;i++)
		tempM[i] = (Complex*)malloc(n*sizeof(Complex));
		
	tempM1 = (Complex**)malloc(n*sizeof(Complex*));
	for(i=0;i<n;i++)
		tempM1[i] = (Complex*)malloc(n*sizeof(Complex));
		
	sum = (Complex**)malloc(n*sizeof(Complex*));
	for(i=0;i<n;i++)
		sum[i] = (Complex*)malloc(n*sizeof(Complex));
		
		
	current = (Complex**)malloc(n*sizeof(Complex*));
	for(i=0;i<n;i++)
		current[i] = (Complex*)malloc(n*sizeof(Complex));

		
	// reserve memory for this procedure
	//c = (ExtComplex *)malloc((n+1)*sizeof(ExtComplex));
	root1 = (Complex *)malloc(n*sizeof(Complex));
	badRoot = (INT32 *)malloc(n*sizeof(INT32));
	p = (Complex*)malloc((n+1)*sizeof(Complex));
	projP = (Polynomial*)malloc(n*sizeof(Polynomial));
	for(i=0;i<n;i++)
		CreateP(projP[i], n-1);
	
	// initialization
	for(i=0;i<n;i++)
	{
		multiplicity[i] = 0;
		assignZeroC(root[i]);
		assignZeroC(root1[i]);
		badRoot[i] = 0;
	}
	
	// find coefficients of characteristic polynomial of M
	CharPolyOld(M, p, n);
	
	if(isEqualZero(p[0]))
		isSingular = true;
	else
		isSingular = false;
	
	// convert to ExtComplex
	for(i=0;i<=n;i++)
	{
		equate(c[i].Re, p[i].Re);
		equate(c[i].Im, p[i].Im);
	}
	
	// get the roots
	if(!myFindRoots(rootE, c, n))
		return false;
	
	// convert to Complex
	
	for(i=0;i<n;i++)
	{
		equate(root1[i].Re, rootE[i].Re);
		equate(root1[i].Im, rootE[i].Im);
	}
	
	
	// following doesn't work for a zero root
	for(i=0;i<n;i++)
	{
		if(dabs(root1[i].Re)<1.e-15*dabs(root1[i].Im))
			root1[i].Re = 0;
		if(dabs(root1[i].Im)<1.e-15*dabs(root1[i].Re))
			root1[i].Im = 0;
	}
	
	// Let's find the multiplicity of the distinct roots, which we put into root
	// from root1
	k = 0; // initialize index of root, k will be number of distinct roots
	for(i=0;i<n;i++)
	{
		badSaveRoot = 1;
		if(badRoot[i]==0)
		{
			badSaveRoot = 0;
			EquateC_C(saveRoot, root1[i]);
			m = 1;
			for(j=i+1;j<n;j++)
			{
				if(isEqual(saveRoot, root1[j]))
				{
					m++;
					badRoot[j] = 1;
				}
			}
		}
		
		if(badSaveRoot==0)
		{
			EquateC_C(root[k], saveRoot);
			multiplicity[k] = m;
			k++;
		}
	}
	
	// coming out, k is the number of distinct roots
	
	// fill the k projection polynomials
	if(!projectionPoly(projP, root, multiplicity, k, n))
		return false;
		
	// create the k projection matrices
	projMatrices(P, M, projP, k, n);
	
	// create the k Omega matrices
	for(i=0;i<k;i++)
	{
		InverseC(tempC, root[i]); // a zero root will give tempC=nan, we can't use that Omega[i]
		for(ii=0;ii<n;ii++)
			for(jj=0;jj<n;jj++)
			{
				Mul(Omega[i][ii][jj], tempC, M[ii][jj]);
				if(ii==jj)
					Omega[i][ii][jj].Re = Omega[i][ii][jj].Re - 1;
			}
	}
	
	// free memory for this procedure
	free(root1);
	free(badRoot);
	free(p);
	
	for(i=0;i<n;i++)
		FreeP(projP[i]);
	free(projP);
	
	for(i=0;i<=n;i++)
	{
		if(c[i].Re.i.n && c[i].Re.i.b)
			free(c[i].Re.i.b);
		if(c[i].Im.i.n && c[i].Im.i.b)
			free(c[i].Im.i.b);
	}
	
	free(c);
	
	for(i=0;i<n;i++)
	{
		if(rootE[i].Re.i.n && rootE[i].Re.i.b)
			free(rootE[i].Re.i.b);
		if(rootE[i].Im.i.n && rootE[i].Im.i.b)
			free(rootE[i].Im.i.b);
	}
	
	free(rootE);
	
	return true;
	
}/* FillArrays */


// kill passed arrays except for M
void killArrays(int n, Complex *root, INT32 *multiplicity, Complex ***P, Complex ***Omega,
				Complex **R, Complex **tempM, Complex **tempM1, Complex **sum,
				Complex **current)
{
	long		i, j;
	
	free(root);
	free(multiplicity);
	
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
			free(P[i][j]);
		free(P[i]);
	}
	free(P);
	
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
			free(Omega[i][j]);
		free(Omega[i]);
	}
	free(Omega);
	
	for(i=0;i<n;i++)
		free(R[i]);
	free(R);
	
	for(i=0;i<n;i++)
		free(tempM[i]);
	free(tempM);
	
	for(i=0;i<n;i++)
		free(tempM1[i]);
	free(tempM1);
	
	for(i=0;i<n;i++)
		free(sum[i]);
	free(sum);
	
	for(i=0;i<n;i++)
		free(current[i]);
	free(current);
	
}/* killArrays */
