/*  pdnmesh - a 2D finite element solver
    Copyright (C) 2001-2005 Sarod Yatawatta <sarod@users.sf.net>  
  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  $Id: eig.c,v 1.10 2005/02/11 18:11:19 sarod Exp $
*/



/* aigenvalue routines for symmetric matrices, storage is reduced */
#include <stdio.h>
#include <math.h> /* for sqrt */
#include <stdlib.h> /* for calloc */
#include <string.h> /* for memcpy */
#include "types.h"


MY_DOUBLE *eig_array; /* array to store solved eigenvalues */


/* macros to access elements in matrices */
/* all these macros assume (i,j) within [0,m-1] */
/* Upper Hessenberg form
 *  is represented by shifting all rows to the left so no zeros
 *  are present */
/* get the (i,j) element from upper Hessenberg A- m by m */
#define eH(A,i,j,m) \
  ((((i)==0)||((i)==1))?A[(i)][(j)]:\
	   ((j)<(i)-1?0:A[(i)][(j)-(i)+1]))
/* tridiagonal form
 * is represented by shifting all rows to the left until no zero appears */
 /* maximum row length is 3 */
#define eT(A,i,j,m) \
  ((((j)>(i)+1)||((j)<(i)-1))?0:\
	  (((i)==0)?A[(i)][(j)]:A[(i)][(j)-(i)+1]))
/* upper tridiagonal form */
/* is represented by shifting all row to left until no zero appears */
/* maximum row length is 3 */
#define eUT(A,i,j,m) \
 ((((j)<(i))||((j)>(i)+2))?0:\
	 ((i)==0?A[(i)][(j)]:A[(i)][(j)-(i)+1]))


static void 
convert_to_hessenberg(MY_DOUBLE **A, MY_DOUBLE **B, int m)
{
/* convert matrix A (m by m), symmetric  to 
 * tridiagonal form B.
 * structure of A - m by m square
 * structure if B - tri diagonal
 * both matrices modified
 */
int i,j,k; 

MY_DOUBLE *v,*u; /* vectors */
MY_DOUBLE norm;


v =(MY_DOUBLE*)calloc(m-1,sizeof(MY_DOUBLE));
if ( v == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
u =(MY_DOUBLE*)calloc(m,sizeof(MY_DOUBLE));
if ( u == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }


/* main loop */
for (k=0; k < m; k++ ) {
 /* copy vector */
 norm =0;
 for ( i=0; i<m-k-1; i++) {
   v[i] = A[i+k+1][k];
   /* also find the norm */
   norm += v[i]*v[i];
 }

 /* store the first element of v temporarily */
 u[0] = v[0];
 /* form the reflector */
 if ( v[0] > 0 )
    v[0] += sqrt(norm);
 else
    v[0] -= sqrt(norm);

 /* find the new norm */
 norm -= u[0]*u[0];
 norm += v[0]*v[0];
 norm = sqrt(norm);
 /* make vector v unit norm */
 for (i=0; i<m-k-1; i++)
   v[i] /= norm;

 /* now form the product v'*A[k+1:m-1][k:m-1] */
 /* and store in u */
 for (i=0; i<m-k; i++ ) {
   u[i] = 0;
   for ( j=0; j <m-k-1; j++)
	u[i] += v[j]*A[k+1+j][k+i];
 }

 /* now update A */
 /* A = A -2v*u' */
 /* note: v = m-k-1 by 1 */
 /* u = m-k by 1 */
 for (i=0; i < m-k-1; i++)
   for (j=0; j<m-k; j++ )
	 A[k+1+i][k+j] -= 2*v[i]*u[j];

 /* form the product A[0:m-1][k+1:m-1]*v */
 /* size m by 1 */
 for (i=0; i<m; i++) {
    u[i] = 0;
    for (j=0; j<m-k-1;j++)
      u[i] += A[i][j+k+1]*v[j];
 }

 /* update A */
 /* A = A - 2*u*v'*/
 /* v - m-k-1 by 1 */
 /* u - m by 1 */
 for (i=0; i<m; i++)
   for (j=0; j<m-k-1; j++)
	A[i][j+k+1] -= 2*u[i]*v[j];
}

/* copy the result to tri diagonal B */
B[0][0]=A[0][0]; B[0][1]=A[0][1];
for (i=1;i<m-1;i++) {
			B[i][0]=A[i][i-1];
			B[i][1]=A[i][i];
			B[i][2]=A[i][i+1];
}
B[m-1][0]=A[m-1][m-2]; B[m-1][1]=A[m-1][m-1];

free(u);
free(v);
}

static int
matrix_mult(MY_DOUBLE **A, MY_DOUBLE **B,MY_DOUBLE **C,int m)
{
	/* multiplies the two matrices */
	/* C = A*B */
	/* A - upper tri diagonal - main, super, super-super diagonals
	 * B - upper Hessenberg
	 * C - Result is tridiagonal, will be copied back to A 
	 */
	int i,j,k;
  
	/* first find rows 1 to m-3, where we have 3 elements in rows of A */
  for (i=1; i<m-2; i++) {
							for (j=i-1;j<i+2;j++) {
								C[i][j-i+1]=0;
								for (k=i;k<=i+2;k++) /* only three terms */
								  C[i][j-i+1] +=eUT(A,i,k,m)*eH(B,k,j,m);
       }
  }
	/* now the first row, only two elements in row of C */
	i=0;
	for (j=i;j<i+2;j++) {
					C[i][j-i]=0;
					for (k=i;k<=i+2;k++) /* only three terms */
					  C[i][j-i] +=eUT(A,i,k,m)*eH(B,k,j,m);
  }
 /* now the m-2 row, only 2 elements in row of A */
	i=m-2;
	for (j=i-1;j<i+2;j++) {
								C[i][j-i+1]=0;
								for (k=i;k<=i+1;k++) /* only two terms */
								  C[i][j-i+1] +=eUT(A,i,k,m)*eH(B,k,j,m);
  }
 /* now the m-1 row, only 1 element in row of A, 2 elements in row of C */
  i=m-1;
	for (j=i-1;j<i+1;j++) {
								/* only one term */
								  C[i][j-i+1] =eUT(A,i,i,m)*eH(B,i,j,m);
  }
	/* copy C back to A */
	A[0][0]=C[0][0]; A[0][1]=C[0][1];
	for (i=1;i<m-1;i++) {
			A[i][0]=C[i][0];
			A[i][1]=C[i][1];
			A[i][2]=C[i][2];
	}
	i=m-1;
  A[i][0]=C[i][0];A[i][1]=C[i][1];
	return(0);
}

static int
qr_decomp(MY_DOUBLE **A, MY_DOUBLE **Q, int m)
{
  /* perform a QR decompositon of A, A=Q.R 
	* R will replace A
	* Q will store Q
	*/
	/* A - tridiagonal 
	 * Q - Upper Hessenberg
	 * R - Upper tridiagonal
	 * so A (tridiagonal) will change to A (Upper tridiagonal)
	 * hence we have 4 diagonals in A
	 */

			/* not optimized, especially in callculating Q, work 
				* directly using A instead of vectors u and v 
				* e being a canonical basis vector is not considered */
				/* when calculating R, no need to calculate last column (1 element only) */

MY_DOUBLE *v,*u,**scratch; /* vectors */
MY_DOUBLE norm;
int i,k,l;

v =(MY_DOUBLE*)calloc(m,sizeof(MY_DOUBLE));
if ( v == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
u =(MY_DOUBLE*)calloc(m,sizeof(MY_DOUBLE));
if ( u == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
scratch= (MY_DOUBLE **) calloc(2, sizeof(MY_DOUBLE *));
  if ( scratch == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  scratch[0] = ( MY_DOUBLE * ) calloc( (size_t)m, sizeof(MY_DOUBLE));
  if ( scratch[0] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
 scratch[1] = ( MY_DOUBLE * ) calloc( (size_t)m, sizeof(MY_DOUBLE));
  if ( scratch[1] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }


for (k=0; k<m-1; k++) {
						/* x=A_[k:m,k] */
						norm=0;
						/* make u=0 */
						memset((void*)u,0,sizeof(MY_DOUBLE)*(size_t)(m-k));
						u[0]=eT(A,k,k,m); norm+=u[0]*u[0];
						u[1]=eT(A,k+1,k,m); norm+=u[1]*u[1];
						/*replaces code: for (i=0; i<m-k; i++) {
										u[i]=eT(A,i+k,k,m);
										norm+=u[i]*u[i];
						}*/

						memset((void*)v,0,sizeof(MY_DOUBLE)*(size_t)(m-k));
					/* v_k=sign(x[0])||x|| e_1 +x */
					v[0]=u[0];
					v[0]+=(u[0]>=0?sqrt(norm):-sqrt(norm));
				/* again calculate norm of v, not u */
					norm +=v[0]*v[0]-u[0]*u[0];
					norm=sqrt(norm);
     v[0]/=norm;
		  /* v_k=v_k/||v_k|| */
          v[1]=u[1]/norm;
				/* all other elements are zero */
#ifdef DEBUG
					printf("qr: Householder vector for col %d\n",k);
					for (i=0;i<m-k;i++) {
          printf("v[%d]="MDF"\n",i,v[i]);
				}
#endif
	
				/* A_[k:m,k:m]=A_[k:m,k:m]-2v_k(v_k'A_[k:m,k:m]) */
				/* first form the product (v_k'A_[k:m,k:m]) */
				/* and store it in u */
        /* product vector has only 3 non zero elements, max */
			   memset((void*)u,0,sizeof(MY_DOUBLE)*(size_t)(m-k));
				 if ( k !=m-2) {
				u[0]=v[0]*eT(A,k,k,m)+v[1]*eT(A,k+1,k,m);
				u[1]=v[0]*eT(A,k,k+1,m)+v[1]*eT(A,k+1,k+1,m);
				u[2]=v[1]*eT(A,k+1,k+2,m);
				 } else { /* k==m-2 */
        u[0]=v[0]*eT(A,k,k,m)+v[1]*eT(A,k+1,k,m);
				u[1]=v[0]*eT(A,k,k+1,m)+v[1]*eT(A,k+1,k+1,m);
        }
#ifdef DEBUG
					printf("qr: product vector for col %d\n",k);
					for (i=0;i<m-k;i++) {
          printf("u[%d]="MDF"\n",i,u[i]);
				}
#endif

				/*replaces code: for (i=0; i< m-k; i++) {
								u[i]=0;
								for (j=0; j<m-k; j++) {
															u[i] +=v[j]*A[j+k][i+k];
								}
				}*/

				/* A_[k:m,k:m]=A_[k:m,k:m]-2v_k( u' ) */
				if (k!=m-2) {
				/* only 6 elements of the matrix is affected */
        /*A[k][k]-=2*v[0]*u[0];
        A[k][k+1]-=2*v[0]*u[1];
        A[k][k+2]=-2*v[0]*u[2]; *//* should be equality */

        /*A[k+1][k]-=2*v[1]*u[0]; this is identically zero
        A[k+1][k+1]-=2*v[1]*u[1];
        A[k+1][k+2]-=2*v[1]*u[2]; */
				/* ugly representation of above code */
				if ( k==0 ) {
           A[k][0]-=2*v[0]*u[0];
           A[k][1]-=2*v[0]*u[1];
           A[k][2]=-2*v[0]*u[2];

           A[k+1][0]-=2*v[1]*u[0];
           A[k+1][1]-=2*v[1]*u[1];
           A[k+1][2]-=2*v[1]*u[2];
				} else { /* k == m-2 and k==m-1 will never happen here */
           A[k][1]-=2*v[0]*u[0];
           A[k][2]-=2*v[0]*u[1];
           A[k][3]=-2*v[0]*u[2];

           A[k+1][0]-=2*v[1]*u[0];
           A[k+1][1]-=2*v[1]*u[1];
           A[k+1][2]-=2*v[1]*u[2];
			 }
			 } else { /* k==m-2 */
       /* A[k][k]-=2*v[0]*u[0];
        A[k][k+1]-=2*v[0]*u[1];
        A[k+1][k]-=2*v[1]*u[0]; this is identically zero
        A[k+1][k+1]-=2*v[1]*u[1]; */
         A[k][1]-=2*v[0]*u[0];
         A[k][2]-=2*v[0]*u[1];

         A[k+1][0]-=2*v[1]*u[0];
         A[k+1][1]-=2*v[1]*u[1];
       }
				
		/* replaces code:  for (i=0; i< m-k; i++ ){
									for (j=0; j<m-k; j++) {
														A[i+k][j+k] -=2*v[i]*u[j];
				     }
			} */

#ifdef DEBUG
  for (i=0;i<m;i++) {
	printf("| ");
	 if ( i==0 ) {
	   printf(""MDF" "MDF" "MDF"",A[i][0], A[i][1], A[i][2]);
        for (l=3;l<m;l++) 
	       printf(""MDF" ",0.0);
	 } else if (i==m-2) {
       for (l=0;l<m-3;l++) 
	       printf(""MDF" ",0.0);
	   printf(""MDF" "MDF" "MDF"",A[i][0], A[i][1], A[i][2]);
	 } else if (i==m-1) {
			for (l=0;l<m-2;l++) 
	      printf(""MDF" ",0.0);
	   printf(""MDF" "MDF"",A[i][0], A[i][1]);
	 } else {
      for (l=0;l<i-1;l++) 
	      printf(""MDF" ",0.0);
	   printf(""MDF" "MDF" "MDF" "MDF"",A[i][0], A[i][1], A[i][2], A[i][3]);
     for (l=i+2;l<m-1;l++) 
	      printf(""MDF" ",0.0);
  }
	printf("|\n");
  }
#endif
  /* save vector v (non zero part) in two arrays, 
	 * since only 2 non zero elements 
	 * to construct Q later */
		scratch[0][k]=v[0];
		scratch[1][k]=v[1];
}

/* note A */
/* for the k th column, only one element */
/* so vector is 1 */
/* note- we have to change sign in last column of Q now */
scratch[0][m-1]=1;
scratch[1][m-1]=0;

#ifdef DEBUG
printf("qr_decomp: R found\n");
  for (i=0;i<m;i++) {
	printf("| ");
	 if ( i==0 ) {
	   printf(""MDF" "MDF" "MDF"",A[i][0], A[i][1], A[i][2]);
        for (l=3;l<m;l++) 
	       printf(""MDF" ",0.0);
	 } else if (i==m-2) {
       for (l=0;l<m-3;l++) 
	       printf(""MDF" ",0.0);
	   printf(""MDF" "MDF" "MDF"",A[i][0], A[i][1], A[i][2]);
	 } else if (i==m-1) {
			for (l=0;l<m-2;l++) 
	      printf(""MDF" ",0.0);
	   printf(""MDF" "MDF"",A[i][0], A[i][1]);
	 } else {
      for (l=0;l<i-1;l++) 
	      printf(""MDF" ",0.0);
	   printf(""MDF" "MDF" "MDF" "MDF"",A[i][0], A[i][1], A[i][2], A[i][3]);
     for (l=i+2;l<m-1;l++) 
	      printf(""MDF" ",0.0);
  }
	printf("|\n");
  }
printf("|| ");
 for (i=0; i<m;i++) {
	   printf(""MDF" ",scratch[0][i]);
 }
printf("||\n");
printf("|| ");
 for (i=0; i<m;i++) {
	   printf(""MDF" ",scratch[1][i]);
 }
printf("||\n");
#endif

/* now construct Q from stored values of v */
for (l=0;l<m;l++) {
					/* unit vector e_l */
			   memset((void*)u,0,sizeof(MY_DOUBLE)*(size_t)(m));
				 u[l]=1;

#ifdef DEBUG
				printf("unit vector %d\n",l);
    for (i=0;i<m;i++) {
				printf(""MDF" ",u[i]);
				}
				printf("\n");
#endif

  for (k=m-1;k>=0; k--) {
				/* retrieve v_k */
			   memset((void*)v,0,sizeof(MY_DOUBLE)*(size_t)(m-k));
						v[0]=scratch[0][k];
						v[1]=scratch[1][k];
#ifdef DEBUG
				printf("using householder %d\n",k);
    for (i=0;i<m-k;i++) {
				printf("i=%d,"MDF" ",i,v[i]);
				}
				printf("\n");
#endif

							/* form product */
				norm=0;
			  norm+=v[0]*u[k];
				if (k==m-1) {
						u[k] -=2*v[0]*norm;
				 } else {
			      norm+=v[1]*u[k+1];
						u[k] -=2*v[0]*norm;
						u[k+1] -=2*v[1]*norm;
				 }
  }
#ifdef DEBUG
				printf("col %d of Q\n",l);
    for (i=0;i<m;i++) {
				printf(""MDF" ",u[i]);
				}
				printf("\n");
#endif
    /* copy to Q, Q is upper hessenberg */
				if ( l != m-1 ) {
				for (i=0; i<l+2;i++) {
								/*Q[i][l]=u[i]; */
								if ( i<2 ) {
                  Q[i][l]=u[i];
								} else {
									Q[i][l-i+1]=u[i];
								}
				}
				} else {
         /* change sign of last column of Q see note A*/
         for (i=0; i<l+1;i++) {
							/*	Q[i][l]=-u[i]; */
                if ( i<2 ) {
                  Q[i][l]=-u[i];
								} else {
									Q[i][l-i+1]=-u[i];
								}
				}
			 }

	}

#ifdef DEBUG
 printf("Q found\n");
 for (i=0;i<m;i++) {
   printf("| ");
	 for (l=0;l<m;l++)
				printf(""MDF" ",eH(Q,i,l,m));
   printf("|\n");
	}
#endif
		free(v);
    free(u);
		free(scratch[0]);
		free(scratch[1]);
		free(scratch);
		return(0);
}

static int
find_eigenvalues_tridiagonal(MY_DOUBLE **H,MY_DOUBLE *v,int m)
{
	/* finds the eigenvalues of the matrix A (in tridiagonal form) and 
	 * stores them in array v
	 * A will be modified
	 */

  MY_DOUBLE **Q,**C,mu;
  int i,k,pivot;
	MY_DOUBLE  *vt;
#ifdef DEBUG
	int j;
#endif
#ifdef DEBUG
		printf("find_eig_tri: solving for size m=%d\n",m);
	  printf("H=\n");
	printf(MDF" "MDF"\n",H[0][0],H[0][1]);
  for (i=1;i<m-1;i++) {
	 printf(MDF" "MDF" "MDF"\n",H[i][0],H[i][1],H[i][2]);
  }
	printf(MDF" "MDF"\n",H[m-1][0],H[m-1][1]);
	printf("fing_eig_tri:=================\n");
  for (i=0;i<m;i++) {
		printf("find_eig_tri: eigenvalue array %d="MDF"\n",i,v[i]);
	}
#endif

	/* FIXME - if m is 2, we used closed form formula to find
	 * the eigenvalues */
	if ( m==2 ) {
#ifdef DEBUG
		printf("find_eig_tri: analytical solution, m=2\n");
#endif
		mu=(H[0][0]-H[1][1])*(H[0][0]-H[1][1])+4*H[0][1]*H[1][0];
		if ( mu>=0 ) {
					mu=sqrtf((float)mu);
		}
		v[0]=0.5*(H[0][0]+H[1][1]+mu);
		v[1]=0.5*(H[0][0]+H[1][1]-mu);
		if ( ABS(v[0])<ABS(v[1]) ) {
				mu=v[0];
				v[0]=v[1];
				v[1]=mu;
		}
#ifdef DEBUG
  for (i=0;i<m;i++) {
		printf("find_eig_tri: eigenvalue array %d="MDF"\n",i,v[i]);
	}
#endif
		return(0);
 }


	/* Q is an Upper Hessenberg matrix */
  Q= (MY_DOUBLE **) calloc(m, sizeof(MY_DOUBLE *));
  if ( Q == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  Q[0] = ( MY_DOUBLE * ) calloc( (size_t)m, sizeof(MY_DOUBLE));
  if ( Q[0] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
	for (i=1;i<m;i++) {
    Q[i] = ( MY_DOUBLE * ) calloc( (size_t)(m-i+1), sizeof(MY_DOUBLE));
    if ( Q[i] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
	}
 /* C is a temporary matrix, tri diagonal */
  C= (MY_DOUBLE **) calloc(m, sizeof(MY_DOUBLE *));
  if ( C == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  for (i = 1; i < m-1; ++i) {
  C[i] = ( MY_DOUBLE * ) calloc( (size_t)3, sizeof(MY_DOUBLE));
  if ( C[i] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
  }
  C[0] = ( MY_DOUBLE * ) calloc( (size_t)2, sizeof(MY_DOUBLE));
  if ( C[0] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
  C[m-1] = ( MY_DOUBLE * ) calloc( (size_t)2, sizeof(MY_DOUBLE));
  if ( C[m-1] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }

/* FIXME improvements
 * 1) shifts
 * 2) divide to submatrices whenever off diagonal becomes almost 0
 */
 for (k=0; k< 8; k++) {
	/* find new shift */
	mu=H[m-1][1]; /* last diagonal element, but we use special storage */
	/* substract the shift from H */
	/* H = H-mu.I */
  for (i=1;i<m; i++) {
			/* H[i][i]-=mu; *
			 * but special storage for H */
			H[i][1]-=mu;
	}
	H[0][0]-=mu;
#ifdef DEBUG
printf("find_eig_tri:iteration %d mu="MDF" \n",k,mu);
#endif
  qr_decomp(H,Q,m); /* H=Q.H */
#ifdef DEBUG
	printf("R after QR=\n");
	printf(MDF" "MDF"\n",H[0][0],H[0][1]);
  for (i=1;i<m-1;i++) {
	 printf(MDF" "MDF" "MDF"\n",H[i][0],H[i][1],H[i][2]);
  }
	printf(MDF" "MDF"\n",H[m-1][0],H[m-1][1]);
	printf("=================\n");
	printf("Q=\n");
  for (i=0;i<m;i++) {
	printf("| ");
        for (j=0;j<m;j++)
	   printf(""MDF" ",eH(Q,i,j,m));
	printf("|\n");
  } 
#endif

  matrix_mult(H,Q,C,m); /* H=H*Q */
  
	/* add the shift back to H */
	/* H = H + mu. I */
  for (i=1;i<m; i++) {
			H[i][1]+=mu;
	}
	H[0][0]+=mu;

#ifdef DEBUG
 printf("RQ=\n");
	printf(MDF" "MDF"\n",H[0][0],H[0][1]);
  for (i=1;i<m-1;i++) {
	 printf(MDF" "MDF" "MDF"\n",H[i][0],H[i][1],H[i][2]);
  }
	printf(MDF" "MDF"\n",H[m-1][0],H[m-1][1]);
	printf("=================\n");
#endif

	/* examine any off diagonal elements are zero
	 * if nearly zero, make them zero */
  for (i=1;i<m-1; i++) {
			if(ABS(H[i][0])<=TOL)
						H[i][0]=0;
			if(ABS(H[i][2])<=TOL)
						H[i][2]=0;
	}
	if ( ABS(H[0][1])<TOL) H[0][1]=0;
	if ( ABS(H[m-1][0])<TOL) H[m-1][0]=0;

	/* now see if any row has both off diagonal elements zero.
	 * if so the diagonal element in that row is an eigenvalue */
	/* since we are shifting with the least eigenvalue,
	 * the chances are this is the first we find. Hence
	 * check the last row */
	pivot=0;
	for (i=1;i<m;i++) {
				if((H[i][0]==0)) {
					/* we assume H to be symmetric here
					 * so if lower diagonal has a zero,
					 * upper diagonal also has a zero */
							pivot=i; /* row */
				}
  }
	/* pivot can be in [1,m-1] only */
  /* now if pivot != 0, that means we have
	 * found an eigenvalue. Hence we split the matrix H
	 * into two at that point and call find_eigenvalue
	 * recursively on the two sub matrices */
  if (pivot==1) {
		 /* first element is an eigenvalue */
		 v[0]=H[0][0];
#ifdef DEBUG
	   printf("find_eig_tri: splitting lower eigenvalue %d="MDF"\n",pivot,v[0]);
#endif
    /* new sub matrix size m-1 */
    /* move elements up one row */
		H[0][0]=H[pivot][1];H[0][1]=H[pivot][2];
		for (i=1;i<m-1-pivot;i++) {
       H[i][0]=H[i+pivot][0];
			 H[i][1]=H[i+pivot][1];
			 H[i][2]=H[i+pivot][2];
		}
		H[m-1-pivot][0]=H[m-1][0];
		H[m-1-pivot][1]=H[m-1][1];

		/* eigenvector array */
		vt=(MY_DOUBLE *) calloc(m-1, sizeof(MY_DOUBLE));
    if ( vt == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
#ifdef DEBUG
printf("find_eig_tri:entering recursive more case 1\n");
#endif
   for(i=0;i<m;i++) {
	    free(Q[i]);
	    free(C[i]);
    }
    free(Q);free(C);

		/* recursive call */
		find_eigenvalues_tridiagonal(H,vt,m-1);
    
		/* copy back result */
		for(i=1;i<m;i++) 
					v[i]=vt[i-1];
#ifdef DEBUG
  for (i=0;i<m;i++) {
		printf("find_eig_tri: eigenvalue array after case 1 %d="MDF"\n",i,v[i]);
	}
#endif

		/* free memory */
		free(vt);

		/* break loop */
		return(0);
	} else if (pivot==m-1) {
  /* last element is an eigenvalue */
					v[m-1]=H[m-1][1];
#ifdef DEBUG
	   printf("find_eig_tri: splitting upper eigenvalue %d="MDF"\n",pivot,v[m-1]);
#endif
		vt=(MY_DOUBLE *) calloc(m-1, sizeof(MY_DOUBLE));
    if ( vt == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
#ifdef DEBUG
printf("find_eig_tri:entering recursive more case 2\n");
#endif
    for(i=0;i<m;i++) {
	    free(Q[i]);
	    free(C[i]);
    }
    free(Q);free(C);

		/* recursive call */
    /* now only the first m-1 rows of H is used */
		find_eigenvalues_tridiagonal(H,vt,m-1);
    
		/* copy back result */
		for(i=0;i<m-1;i++) 
					v[i]=vt[i];
#ifdef DEBUG
  for (i=0;i<m;i++) {
		printf("find_eig_tri: eigenvalue array after case 2 %d="MDF"\n",i,v[i]);
	}
#endif

		free(vt);

    /* break loop */
	   return(0);
	} else if (pivot) {
			/* no eigenvalue found, but we 
			 * can subdivide the matrix into two */
			/* first the upper matrix */
			/* rows 0 to pivot-1, size pivot */
		/* allocate memory */
#ifdef DEBUG
	   printf("find_eig_tri: splitting to two no eigenvalue, pivot at %d\n",pivot);
#endif
    /* new sub matrix size pivot */
		
		/* eigenvector array */
		vt=(MY_DOUBLE *) calloc(pivot, sizeof(MY_DOUBLE));
    if ( vt == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
#ifdef DEBUG
printf("find_eig_tri:entering recursive more case 3 a\n");
#endif

    for(i=0;i<m;i++) {
	    free(Q[i]);
	    free(C[i]);
    }
    free(Q);free(C);

		/* recursive call */
		find_eigenvalues_tridiagonal(H,vt,pivot);
    
		/* copy back result */
		for(i=0;i<pivot;i++) 
					v[i]=vt[i];
		/* free memory */
		free(vt);

    /* now the lower matrix */
		/* rows pivot to m-1 */
    /* new sub matrix size m-pivot */
	  /* move rows upwards by pivot */	
		H[0][0]=H[pivot][1];H[0][1]=H[pivot][2];
		for (i=1;i<m-1-pivot;i++) {
       H[i][0]=H[i+pivot][0];
			 H[i][1]=H[i+pivot][1];
			 H[i][2]=H[i+pivot][2];
		}
		H[m-1-pivot][0]=H[m-1][0];
		H[m-1-pivot][1]=H[m-1][1];


		/* eigenvector array */
		vt=(MY_DOUBLE *) calloc(m-pivot, sizeof(MY_DOUBLE));
    if ( vt == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
#ifdef DEBUG
printf("find_eig_tri:entering recursive more case 3 b\n");
#endif


		/* recursive call */
		find_eigenvalues_tridiagonal(H,vt,m-pivot);
    
		/* copy back result */
		for(i=0;i<m-pivot;i++) 
					v[i+pivot]=vt[i];
		free(vt);

		/* break loop */
	  return(0);
	}
	 }
  
 /* if we are here, no matrix subdivision was done
	* and we exited the loop simply due to
	* exceeding the number of iterations. need to copy the eigenvalues
	* we have estimated so far */
  for (i=0;i<m;i++) {
#ifdef DEBUG
		printf("find_eig_tri: eigenvalues estimated %d="MDF"\n",i,eT(H,i,i,m));
#endif
		v[i]=eT(H,i,i,m);
	}
  for(i=0;i<m;i++) {
	    free(Q[i]);
	    free(C[i]);
    }
    free(Q);free(C);


  return(0);
 
}

static int
find_eigenvalues(MY_DOUBLE **A,MY_DOUBLE *v,int m)
{
	/* finds the eigenvalues of the matrix A and stores them
	 * in array v
	 * A will be modified
	 */

  MY_DOUBLE **H,**Q,**C;
  int i;
#ifdef DEBUG
	int j;
#endif
  /* setup matrix H 4 diagonal */
  H= (MY_DOUBLE **) calloc(m, sizeof(MY_DOUBLE *));
  if ( H == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  for (i = 1; i < m-2; ++i) {
  H[i] = ( MY_DOUBLE * ) calloc( (size_t)4, sizeof(MY_DOUBLE));
  if ( H[i] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
  }
  H[0] = ( MY_DOUBLE * ) calloc( (size_t)3, sizeof(MY_DOUBLE));
  if ( H[0] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
  H[m-2] = ( MY_DOUBLE * ) calloc( (size_t)3, sizeof(MY_DOUBLE));
  if ( H[m-2] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
  H[m-1] = ( MY_DOUBLE * ) calloc( (size_t)2, sizeof(MY_DOUBLE));
  if ( H[m-1] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }

	/* Q is an Upper Hessenberg matrix */
  Q= (MY_DOUBLE **) calloc(m, sizeof(MY_DOUBLE *));
  if ( Q == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  Q[0] = ( MY_DOUBLE * ) calloc( (size_t)m, sizeof(MY_DOUBLE));
  if ( Q[0] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
	for (i=1;i<m;i++) {
    Q[i] = ( MY_DOUBLE * ) calloc( (size_t)(m-i+1), sizeof(MY_DOUBLE));
    if ( Q[i] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
	}
 /* C is a temporary matrix, tri diagonal */
  C= (MY_DOUBLE **) calloc(m, sizeof(MY_DOUBLE *));
  if ( C == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  for (i = 1; i < m-1; ++i) {
  C[i] = ( MY_DOUBLE * ) calloc( (size_t)3, sizeof(MY_DOUBLE));
  if ( C[i] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
  }
  C[0] = ( MY_DOUBLE * ) calloc( (size_t)2, sizeof(MY_DOUBLE));
  if ( C[0] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
  C[m-1] = ( MY_DOUBLE * ) calloc( (size_t)2, sizeof(MY_DOUBLE));
  if ( C[m-1] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }

  convert_to_hessenberg(A,H,m);
#ifdef DEBUG
	printf("A, Hessenberg=\n");
  for (i=0;i<m;i++) {
	printf("| ");
        for (j=0;j<m;j++)
	   printf(""MDF" ",eT(H,i,j,m));
	printf("|\n");
  }
	printf("A =\n");
  for (i=0;i<m;i++) {
	printf("| ");
        for (j=0;j<m;j++)
	   printf(""MDF" ",A[i][j]);
	printf("|\n");
  } 
#endif

	/* call the recursive routine */
  find_eigenvalues_tridiagonal(H,v,m);
#ifdef DEBUG
	printf("find_eig: eigenvalues found\n");
	for(i=0;i<m;i++) 
			printf("find_eig: eigenvalue %d="MDF"\n",i,v[i]);
#endif
	for(i=0;i<m;i++) {
	free(H[i]);
	free(Q[i]);
	free(C[i]);
 }
free(H);free(Q);free(C);
  return(0);
 
}


static int 
lu_solve(MY_DOUBLE **A, MY_DOUBLE *x, MY_DOUBLE *y, int m)
{
	/* solves Ax=y by LU decomposition with pivoting */
	/* A,x, y will be changed */
  MY_DOUBLE max;
	int i,j,k,*v;
	/* decompose A=L. U  in place */
 /* row vector to store pivot data instead of P */	
	v =(int*)calloc(m,sizeof(int));
if ( v == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
 /* U = A, P=I; L=I initially */
 for (i=0;i<m;i++) {
		v[i]=i; /* initially no permutation */
 }

 for (k=0; k<m-1;k++) {
    /* select i (>=k) with max |U_ik| */
		i=k;max=ABS(A[v[i]][k]);
		for (j=k;j<m;j++){
				if (ABS(A[v[j]][k]) >max){
		     i=j;max=ABS(A[v[i]][k]);
        }
	  }

		/* interchange rows */
		/* U_k,k:m-1 <-> U_i,k:m-1 and
		 * L_k,0:k-1 <-> L_i,0:k-1
		 */
		/* instead we interchange the row index */
		j=v[k];
		v[k]=v[i];
		v[i]=j;

		/* now U_k,k should have the max value from column k
		 * if U_k,k is zero matrix is singular
		 * hence substitude a small value*/
		if (A[v[k]][k]==0){
			printf("***: warning: matrix is singular\n");
			A[v[k]][k]=0.0000001;
		}

		for (j=k+1;j<m;j++) {
					/* L_j,k=U_j,k/U_k,k */
					max=A[v[j]][k]/A[v[k]][k];
					/*A[v[j]][k]=A[v[j]][k]/A[v[k]][k]; */
					/* U_j,k:m-1=U_j,k:m-1-L_j,k*U_k,k:m-1 */
					for (i=k;i<m;i++) {
							A[v[j]][i]-=max*A[v[k]][i];
					}
					A[v[j]][k]=max;
		}
  }
 /* now we have A = L. U */
 /* solve system Ax=y */
  /* forward elimination */
	for (i=0; i<m; i++) {
		max=0;
		for (j=0;j<i;j++) {
				max+=A[v[i]][j]*y[v[j]];
		}
		y[v[i]]-=max;
	}
 /* back substitution */
	for (i=m-1;i>=0;i--) {
			max=0;
			for (j=i+1;j<m;j++) {
				max+=A[v[i]][j]*x[v[j]];
			}
			x[v[i]]=(y[v[i]]-max)/A[v[i]][i];
	}

	/* put solution in correct place */
	 for (i=0; i<m; i++) {
			y[i]=x[v[i]];
	 }
	 for (i=0;i<m;i++) {
			x[i]=y[i];
	 }
  free(v); 
	return(0);
}





/* used in sorting eigenvalues, comparison */
static int
compare_doubles(const void *a,const void *b)
{
	 const MY_DOUBLE *da=(const MY_DOUBLE *)a;
	 const MY_DOUBLE *db=(const MY_DOUBLE *)b;
  
	 return((ABS(*da)<=ABS(*db)?1:-1));
}

/* finds all eigenvalues of the matrix a */
static void
find_all_eigenvalues(MY_DOUBLE **a, MY_DOUBLE *g, MY_INT m)
{ 
	/* finds all eigenvalues of a m by m symmetric matrix a
	 * a - m by m matrix
	 * and returns them using an array g - size m by 1
	 */
  MY_DOUBLE **B;

  int i;
#ifdef DEBUG
	int k,j;
#endif

	/* temporary matrix */
  B= (MY_DOUBLE **) calloc(m, sizeof(MY_DOUBLE *));
  if ( B == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  for (i = 0; i < m; ++i) {
  B[i] = ( MY_DOUBLE * ) calloc( (size_t)m, sizeof(MY_DOUBLE));
  if ( B[i] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
	   memcpy((void*)B[i],(void*)a[i],(size_t)(m)*sizeof(MY_DOUBLE));
  }

	
  /* display */
#ifdef DEBUG
	printf("A =\n");
  for (i=0;i<m;i++) {
	printf("| ");
        for (j=0;j<m;j++)
	   printf(""MDF" ",a[i][j]);
	printf("|\n");
  } 
#endif

/* find eigenvalues */ 
 find_eigenvalues(B,g,m);

 /* sort the eigenvalues */
 qsort(g,m,sizeof(MY_DOUBLE),compare_doubles);
#ifdef DEBUG
 printf("find_eigenvalues_all: eigenvalues found\n");
  for (i=0;i<m;i++) {
	   printf(""MDF" ",g[i]);
	}
	printf("\n");
#endif


 for(i=0;i<m;i++) {
	free(B[i]);
 }
 free(B);

}

/* find the eigenvector(s) given an eigenvalue(s) */
void
find_eigenvectors(MY_DOUBLE **a, MY_DOUBLE **x,  MY_DOUBLE *ev, MY_INT m, MY_INT n)
{ 
	/* finds the eigenvector of a m by m symmetric matrix a
	 * a - m by m matrix
	 * x - eigenvector matrix size (m by n)
   * ev - eigenvalue array size n by 1
	 * n - how many eigenvectors to find starting from the lowest eigenvalue
	 * m - matrix size of a
	 */
  MY_DOUBLE **B,*y,*v,*vec,norm;

  int i,j,en;
#ifdef DEBUG
	int k;
#endif

	/* temporary matrix */
  B= (MY_DOUBLE **) calloc(m, sizeof(MY_DOUBLE *));
  if ( B == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
  for (i = 0; i < m; ++i) {
  B[i] = ( MY_DOUBLE * ) calloc( (size_t)m, sizeof(MY_DOUBLE));
  if ( B[i] == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
  }
	   memcpy((void*)B[i],(void*)a[i],(size_t)(m)*sizeof(MY_DOUBLE));
  }

  /* allocate memory for array of eigenvalues */
  v= (MY_DOUBLE *) calloc(m, sizeof(MY_DOUBLE));
  if ( v == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
   }


  /* display */
#ifdef DEBUG
	printf("A =\n");
  for (i=0;i<m;i++) {
	printf("| ");
        for (j=0;j<m;j++)
	   printf(""MDF" ",a[i][j]);
	printf("|\n");
  } 
#endif

 /* find all eigenvalues */ 
 find_all_eigenvalues(a,v,m);

#ifdef DEBUG
 printf("eigenvalues found\n");
  for (i=0;i<m;i++) {
	   printf(""MDF" ",v[i]);
	}
	printf("\n");
#endif

	/* do an inverse iteration to find the eigenvector */
 	y =(MY_DOUBLE*)calloc(m,sizeof(MY_DOUBLE));
if ( y == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
 	vec =(MY_DOUBLE*)calloc(m,sizeof(MY_DOUBLE));
if ( vec == 0 ) {
      fprintf(stderr,"%s: %d: no free memory",__FILE__,__LINE__);
      exit(1);
    }
for (en=m-1;en>=m-n;en--) {
 /* initial guess */
memset((void*)vec,0,(size_t)m*sizeof(MY_DOUBLE));
vec[0]=1; /* start with unit vector */
   
 for (j=0; j<EIGENVECTOR_ITERATION_LIMIT; j++) { /* inverse iteration 
													iterative step: only 1 iteration because
													we have found an eigenvalue */
				 /* copy A to B */
				 /* B=A-mu.I */
				 for (i=0; i<m; i++) {
							memcpy((void*)B[i],(void*)a[i],(size_t)(m)*sizeof(MY_DOUBLE));
							B[i][i] -=v[en]; /* last eigenvalue */
							y[i]=vec[i];
         }
#ifdef DEBUG
  printf("solving system\n");
  for (i=0;i<m;i++) {
	printf("| ");
        for (k=0;k<m;k++)
	   printf(""MDF" ",B[i][k]);
	printf("|| "MDF"\n",y[i]);
  } 
#endif

         lu_solve(B, vec, y, m);
				 /* normalize x */
				 norm=0;
				 for(i=0;i<m;i++) {
							norm+=vec[i]*vec[i];
				 }
				 norm=sqrt(norm);
				 for (i=0; i<m;i++) {
							/*vec[i]/=sqrt(norm); */
							vec[i]=vec[i]/norm;
				 }
#ifdef DEBUG
				 printf("find_eigvec: eigenvector estimate: for eigenvalue "MDF" iteration %d\n",v[en],j);
				 for (i=0;i<m;i++) {
	         printf(""MDF" ",vec[i]);
				 }
				 printf("\n");
#endif
 }

 for (i=0;i<m;i++) {
  x[i][m-1-en]=vec[i];
#ifdef DEBUG
	printf(MDF" ",x[i][m-1-en]);
#endif
 }
#ifdef DEBUG
	printf("\n");
  printf("find_eigvec: eigenvalue %d estimate:  "MDF" \n",en, v[en]);
#endif

 }
 i=0;
 for (en=m-1;en>=m-n;en--) {
  /* copy eigenvalue to output */
  ev[i++]=v[en];
 }

 for(i=0;i<m;i++) {
	free(B[i]);
 }
 free(B);
 free(y);
 free(v);
 free(vec);
}