// $Id: CovMatrixDiagonalizeV.cc,v 1.6 2002/09/19 16:45:18 sachs Exp $
//
// Compute Eigenvalues and  Eigenvectors of a 
//	Positive definite symmetric matrix.
//	Return true if the computation was successful.
//	Return false if the matrix could not be diagonalized.
//
// David Sachs, Fermilab - April 2002
//
// Based on LAPACK routine dspev.f

#include "CovMatrices/CovMatrix.h"
#include "CovMatrixEigCommon.h"

using namespace CovMatrices::LAPACK;

bool CovMatrices::CovMatrix::diagonalize( std::vector<double>& eigenvalues, std::vector<std::vector<double> >& eigenvectors ) const
{

  // Compute Eigenvalues and  Eigenvectors 
  // by using C++ versions of proper LAPACK routines
  //
  // Arguments are:
  //
  //  vector<double>& eigenvalues:	OUTPUT eignvalues
  //
  //  vector<vector<double>& eigenvectors:	OUTPUT eigenvectors
  //
  // Return value is true if computation was successful
  //

  if (n < 3) // Special cases for low n
  {
    switch(n)
    {
      case 1:	// n = 1 is trivial
      {
        eigenvectors.resize(1);
        eigenvectors[0].resize(1);
        eigenvalues.resize(1);
        eigenvalues[0] == M[0];
	eigenvectors[0][0] = 1.0;
	return true;
      }
      case 2:  // Use Exact formula for n = 2
      //	Based on LAPACK routine dlaev2.f
      {
        eigenvectors.resize(2);
        eigenvectors[0].resize(2);
        eigenvectors[1].resize(2);
        eigenvalues.resize(2);
        double sm = M[0]+M[2];
        double df = M[0]-M[2];
        double tb = M[1]+M[1];
        double ab = abs(tb);
        double adf= abs(df);
        double acmx, acmn, rt, r1, r2, cs, cs1, sn1, ct, tn, acs;
        bool x = true;  // Flag for eigenvector rotation needed
    
        // Compute EigenValues, handling all cases
        if ( abs(M[0]) > abs(M[2]) )
        {
          acmx = M[0];
          acmn = M[2];
        }
        else 
        {
          acmx = M[2];
          acmn = M[0];
        }
        
        if ( adf > ab )
        {
          rt = adf*sqrt( 1. + (ab/adf)*(ab/adf));
        }
        else if ( adf < ab )
        {
          rt = ab*sqrt( 1. + (adf/ab)*(adf/ab));
        }
        else  // adf == ab includes case of all zero matrix
        {
          rt = ab * sqrt(2.0);
        }
    
        if (sm < 0)
        {
          r1 = .5 * (sm - rt);
          r2 = ( acmx/r1 ) * acmn - (M[1]/r1)*M[1];
          x = !x;
        }
        else if ( sm > 0 )
        {
          r1 = .5 * (sm + rt);
          r2 = ( acmx/r1 ) * acmn - (M[1]/r1)*M[1];
        }
        else
        {
          r1 = .5*rt;
          r2 = -r1;
        }
    
        eigenvalues[0] = r1;
        eigenvalues[1] = r2;
    
        // Compute EigenVectors
    
        if ( df >= 0. )
        {
          cs = df + rt;
        }
        else
        {
          cs = df - rt;
          x = !x;
        }
    
        acs = abs(cs);
        if (acs > ab)
        {
          ct = -tb/cs;
          sn1 = 1./sqrt(1. + ct*ct);
          cs1 = ct*sn1;
        }
        else if (ab == 0.)
        {
          cs1 = 1.;
          sn1 = 0.;
        }
        else
        {
          tn = -cs/tb;
          cs1 = 1./sqrt(1.+tn*tn);
          sn1 = tn*cs1;
        }
    
        if (x)
        {
          tn = cs1;
          cs1 = -sn1;
          sn1 = tn;
        }
    
        eigenvectors[0][0] = cs1;
        eigenvectors[0][1] = sn1;
        eigenvectors[1][0] = -sn1;
        eigenvectors[1][1] = cs1;
    
        return true;
      }

      default:  // n <= 0 ** Should not happen
      return false;
    }
  }
  else // Normal computation for n >= 3
  {
    int i;
    int sz = (n*(n+1))/2;	// Number of elemetns in matrix
    std::vector<double> v(sz + n*n + 3*n -2);// Temporaries
    double* in= &v[0];		// Copy of original matrix
    double* val= in+sz;		// Temporary to hold Eigenvalues
    double* vec= val+ n;	// Temporary to hold Eigenvectors
    double* e = vec + n*n;	// Temporary for tridagonal form
    double* t = e + n-1;	// Temporary for tridagonal form
    
    // Prepare output vectors 
    eigenvalues.resize(n);
    eigenvectors.resize(n);

    memcpy(in, &M[0], sz*sizeof(double));	// Copy input matrix
  
    // Convert Matrix to TriDiagonal form
    dsptrd(n, in, val, e, t);
  
    // For eigenvectors call dopgtr and dsteqr
    dopgtr( n, in, t, vec);
    sz = dsteqr(n, val, e, vec);
    if (sz !=0) return false;
  
    // store eigenvector and eigenvalues
    memcpy( &eigenvalues[0], val, n*sizeof(double));
    for (i = 0; i < n; i++)
    {
      eigenvectors[i].resize(n);
      memcpy( &eigenvectors[i][0], vec+i*n, n*sizeof(double));
    }
    return true;
  }
}