/* file: makeev.c	G. Moody	12 July 1988
			Last revised:	21 May 2009
-------------------------------------------------------------------------------
makeev: Derive eigenvectors of covariance matrix
Copyright (C) 1988-2009 George B. Moody

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.

You may contact the author by e-mail (george@mit.edu) or postal mail
(MIT Room E25-505A, Cambridge, MA 02139 USA).  For updates to this software,
please visit PhysioNet (http://physionet.org/).
_______________________________________________________________________________

See Press et al., Numerical Recipes, Cambridge University Press, for
details of the algorithm used for the Jacobi rotations.

Thanks to Valentin Reuter for pointing out an error in output of the
eigenvalues, which has been corrected below.
*/

#include <stdio.h>
#include <math.h>
#include "defs.h"

#define V(I,J)  v[(I)*PVDIM + (J)]

main()
{
  static double a[PVDIM][PVDIM];
  static double d[PVDIM], v[PVDIM*PVDIM];
  double *ap, ratio; 
  int i, j, k, kk, l, ll, jacobi();
  char buf[10];
  void eigsrt();

  /* Read the covariance matrix from the standard input. */
  for (j = 0, ap = &a[0][0]; j < PVDIM*PVDIM; j++)
    scanf("%lf", ap++);
  
  /* Find the eigenvectors by Jacobi rotations. */
  if (jacobi(a, d, v) < 0)
    exit(1);
  
  /* Sort them by eigenvalue. */
  eigsrt(d, v);
  
  /* Print the eigenvalues and eigenvectors. */
  for (j = 0; j < PVDIM; j++) {
    printf("%g ", d[j]);	/* eigenvalue */
    for (k = 0; k < PVDIM; k++)
      printf("%g%c", V(k, j), (k == PVDIM-1) ? '\n' : ' ');
  }
  exit(0);
}

#define A(I,J)  a[(I)*PVDIM + (J)]
#define IMAX    50      /* maximum number of iterations */

jacobi(double *a,     /* input matrix (PVDIM x PVDIM elements) */
       double *d,     /* (output) eigenvalues of *a (PVDIM elements) */
       double *v)     /* (output) eigenvectors of *a (PVDIMxPVDIM elements) */
{
  double *b, *z;
  int i, ip, iq, j, nrot;
  double c, g, h, s, sm, t, tau, theta, thresh;
  char *malloc();
  
  /* allocate workspace */
  if ((b = (double *)malloc(PVDIM*sizeof(double))) == NULL ||
      (z = (double *)malloc(PVDIM*sizeof(double))) == NULL) {
    fprintf(stderr, "jacobi: insufficient memory\n");
    return (-1);
  }
  
  for (ip = 0; ip < PVDIM; ip++) {
    /* initialize V to the identity matrix */
    for (iq = 0; iq < PVDIM; iq++)
      V(ip, iq) = 0.;
    V(ip, ip) = 1.;
    /* initialize b and d to the diagonal of A */
    b[ip] = d[ip] = A(ip, ip);
    z[ip] = 0.;
  }
  nrot = 0;
  
  for (i = 0; i < IMAX; i++) {
    
    /* sum off-diagonal elements */
    for (sm = 0., ip = 0; ip < PVDIM-1; ip++)
      for (iq = ip+1; iq < PVDIM; iq++)
	sm += fabs(A(ip, iq));
    if (sm == 0.)
      return (nrot); /* the normal return, which relies on quadratic
			convergence to machine underflow */
    
    if (i < 3)
      thresh = 0.2*sm/(PVDIM*PVDIM);   /* ... on the first 3 sweeps */
    else
      thresh = 0.;             /* ... thereafter */
    
    for (ip = 0; ip < PVDIM-1; ip++) {
      for (iq = ip+1; iq < PVDIM; iq++) {
	g = 100.*fabs(A(ip, iq));
	/* after 4 sweeps, skip the rotation if the
	   off-diagonal element is small */
	if (i > 3 && fabs(d[ip])+g == fabs(d[ip]) &&
	    fabs(d[iq])+g == fabs(d[iq]))
	  A(ip, iq) = 0.;
	else if (fabs(A(ip, iq)) > thresh) {
	  h = d[iq] - d[ip];
	  if (fabs(h)+g == fabs(h))
	    t = A(ip, iq)/h;        /* t = 1/(2*theta) */
	  else {
	    theta = 0.5*h/A(ip, iq);
	    t = 1./(fabs(theta)+sqrt(1.+theta*theta));
	    if (theta < 0.)
	      t = -t;
	  }
	  c = 1./sqrt(1.+t*t);
	  s = t*c;
	  tau = s/(1.+c);
	  h = t*A(ip, iq);
	  z[ip] -= h;
	  z[iq] += h;
	  d[ip] -= h;
	  d[iq] += h;
	  A(ip, iq) = 0.;
	  
	  /* case of rotations 0 <= j < p */
	  for (j = 0; j < ip; j++) {
	    g = A(j, ip);
	    h = A(j, iq);
	    A(j, ip) = g - s*(h + g*tau);
	    A(j, iq) = h + s*(g - h*tau);
	  }
	  
	  /* case of rotations p < j < q */
	  for (j = ip+1; j < iq; j++) {
	    g = A(ip, j);
	    h = A(j, iq);
	    A(ip, j) = g - s*(h + g*tau);
	    A(j, iq) = h + s*(g - h*tau);
	  }
	  
	  /* case of rotations q < j < n */
	  for (j = iq+1; j < PVDIM; j++) {
	    g = A(ip, j);
	    h = A(iq, j);
	    A(ip, j) = g - s*(h + g*tau);
	    A(iq, j) = h + s*(g - h*tau);
	  }
	  
	  for (j = 0; j < PVDIM; j++) {
	    g = V(j, ip);
	    h = V(j, iq);
	    V(j, ip) = g - s*(h + g*tau);
	    V(j, iq) = h + s*(g - h*tau);
	  }
	  
	  nrot++;
	}
      }
    }
    for (ip = 0; ip < PVDIM; ip++) {
      d[ip] = b[ip] += z[ip];
      z[ip] = 0.;
    }
  }
  fprintf(stderr,
	  "Cannot derive eigenvectors -- no convergence after %d rotations\n",
	  IMAX);
  return (-2);
}

void eigsrt(double *d,	/* array of eigenvalues (PVDIM elements) */
	    double *v)	/* array of eigenvectors (PVDIM x PVDIM elements) */
{
  int i, j, k;
  double p;
  
  for (i = 0; i < PVDIM-1; i++) {
    k = i;
    p = d[i];
    for (j = i+1; j < PVDIM; j++)
      if (d[j] >= p) {
	k = j;
	p = d[j];
      }
    if (k != i) {
      d[k] = d[i];
      d[i] = p;
      for (j = 0; j < PVDIM; j++) {
	p = V(j, i);
	V(j, i) = V(j, k);
	V(j, k) = p;
      }
    }
  }
}