/*
   CEigensystem.c
   diagonalization of a complex 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 24 Aug 15 th
*/

#include "diag-c.h"


/*
   CEigensystem diagonalizes a general complex n-by-n matrix.
   Input:	n, A = n-by-n matrix,
   Output:	d = vector of eigenvalues,
		U = transformation matrix,
   these fulfill
	d = U A U^-1,  A = U d U^-1,  U A = d U  (UCOLS=0),
	d = U^-1 A U,  A = U^-1 d U,  A U = U d  (UCOLS=1).
*/

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

  for( p = 0; p < n; ++p ) {
    ev[p][0] = 0;
    ev[p][1] = d[p] = 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)) + Sq(A(q,p));
    if( !(thresh > 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);
        cComplexType Aqp = A(q,p);
        cRealType off = Sq(Apq) + Sq(Aqp);
        if( nsweeps > 4 && off < EPS*(Sq(ev[p][1]) + Sq(ev[q][1])) ) {
          A(p,q) = 0;
          A(q,p) = 0;
        }
        else if( off > thresh ) {
          ComplexType delta, x, y, t, s, invc, sx, sy, tx, ty;
          int j;

          delta = Apq*Aqp;
          x = .5*(ev[p][1] - ev[q][1]);
          y = Sqrt(x*x + delta);
          if( Sq(t = x - y) < Sq(s = x + y) ) t = s;

          t = 1/t;
          delta *= 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 /= invc + 1;
          sx = s*Apq;
          ty = t*Apq;
          sy = s*Aqp;
          tx = t*Aqp;

          for( j = 0; j < n; ++j ) {
            ComplexType x = A(j,p);
            ComplexType y = A(j,q);
            A(j,p) = x + sy*(y - ty*x);
            A(j,q) = y - sx*(x + tx*y);
            x = A(p,j);
            y = A(q,j);
            A(p,j) = x + sx*(y - tx*x);
            A(q,j) = y - sy*(x + ty*y);
          }

          A(p,q) = 0;
          A(q,p) = 0;

          for( j = 0; j < n; ++j ) {
            cComplexType x = UL(p,j);
            cComplexType y = UL(q,j);
#if UCOLS
            UL(p,j) = x + sy*(y - ty*x);
            UL(q,j) = y - sx*(x + tx*y);
#else
            UL(p,j) = x + sx*(y - tx*x);
            UL(q,j) = y - sy*(x + ty*y);
#endif
          }
        }
      }

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

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

done:

/* normalize the eigenvectors */
  for( p = 0; p < n; ++p ) {
    RealType norm = 0;
    for( q = 0; q < n; ++q ) norm += Sq(UL(p,q));
    norm = 1/sqrt(norm);
    for( q = 0; q < n; ++q ) UL(p,q) *= norm;
  }

  if( sort == 0 ) return;

/* sort the eigenvalues by their real part */
  for( p = 0; p < n - 1; ++p ) {
    int j = p;
    ComplexType t = d[p];
    for( q = p + 1; q < n; ++q )
      if( sort*(Re(t) - Re(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;
    }
  }
}