/*
   HEigensystem.c
   diagonalization of a Hermitian n-by-n matrix using the Jacobi algorithm
   code adapted from the "Handbook" routines for complex A
   (Wilkinson, Reinsch: Handbook for Automatic Computation, p. 202)
   this file is part of the Diag library
   last modified 20 Aug 15 th
*/

#include "diag-c.h"


/*
   HEigensystem diagonalizes a Hermitian n-by-n matrix.
   Input:	n, A = n-by-n matrix, Hermitian
		(only the upper triangle of A needs to be filled),
   Output:	d = vector of eigenvalues,
		U = transformation matrix, unitary (U U^+ = 1),
   these fulfill
	d = U A U^+,  A = U^+ d U,  U A = d U  (UCOLS=0),
	d = U^+ A U,  A = U d U^+,  A U = U d  (UCOLS=1).
*/

void HEigensystem(cint n, ComplexType *A, cint ldA,
  RealType *d, ComplexType *U, cint ldU, cint sort)
{
  int p, q;
  cRealType red = .04/(n*n*n*n);
  RealType ev[n][2];

  for( p = 0; p < n; ++p ) {
    ev[p][0] = 0;
    ev[p][1] = d[p] = Re(A(p,p));
  }

  for( p = 0; p < n; ++p ) {
    memset(&U(p,0), 0, n*sizeof(ComplexType));
    U(p,p) = 1;
  }

  for( nsweeps = 1; nsweeps <= 50; ++nsweeps ) {
    RealType thresh = 0;
    for( q = 1; q < n; ++q )
      for( p = 0; p < q; ++p )
        thresh += Sq(A(p,q));
    if( !(thresh > SYM_EPS) ) goto done;

    thresh = (nsweeps < 4) ? thresh*red : 0;

    for( q = 1; q < n; ++q )
      for( p = 0; p < q; ++p ) {
        cComplexType Apq = A(p,q);
        cRealType off = Sq(Apq);
        if( nsweeps > 4 && off < SYM_EPS*(sq(ev[p][1]) + sq(ev[q][1])) )
          A(p,q) = 0;
        else if( off > thresh ) {
          RealType delta, t, s, invc;
          int j;

          t = .5*(ev[p][1] - ev[q][1]);
          t = 1/(t + sign(sqrt(t*t + off), t));
          delta = off*t;
          ev[p][1] = d[p] + (ev[p][0] += delta);
          ev[q][1] = d[q] + (ev[q][0] -= delta);
          invc = sqrt(delta*t + 1);
          s = t/invc;
          t = delta/(invc + 1);

          for( j = 0; j < p; ++j ) {
            cComplexType x = A(j,p);
            cComplexType y = A(j,q);
            A(j,p) = x + s*(Conjugate(Apq)*y - t*x);
            A(j,q) = y - s*(Apq*x + t*y);
          }

          for( j = p + 1; j < q; ++j ) {
            cComplexType x = A(p,j);
            cComplexType y = A(j,q);
            A(p,j) = x + s*(Apq*Conjugate(y) - t*x);
            A(j,q) = y - s*(Apq*Conjugate(x) + t*y);
          }

          for( j = q + 1; j < n; ++j ) {
            cComplexType x = A(p,j);
            cComplexType y = A(q,j);
            A(p,j) = x + s*(Apq*y - t*x);
            A(q,j) = y - s*(Conjugate(Apq)*x + t*y);
          }

          A(p,q) = 0;

          for( j = 0; j < n; ++j ) {
            cComplexType x = UL(p,j);
            cComplexType y = UL(q,j);
#if UCOLS
            UL(p,j) = x + s*(Conjugate(Apq)*y - t*x);
            UL(q,j) = y - s*(Apq*x + t*y);
#else
            UL(p,j) = x + s*(Apq*y - t*x);
            UL(q,j) = y - s*(Conjugate(Apq)*x + t*y);
#endif
          }
        }
      }

    for( p = 0; p < n; ++p ) {
      ev[p][0] = 0;
      d[p] = ev[p][1];
    }
  }

  fputs("Bad convergence in HEigensystem\n", stderr);

done:
  if( sort == 0 ) return;

/* sort the eigenvalues */

  for( p = 0; p < n - 1; ++p ) {
    int j = p;
    RealType t = d[p];
    for( q = p + 1; q < n; ++q )
      if( sort*(t - d[q]) > 0 ) t = d[j = q];
    if( j == p ) continue;
    d[j] = d[p];
    d[p] = t;
    for( q = 0; q < n; ++q ) {
      cComplexType x = UL(p,q);
      UL(p,q) = UL(j,q);
      UL(j,q) = x;
    }
  }
}