/* diagh.cp */

/*
	written by:
	Robert M. Delaney
	Emeritus Professor of Physics
	Saint Louis University
	September, 2003
*/
extern  double zeroDouble;
static double dabs(double x)
{
    if(x<0)
        x = -x;
    
    return x;
    
}/* dabs */

/*
	diagonalizes a Hermitian matrix using the Jacobi method. Only the upper triangular
	part, including the diagonal, needs to be inputted.

	M is the matrix to be diagonalized. the matrix EigenM is returned with its columns the
	eigenvectors. n is the size of the matrices, which should have memory dynamically allocated by
	the calling program. eps is a measure of the acceptable largest off-diagonal element
	in absolute value.

	Basic method:
	For a Hermitian sub-matrix of dimension 2
	A = x  z
		z* y
		
	where x an y are M[i][i] and M[j][j] (i<j)
	z is M[i][j] and z* is M[j][i]
	where z is largest off-diagonal element in absolute value
	
	U A U* , where here * means transpose-complex conjugate of the matrix, diagonalizes A where:
	
	z = |z|e^(i*gamma)
	lambda = |z|
	mu = (x-y)/2
	f = sign(mu)*lambda/sqrt(lambda^2 + mu^2)  (if mu=0 then sign(mu)=1)
	s = f/sqrt(2*(1+sqrt(1-f^2)))
	c = sqrt(1-s^2)
	
	Then with
	U = c                 s*e^(i*gamma)
		-s*e^(-i*gamma)   c
		
	U* = c                -s*e^(i*gamma)
		 s*e^(-i*gamma)   c
		 
	A is diagonalized with new diagonal elements:
	x*c^2 + y*s^2 + 2*lambda*s*c
	x*s^2 + y*c^2 - 2*lambda*s*c
	
	Apply an expanded U (with suitably inserted 1's and 0's) by M = U M U* to generate a new M
	
	We start with EigenM matrix a unit matrix, and multiply it from the right by expanded U*
	At the end the columns of the EigenM matrix are the eigenvectors
	
	repeat until absolute value of largest off-diagonal element is less that some given eps


	myTypedef.h contains:
	
	typedef struct
	{
		double				Re;
		double				Im;
		
	} Complex;
	
*/


#include "myTypedef.h"

#include <math.h>
#include "diagh.h"



static inline double absC(Complex z)
{
	return sqrt(z.Re*z.Re + z.Im*z.Im);
	
}/* absC */

static inline void Equate(Complex& z, const Complex& x)
{
	z.Re = x.Re;
	z.Im = x.Im;
	
}/* EquateC_C */

static inline void Add(Complex& z, const Complex& x, const Complex& y)
{
	z.Re = x.Re + y.Re;
	z.Im = x.Im + y.Im;
	
}/* Add */


static inline void Sub(Complex& z, const Complex& x, const Complex& y)
{
	z.Re = x.Re - y.Re;
	z.Im = x.Im - y.Im;
	
}/* Sub */

static inline void Mul(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;
	}
	
}/* Mul */

void diagh(Complex **M, Complex **EigenM, long n, double eps)
{
	long		i, j, is, js;
	double		f, lambda, mu, gamma, sig, norm, maxValue, maxValue1, temp;
	double		s, c, x, y, s2, c2;
	Complex		w, wStar, M1, M2, temp1, temp2;
	
	if(n==1 || eps<=0)
		return;
		
	// make EigenM a unit matrix
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			EigenM[i][j].Im = 0;
			if(i==j)
				EigenM[i][j].Re = 1;
			else
				EigenM[i][j].Re = 0;
		}
		
	// fill in lower triangular part of M and make sure diagonal is real
	for(i=0;i<n-1;i++)
	{
		M[i][i].Im = 0;
		for(j=i+1;j<n;j++)
		{
			M[j][i].Re = M[i][j].Re;
			M[j][i].Im = -M[i][j].Im;
		}
	}
		
	// find norm value as the largest off-diagonal element in absolute value
	norm = 0;
	for(i=0;i<n-1;i++)
		for(j=i+1;j<n;j++)
			if(absC(M[i][j])>norm)
				norm = absC(M[i][j]);
		
	if(norm==0) // M is already diagonal
		return;
	
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			M[i][j].Re = M[i][j].Re/norm;
			M[i][j].Im = M[i][j].Im/norm;
		}
			
	do
	{
		maxValue = absC(M[0][1]);
		is = 0;
		js = 1;
		
		// find the largest off-diagonal element in absolute value
		for(i=0;i<n-1;i++)
			for(j=i+1;j<n;j++)
			{
				maxValue1 = absC(M[i][j]);
				if(maxValue1>maxValue)
				{
					is = i;
					js = j;
					maxValue = maxValue1;
				}
			}
			
		if(maxValue>=eps)
		{
			lambda = absC(M[is][js]);
			gamma = atan2(M[is][js].Im, M[is][js].Re);
			
			mu = (M[is][is].Re - M[js][js].Re)/2;
			
			if(mu>=0)
				sig = 1;
			else
				sig = -1;
				
			f = sig*lambda/sqrt(lambda*lambda + mu*mu);
			
			s = f/sqrt(2*(1+sqrt(1-f*f)));
			c = sqrt(1-s*s);
			
			// w = e^(i*gamma) * sin(theta)
			wStar.Re = w.Re = s*cos(gamma);
			w.Im = s*sin(gamma);
			wStar.Im = - w.Im;
			
			
			x = M[is][is].Re;
			y = M[js][js].Re;
			
			temp = 2*lambda*s*c;
			s2 = s*s;
			c2 = c*c;
			M[is][is].Re = x*c2 + y*s2 + temp;
			M[js][js].Re = x*s2 + y*c2 - temp;
			M[is][js].Re = M[js][is].Re = M[is][js].Im = M[js][is].Im = 0;
			
			for(i=0;i<n;i++)
			{
				if(i!=is && i!=js)
				{
					Equate(M1, M[i][is]);
					Equate(M2, M[i][js]);
					
					Equate(temp1, M1);
					temp1.Re = temp1.Re*c;
					temp1.Im = temp1.Im*c;
					Mul(temp2, M2, wStar);
					Add(M[i][is], temp1, temp2);
					
					Equate(temp1, M2);
					temp1.Re = temp1.Re*c;
					temp1.Im = temp1.Im*c;
					Mul(temp2, M1, w);
					Sub(M[i][js], temp1, temp2);
					
					M[is][i].Re = M[i][is].Re;
					M[is][i].Im = -M[i][is].Im;
					
					M[js][i].Re = M[i][js].Re;
					M[js][i].Im = -M[i][js].Im;
				}
				
				Equate(M1, EigenM[i][is]);
				Equate(M2, EigenM[i][js]);
				
				Equate(temp1, M1);
				temp1.Re = temp1.Re*c;
				temp1.Im = temp1.Im*c;
				Mul(temp2, M2, wStar);
				Add(EigenM[i][is], temp1, temp2);
				
				Equate(temp1, M2);
				temp1.Re = temp1.Re*c;
				temp1.Im = temp1.Im*c;
				Mul(temp2, M1, w);
				Sub(EigenM[i][js], temp1, temp2);
			}
		}
	}
	while (maxValue >= eps);
		
	for(i=0;i<n;i++)
		M[i][i].Re = norm*M[i][i].Re;
    
    for(i=0;i<n;++i)
        for(j=0;j<n;++j)
        {
            if(dabs(M[i][j].Re)<zeroDouble)
                M[i][j].Re = 0;
            
            if(dabs(M[i][j].Im)<zeroDouble)
                M[i][j].Im = 0;
            
            if(dabs(EigenM[i][j].Re)<zeroDouble)
                EigenM[i][j].Re = 0;
            
            if(dabs(EigenM[i][j].Im)<zeroDouble)
                EigenM[i][j].Im = 0;
        }
	
}/* diagh */