#ifndef __ESKOW_CHOLESKY_HPP
#define __ESKOW_CHOLESKY_HPP

#include <cmath>
#include <vector>
#include "mach.hpp"

/* Implementation of Schnabel & Eskow, 1999, Vol. 9, No. 4, pp. 1135-148, SIAM J. OPTIM. */

namespace CholeskyEskow
{

  template<typename T, typename A>
  T max_diag(A& m, int j, int N)
  {
    T maxval = m(j,j);

    for (int k = j+1; k < N; k++)
      {
	maxval = std::max(maxval, m(k,k));
      }
    return maxval;
  }

  template<typename T, typename A>
  void minmax_diag(A& m, int j, int N, T& minval, T& maxval, int& i_min, int& i_max)
  {
    minval = maxval = m(j,j);

    for (int k = j+1; k < N; k++)
      {
	maxval = std::max(maxval, m(k,k));
	minval = std::min(minval, m(k,k));
      }
  
    for (int k = j; k < N; k++)
      {
	if (m(k,k) == minval)
	  i_min = k;
	if (m(k,k) == maxval)
	  i_max = k;
      }
  }

  template<typename T, typename A>
  void swap_rows(A& m, int N, int i0, int i1)
  {
    for (int r = 0; r < N; r++)
      std::swap(m(r,i0), m(r,i1));
  }

  template<typename T, typename A>
  void swap_cols(A& m, int N, int i0, int i1)
  {
    for (int c = 0; c < N; c++)
      std::swap(m(i0,c), m(i1,c));
  }

  template<typename T>
  T square(T x)
  {
    return x*x;
  }

  template<typename T, typename A>
  T min_row(A& m, int j, int N)
  {
    T a = 1/m(j,j);
    T v = m(j+1,j+1) - square(m(j+1,j))*a;
    
    for (int i = j+2; i < N; i++)
      {
	v = std::max(v, m(i, i) - square(m(i,j))*a);
      }

    return v;
  }

  template<typename T>
  int g_max(const std::vector<T>& g, int j, int N)
  {
    T a = g[j];
    int k = j;

    for (int i = j+1; i < N; i++)
      {
	if (a < g[i])
	  {
	    a = g[i];
	    k = i;
	  }
      }
    return k;
  }

  template<typename T, typename A>
  void cholesky_eskow(A& m, int N, T& norm_E)
  {
    T tau_bar = std::pow(mach_epsilon<T>(), 2./3);
    T tau = std::pow(mach_epsilon<T>(), 1./3);
    T mu = 0.1;
    bool phaseone = true;
    T gamma = max_diag<T,A>(m, 0, N);
    int j;

    norm_E = 0;

    for (j = 0; j < N && phaseone; j++)
      {
	T minval, maxval;
	int i_min, i_max;
      
	minmax_diag<T,A>(m, j, N, minval, maxval, i_min, i_max);
	if (maxval < tau_bar*gamma || minval < -mu*maxval)
	  {
	    phaseone = false;
	    break;
	  }
      
	if (i_max != j)
	  {
	    swap_cols<T,A>(m, N, i_max, j);
	    swap_rows<T,A>(m, N, i_max, j);
	  }
      
	if (min_row<T,A>(m, j, N) < -mu*gamma)
	  {
	    phaseone = false;
	    break;
	  }
      
	T L_jj = std::sqrt(m(j,j));

	m(j,j) = L_jj;
	for (int i = j+1; i < N; i++)
	  {
	    m(i,j) /= L_jj;
	    for (int k = j+1; k < i; k++)
	      m(i,k) -= m(i,j)*m(k,j);
	  }
      }


    if (!phaseone && j == N-1)
      {
	T A_nn = m(N-1,N-1);
	T delta = -A_nn + std::max(tau*(-A_nn)/(1-tau), tau_bar*gamma);
      
	m(N-1,N-1) = std::sqrt(m(N-1,N-1) + delta);	  	  
      }

    


    if (!phaseone && j < (N-1))
      {
	int k = j-1;
	std::vector<T> g(N);

	for (int i = k+1; i < N; i++)
	  {
	    g[i] = m(i,i);
	    for (int j = k+1; j < i; j++)
	      g[i] -= std::abs(m(i,j));
	    for (int j = i+1; j < N; j++)
	      g[i] -= std::abs(m(j,i));
	  }

	T delta, delta_prev = 0;
	
	for (int j = k+1; j < N-2; j++)
	  {
	    int i = g_max(g, j, N);
	    T norm_j;

	    if (i != j)
	      {
		swap_cols<T,A>(m, N, i, j);
		swap_rows<T,A>(m, N, i, j);		
	      }

	    for (int i = j+1; j < N; j++)
	      {
		norm_j += std::abs(m(i,j));
	      }
	    
	    delta = std::max(delta_prev, std::max((T)0, -m(j,j) + std::max(norm_j,tau_bar*gamma)));
	    if (delta > 0)
	      {
		m(j,j) += delta;
		delta_prev = delta;
	      }
	    
	    if (m(j,j) != norm_j)
	      {
		T temp = 1 - norm_j/m(j,j);
		
		for (int i = j+1; j < N; j++)
		  {
		    g[i] += std::abs(m(i,j))*temp;
		  }
	      }

	    // Now we do the classic cholesky iteration
	    T L_jj = std::sqrt(m(j,j));
	    
	    m(j,j) = L_jj;
	    for (int i = j+1; i < N; i++)
	      {
		m(i,j) /= L_jj;
		for (int k = j+1; k < i; k++)
		  m(i,k) -= m(i,j)*m(k,j);
	      }
	  }

	// The final 2x2 submatrix is special
	T A00 = m(N-2, N-2), A01 = m(N-2, N-1), A11 = m(N-1,N-1);
	T sq_DELTA = std::sqrt(square(A00-A11) + square(A01));
	T lambda_hi = 0.5*((A00+A11) + sq_DELTA);
	T lambda_lo = 0.5*((A00+A11) - sq_DELTA);
       
	delta = std::max(std::max((T)0, -lambda_lo + std::max(tau*sq_DELTA/(1-tau), tau_bar*gamma)),delta_prev);
	if (delta > 0)
	  {
	    m(N-1,N-1) += delta;
	    m(N,N) += delta;
	    delta_prev = delta;
	  }
	m(N-2,N-2) = A00 = std::sqrt(A00);
	m(N-1,N-2) = (A01 /= A00);
	m(N-1,N-1) = std::sqrt(A11-A01*A01);
	norm_E = delta_prev;
      }
  }

};


#endif