/*
 * Copyright University of Reims Champagne-Ardenne
 * Authors and Contributors: Akilan RAJAMANI, Corentin LEFEBVRE, Johanna KLEIN,
 *                           Emmanuel PLUOT, Gaetan RUBEZ, Hassan KHARTABIL,
 *                           Jean-Charles BOISSON and Eric HENON
 * (24/07/2017)
 * jean-charles.boisson@univ-reims.fr, eric.henon@univ-reims.fr
 *
 * This software is a computer program whose purpose is to
 * detect and quantify interactions from electron density
 * obtained either internally from promolecular density or
 * calculated from an input wave function input file. It also
 * prepares for the visualization of isosurfaces representing
 * several descriptors (dg) coming from the IGM methodology.
 *
 * This software is governed by the CeCILL-C license under French law and
 * abiding by the rules of distribution of free software.  You can  use,
 * modify and/ or redistribute the software under the terms of the CeCILL-C
 * license as circulated by CEA, CNRS and INRIA at the following URL
 * "http://www.cecill.info".
 *
 * As a counterpart to the access to the source code and  rights to copy,
 * modify and redistribute granted by the license, users are provided only
 * with a limited warranty  and the software's author,  the holder of the
 * economic rights,  and the successive licensors  have only  limited
 * liability.
 *
 * In this respect, the user's attention is drawn to the risks associated
 * with loading,  using,  modifying and/or developing or reproducing the
 * software by the user in light of its specific status of free software,
 * that may mean  that it is complicated to manipulate,  and  that  also
 * therefore means  that it is reserved for developers  and  experienced
 * professionals having in-depth computer knowledge. Users are therefore
 * encouraged to load and test the software's suitability as regards their
 * requirements in conditions enabling the security of their systems and/or
 * data to be ensured and,  more generally, to use and operate it in the
 * same conditions as regards security.
 *
 * The fact that you are presently reading this means that you have had
 * knowledge of the CeCILL-C license and that you accept its terms.
 *
 * */
/**
 * @file eig3.cpp
 * @brief Implementation of eig3.h definitions */

#ifndef _EIG3_CPP_
#define _EIG3_CPP_

// LOCAL
#include <eig3.h>

// STL
#include <iostream>

//JC : only computed one time
double eig3__eps = pow(2.0,-52.0);

double
hypot2(double x, double y)
{
  return sqrt(x*x+y*y);
}

// Symmetric Householder reduction to tridiagonal form.

void
tred2(double V[3][3], double d[3], double e[3])
{
  int n = 3;
  
  for (int j = 0; j < n; j++)
    {
      d[j] = V[n-1][j];
    }
  
  // Householder reduction to tridiagonal form.

  for (int i = n-1; i > 0; i--) {

    // Scale to avoid under/overflow.

    double scale = 0.0;
    double h = 0.0;
    for (int k = 0; k < i; k++)
      {
	scale = scale + std::abs(d[k]);
      }
    if (scale == 0.0)
      {
	e[i] = d[i-1];
	for (int j = 0; j < i; j++)
	  {
	    d[j] = V[i-1][j];
	    V[i][j] = 0.0;
	    V[j][i] = 0.0;
	  }
      }
    else
      {
	
	// Generate Householder vector.
	
	for (int k = 0; k < i; k++)
	  {
	    d[k] /= scale;
	    h += d[k] * d[k];
	  }
	double f = d[i-1];
	double g = sqrt(h);
	if (f > 0)
	  {
	    g = -g;
	  }
	e[i] = scale * g;
	h = h - f * g;
	d[i-1] = f - g;

	for (int j = 0; j < i; j++)
	  {
	    e[j] = 0.0;
	  }
	
	// Apply similarity transformation to remaining columns.

	for (int j = 0; j < i; j++)
	  {
	    f = d[j];
	    V[j][i] = f;
	    g = e[j] + V[j][j] * f;
	    for (int k = j+1; k <= i-1; k++)
	      {
		g += V[k][j] * d[k];
		e[k] += V[k][j] * f;
	      }
	    e[j] = g;
	  }
	f = 0.0;
	for (int j = 0; j < i; j++)
	  {
	    e[j] /= h;
	    f += e[j] * d[j];
	  }
	double hh = f / (h + h);
	for (int j = 0; j < i; j++)
	  {
	    e[j] -= hh * d[j];
	  }
	for (int j = 0; j < i; j++)
	  {
	    f = d[j];
	    g = e[j];
	    for (int k = j; k <= i-1; k++)
	      {
		V[k][j] -= (f * e[k] + g * d[k]);
	      }
	    d[j] = V[i-1][j];
	    V[i][j] = 0.0;
	  }
      }
    d[i] = h;
  }

  // Accumulate transformations.

  for (int i = 0; i < n-1; i++)
    {
      V[n-1][i] = V[i][i];
      V[i][i] = 1.0;
      double h = d[i+1];
      if (h != 0.0) {
	for (int k = 0; k <= i; k++)
	  {
	    d[k] = V[k][i+1] / h;
	  }
	for (int j = 0; j <= i; j++)
	  {
	    double g = 0.0;
	    for (int k = 0; k <= i; k++)
	      {
		g += V[k][i+1] * V[k][j];
	      }
	    for (int k = 0; k <= i; k++)
	      {
		V[k][j] -= g * d[k];
	      }
	  }
      }
      for (int k = 0; k <= i; k++)
	{
	  V[k][i+1] = 0.0;
	}
    }
  for (int j = 0; j < n; j++)
    {
      d[j] = V[n-1][j];
      V[n-1][j] = 0.0;
    }
  V[n-1][n-1] = 1.0;
  e[0] = 0.0;
} 

// Symmetric tridiagonal QL algorithm.

void
tql2(double V[3][3], double d[3], double e[3])
{
/*       D CONTAINS THE EIGENVALUES IN ASCENDING ORDER.  IF AN
            ERROR EXIT IS MADE, THE EIGENVALUES ARE CORRECT BUT
            UNORDERED FOR INDICES 1,2,...,IERR-1.
  
          E HAS BEEN DESTROYED.
  
          V CONTAINS ORTHONORMAL EIGENVECTORS OF THE SYMMETRIC
            TRIDIAGONAL (OR FULL) MATRIX.  IF AN ERROR EXIT IS MADE,
            Z CONTAINS THE EIGENVECTORS ASSOCIATED WITH THE STORED
            EIGENVALUES.
*/
  
  int n = 3;
  
  for (int i = 1; i < n; i++)
    {
      e[i-1] = e[i];
    }
  
  e[n-1] = 0.0;

  double f = 0.0;
  double tst1 = 0.0;

  //JC old place
  //double eig3__eps = pow(2.0,-52.0);
  
  // JC
  //double temp;

  for (int l = 0; l < n; l++)
    {
      
      // Find small subdiagonal element
      
      //tst1 = MAX(tst1,std::abs(d[l]) + std::abs(e[l]));
      /*temp=std::abs(d[l]) + std::abs(e[l]);
      if(tst1<temp)
	{
	  tst1=temp;
	  }*/
      tst1=std::max(tst1,std::abs(d[l]) + std::abs(e[l]));
      

      //std::cout << "tst1 = " << tst1 << std::endl;
      //std::cout << "sum  = " << std::abs(d[l]) + std::abs(e[l]) << std::endl;
      //std::cout << "sum  = " << d[l] << " + " << e[l] << std::endl;
      //std::cout << "sum  = " << std::abs(d[l]) << " + " << std::abs(e[l]) << std::endl;
      
      int m = l;
      while (m < n)
	{
	  if (std::abs(e[m]) <= eig3__eps*tst1)
	    {
	      break;
	    }
	  m++;
	}

      // If m == l, d[l] is an eigenvalue,
      // otherwise, iterate.

       
      if (m > l)
	{
	  /*if(l>=2)
	    {
	      std::cout << "l = " << l << std::endl;
	      std::cout << "m = " << m << std::endl;
	      }*/
	  int iter = 0;
	  do {
	    iter = iter + 1;  // (Could check iteration count here.)
	    
	    // Compute implicit shift

	    double g = d[l];
	    double p = (d[l+1] - g) / (2.0 * e[l]);
	    double r = hypot2(p,1.0);
	    if (p < 0)
	      {
		r = -r;
	      }
	    d[l] = e[l] / (p + r);
	    d[l+1] = e[l] * (p + r);
	    double dl1 = d[l+1];
	    double h = g - d[l];
	    for (int i = l+2; i < n; i++)
	      {
		d[i] -= h;
	      }
	    f = f + h;

	    // Implicit QL transformation.

	    p = d[m];
	    double c = 1.0;
	    double c2 = c;
	    double c3 = c;
	    double el1 = e[l+1];
	    double s = 0.0;
	    double s2 = 0.0;
	    for (int i = m-1; i >= l; i--)
	      {
		c3 = c2;
		c2 = c;
		s2 = s;
		g = c * e[i];
		h = c * p;
		r = hypot2(p,e[i]);
		e[i+1] = s * r;
		s = e[i] / r;
		c = p / r;
		p = c * d[i] - s * g;
		d[i+1] = h + s * (c * g + s * d[i]);

		// Accumulate transformation.

		for (int k = 0; k < n; k++)
		  {
		    h = V[k][i+1];
		    V[k][i+1] = s * V[k][i] + c * h;
		    V[k][i] = c * V[k][i] - s * h;
		  }
	      }
	    p = -s * s2 * c3 * el1 * e[l] / dl1;
	    e[l] = s * p;
	    d[l] = c * p;

	    // Check for convergence.

	    /*std::cout << std::abs(e[l]) << " " << eig3__eps*tst1 << std::endl;

	      std::cout << eig3__eps << std::endl;
	      std::cout << tst1 << std::endl;
	      std::cout << tst1*eig3__eps << std::endl;*/
	    
	    
	  } while (std::abs(e[l]) > eig3__eps*tst1);
	}
      d[l] = d[l] + f;
      e[l] = 0.0;
    }
  
  // Sort eigenvalues and corresponding vectors.

  for (int i = 0; i < n-1; i++)
    {
      int k = i;
      double p = d[i];
      for (int j = i+1; j < n; j++)
	{
	  if (d[j] < p)
	    {
	      k = j;
	      p = d[j];
	    }
	}
      if (k != i)
	{
	  d[k] = d[i];
	  d[i] = p;
	  for (int j = 0; j < n; j++)
	    {
	      p = V[j][i];
	      V[j][i] = V[j][k];
	      V[j][k] = p;
	    }
	}
    }
}

void
eigen_decomposition(double** A, double V[3][3], double d[3])
{

// A: a 3x3 matrix (Hessian Matrix for instance)
// V: a 3x3 matrix containing the 3 eigenvectors

  double e[3];
  for (int i = 0; i < 3; i++)
    {
      for (int j = 0; j < 3; j++)
	{
	  V[i][j] = A[i][j];
	}
    }

// make the matrix tridiagonal
  tred2(V, d, e);

// diagonalize
// V = eigenVectors
// d = eigenvalues in ascending order
// e = (subdiagonal elements, has been destroyed at the end)
  tql2(V, d, e);

}

#endif
