/*
 * $Id: eigen_dmat.cpp 578 2012-08-28 00:07:44Z johnoel $
 *
 * Copyright (c) 2009-2012 ADMB foundation
 */
/**
 * \file
 * Contains routines to find eigenvectors and eigenvalues
 * for constant types.
 */
#include <fvar.hpp>

void eigens(const dmatrix & m, const dmatrix & _evecs,
	    const dvector & _evals);

/**
 * \ingroup eigen
 * Eigenvalues
 * \param m \f$m\f$
 * \return a variable vector containing the
 *         eigenvalues of \f$m\f$.
 */
dvector eigenvalues(const dmatrix &m)
{
   if (m.rowsize() != m.colsize())
   {
      cerr <<
	 "error -- non square matrix passed to dvector eigenvalues(const dmatrix& m)\n";
      ad_exit(1);
   }

   int rmin = m.rowmin();
   int rmax = m.rowmax();

   dmatrix evecs(rmin, rmax, rmin, rmax);
   dvector evals(rmin, rmax);

   eigens(m, evecs, evals);

   return evals;
}

/**
 * \ingroup eigen
 * Eigenvectors
 * \param m \f$m\f$
 * \return a variable matrix with the
 *         eigenvectors of \f$m\f$ stored in its columns.
 */
dmatrix eigenvectors(const dmatrix &m)
{
   if (m.rowsize() != m.colsize())
   {
      cerr <<
	 "error -- non square matrix passed to dmatrix eigenvectors(const dmatrix& m)\n";
      ad_exit(1);
   }

   int rmin = m.rowmin();
   int rmax = m.rowmax();

   dmatrix evecs(rmin, rmax, rmin, rmax);
   dvector evals(rmin, rmax);

   eigens(m, evecs, evals);

   return evecs;
}

/**
 * \ingroup eigen
 * Eigenvectors and Eigenvalues
 * \param m \f$m\f$
 * \param _diag
 * \return a constant matrix with the
 *         eigenvectors of \f$m\f$ stored in its columns.
 * On return _diag contains the eigenvalues of \f$m\f$.
 */
dmatrix eigenvectors(const dmatrix &m, const dvector &_diag)
{
   ADUNCONST(dvector, diag) if (m.rowsize() != m.colsize())
   {
      cerr << "error -- non square matrix passed to dmatrix "
	 << "eigenvectors(const dmatrix& m, const dvector& _diag)\n";
      ad_exit(1);
   }

   int rmin = m.rowmin();
   int rmax = m.rowmax();

   dmatrix evecs(rmin, rmax, rmin, rmax);
   dvector evals(rmin, rmax);
   eigens(m, evecs, evals);
   diag = evals;

   return evecs;
}


/**
 * \ingroup eigen
 * Eigenvector and Eigenvalue algorithm
 * \param m \f$m\f$
 * \param _evecs
 * \param _evals
 *
 * \return
 * On return _evecs will be a constant matrix with the
 *  eigenvectors of \f$m\f$ stored in its columns.
 * On return _evals will be a constant vector containing the
 *  eigenvalues of \f$m\f$.
 * 
 * \n\n Cephes Math Library Release 2.1:  December, 1988
 * Copyright 1984, 1987, 1988 by Stephen L. Moshier 
 * Direct inquiries to 30 Frost Street, Cambridge, MA 02140
 */
void eigens(const dmatrix & m, const dmatrix & _evecs,
	    const dvector & _evals)
{
   ADUNCONST(dmatrix, evecs)
      ADUNCONST(dvector, evals) if (m.rowsize() != m.colsize())
   {
      cerr << "error -- non square matrix passed to "
	 <<
	 "void (const dmatirx& m, const dmatirx& evecs, const dvector& evals)\n";
      ad_exit(1);
   }
   if ((m.rowsize() != evecs.rowsize())
       || (m.colsize() != evecs.colsize()))
   {
      cerr << "error -- eigenvector array not compatible with matrix in "
	 <<
	 "void (const dmatirx& m, const dmatirx& evecs, const dvector& evals)\n";
      ad_exit(1);
   }
   if (m.rowsize() != evals.size())
   {
      cerr << "error -- eigenvalue array not compatible with matrix in "
	 <<
	 "void (const dmatirx& m, const dmatirx& evecs, const dvector& evals)\n";
      ad_exit(1);
   }

   int evecslb = evecs.rowmin();
   int evalslb = evals.indexmin();

   int N = m.colsize();

   dmatrix mat = symmetrize(m);

   //shift m, evecs, and evals so they are compatible with algorithm
   mat.rowshift(0);
   mat.colshift(0);
   evecs.rowshift(0);
   evecs.colshift(0);
   evals.shift(0);


   //mat[ row, column ] = A[ (row*row+row)/2 + column ]
   dvector A(0, N * (N + 1) / 2);

   for (int i = 0; i < N; i++)
   {
      for (int j = 0; j <= i; j++)
      {
	 A((i * i + i) / 2 + j) = mat(i, j);
      }
   }

   dvector RR(0, N * N - 1);
   dvector E(0, N - 1);

   // run cephes code
   int IND, L, LL, LM, M, MM, MQ, I, J, IA, LQ;
   int IQ, IM, IL, NLI, NMI;
   double ANORM, ANORMX, AIA, THR, ALM, ALL, AMM, X, Y;
   double SINX, SINX2, COSX, COSX2, SINCS, AIL, AIM;
   double RLI, RMI;
   static double RANGE = 1.0e-10;	/*3.0517578e-5; */


   /* Initialize identity matrix in RR[] */
   for (J = 0; J < N * N; J++)
      RR[J] = 0.0;
   MM = 0;
   for (J = 0; J < N; J++)
   {
      RR[MM + J] = 1.0;
      MM += N;
   }

   ANORM = 0.0;
   for (I = 0; I < N; I++)
   {
      for (J = 0; J < N; J++)
      {
	 if (I != J)
	 {
	    IA = I + (J * J + J) / 2;
	    AIA = A[IA];
	    ANORM += AIA * AIA;
	 }
      }
   }
   if (ANORM <= 0.0)
      goto done;
   ANORM = sqrt(ANORM + ANORM);
   ANORMX = ANORM * RANGE / N;
   THR = ANORM;

   while (THR > ANORMX)
   {
      THR = THR / N;

      do
      {				/* while IND != 0 */
	 IND = 0;

	 for (L = 0; L < N - 1; L++)
	 {

	    for (M = L + 1; M < N; M++)
	    {
	       MQ = (M * M + M) / 2;
	       LM = L + MQ;
	       ALM = A[LM];
	       if (fabs(ALM) < THR)
		  continue;

	       IND = 1;
	       LQ = (L * L + L) / 2;
	       LL = L + LQ;
	       MM = M + MQ;
	       ALL = A[LL];
	       AMM = A[MM];
	       X = (ALL - AMM) / 2.0;
	       Y = -ALM / sqrt(ALM * ALM + X * X);
	       if (X < 0.0)
		  Y = -Y;
	       SINX = Y / sqrt(2.0 * (1.0 + sqrt(1.0 - Y * Y)));
	       SINX2 = SINX * SINX;
	       COSX = sqrt(1.0 - SINX2);
	       COSX2 = COSX * COSX;
	       SINCS = SINX * COSX;

	       /*       ROTATE L AND M COLUMNS */
	       for (I = 0; I < N; I++)
	       {
		  IQ = (I * I + I) / 2;
		  if ((I != M) && (I != L))
		  {
		     if (I > M)
			IM = M + IQ;
		     else
			IM = I + MQ;
		     if (I >= L)
			IL = L + IQ;
		     else
			IL = I + LQ;
		     AIL = A[IL];
		     AIM = A[IM];
		     X = AIL * COSX - AIM * SINX;
		     A[IM] = AIL * SINX + AIM * COSX;
		     A[IL] = X;
		  }
		  NLI = N * L + I;
		  NMI = N * M + I;
		  RLI = RR[NLI];
		  RMI = RR[NMI];
		  RR[NLI] = RLI * COSX - RMI * SINX;
		  RR[NMI] = RLI * SINX + RMI * COSX;
	       }

	       X = 2.0 * ALM * SINCS;
	       A[LL] = ALL * COSX2 + AMM * SINX2 - X;
	       A[MM] = ALL * SINX2 + AMM * COSX2 + X;
	       A[LM] = (ALL - AMM) * SINCS + ALM * (COSX2 - SINX2);
	    }			/* for M=L+1 to N-1 */
	 }			/* for L=0 to N-2 */

      }
      while (IND != 0);

   }				/* while THR > ANORMX */

 done:;

   /* Extract eigenvalues from the reduced matrix */
   L = 0;
   for (J = 1; J <= N; J++)
   {
      L = L + J;
      E[J - 1] = A[L - 1];
   }

   // end of cephes code

   //put data into evecs and evals
   //EV[ n*i+j ] = evecs[i][j]
   for (int i = 0; i < N; i++)
   {
      for (int j = 0; j < N; j++)
      {
	 evecs(j, i) = RR[N * i + j];
      }
   }

   evals = E;

   //shift evecs and evals back
   evecs.rowshift(evecslb);
   evecs.colshift(evecslb);
   evals.shift(evalslb);

}
