/************************************************************************
 *                                                                      *
 *  Program package         T O O L D I A G                             *
 *                                                                      *
 *  Version   1.5                                                       *
 *  Date:     Tue Feb  8 13:39:05 1994                                  *
 *                                                                      *
 *  NOTE: This program package is copyrighted in the sense that it      *
 *  may be used for scientific purposes. The package as a whole, or     *
 *  parts thereof, cannot be included or used in any commercial         *
 *  application without written permission granted by the author.       *
 *  No programs contained in this package may be copied for commercial  *
 *  distribution.                                                       *
 *                                                                      *
 *  All comments  concerning this program package may be sent to the    *
 *  e-mail address 'tr@fct.unl.pt'.                                     *
 *                                                                      *
 ************************************************************************/



#include <stdio.h>
#include <math.h>
#include "def.h"
#include "matrix.h"

extern universe *U;
extern float vector_len();
extern float Euclidian_Distance();
extern float dot_product();

static str80 buf;

static void nrerror(error_text)
char error_text[];
{
  fprintf(stderr,"Numerical Recipes run-time error...\n");
  fprintf(stderr,"%s\n",error_text);
  fprintf(stderr,"...now exiting to system...\n");
  exit(1);
}


static void debug_a( a, n, m )
float **a; int n, m;
{
 int i, j;

 printf("\nPeeping matrix...\n");
 for( i = 0; i < n; i++ )
 {
   for( j = 0; j < m; j++)
     printf(" %7.5f ", a[i][j] );
   printf("\n");
 }
 printf("\n");
}

static void debug_vec( v, n )
float *v; int n;
{
 int j;

 printf("\n");
 for( j = 0; j < n; j++)
   printf(" %7.20f\n", v[j] );
 printf("\n");
}


#define RADIX 2.0

void balanc(M)
Matrix *M;
{
  int n, last,j,i;
  float s,r,g,f,c,sqrdx;
  float **a = NULL;

  n = M->col;

  a = (float**) malloc( n * sizeof(float*) );
  for( i = 0; i < n; i++ )
    a[i] = (float*) malloc( n * sizeof(float) );

  for( i = 0; i < n; i++ )
    for( j = 0; j < n; j++ )
      a[i][j] = (float)M->Elem[i*n+j];

  sqrdx=RADIX*RADIX;
  last=0;
  while (last == 0) {
    last=1;
    for (i=1;i<=n;i++) {
      r=c=0.0;
      for (j=1;j<=n;j++)
        if (j != i) {
          c += fabs(a[j-1][i-1]);
          r += fabs(a[i-1][j-1]);
        }
      if (c && r) {
        g=r/RADIX;
        f=1.0;
        s=c+r;
        while (c<g) {
          f *= RADIX;
          c *= sqrdx;
        }
        g=r*RADIX;
        while (c>g) {
          f /= RADIX;
          c /= sqrdx;
        }
        if ((c+r)/f < 0.95*s) {
          last=0;
          g=1.0/f;
          for (j=1;j<=n;j++) a[i-1][j-1] *= g;
          for (j=1;j<=n;j++) a[j-1][i-1] *= f;
        }
      }
    }
  }

  for( i = 0; i < n; i++ )
    for( j = 0; j < n; j++ )
      M->Elem[i*n+j] = (MatElem)a[i][j];

  for( i = 0; i < n; i++ )
    FREE(a[i]);
  FREE(a);
}
#undef RADIX


#define SWAP(g,h) {y=(g);(g)=(h);(h)=y;}

void elmhes(M)
Matrix *M;
{
  int n,m,j,i;
  float y,x;
  float **a = NULL;

  n = M->col;

  a = (float**) malloc( n * sizeof(float*) );
  for( i = 0; i < n; i++ )
    a[i] = (float*) malloc( n * sizeof(float) );

  for( i = 0; i < n; i++ )
    for( j = 0; j < n; j++ )
      a[i][j] = (float)M->Elem[i*n+j];


  for (m=2;m<n;m++) {
    x=0.0;
    i=m;
    for (j=m;j<=n;j++) {
      if (fabs(a[j-1][m-2]) > fabs(x)) {
        x=a[j-1][m-2];
        i=j;
      }
    }
    if (i != m) {
      for (j=m-1;j<=n;j++) SWAP(a[i-1][j-1],a[m-1][j-1])
      for (j=1;j<=n;j++) SWAP(a[j-1][i-1],a[j-1][m-1])
    }
    if (x) {
      for (i=m+1;i<=n;i++) {
        if (y=a[i-1][m-2]) {
          y /= x;
          a[i-1][m-2]=y;
          for (j=m;j<=n;j++)
            a[i-1][j-1] -= y*a[m-1][j-1];
          for (j=1;j<=n;j++)
            a[j-1][m-1] += y*a[j-1][i-1];
        }
      }
    }
  }

  for( i = 0; i < n; i++ )
    for( j = 0; j < n; j++ )
      M->Elem[i*n+j] = (MatElem)a[i][j];

  for( i = 0; i < n; i++ )
    FREE(a[i]);
  FREE(a);
}
#undef SWAP


#define SIGN(a,b) ((b) > 0 ? fabs(a) : -fabs(a))

void hqr(M,wr,wi)
Matrix *M;
float wr[],wi[];
{
  int n,nn,m,l,k,j,its,i,mmin;
  float z,y,x,w,v,u,t,s,r,q,p,anorm;
  float **a = NULL;

  n = M->col;

  a = (float**) malloc( n * sizeof(float*) );
  for( i = 0; i < n; i++ )
    a[i] = (float*) malloc( n * sizeof(float) );

  for( i = 0; i < n; i++ )
    for( j = 0; j < n; j++ )
      a[i][j] = (float)M->Elem[i*n+j];


  anorm=fabs(a[0][0]);
  for (i=2;i<=n;i++)
    for (j=(i-1);j<=n;j++)
      anorm += fabs(a[i-1][j-1]);
  nn=n;
  t=0.0;
  while (nn >= 1) {
    its=0;
    do {
      for (l=nn;l>=2;l--) {
        s=fabs(a[l-2][l-2])+fabs(a[l-1][l-1]);
        if (s == 0.0) s=anorm;
        if ((float)(fabs(a[l-1][l-2]) + s) == s) break;
      }
      x=a[nn-1][nn-1];
      if (l == nn) {
        wr[nn-1]=x+t;
        wi[nn-1]=0.0; nn--;
      } else {
        y=a[nn-2][nn-2];
        w=a[nn-1][nn-2]*a[nn-2][nn-1];
        if (l == (nn-1)) {
          p=0.5*(y-x);
          q=p*p+w;
          z=sqrt(fabs(q));
          x += t;
          if (q >= 0.0) {
            z=p+SIGN(z,p);
            wr[nn-2]=wr[nn-1]=x+z;
            if (z) wr[nn-1]=x-w/z;
            wi[nn-2]=wi[nn-1]=0.0;
          } else {
            wr[nn-2]=wr[nn-1]=x+p;
            wi[nn-2]= -(wi[nn-1]=z);
          }
          nn -= 2;
        } else {
          if (its == 30) nrerror("Too many iterations in HQR");
          if (its == 10 || its == 20) {
            t += x;
            for (i=1;i<=nn;i++) a[i-1][i-1] -= x;
            s=fabs(a[nn-1][nn-2])+fabs(a[nn-2][nn-3]);
            y=x=0.75*s;
            w = -0.4375*s*s;
          }
          ++its;
          for (m=(nn-2);m>=l;m--) {
            z=a[m-1][m-1];
            r=x-z;
            s=y-z;
            p=(r*s-w)/a[m][m-1]+a[m-1][m];
            q=a[m][m]-z-r-s;
            r=a[m+1][m];
            s=fabs(p)+fabs(q)+fabs(r);
            p /= s;
            q /= s;
            r /= s;
            if (m == l) break;
            u=fabs(a[m-1][m-2])*(fabs(q)+fabs(r));
            v=fabs(p)*(fabs(a[m-2][m-2])+fabs(z)+fabs(a[m][m]));
            if ((float)(u+v) == v) break;
          }
          for (i=m+2;i<=nn;i++) {
            a[i-1][i-3]=0.0;
            if  (i != (m+2)) a[i-1][i-4]=0.0;
          }
          for (k=m;k<=nn-1;k++) {
            if (k != m) {
              p=a[k-1][k-2];
              q=a[k][k-2];
              r=0.0;
              if (k != (nn-1)) r=a[k+1][k-2];
              if (x=fabs(p)+fabs(q)+fabs(r)) {
                p /= x;
                q /= x;
                r /= x;
              }
            }
            if (s=SIGN(sqrt(p*p+q*q+r*r),p)) {
              if (k == m) {
                if (l != m)
                a[k-1][k-2] = -a[k-1][k-2];
              } else
                a[k-1][k-2] = -s*x;
              p += s;
              x=p/s;
              y=q/s;
              z=r/s;
              q /= p;
              r /= p;
              for (j=k;j<=nn;j++) {
                p=a[k-1][j-1]+q*a[k][j-1];
                if (k != (nn-1)) {
                  p += r*a[k+1][j-1];
                  a[k+1][j-1] -= p*z;
                }
                a[k][j-1] -= p*y;
                a[k-1][j-1] -= p*x;
              }
              mmin = nn<k+3 ? nn : k+3;
              for (i=l;i<=mmin;i++) {
                p=x*a[i-1][k-1]+y*a[i-1][k];
                if (k != (nn-1)) {
                  p += z*a[i-1][k+1];
                  a[i-1][k+1] -= p*r;
                }
                a[i-1][k] -= p*q;
                a[i-1][k-1] -= p;
              }
            }
          }
        }
      }
    } while (l < nn-1);
  }

  for( i = 0; i < n; i++ )
    for( j = 0; j < n; j++ )
      M->Elem[i*n+j] = (MatElem)a[i][j];

  for( i = 0; i < n; i++ )
    FREE(a[i]);
  FREE(a);
}


#define SWAP(a,b) {float temp=(a);(a)=(b);(b)=temp;}

void gaussj(a,n,b,m)
float **a,**b;
int n,m;
{
  int *indxc=NULL,*indxr=NULL,*ipiv=NULL;
  int i,icol,irow,j,k,l,ll;
  float big,dum,pivinv;

  indxc = (int*) malloc( n * sizeof(int) ); CHKPTR( indxc );
  indxr = (int*) malloc( n * sizeof(int) ); CHKPTR( indxr );
  ipiv = (int*) malloc( n * sizeof(int) ); CHKPTR( ipiv );


  for (j=0;j<n;j++) ipiv[j]=0;
  for (i=0;i<n;i++) {
    big=0.0;
    for (j=0;j<n;j++)
      if (ipiv[j] != 1)
        for (k=0;k<n;k++) {
          if (ipiv[k] == 0) {
            if (fabs(a[j][k]) >= big) {
              big=fabs(a[j][k]);
              irow=j;
              icol=k;
            }
          } else if (ipiv[k] > 1) nrerror("GAUSSJ: Singular Matrix-1");
        }
    ++(ipiv[icol]);
    if (irow != icol) {
      for (l=0;l<n;l++) SWAP(a[irow][l],a[icol][l])
      for (l=0;l<m;l++) SWAP(b[irow][l],b[icol][l])
    }
    indxr[i]=irow;
    indxc[i]=icol;
    if (a[icol][icol] == 0.0) nrerror("GAUSSJ: Singular Matrix-2");
    pivinv=1.0/a[icol][icol];
    a[icol][icol]=1.0;
    for (l=0;l<n;l++) a[icol][l] *= pivinv;
    for (l=0;l<m;l++) b[icol][l] *= pivinv;
    for (ll=0;ll<n;ll++)
      if (ll != icol) {
        dum=a[ll][icol];
        a[ll][icol]=0.0;
        for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
        for (l=0;l<m;l++) b[ll][l] -= b[icol][l]*dum;
      }
  }
  for (l=n-1;l>=0;l--) {
    if (indxr[l] != indxc[l])
      for (k=0;k<n;k++)
        SWAP(a[k][indxr[l]],a[k][indxc[l]]);
  }
  FREE( ipiv ); FREE( indxr ); FREE( indxc );
}
#undef SWAP


#define TINY 1.0e-20;

static void ludcmp(a,n,indx,d)
int n,*indx;
float **a,*d;
{
  int i,imax,j,k;
  float big,dum,sum,temp;
  float *vv = NULL;

  vv = (float*) malloc( n * sizeof(float) );

  *d=1.0;
  for (i=0;i<n;i++) {
    big=0.0;
    for (j=0;j<n;j++)
      if ((temp=fabs(a[i][j])) > big) big=temp;
    if (big == 0.0) nrerror("Singular matrix in routine LUDCMP");
    vv[i]=1.0/big;
  }
  for (j=0;j<n;j++) {
    for (i=0;i<j;i++) {
      sum=a[i][j];
      for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
      a[i][j]=sum;
    }
    big=0.0;
    for (i=j;i<n;i++) {
      sum=a[i][j];
      for (k=0;k<j;k++)
        sum -= a[i][k]*a[k][j];
      a[i][j]=sum;
      if ( (dum=vv[i]*fabs(sum)) >= big) {
        big=dum;
        imax=i;
      }
    }
    if (j != imax) {
      for (k=0;k<n;k++) {
        dum=a[imax][k];
        a[imax][k]=a[j][k];
        a[j][k]=dum;
      }
      *d = -(*d);
      vv[imax]=vv[j];
    }
    indx[j]=imax;
    if (a[j][j] == 0.0) a[j][j]=TINY;
    if (j != n-1) {
      dum=1.0/(a[j][j]);
      for (i=j+1;i<n;i++) a[i][j] *= dum;
    }
  }
  FREE( vv );
}
#undef TINY


static void lubksb(a,n,indx,b)
float **a,b[];
int n,*indx;
{
  int i,ii=0,ip,j;
  float sum;

  for (i=0;i<n;i++) {
    ip=indx[i];
    sum=b[ip];
    b[ip]=b[i];
    if (ii)
      for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
    else if (sum) ii=i;
    b[i]=sum;
  }
  for (i=n-1;i>=0;i--) {
    sum=b[i];
    for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
    b[i]=sum/a[i][i];
  }
}


#define MY_MAXINT 32000
/* generate a random vector of length dim with unit size */
static void randinit( x, dim )
float *x;
int dim;
{
 int i, rand;
 float len;

 for( i = 0; i < dim; i++ )
 {
   get_random( MY_MAXINT, &rand );
   x[i] = (float)rand / (float)MY_MAXINT;
 }
 /* showFV( dim, x ); DBG; /**/
 norm_vector( x, dim );
}


#define LU_DECOMP
static void linearSolve( M, eigVal, y, x )
Matrix *M;
float eigVal, *y, *x;
{
 float **a = NULL, **b = NULL, d, *bVec = NULL;
 int n, m, i, j, *indx = NULL;

 n = M->row; m = 1;

 a = (float**) malloc( n * sizeof(float*) );
 for( i = 0; i < n; i++ )
   a[i] = (float*) malloc( n * sizeof(float) );
 for( i = 0; i < n; i++ )
   for( j = 0; j < n; j++ )
     a[i][j] = (float)M->Elem[i*n+j];

 for( i = 0; i < n; i++ )
   a[i][i] -= eigVal;


#ifdef GAUSS 
 b = (float**) malloc( n * sizeof(float*) );
 for( i = 0; i < n; i++ )
   b[i] = (float*) malloc( m * sizeof(float) );
 for( i = 0; i < n; i++ )
   for( j = 0; j < m; j++ )
     b[i][j] = x[i];

 debug_a( a, n, n );
 printf("Vector X:\n"); debug_a( b, n, m );
 gaussj(a,n,b,m);
 printf("Solution vector Y:\n"); debug_a( b, n, m );

 for( i = 0; i < n; i++ )
   for( j = 0; j < m; j++ )
     y[i] = b[i][j];

 for( i = 0; i < n; i++ )
 {
   FREE(a[i]); FREE(b[i]);
 }
 FREE(a); FREE(b);
#endif

#ifdef LU_DECOMP
 bVec = (float*) malloc( n * sizeof(float) ); CHKPTR(bVec);
 indx = (int*) malloc( n * sizeof(int) ); CHKPTR(indx);
 copy_FV( x, bVec, n );

 /* debug_a( a, n, n );
 printf(" @@@@@@@@ Input vector X:\n"); debug_vec( bVec, n ); /**/
 ludcmp(a,n,indx,&d);
 lubksb(a,n,indx,bVec);
 /* printf("Solution vector Y:\n"); debug_vec( bVec, n ); /**/

 copy_FV( bVec, y, n );
 FREE( bVec ); FREE( indx );
#endif
}


/*
 * Given a matrix M, an eigenvalue eigVal, calculate the corresponding
 * eigenvector eigVec.
 * Use the method of inverse iteration: see:
 * "Numerical Recipes in C: The Art of Scientific Computing"
 *  published by Cambridge University Press (1988), pp. 396;
 *  Solve the linear equation problem by LU decomposition
*/
#define MIN_INITIAL_GROWTH (1000.0)
#define MAX_ITER 8
#define MAX_L_UPDATE 5
#define MAX_INITIAL_ITER 10
#define MIN_DECREASE (0.01)
#define EPSILON (0.0000001)

#define VERIFY
#define MAX_DIFF_VERIFY (1.0)
#define xxxDEBUG

void calcEigVec( M, eigVal, eigVec, nr )
Matrix *M;
float *eigVal, *eigVec;
int nr;
{
 float lambdaI, lambdaI1;
 float *xi = NULL, *xi1 = NULL, *y = NULL, *y_init_max = NULL;
 float deltaL, di, di1, len_xi, len_y, sum_nom, sum_denom, maxGF, decrease;
 int n, i, nrIter = 0, nrInitialIter = 0, nrLambdaUpdates = 0;
 bool done = FALSE, betterEigVal = FALSE;
#ifdef VERIFY
 float eigenDiff;
 Matrix aux1, aux2, aux3;
 init_Matrix( &aux1 ); init_Matrix( &aux2 ); init_Matrix( &aux3 );
 Copy_Matrix( M, &aux1 );
 aux2.row = M->row; aux2.col = 1; Matrix_Alloc( &aux2 );
#endif

 n = M->row;

 xi = (float*) malloc( n * sizeof(float) ); CHKPTR(xi);
 xi1 = (float*) malloc( n * sizeof(float) ); CHKPTR(xi1);
 y = (float*) malloc( n * sizeof(float) ); CHKPTR(y);
 y_init_max = (float*) malloc( n * sizeof(float) ); CHKPTR(y_init_max);

 lambdaI = *eigVal;

 maxGF = -INFINITY;
 do	/* first case */
 {
   nrInitialIter++;
   randinit( xi, n );
   linearSolve( M, lambdaI, y, xi );
   len_y = vector_len( y, n );
#ifdef DEBUG
   printf("Initial growth factor of y = %f\n", len_y); /**/
#endif
   if( len_y > maxGF )
   {
     maxGF = len_y;
     copy_FV( y, y_init_max, n );
   }
   /* printf("Actual maximum initial growth = %f\n", maxGF ); /**/
 }
 while( len_y < MIN_INITIAL_GROWTH && nrInitialIter < MAX_INITIAL_ITER );
 copy_FV( y_init_max, y, n );

 nrIter = 0;
 di = 0.0;
 for( i = 0; i < n; i++ )
   xi1[i] = y[i] / len_y;

 do
 {
   nrIter++;
   di1 = Euclidian_Distance( xi1, xi, n );
   /* printf("Vector Xi:\n"); debug_vec( xi, n );
   printf("Vector Xi1:\n"); debug_vec( xi1, n );
   printf("di1 = %.30f\n\n", di1 ); /**/

   if( di1 < EPSILON || nrIter >= MAX_ITER || nrLambdaUpdates >= MAX_L_UPDATE )
     done = TRUE;
   else
   {
     decrease = (float) fabs( di1 - di);
     if( decrease < MIN_DECREASE )
     {
       /* update lambda */
       nrLambdaUpdates++;
       sum_nom = 0.0; sum_denom = 0.0;
       for( i = 0; i < n; i++ )
       {
         sum_nom += xi[i] * xi[i];
         sum_denom += xi[i] * y[i];
       }
       deltaL = sum_nom / (float)fabs(sum_denom);
       lambdaI1 = lambdaI + deltaL;
#ifdef DEBUG
       printf("UPDATE deltaL=%.40f\n", deltaL );
#endif
       betterEigVal = TRUE;
       lambdaI = lambdaI1;
       if( deltaL == 0.0 )
         done = TRUE;
       else
         nrIter = 0;	/* start new cycle with new eigenvalue */
     }
     if( ! done )
     {
       copy_FV( xi1, xi, n );
       di = di1;
       /* Solve the set of linear equations : (M-lambda*UnitMatrix)*y = xi  */
       linearSolve( M, lambdaI, y, xi );
       len_y = vector_len( y, n );
       for( i = 0; i < n; i++ )
         xi1[i] = y[i] / len_y;
     }
   }
 }
 while( ! done );

 copy_FV( xi1, eigVec, n );
 if( betterEigVal )
   *eigVal = lambdaI;

#ifdef VERIFY
 for( i = 0; i < n; i++ )
   aux2.Elem[i] = xi1[i];

 Mult_Matrix( M, &aux2, &aux3 );
 ScalarMult_Matrix( 1.0/lambdaI, &aux3 );

 for( i = 0; i < n; i++ )
   y[i] = aux3.Elem[i];
 eigenDiff = Euclidian_Distance( xi1, y, n );
#ifdef DEBUG
 if( eigenDiff > MAX_DIFF_VERIFY )
 {
   printf("Had a big difference: %f\n", eigenDiff );
   printf("Number of iterations: --- %d ---\nVector X:\n", nrIter);
   for( i = 0; i < n; i++ )
     printf("%.30f\n", xi1[i] );
   printf("Vector should be X:\n"); 
   for( i = 0; i < n; i++ )
     printf("%.30f\n", aux3.Elem[i] );
 }
 else
   printf("\n --------------  V E R I F I E D\n");
 printf(" REASON:\n");
 printf("di1=%f  decrease=%f  nrIter=%d  deltaL=%.20f\n  nrLambdaUpdates=%d\n",
	di1, decrease, nrIter, deltaL, nrLambdaUpdates );
 if( di1 < EPSILON ) printf("di1 < EPSILON\n");
 if( nrIter >= MAX_ITER ) printf("nrIter >= MAX_ITER\n");
 if( deltaL == 0.0 ) printf("deltaL == 0.0\n");
 if( nrLambdaUpdates >= MAX_L_UPDATE )printf("LambdaUpdates >= MAX_L_UPDATE\n");
 printf("-------------------------------------------------------\n");
#endif
 if( eigenDiff > MAX_DIFF_VERIFY )
   { printf("Had trouble calculating eigenvector nr. %d\n", nr+1 ); }
 Matrix_Free( &aux1 ); Matrix_Free( &aux2 ); Matrix_Free( &aux3 );
#endif

 FREE(xi); FREE(xi1); FREE(y); FREE(y_init_max);
}