// $Id: CovMatrixEigCommon.cc,v 1.22 2002/10/22 17:59:37 sachs Exp $
//
// Common EigenValue and EigenVector routines
// 
// David Sachs - Fermilab - March 2002
//
// These routines are modified C++ versions of selected routines from
// Version 3.0 of the LAPACK library .
//
// The original LAPACK library may be obtained from 
// http://www.netlib.org/lapack/

#include <cmath>
#include <vector>
#include "CovMatrixEigCommon.h"

// Simplify access to mathematical functions from standard library
using std::abs;
using std::sqrt;

using namespace CovMatrices::LAPACK;

// *** WARNING *** Old method used because some compilers 
//                 do not support C++ standard <limits>
// Testing has determined that these limits should work
// on IEEE floating point arithmetic compliant computers
#include <cfloat>
namespace
{
  const double eps2 = (DBL_EPSILON)*(DBL_EPSILON);
  const double safmin = DBL_MIN; 
	
// Auxiliary functions 

inline double dnrm2(int n, double* x)
{
  // CovMatrix version of LAPACK function dnrm2.fa with increment 1
  // Compute norm of size n vector 
  // This version does not perform scaling

  if (n < 1) return 0.;
  if (n == 1) return abs(*x);
  double sum = (*x)*(*x);

  while (--n)
  {
    x ++;
    sum += (*x)*(*x);
  }
  return (sqrt(sum));
}

inline double dlarfg(int n, double& alpha, double* x)
{
  // CovMatrix version of LAPACK routine dlarfg.f with stride 1
  // Generate Housholder reflector of order n such that
  // H * (alpha, x)' = (beta, 0) H=H'  H*H = I
  // H is represented as tau * (1, V)' * (1, V)
  // where tau is a scalar and V is a (n-1) element vector
  // This version does not perform scaling and is for Upper Triangular Matrices
  //
  // arguments:
  //	n	Order of reflector
  //
  //	alpha	Input: value alpha
  //		Output: value beta
  //	
  //	x	Input: source vector X
  //		Output: Output vector V
  //
  // Return value is tau

  int i;
  if (n <= 1) return 0.;

  double xnorm = dnrm2( n-1, x);
  if (xnorm == 0.) return 0.;
  double beta = sqrt(alpha*alpha + xnorm*xnorm);
  if (alpha >= 0.) beta = -beta;
  double tau = (beta - alpha)/beta;
  for (i = 0; i < n-1; i++) x[i] /= (alpha - beta);
  alpha = beta;
  return tau;
}

inline void dspmvu011(int n, double alpha, const double* ap,
		const double* x, double* y)
{
  // Special case of LAPACK routine dspmv.f
  // (Upper triangular stride 1, initialize vector y)
  //
  // dspmvu011 performs matrix-vector operation
  //
  // y := alpha*ap*x
  //
  // Arguments:
  //	n	Order of Matrix and Vectors
  //
  //	ap	n by n symmetric matrix in packed upper triangle form
  //
  //	alpha	scalar multiplication factor
  //
  //	x	n element vector
  //
  //	y	OUTPUT n element vector

  int i, j, k;
  double t;

  for ( i = 0; i < n; i++) y[i]= 0.;

  k = 0;
  for ( j = 0; j < n; j++)
  {
    k += j;	// k = j*(j+1)/2 - Index to next part of matrix
    t = y[j];	// Used to let optimizing compilers know y[i] and y[j]
		// do not conflict
    for ( i = 0; i < j; i++)
    {
      y[i] += x[j]*ap[k+i];	// ap(i,j)*x(j)  i<j
      t += x[i]*ap[k+i];	// ap(j,i)*x(i)	 j>i
    }
    y[j] = x[j]*ap[k+j] + t;	// ap(j,j)*x(j)
  }
  for ( i = 0; i < n; i++) y[i] *= alpha;
}

inline void dspr2u11( int n, double alpha, const double* x,
		const double* y, double* ap)
{
  // Special case of LAPACK routine dspr2.f
  // (Upper triangular, stride 1)
  //
  // dspr2u11 performs the symmetric rank 2 operation
  // ap += alpha*x*y' + alpha*y*x'
  //
  // Arguments:
  //	n       Order of Matrix and Vectors
  //
  //	alpha	scalar multiplication factor
  //
  //	x	n element vector
  //
  //	y	n element vector
  //
  //	ap      n by n symmetric matrix in packed upper triangle form
  //		MODIFIED on exit

  int i, j, k;
  double t1, t2;

  k = 0;
  for (j = 0; j < n; j++)
  {
    k += j;	// k = j*(j+1)/2 = index for section j
    if ( x[j] || y[j] )	// skip inner loop if unneeded
    {
      t1 = alpha*y[j];
      t2 = alpha*x[j];
      for (i = 0; i <= j; i++)
      {
	ap[k+i] += t1*x[i] + t2*y[i];  // a(i,j) += t*x(i)*y(j) + t*y(i)*x(j)
      }
    }
  }
}

void dlartg( double f, double g, double& cs,
			double& sn, double& r)
{
  // CovMatrix version of LAPACK routine dlartg.f
  // This version does not perform scaling
  //
  // Generate plane rotation such that
  // |  CS  SN | * | F | =  | R |
  // | -SN  CS |   | G |    | 0 |
  //
  // If F has greater magnitude, CS will be positive
  // If G has greater magniture, R will be positive
  //
  // arguments:
  //
  //  f:	INPUT	First component of input vector
  //  g:	INPUT	Second component of input vector
  // cs:	OUTPUT	Cosine of rotation
  //
  // sn:	OUTPUT	Sine of rotation
  //  r:	OUTPUT	Nonzero component of rotated vector

  double fa = abs(f);
  double ga = abs(g);

  if (!g)
  {
    cs = 1.0;
    sn = 0.;
    r = f;
  }
  else if (!f)
  {
    cs = 0.;
    sn = 1.;
    r = g;
  }
  else if (fa > ga)
  {
    r = f*sqrt(1. + (ga/fa)*(ga/fa));
    // NOTE: In this case rotation angle has a positive cosine
    // and r may be negative
    cs = f/r;
    sn = g/r;
  }
  else
  {
    r =  ga*sqrt(1. + (fa/ga)*(fa/ga));
    // NOTE: In this case cs and sn have signs of f and g
    // and r is always positive
    cs = f/r;
    sn = g/r;
  }
}

} // END unnamed namespace

bool CovMatrices::LAPACK::dsptrd(int n, double* ap, double* d, double* e, double* tau)
{
  // CovMatrix version of LAPACK routine dsptrd.d
  // Convert Symmetric Matrix to Tridiagonal form
  // This version only processes upper triangular form
  //
  // Arguments:
  //	n	Order of Matrix
  //
  //	ap	INPUT: Original Matrix as packed upper triangle
  //		OUTPUT: Tridiagonal Matrix and Reflectors
  //		See LAPACK documentation for further details
  //
  //	d	OUTPUT:Diagonal Elements of Tridiagonal form
  //		Size = n
  //
  //	e	OUTPUT:Off-diagonal  Elements of Tridiagonal form
  //		Size = n-1
  //
  //	tau	OUTPUT:Scalar factors of elementary reflectors
  //		Size = n-1
  //
  // Returns true if successful or false if there was an error

  int i, j;
  double taui;
  double alpha;

  if (n < 1) return false;

  int i1 = ( n * (n - 1))/2;	// Index for start of working column (i+1)
				// Starts pointing to last column
  for ( i = n-1; i >= 1; i--)
  {
    // Generate elementary reflector for column i+1
    // to annihilate i-1 off tridiagonal elements
    taui = dlarfg(i, ap[i1+i-1], &ap[i1]);
    e[i-1] = ap[i1+i-1];
    if (taui)
    {
      // Apply elementary reflector to both sides of ap(0:i-1,0:i-1)
      ap[i1+i-1] = 1.0;
      // Compute tau(*) = a(*,*)*taui*h
      dspmvu011(i, taui, ap, ap+i1, tau);

      // Compute w := y - .5*tau*(y'*v)*v
      alpha = tau[0]*ap[i1];
      // Loop unrolling code from LAPACK ddot.f is omitted for now
      for (j = 1; j < i; ++j) 
      {
	alpha += tau[j] * ap[i1+j];
      }
      alpha *= -.5 * taui;
      for (j = 0; j < i; ++j)
      {
	tau[j] += alpha * ap[i1+j];
      }

      // Apply the rank-2 update A = A - v * w' - w * v'
      dspr2u11( i, -1., &ap[i1], tau, ap );
      ap[i1+i-1] = e[i-1];
    }

    d[i] = ap[i1+i];
    tau[i-1] = taui;
    i1 -= i;
  }
  d[0] = ap[0];
  return true;
}

// Routines dlae2 and dsterf are no longer used by CovMatrices
#if 0 
void CovMatrices::LAPACK::dlae2(double a, double b, double c, double& e1, double& e2)
{
  // CovMatrix version of LAPACK routine dlae2,f
  // Compute Eignevalues of a 2x2 symmetric matrix
  //
  // arguments
  //  a:	INPUT:	First diagnonal element
  //  b:	INPUT:  Off diagonal element
  //  c:	INPUT:  Second diagonal element
  //
  //  e1:	OUTPUT:	Larger Eigenrvalue
  //  e2:	OUTPUT: Smaller Eignevalue
  //
  // This routine uses the standard formula for solving
  // the quadratic equation for the eigenvalues with
  // provisions for minimizing loss of precision.

  double sm, acmn, acmx, adf, df, rt, tb, ab;

  sm = a+c;
  df = a-c;
  adf = abs(df);
  tb = b+b;
  ab = abs(b);
  if (abs(a)>abs(c))
  {
    acmx = a;
    acmn = c;
  }
  else
  {
    acmx = c;
    acmn = a;
  }
  if ( adf > ab) rt = adf*sqrt(1.+(ab/adf)*(ab/adf));
  else           rt = ab*sqrt(1.+(adf/ab)*(adf/ab));

  if (sm<0)
  {
    e1 = .5*(sm-rt);
    e2 = (acmx/e1)*acmn - (b/e1)*b;
  }
  else if (sm>0)
  {
    e1 = .5*(sm+rt);
    e2 = (acmx/e1)*acmn - (b/e1)*b;
  }
  else
  {
    e1 = .5*rt;
    e2 = -.5*rt;
  }
}

int CovMatrices::LAPACK::dsterf(int n, double* d, double* e)
{
  // CovMatrix version of LAPACK routine dsterf.f
  // Compute all eignnvalues of a symmetric tridiagonal matrix
  // using the QL and/or QR methods.
  //
  // arguments:
  //
  //  n:	Order of the matrix (n > 0)
  //
  //  d:	INPUT:	Diagonal elements
  //		OUTPUT:	Eigenvalues
  //
  //  e:	INPUT:	Off diagonal elements
  //		OUTPUT: (Overwritten)
  //
  // return value
  //	=0	success
  //	<0	error other than non-convergence
  //	>0	number of failed eigenvalues

  const int maxit = 30;	// Maximum iterations allowed per value

  if (n<0) return -1;	// argument error
  if (n<1) return 0;	// Automatic success for size 0 or 1

  int nmaxit = n*maxit;
  double sigma = 0.;
  int jtot = 0;
  int lend;
  int l1 = 0;
  int i, m, lsv, lendsv, l;
  double p, rte, r, c, s, gamma, bb, oldc, oldgam, alpha;

  while ( l1 < n)
  {
    // Determine if the matrix splits and handle each portion
    if(l1) e[l1-1]=0.;
    for (m = l1; m < n-1; m++)
    {
      if (e[m]*e[m] <= abs((eps2*d[m])*d[m+1])+safmin)
      {
	e[m]=0.;
	break;
      }
    }
    l = l1;
    lsv = l;
    lend = m;
    lendsv=lend;
    l1 = m + 1;
    if (lend == l) continue; // 1x1 part requires no action
    if (lend == l+1)  // 2x2 - Use exact solution
    {
      dlae2(d[l],e[l],d[lend],d[l],d[lend]);
      continue;
    }
    
    // Square off-diagonal elements for QR/QL calculations
    for (i=l; i<lend; i++) e[i] *= e[i];

    // Determine whether to use QL or QR based on relative sizes of diagonal elements
    if ( abs(d[l]) < abs(d[lend]))
    {
      // QL iteration 
      while  ( l<=lend )
      {
	//  Look for small off-diagonal elements
        for (m=l; m<lend; m++)
        {
	  if (abs(e[m]) <= abs((eps2*d[m])*d[m+1])+safmin) break;
        }
        if (m<lend) e[m]=0.;
	p = d[l];
	if (m == l) 
	{
	  l++;	// If block is 1x1 go for next eigenvalue
	  continue;
	}
	if ( m == l+1 )
	{
	  dlae2( d[l], sqrt(e[l]), d[l+1], d[l], d[l+1]);	// process 2x2 block
	  e[l] = 0.;
	  l += 2;
	  continue;
	}
	if (++jtot > nmaxit) return(n-m);  // *** Failed to converge ***

	// Form shift for next iteration
	rte = sqrt(e[l]);
	sigma = (d[l+1] - p)/(2. * rte);
	r = sqrt(1. + sigma*sigma);
	if (sigma < 0) r = -r;
	sigma = p - rte/(sigma+r);

	c = 1.0;
	s = 0.;
	gamma = d[m] - sigma;
	p = gamma*gamma;

	// Inner loop
	for ( i = m-1; i >= l; i--)
	{
	  bb = e[i];
	  r = p+bb;
	  if (i != m-1 ) e[i+1]=s*r;
	  oldc = c;
	  c = p/r;
	  s = bb/r;
	  oldgam = gamma;
	  alpha = d[i];
	  gamma = c*(alpha-sigma) - s*oldgam;
	  d[i+1] = oldgam + (alpha -gamma);
	  p = c ? (gamma*gamma)/c : oldc*bb;
	}
	e[l]= s*p;
	d[l]=sigma+gamma;
      }
    }
    else
    {
      lend = lsv;
      l = lendsv;
      // QR iteration 
      while  ( l>=lend )
      {
	//  Look for small off-diagonal elements
        for (m=l; m>lend; m--)
        {
	  if (abs(e[m-1]) <= abs((eps2*d[m])*d[m-1])+safmin) break;
        }
        if (m>lend) e[m-1]=0.;
	p = d[l];
	if (m == l) 
	{
	  l--;
	  continue;
	}
	if ( m == l-1 )
	{
	  dlae2( d[l], sqrt(e[l-1]), d[l-1], d[l], d[l-1]);
	  e[l-1] = 0.;
	  l -= 2;
	  continue;
	}
	if (++jtot > nmaxit) return(n-lend);  // *** Failed to converge ***

	// Form shift for next iteration
	rte = sqrt(e[l-1]);
	sigma = (d[l-1] - p)/(2. * rte);
	r = sqrt(1. + sigma*sigma);
	if (sigma < 0) r = -r;
	sigma = p - rte/(sigma+r);

	c = 1.0;
	s = 0.;
	gamma = d[m] - sigma;
	p = gamma*gamma;

	// Inner loop
	for ( i = m; i < l; i++)
	{
	  bb = e[i];
	  r = p+bb;
	  if (i != m ) e[i-1]=s*r;
	  oldc = c;
	  c = p/r;
	  s = bb/r;
	  oldgam = gamma;
	  alpha = d[i+1];
	  gamma = c*(alpha-sigma) - s*oldgam;
	  d[i] = oldgam + (alpha - gamma);
	  p = c ? (gamma*gamma)/c : oldc*bb;
	}
	e[l-1]= s*p;
	d[l]=sigma+gamma;
      }
    }
  }
  // Sort the Eigenvalues in increasing order.
  // Because the order of the matrix is likely to be small a simple exchange sort is used
  for ( i = 1; i< n; i ++)
  {
    for( m = 0; m < i; m++)
    {
      if (d[m]>d[i])
      {
	p = d[m];
	d[m] = d[i];
	d[i] = p;
      }
    }
  }
  return 0;
}
#endif

bool CovMatrices::LAPACK::dopgtr( int n, const double* ap, const double* tau,
			double* q)
{
  // CovMatrix version of LAPACK routines :
  // dopgtr.f, dorg2l.f, dlarf.f and dscal.f 
  // Restricted to upper triagonal case with minimum sized q matrix
  // Compute orthogonal matrix q from elementarey reflectors generated
  // by dsptrd from upper triagonal symmetric matrix representation
  //
  // arguments:
  //
  //  n		INPUT:	Order of matrices
  //
  //  ap	INPUT: 	Reflectors generated by dsptrd
  //
  //  tau	INPUT:	Scalar factors generated by dsptrd
  //
  //  q		OUTPUT: n*n orthogonal matrix stored in FORTRAN order
  //
  // return value:
  //	true = Matrix generated
  //	false = error

  if (n<2) return false;	// n bmust be >= 2

  std::vector<double> workarea(n-1); // temporary work area
  double* w = &workarea[0];
  int i, j, ij, nm, np, k;
  double temp;

  // Initialize q to results from reflectors
  // The last row and column of q are initialized to unit matrix
  ij = 1;  // ij = i + (j+1)*(j+2)/2
  for (j = 0; j < n-1; j++)
  {
    for ( i = 0; i < j; i++)
    {
      q[i+j*n] = ap[ij++];	//q(i,j) = a(i,j+1) (i<j)
    }
    ij += 2;
    q[n-1+n*j] = 0.;		//q(n,j) = 0.
    q[n*n-n+j] = 0.;		//q(j,n) = 0.
  }
  q[n*n-1]= 1.0;		//q(n,n) = 1.
 
  // dorg2l n-1 n-1 n-1 q n t w

  nm = n-1;
  np = n+1;
  for (i=0; i<nm; i++)
  {
    q[np*i]=1.0;	//q(i,i)=1.
    //***DLARF Left i+1 i q[0,i] 1 tau[i] q n w
    if (tau[i]) // If zero, reflector has no effect
    {
      // DGEMV T i+1 i 1.0 q n q(0,i) 1 0 w
      // Compute reflector i
      for ( j=0; j<i; j++) w[j]=0.;
      for ( j=0; j<i; j++)
      {
	for (k = 0; k<=i; k++)
	{
	  w[j] += q[n*i+k]*q[n*j+k];
	}
      }

      // Apply reflector
      // DGER i+1 i -tau[i] w 1 q(0,i) 1 q n
      for (j=0; j<i; j++)
      {
	if(w[j])
	{
	  temp = tau[i]*w[j];
	  for(k=0; k<=i; k++) q[n*j+k] -= temp*q[n*i+k];
	}
      }

      //***DSCAL i -tau(i) &q[n*i] 1
      for (j=0; j<i; j++) q[n*i+j] *= -tau[i];
    }

    q[np*i] = 1. - tau[i];
    // Zero elements beyond scope of this reflector
    for (k=i+1;  k<n; k++) q[n*i+k] = 0.;

  }
  return true;
}


int CovMatrices::LAPACK::dsteqr( int n, double* d, double* e, double* z)
{
  // CovMatrix version of LAPACK routine dsteqr.f
  // Compute all eignnvalues and eigenvectorsof a
  // symmetric tridiagonal matrix using the QL and/or QR methods.
  //
  // arguments:
  //
  //  n:	Order of the matrix (n > 0)
  //
  //  d:	INPUT:	Diagonal elements
  //		OUTPUT:	Eigenvalues
  //
  //  e:	INPUT:	Off diagonal elements
  //		OUTPUT: (Overwritten)
  //
  //  z:	INPUT:	Orthogonal  matrix from tridiagonal reduction
  //		OUTPUT: Eignevectors of original Matix 
  //		Specify a NULL ppointer if only EigenValues are desired
  //		NOTE: Eignevector Matrix is stored in FORTRAN order
  //
  // return value
  //	=0	success
  //	<0	error other than non-convergence
  //	>0	number of failed eigenvalues

  const int maxit = 30;	// Maximum iterations allowed per value

  if (n<0) return -1;	// argument error
  if (n<=1) return 0;	// Automatic success for size 0 or 1

  int nmaxit = n*maxit;
  double g, f, b;
  int jtot = 0;
  int lend;
  int l1 = 0;
  int i, m, lsv, lendsv, k, j;
  double p, r, c, s;

  while ( l1 < n)
  {
    // Determine if the matrix splits and handle each portion
    if(l1) e[l1-1]=0.;
    for (m = l1; m < n-1; m++)
    {
      if (e[m]*e[m] <= abs((eps2*d[m])*d[m+1])+safmin)
      {
	e[m]=0.;
	break;
      }
    }
    k = l1;
    lsv = k;
    lend = m;
    lendsv=lend;
    l1 = m + 1;
    if (lend == k) continue; // 1x1 part requires no action
    
    // Determine whether to use QL or QR based on relative sizes of diagonal elements
    if ( abs(d[k]) < abs(d[lend]))
    {
      // QL iteration 
      while  ( k<=lend )
      {
	//  Look for small off-diagonal elements
        for (m=k; m<lend; m++)
        {
	  if (e[m]*e[m] <= abs((eps2*d[m])*d[m+1])+safmin) 
	  {
	    e[m] = 0.;
	    break;
	  }
        }
	if (m == k) 
	{
	  k++;		// Eigenvalue found - continue with rest
	  continue;
	}
	if (++jtot > nmaxit) return(n-m);  // *** Failed to converge ***

	// Form shift for next iteration
	p = d[k];
	g = (d[k+1] - p)/(2. * e[k]);	// Form shift
	r = sqrt(1. + g*g);
	if  (g<0.) r = -r;
	g = d[m] - p + e[k]/(g+r);	// This is d[m] - k

	c = 1.0;
	s = 1.0; 
	p = 0.;

	// Inner loop
	for ( i = m-1; i >= k; i--)
	{
	  f = s*e[i];
	  b = c*e[i];
	  dlartg( g, f, c, s, r);
	  if (i != m-1 ) e[i+1] = r;
	  g = d[i+1] -p;
	  r = (d[i] - g)*s + 2.*c*b;
	  p = s*r;
	  d[i+1] = g + p;
	  g = c*r - b;
	  if (z)  // Skip EigenVector code if only Eigenvalues are needed
	  {
	    for (j=0; j<n; j++) // Apply rotation to EigenVector matrix
	    {
	      f = z[n*(i+1)+j];
	      z[n*(i+1)+j] = s*z[i*n+j] + c*f;
	      z[i*n+j] = c*z[i*n+j] - s*f;
	    }
	  }
	}

	e[k] = g;
	d[k] -= p;
      }
    }
    else
    {
      lend = lsv;
      k = lendsv;
      // QR iteration 
      while  ( k>=lend )
      {
	//  Look for small off-diagonal elements
        for (m=k; m>lend; m--)
        {
	  if (e[m-1]*e[m-1] <= abs((eps2*d[m])*d[m-1])+safmin) 
	  {
	    e[m-1] = 0.;
	    break;
	  }
        }
	if (m == k) 
	{
	  k--;
	  continue;
	}
	if (++jtot > nmaxit) return(n-lend);  // *** Failed to converge ***

	// Form shift for next iteration
	p = d[k];
	g = (d[k-1] - p)/(2. * e[k-1]);
	r = sqrt(1. + g*g);
	if  (g<0.) r = -r;
	g = d[m] - p + e[k-1]/(g+r);

	c = 1.0;
	s = 1.0;
	p = 0.;

	// Inner loop
	for ( i = m; i < k; i++)
	{
	  f = s*e[i];
	  b = c*e[i];
	  dlartg( g, f, c, s, r);
	  if (i != m ) e[i-1] = r;
	  g = d[i] -p;
	  r = (d[i+1] - g)*s + 2.*c*b;
	  p = s*r;
	  d[i] = g + p;
	  g = c*r - b;
	  if (z)  // Skip EigenVector code if only Eigenvalues are needed
	  {
	    for (j=0; j<n; j++) // Apply rotation to Eigenvector matrix
	    {
	      f = z[n*(i+1)+j];
	      z[n*(i+1)+j] = f*c - z[n*i+j]*s;
	      z[n*i+j] = z[n*i+j]*c + f*s;
	    }
	  }
	}

	e[k-1] = g;
	d[k] -= p;
      }
    }
  }
  // Sort the Eigenvalues in increasing order.
  // Use Selection sort to minimize swaps
  /*for ( i = 1; i < n; i++)   // This code is skipped
  {
    l1 = i - 1;
    m = l1;
    p = d[l1];
    for ( j = i; j < n; j++)
    {
      if (d[j] < p)
      {
	m = j;
	p = d[j];
      }
    }
    if (m != l1)
    {
      d[m] = d[l1];
      d[l1] = p;
      //dswap( n, &z[1,l1],i 1, &z[1,m], 1);
      // simplified swap without loop unrolling used here
      if (z)
      {
        for (j = 0; j < n; j++)
        {
	  p = z[n*l1+j];
	  z[n*l1+j] = z[n*m+j];
	  z[n*m+l1] = p;
        }
      }
    }
  } */
  return 0;
}