// $Id: CovMatrix4Diagonalize.cc,v 1.7 2002/10/22 17:52:36 sachs Exp $
//
// Compute Eigenvalues and optionally Eigenvectors of a 4x4
//	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/CovMatrix4.h"
#include "CovMatrixEigCommon.h"

using namespace CovMatrices::LAPACK;

bool CovMatrices::CovMatrix4::diagonalize ( Vector4& eigenvalues, Vector4 eigenvectors[] ) const
{

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

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

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

    // store eigenvector and eigenvalues
    eigenvalues = val;
    eigenvectors[0] = vec;
    eigenvectors[1] = vec+4;
    eigenvectors[2] = vec+8;
    eigenvectors[3] = vec+12;
    return true;
  }
}