// $Id: CovMatrix6Diagonalize.cc,v 1.7 2003/07/01 21:16:46 sachs Exp $
//
// Compute Eigenvalues and optionally Eigenvectors of a 6x6
//	Positive definite symmetric matrix.
//	Return true if the computation was successful.
//	Return false if the matrix could not be diagonalized.
//
// David Sachs, Fermilab - March 2002
//
// Based on LAPACK routine dspev.f

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

using namespace CovMatrices::LAPACK;

bool CovMatrices::CovMatrix6::diagonalize ( Vector6& eigenvalues, Vector6 eigenvectors[] ) const
{

  // Compute Eigenvalues and optionally Eigenvectors 
  // by using C++ versions of proper LAPACK routines
  //
  // Arguments are:
  //
  //  Vector6& eigenvalues:	OUTPUT eignvalues
  //
  //  Vector6 eigenvectors[5]:	OUTPUT eigenvectors
  //	Supply a null pointer to compute Eigenvectors only
  //
  // Return value is true if computation was successful
  //
  
  double in[21];	// Copy of original matrixC
  double val[6];	// Temporary to hold Eigenvalues
  double vec[36];	// Temporary to hold Eigenvectors
  double e[5], t[5];	// Temporary for tridagonal form
  
  memcpy(in, &M, sizeof(M));	// Copy input matrix

  // Convert Matrix to TriDiagonal form
  dsptrd(6, in, val, e, t);

  // For eigenvalues only call dsteqr
  // For eigenvectors call dopgtr and dsteqr
  if (!eigenvectors)
  {
    if (dsteqr(6, val, e) != 0) return false;
    eigenvalues = val;
    return true;
  }
  else
  {
    dopgtr( 6, in, t, vec);
    if (dsteqr(6, val, e, vec) !=0) return false;

    // store eigenvector and eigenvalues
    eigenvalues = val;
    eigenvectors[0] = vec;
    eigenvectors[1] = vec+6;
    eigenvectors[2] = vec+12;
    eigenvectors[3] = vec+18;
    eigenvectors[4] = vec+24;
    eigenvectors[5] = vec+30;
    return true;
  }
}