//$$jacobi.cxx                           jacobi eigenvalue analysis

// Copyright (C) 1991: R B Davies and DSIR

#define WANT_MATH

#include "include.hxx"
#include "newmat.hxx"
#include "precisio.hxx"
#include "newmatrm.hxx"



void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A,
   Matrix& V, BOOL eivec)
{
   int n = X.Nrows(); DiagonalMatrix B(n), Z(n); D.ReDimension(n); A = X.c();
   if (eivec) { V.ReDimension(n,n); D = 1.0; V = D; }
   B << A; D = B; Z = 0.0; A.Inject(Z);
   for (int i=1; i<=50; i++)
   {
      real sm=0.0; real* a = A.Store(); int p = A.Storage();
      while (p--) sm += fabs(*a++);            // have previously zeroed diags
      if (sm==0.0) return;
      real tresh = (i<4) ? 0.2 * sm / square(n) : 0.0; a = A.Store();
      for (p = 0; p < n; p++)
      {
	 real* ap1 = a + (p*(p+1))/2;
         real& zp = Z.element(p); real& dp = D.element(p);
	 for (int q = p+1; q < n; q++)
	 {
	    real* ap = ap1; real* aq = a + (q*(q+1))/2;
            real& zq = Z.element(q); real& dq = D.element(q);
            real& apq = A.element(q,p);
            real g = 100 * fabs(apq); real adp = fabs(dp); real adq = fabs(dq);
	    if (i>4 && adp+g==adp && adq+g==adq) apq = 0.0;
	    else if (fabs(apq) > tresh)
	    {
	       real t; real h = dq - dp; real ah = fabs(h);
	       if (ah+g==ah) t = apq / h;
	       else
	       {
		  real theta = 0.5 * h / apq;
		  t = 1.0 / ( fabs(theta) + sqrt(1.0 + square(theta)) );
		  if (theta<0.0) t = -t;
	       }
	       real c = 1.0 / sqrt(1.0 + square(t)); real s = t * c;
	       real tau = s / (1.0 + c); h = t * apq;
               zp -= h; zq += h; dp -= h; dq += h; apq = 0.0;
	       int j = p;
	       while (j--)
	       {
		  g = *ap; h = *aq;
		  *ap++ = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
	       }
	       int ip = p+1; j = q-ip; ap += ip++; aq++;
	       while (j--)
	       {
		  g = *ap; h = *aq;
                  *ap = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
		  ap += ip++;
	       }
	       int iq = q+1; j = n-iq; ap += ip++; aq += iq++;
	       while (j--)
	       {
		  g = *ap; h = *aq;
		  *ap = g-s*(h+g*tau); *aq = h+s*(g-h*tau);
		  ap += ip++; aq += iq++;
	       }
	       if (eivec)
	       {
                  RectMatrixCol VP(V,p); RectMatrixCol VQ(V,q);
                  Rotate(VP, VQ, tau, s);
	       }
	    }
	 }
      }
      B = B + Z; D = B; Z = 0.0;
   }
}

void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D)
{ SymmetricMatrix A; Matrix V; Jacobi(X,D,A,V,FALSE); }

void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A)
{ Matrix V; Jacobi(X,D,A,V,FALSE); }

void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, Matrix& V)
{ SymmetricMatrix A; Jacobi(X,D,A,V,TRUE); }


