/***********************************************************************
*    Eigenvalues of a real nonsymmetric square matrix using the HQR    *
*    algoritm.                                                         *
*                                                                      *
*                                C++ version by J-P Moreau, Paris      *
*                                        (www.jpmoreau.fr)             *
* -------------------------------------------------------------------- *
* SAMPLE RUN:                                                          *
*                                                                      *
* Input matrix (symmetric for comparison with Jacobi):                 *
*                                                                      *
*        1   2   3  -7     12                                          *
*        2   4   7   3     -1                                          *
*        3   7  10   8      4                                          *
*       -7   3   8  -0.75  -9                                          *
*       12  -1   4  -9     10                                          *
*                                                                      *
*                                                                      *
* Output file (test_hqr.lst):                                          *
*                                                                      *
* =============================================================        *
*   TESTING THE QR ALGORITHM TO FIND THE REAL OR COMPLEX               *
*     EIGENVALUES OF A REAL SQUARE NON SYMMETRIC MATRIX                *
*            IN DOUBLE PRECISION (16 digits)                           *
* =============================================================        *
*                                                                      *
* GIVEN MATRIX A:                                                      *
*                                                                      *
*    1.00    2.00    3.00   -7.00   12.00                              *
*    2.00    4.00    7.00    3.00   -1.00                              *
*    3.00    7.00   10.00    8.00    4.00                              *
*   -7.00    3.00    8.00   -0.75   -9.00                              *
*   12.00   -1.00    4.00   -9.00   10.00                              *
*                                                                      *
* MATRIX A AFTER BALANC:                                               *
*                                                                      *
*    1.0000    2.0000    3.0000   -7.0000   12.0000                    *
*    2.0000    4.0000    7.0000    3.0000   -1.0000                    *
*    3.0000    7.0000   10.0000    8.0000    4.0000                    *
*   -7.0000    3.0000    8.0000   -0.7500   -9.0000                    *
*   12.0000   -1.0000    4.0000   -9.0000   10.0000                    *
*                                                                      *
* ElmHes done.                                                         *
*                                                                      *
* MATRIX A IN HESSENBERG FORM:                                         *
*                                                                      *
*    1.000000   17.166667   -9.738494    2.674367    3.000000          *
*   12.000000   16.083333   -9.322176   -0.100844    4.000000          *
*    0.250000    3.319444  -11.372036    4.739487   10.333333          *
*   -0.583333   -0.307531  -11.556061    9.893547   15.715481          *
*    0.166667   -0.907950    0.224789    8.506681    8.645156          *
*                                                                      *
*                                                                      *
* HQR done.                                                            *
*                                                                      *
* EIGEN VALUES, REAL AND IMAGINARY PARTS:                              *
*                                                                      *
*  -10.486545    0.000000                                              *
*   -7.774580    0.000000                                              *
*   23.755955    0.000000                                              *
*   18.291821    0.000000                                              *
*    0.463350    0.000000                                              *
*                                                                      *
* End of file Test_hqr.lst.                                            *
*                                                                      *
***********************************************************************/
// Note: index zero is used (for hqr only) in h,niter,wr,wi  
#include <stdio.h>
#include <math.h>

#define ZERO  0
#define ONE   1
#define MACH_EPS  1e-16

#define SIZE  25

typedef double MAT[SIZE][SIZE];

MAT  A; //input matrix
double  D[SIZE],wr[SIZE],wi[SIZE];
int     niter[SIZE];

int n;   //order of square matrix A
int i,j,low,hi,rc;

FILE *fp_out;


//--------------------------------------------------------------
void Exc(int j,int k,int l,int m)  {  // used by Balanc
  int ii; double f;
  D[m] = (double) j;
  if (j != m) {
    for (ii = 1; ii<k+1; ii++) {
      f = A[ii][j];
      A[ii][j] = A[ii][m];
      A[ii][m] = f;
    }
    for (ii = l; ii<n+1; ii++) {
      f = A[j][ii];
      A[j][ii] = A[m][ii];
      A[m][ii] = f;
    }
  }
}
//--------------------------------------------------------------

/**********************************************************************
* Balanc:   From Pascal version translated from ALGOL by J-P Dumont   *
*                                    C++ version by J-P Moreau        *
* ------------------------------------------------------------------- *
* Ref.: "Handbook for automatic computation Volume II Linear Algebra  *
*        J.H. Wilkinson et C.Reinsch, Springer Verlag, 1971"          *
* ------------------------------------------------------------------- *
*   This procedure balances the elements of a non symmetric square    *
* matrix. Given a matrix n x n stored in a table of size n x n, that  *
* matrix is replaced by a balanced matrix having the same eigenvalues *
* A symmetric matrix is not affected by this procedure.               *
* ------------------------------------------------------------------- *
* Inputs:                                                             *
*       n       Order of non symmetric square matrix A (n x n)        *
*       A       Table n*n storing the  elements of A                  *
* ------------------------------------------------------------------- *
* Outputs:                                                            *
*       A       The original A matrix is replaced by the balanced     *
*               matrix. However, it is possible to recover exactly    *
*               the original matrix.                                  *
*  low,hi       Two integers such as A[i][j] = 0 if                   *
*               (1) i > j  AND                                        *
*               (2) j =1,...,Low-1 OU i = hi+1,...,n                  *
*       D       Vector [1..n] containing enough information to trace  *
*               the done permutations and the used scale factors.     *
**********************************************************************/
void Balanc()  {
//LABELS: Iteration(1100)][L1(1200)][L2(1300)][L3(1400)][L4(1500)

  int i,j,k,l,m;
  double b,b2,c,f,g,r,s;
  int noconv;  // boolean 0 or 1

  b = 2;       // Floating point basis of used CPU
  b2 = b*b;
  l = 1;
  k = n;
  //Search lines isolating an eigenvalue and shift downwards
L1: 
  for (j = k; j>0; j--) {
    r = ZERO;
    for (i = 1; i<j; i++)  r = r + fabs(A[j][i]);
    for (i = j+1; i<k+1; i++)  r = r + fabs(A[j][i]);
    if (r == ZERO) {
      m=k; Exc(j,k,l,m);
      k--;
      goto L1;
    }
  }
  //Search columns isolating an eigenvalue and shift to the left
L2:
  for (j = l; j<k+1; j++) {
    c = ZERO;
    for (i = l; i<j; i++)  c = c + fabs(A[i][j]);
    for (i = j+1; i<k+1; i++)  c = c + fabs(A[i][j]);
    if (c == ZERO) {
      m=l; Exc(j,k,l,m);
      l++;
      goto L2;
    }
  }
  //Now balance submatrix from ligne l to line k
  low = l;
  hi  = k;
  for (i = 1; i<k+1; i++)  D[i] = ONE;
Iterations:  noconv = 0;
  for (i = l; i<k+1; i++) {
    c = ZERO;
    r = c;
    for (j = l; j<i; j++) {
      c = c + fabs(A[j][i]);
      r = r + fabs(A[i][j]);
    }
    for (j = i+1; j<k+1; j++) {
      c = c + fabs(A[j][i]);
      r = r + fabs(A[i][j]);
    }
    g = r/b;
    f = ONE;
    s = c+r;
L3: if (c < g) {
      f = f*b;
      c = c*b2;
      goto L3;
    }
    g = r*b;
L4: if (c>=g) {
      f = f/b;
      c = c/b2;
      goto L4;
    }

    // Balancing the elements of submatrix
    if ((c+r)/f < 0.95*s) {
      g = ONE/f;
      D[i] *= f;
      noconv = 1;
      for (j = l; j<n+1; j++)  A[i][j] *= g;
      for (j = 1; j<k+1; j++)  A[j][i] *= f;
    }
  } 
  if (noconv==1) goto Iterations;

} //Balanc()

/*
{-------------------------Documentation-------------------------------}
{               VAR A: Square_Matrix; n: INTEGER);                    }
{ Reduction of a non symmetric real matrix to Hessenberg form by the  }
{ elimination method. Matrix A (n x n), stored in a table of size     }
{ Maxc x Maxc, is replaced by a superior Hessenberg matrix having the }
{ same eigenvalues. It is recommanded to call previously the Balanc   }
{ procedure. In output,the Hessenberg matrix has elements A[i][j] with}
{ i<=j+1. The elements for i>j+1, that in theory equal zero, are      }
{ actually filled with random (not used) values.                      }
{---------------------------------------------------------------------}
{ From Pascal version by J.P.DUMONT - Extracted from reference:       }
{       "William H.PRESS, Brian P.FLANNERY, Saul A.TEUKOLSKY AND      }
{        William T.VETTERLING                                         }
{              N U M E R I C A L  R E C I P E S                       }
{              The Art OF Scientific Computing                        }
{              CAMBRIDGE UNIVERSITY PRESS 1987" [BIBLI 08].           }
{                                         C++ version by J-P Moreau   }
{/////////////////////////////////////////////////////////////////////}
*/
void ElmHes()  {
  int i,j,m;
  double x,y;
  if  (n > 2) {
    for (m = 2; m<n; m++) {
      x = ZERO;
      i = m;
      for (j= m; j<n+1; j++) {
        if (fabs(A[j][m-1]) > fabs(x)) {
          x = A[j][m-1];
          i = j;
        }
      } // j loop
      if (i != m) {
        for (j = m-1; j<n+1; j++) {
          y = A[i][j];
          A[i][j] = A[m][j];
          A[m][j] = y;
        }
        for (j = 1; j<n+1; j++) {
          y = A[j][i];
          A[j][i] = A[j][m];
          A[j][m] = y;
        }
      } //if i != m
      if (x != ZERO) {
        for (i = m+1; i<n+1; i++) {
          y = A[i][m-1];
          if (y != ZERO)  {
            y /= x;
            A[i][m-1] = y;
            for (j = m; j<n+1; j++)  A[i][j] -= y*A[m][j];
            for (j = 1; j<n+1; j++)  A[j][m] += y*A[j][i];
          } // if y
		} // i loop
      } // if x
    } // m loop
  } //if n>2
  //Put lower triangle to zero (optional)
  for (i=1; i<n+1; i++)
    for (j=1; j<n+1; j++)
	   if (j<i-1) A[i][j] = ZERO;
} // ElmHes()


static int hqr           // compute eigenvalues ......................*/
               ( int     n,           /* Dimension of matrix .........*/
                 int     low,         /* first nonzero row ...........*/
                 int     high,        /* last nonzero row ............*/
                 MAT     h,           /* Hessenberg matrix ...........*/
                 double  wr[],        /* Real parts of eigenvalues....*/
                 double  wi[],        /* Imaginary parts of evalues ..*/
                 int     *cnt         /* Iteration counter ...........*/
               ) 
/*====================================================================*
 *                                                                    *
 *  hqr computes the eigenvalues of an n * n upper Hessenberg matrix  *
 *                                                                    *
 *====================================================================*
 *                                                                    *
 *   Input parameters:                                                *
 *   ================                                                 *
 *      n        int n;  ( n > 0 )                                    *
 *               Dimension of  matrix h     ,                         *
 *               length of the double parts vector  wr and of the     *
 *               imaginary parts vector  wi of the eigenvalues.       *
 *      low      int low;                                             *
 *      high     int high; see  balance                               *
 *      h        upper  Hessenberg matrix (output of ElmHes)          *
 *                                                                    *
 *   Output parameters:                                               *
 *   ==================                                               *
 *      wr       double   wr[n];                                      *
 *               double part of the n eigenvalues.                    *
 *      wi       double   wi[n];                                      *
 *               Imaginary parts of the eigenvalues                   *
 *      cnt      int cnt[n];                                          *
 *               vector of iterations used for each eigenvalue.       *
 *               For a complex conjugate eigenvalue pair the second   *
 *               entry is negative.                                   *
 *                                                                    *
 *   Return value :                                                   *
 *   =============                                                    *
 *      =   0    all ok                                               *
 *      = 4xx    Iteration maximum exceeded when computing evalue xx  *
 *      =  99    zero  matrix                                         *
 *                                                                    *
 *====================================================================*
 * Reference: "Numerical Algorithms with C By G. Engeln-Mueller and   *
 *             F. Uhlig, Springer-Verlag, 1996" [BIBLI 11].           *
 *====================================================================*/
{
  int  i,j,MAXIT=30,vec=0;
  int  na, en, iter, k, l, m;
  double p = ZERO, q = ZERO, r = ZERO, s, t, w, x, y, z;

  // put h with index from zero
  for (i=1; i<n+1; i++)
    for (j=1; j<n+1; j++)   
      h[i-1][j-1]=h[i][j];

  for (i = 0; i < n; i++)
    if (i < low || i > high)
    {
      wr[i] = h[i][i];
      wi[i] = ZERO;
      cnt[i] = 0;
    }

  en = high;
  t = ZERO;

  while (en >= low)
  {
    iter = 0;
    na = en - 1;

    for ( ; ; )
    {
      for (l = en; l > low; l--)             /* search for small      */
        if ( fabs(h[l][l-1]) <=               /* subdiagonal element   */
              MACH_EPS * (fabs(h[l-1][l-1]) + fabs(h[l][l])) )  break;

      x = h[en][en];
      if (l == en)                            /* found one evalue     */
      {
        wr[en] = h[en][en] = x + t;
        wi[en] = ZERO;
        cnt[en] = iter;
        en--;
        break;
      }

      y = h[na][na];
      w = h[en][na] * h[na][en];

      if (l == na)                            /* found two evalues    */
      {
        p = (y - x) * 0.5;
        q = p * p + w;
        z = sqrt (fabs (q));
        x = h[en][en] = x + t;
        h[na][na] = y + t;
        cnt[en] = -iter;
        cnt[na] = iter;
        if (q >= ZERO)
        {                                     /* double eigenvalues     */
          z = (p < ZERO) ? (p - z) : (p + z);
          wr[na] = x + z;
          wr[en] = s = x - w / z;
          wi[na] = wi[en] = ZERO;
          x = h[en][na];
          r = sqrt (x * x + z * z);
        }  /* end if (q >= ZERO) */
        else                                  /* pair of complex      */
        {                                     /* conjugate evalues    */
          wr[na] = wr[en] = x + p;
          wi[na] =   z;
          wi[en] = - z;
        }

        en -= 2;
        break;
      }  /* end if (l == na) */

      if (iter >= MAXIT)
      {
        cnt[en] = MAXIT + 1;
        return (en);                         /* MAXIT Iterations     */
      }

      if ( (iter != 0) && (iter % 10 == 0) )
      {
        t += x;
        for (i = low; i <= en; i++) h[i][i] -= x;
        s = fabs (h[en][na]) + fabs (h[na][en-2]);
        x = y = (double)0.75 * s;
        w = - (double)0.4375 * s * s;
      }

      iter ++;

      for (m = en - 2; m >= l; m--)
      {
        z = h[m][m];
        r = x - z;
        s = y - z;
        p = ( r * s - w ) / h[m+1][m] + h[m][m+1];
        q = h[m + 1][m + 1] - z - r - s;
        r = h[m + 2][m + 1];
        s = fabs (p) + fabs (q) + fabs (r);
        p /= s;
        q /= s;
        r /= s;
        if (m == l) break;
        if ( fabs (h[m][m-1]) * (fabs (q) + fabs (r)) <=
                 MACH_EPS * fabs (p)
                 * ( fabs (h[m-1][m-1]) + fabs (z) + fabs (h[m+1][m+1])) )
          break;
      }

      for (i = m + 2; i <= en; i++) h[i][i-2] = ZERO;
      for (i = m + 3; i <= en; i++) h[i][i-3] = ZERO;

      for (k = m; k <= na; k++)
      {
        if (k != m)             /* double  QR step, for rows l to en  */
        {                       /* and columns m to en                */
          p = h[k][k-1];
          q = h[k+1][k-1];
          r = (k != na) ? h[k+2][k-1] : ZERO;
          x = fabs (p) + fabs (q) + fabs (r);
          if (x == ZERO) continue;                  /*  next k        */
          p /= x;
          q /= x;
          r /= x;
        }
        s = sqrt (p * p + q * q + r * r);
        if (p < ZERO) s = -s;

        if (k != m) h[k][k-1] = -s * x;
          else if (l != m)
                 h[k][k-1] = -h[k][k-1];
        p += s;
        x = p / s;
        y = q / s;
        z = r / s;
        q /= p;
        r /= p;

        for (j = k; j < n; j++)               /* modify rows          */
        {
          p = h[k][j] + q * h[k+1][j];
          if (k != na)
          {
            p += r * h[k+2][j];
            h[k+2][j] -= p * z;
          }
          h[k+1][j] -= p * y;
          h[k][j]   -= p * x;
        }

        j = (k + 3 < en) ? (k + 3) : en;
        for (i = 0; i <= j; i++)              /* modify columns       */
        {
          p = x * h[i][k] + y * h[i][k+1];
          if (k != na)
          {
            p += z * h[i][k+2];
            h[i][k+2] -= p * r;
          }
          h[i][k+1] -= p * q;
          h[i][k]   -= p;
        }
      }    /* end k          */
    }    /* end for ( ; ;) */
  }    /* while (en >= low)                      All evalues found    */
  return (0);
} //hqr()


void main()  {
  
  n    = 5;    //order of square matrix

  // test matrix for HQR method
  A[1][1]= 1.0; A[1][2]= 2.0; A[1][3]= 3.0; A[1][4]=-7.0 ; A[1][5]=12.0;
  A[2][1]= 2.0; A[2][2]= 4.0; A[2][3]= 7.0; A[2][4]= 3.0 ; A[2][5]=-1.0;
  A[3][1]= 3.0; A[3][2]= 7.0; A[3][3]=10.0; A[3][4]= 8.0 ; A[3][5]= 4.0;
  A[4][1]=-7.0; A[4][2]= 3.0; A[4][3]= 8.0; A[4][4]=-0.75; A[4][5]=-9.0;
  A[5][1]=12.0; A[5][2]=-1.0; A[5][3]= 4.0; A[5][4]=-9.0 ; A[5][5]=10.0;

  //open output file
  fp_out = fopen("test_hqr.lst","w");

  fprintf(fp_out,"\n");
  fprintf(fp_out," ===========================================================\n");
  fprintf(fp_out,"   TESTING THE QR ALGORITHM TO FIND THE REAL OR COMPLEX\n");
  fprintf(fp_out,"     EIGENVALUES OF A REAL SQUARE NON SYMMETRIC MATRIX\n");
  fprintf(fp_out,"            IN DOUBLE PRECISION (16 digits)\n");
  fprintf(fp_out," ===========================================================\n\n");

  fprintf(fp_out," GIVEN MATRIX A:\n\n");
  for (i = 1; i<n+1; i++) {
    for (j = 1; j<n+1; j++)
      fprintf(fp_out," %10.6f",A[i][j]);
    fprintf(fp_out,"\n");
  }
  fprintf(fp_out,"\n");

  low = 1;
  hi  = n;

  Balanc();

  printf("\n Balanc done.\n");
  fprintf(fp_out," MATRIX A AFTER BALANC:\n\n");
  for (i = 1; i<n+1; i++) {
    for (j = 1; j<n+1; j++)
    fprintf(fp_out," %10.6f",A[i][j]);
    fprintf(fp_out,"\n");
  }
  fprintf(fp_out,"\n");  

  ElmHes();

  printf("\n ElmHes done.\n");

  fprintf(fp_out," MATRIX A IN HESSENBERG FORM:\n\n");
  for (i = 1; i<n+1; i++) {
    for (j = 1; j<n+1; j++)
    fprintf(fp_out," %10.6f",A[i][j]);
    fprintf(fp_out,"\n");
  }
  fprintf(fp_out,"\n");  

  // call QR algorithm
  rc = hqr(n,low-1,hi-1,A,wr,wi,niter);

  printf(" \n HQR done: rc=%d\n",rc);
  fprintf(fp_out," EIGEN VALUES, double AND IMAGINARY PARTS:\n\n");

  for (i = 0; i<n; i++)
    fprintf(fp_out," %10.6f  %10.6f\n",wr[i],wi[i]);
  
  fprintf(fp_out,"\n End of file Test_hqr.lst.\n");
  fclose(fp_out);
  
  printf("\n Results in file test_hqr.lst.\n\n");

}

//End of file Test_hqr.cpp