// $Id: CovMatrix2Diagonalize.cc,v 1.7 2002/10/22 16:20:54 mf Exp $
//
// Compute Eigenvalues and optionally Eigenvectors of a 2x2
//	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
// 
// 02-10-22 MF  Removed redundat and technically illegal re-spec of 
//              default for eigenvectors[] argument

#include "CovMatrices/CovMatrix2.h"
#include <cmath>

using std::abs;		// Be sure to use C++ abs and sqrt functions
using std::sqrt;	

bool CovMatrices::CovMatrix2::diagonalize ( Vector2& eigenvalues, 
	Vector2 eigenvectors[] ) const
{

  // Compute Eigenvalues and optionally Eigenvectors 
  // by using exact formulas: Based on LAPACK routine dlaev2.f
  //
  // Arguments are:
  //
  //  Vector2& eigenvalues:	OUTPUT eignvalues
  //
  //  Vector2 eigenvectors[2]:	OUTPUT eigenvectors
  //	Supply a null pointer to compute Eigenvectors only
  //
  // Always returns true, as the exact formulas never fail. for the 2x2 case
  //
  
    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;

    if (!eigenvectors) return true;  // EigenVectors not required

    // 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;
  }