// $Id: CovMatrixDiagonalize.cc,v 1.5 2002/10/22 18:08:19 sachs Exp $
//
// Compute Eigenvalues 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) const
{

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

  if (n < 3) // Special cases for low n
  {
    switch(n)
    {
      case 1:	// n = 1 is trivial
      {
        eigenvalues.resize(1);
        eigenvalues[0] == M[0];
	return true;
      }
      case 2:  // Use Exact formula for n = 2
      //	Based on LAPACK routine dlaev2.f
      {
        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;
        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;
    
        return true;
      }

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

    memcpy(in, &M[0], sz*sizeof(double));	// Copy input matrix
  
    // Convert Matrix to TriDiagonal form
    dsptrd(n, in, val, e, t);
  
    sz = dsteqr(n, val, e, 0);
    if (sz !=0) return false;
  
    // store eigenvalues
    memcpy( &eigenvalues[0], val, n*sizeof(double));
    return true;
  }
}