#include <iostream>
#include <cmath>
#include <vector>
#include <chrono>
#include <memory>
#include <thread>
#include <random>
#include <limits>
#include <boost/range/irange.hpp>
#include <Eigen/Dense>
#include "GPs.h"

// Retrieve aliases from GP namescope
using Matrix = GP::Matrix;
using Vector = GP::Vector;

// Compute pairwise distance between lists of points
void GP::pdist(Matrix & Dv, Matrix & X1, Matrix & X2)
{
  auto n = static_cast<int>(X1.rows());
  auto entryCount = static_cast<int>( (n*(n-1))/2);
  Dv.resize(entryCount, 1);

  // Get thread count
  int threadCount = Eigen::nbThreads( );

  // Get problem dimension d per thread
  auto d = static_cast<int>(n/threadCount);

  std::vector<int> startVals;
  for ( auto i : boost::irange(0,threadCount) )
    startVals.emplace_back(i*d);

  std::vector<int> endVals;
  for ( auto i : boost::irange(1,threadCount) )
    endVals.emplace_back(i*d);
  endVals.emplace_back(n-1);

  // Define lambda function specifying each threads solver task
  auto lambda = [&Dv,&X1,&X2,n](int startInd, int endInd) {
                  for ( auto i : boost::irange(startInd, endInd) )
                    {
                      for ( auto j : boost::irange(i+1,n) )
                        Dv(static_cast<int>(i*n-(i*(i+1))/2+j-i-1), 0) = (static_cast<Vector>(X1.row(i)-X2.row(j))).squaredNorm();
                    }
                };

  // Initialize thread list
  std::vector<std::thread> threadList;

  // Assign tasks to threads
  for ( auto i : boost::irange(0,threadCount) )
    threadList.emplace_back(lambda,startVals[i],endVals[i]);

  // Join threads
  for ( auto & thread : threadList )
    thread.join();

}

// Re-assemble pairwise distances into a dense matrix
void GP::squareForm(Matrix & D, Matrix & Dv, int n, double diagVal)
{
  D.resize(n,n);

  // Get thread count
  int threadCount = Eigen::nbThreads( );

  // Get problem dimension d per thread
  auto d = static_cast<int>(n/threadCount);

  std::vector<int> startVals;
  for ( auto i : boost::irange(0,threadCount) )
    startVals.emplace_back(i*d);

  std::vector<int> endVals;
  for ( auto i : boost::irange(1,threadCount) )
    endVals.emplace_back(i*d);
  endVals.emplace_back(n-1);

  // Define lambda function specifying each threads solver task
  auto lambda = [&D,&Dv,n](int startInd, int endInd) {
                  for ( auto i : boost::irange(startInd,endInd) )
                    {
                      for ( auto j : boost::irange(i+1, n) )
                       D(i,j) = D(j,i) = Dv(static_cast<int>(i*n-(i*(i+1))/2+j-i-1), 0);
                    }
                };

  // Initialize thread list
  std::vector<std::thread> threadList;

  // Assign tasks to threads
  for ( auto i : boost::irange(0,threadCount) )
    threadList.emplace_back(lambda,startVals[i],endVals[i]);

  // Join threads
  for ( auto & thread : threadList )
    thread.join();

  // Add diagonal values to distance matrix
  D.diagonal() = diagVal * Eigen::MatrixXd::Ones(n,1);
}

// Parse kernel parameter vector, separating noise from the kernel hyperparameters
void GP::Kernel::parseParams(const Vector & params, Vector & kernelParams, std::vector<double> & nonKernelParams)
{
  if ( !fixedNoise )
    {
      // Noise = params(0)
      nonKernelParams.emplace_back(params(0));
      if ( !fixedScaling )
        nonKernelParams.emplace_back(params(1));
      else
        nonKernelParams.emplace_back(scalingLevel);
    }
  else
    {
      // Scaling = params(0)
      nonKernelParams.emplace_back(noiseLevel);
      if ( !fixedScaling )
        nonKernelParams.emplace_back(params(0));
      else
        nonKernelParams.emplace_back(scalingLevel);
    }

  // Trim kernel parameter vector
  kernelParams = params.tail(paramCount);

}

// Compute covariance matrix (and gradients) from a vector of squared pairwise distances Dv
void GP::RBF::computeCov(Matrix & K, Matrix & obsX, Vector & params, std::vector<Matrix> & gradList, double jitter, bool evalGrad)
{
  auto n = static_cast<int>(K.rows());

  // Separate noise and scaling parameters from kernel hyperparameters
  Vector kernelParams;
  std::vector<double> noiseAndScaling;
  parseParams(params, kernelParams, noiseAndScaling);
  double noise = noiseAndScaling[0];
  double scaling = noiseAndScaling[1];

  // Compute distance matrix for each call
  Matrix Dv;
  pdist(Dv, obsX, obsX);

  // Evaluate covariance kernel on pairwise distance vector
  Matrix Kv;
  Kv.noalias() = scaling * ( (-0.5 / std::pow(kernelParams(0),2)) * Dv ).array().exp().matrix();

  // Make sure not to scale the jitter and noise terms
  squareForm(K, Kv, n, scaling*1.0 + jitter + noise);

  // Compute gradients w.r.t. kernel hyperparameters
  if ( evalGrad )
    {
      // Prepend gradient list with scaling term
      //int index = 0;
      //if (!fixedScaling)
      //  {
      //    Matrix dK_scaling;
      //    squareForm(dK_scaling, Kv, n, -noise);
      //    gradList[index++] = dK_scaling;
      //  }

      Matrix dK_i;
      Matrix dK_iv;
      dK_iv.noalias() = 1/std::pow(kernelParams(0),2) * ( Dv.array() * Kv.array() ).matrix();
      squareForm(dK_i, dK_iv, n); // Note: diagVal = 0.0
      gradList[0] = dK_i.eval();
      //gradList[index] = dK_i.eval();
    }

};

// Define distance kernel function for RBF
// [ Note: Optimize w.r.t. theta = log(l) for stability  ==>   / l^2  instead of  / l^3 ]
double GP::RBF::evalDistKernel(double d, Vector & params, int n)
{
  switch (n)
    {
    case 0: return std::exp( -d / (2.0*std::pow(params(0),2)));
    case 1: return d / std::pow(params(0),2) * std::exp( -d / (2.0*std::pow(params(0),2)));
    default: std::cout << "\n[*] UNDEFINED DERIVATIVE\n"; return 0.0;
    }
};

// Compute cross covariance between two input vectors using kernel parameters params
void GP::RBF::computeCrossCov(Matrix & K, Matrix & X1, Matrix & X2, Vector & params)
{
  // Get prediction count
  auto m = static_cast<int>(X2.rows());

   // Define lambda function to create unary operator (by clamping kernelParams argument)
  auto lambda = [=,&params](double d)->double { return evalDistKernel(d, params, 0); };
  for ( auto j : boost::irange(0,m) )
    {
      K.col(j) = ((X1.rowwise() - X2.row(j)).rowwise().squaredNorm()).unaryExpr(lambda);
    }

};

// Evaluate NLML for specified kernel hyperparameters p
double GP::GaussianProcess::evalNLML(const Vector & p, Vector & g, bool evalGrad)
{
  time EVAL_start = high_resolution_clock::now();

  // Get matrix input observation count
  auto n = static_cast<int>(obsX.rows());

  // ASSUME OPTIMIZATION OVER LOG VALUES
  auto params = static_cast<Vector>(p);
  params = params.array().exp().matrix();

  // Compute covariance matrix and store Cholesky factor
  Matrix K(n,n);
  time start = high_resolution_clock::now();
  (*kernel).computeCov(K, obsX, params, gradList, jitter, evalGrad);
  time end = high_resolution_clock::now();
  time_computecov += getTime(start, end);

  start = high_resolution_clock::now();
  Eigen::LLT<Matrix> _cholesky(n);
  _cholesky = K.llt();
  end = high_resolution_clock::now();
  time_cholesky_llt += getTime(start, end);

  start = high_resolution_clock::now();
  Matrix _alpha = _cholesky.solve(obsY);
  end = high_resolution_clock::now();
  time_alpha += getTime(start, end);

  // Compute NLML value
  start = high_resolution_clock::now();
  double NLML_value = (obsY.transpose()*_alpha)(0);
  NLML_value += n*std::log(2*PI);
  NLML_value *= 0.5;
  NLML_value += _cholesky.matrixLLT().diagonal().array().log().sum();
  end = high_resolution_clock::now();
  time_NLML += getTime(start, end);

  if ( evalGrad )
    {

      //
      // Precompute the multiplicative term in derivative expressions
      //
      // [ THIS APPEARS TO BE A COMPUTATIONAL BOTTLE-NECK ]
      //

      start = high_resolution_clock::now();

      // Direct Implementation
      //Matrix term(n,n);
      //term.noalias() = _cholesky.solve(Matrix::Identity(n,n));
      //term.noalias() -= _alpha*_alpha.transpose();

      // Using Solve In Place
      //Matrix term = Matrix::Identity(n,n);
      //_cholesky.solveInPlace(term);
      //term.noalias() -= _alpha*_alpha.transpose();

      //
      //  MULTI-THREADED IMPLEMENTATION
      //
      Matrix term = Matrix::Identity(n,n);

      // Get thread count
      int threadCount = Eigen::nbThreads( );

      // Get problem dimension d per thread
      auto d = static_cast<int>(n/threadCount);

      // Construct partitioned list of block terms for solver
      std::vector<Matrix> termList;
      for ( auto i : boost::irange(0,threadCount-1) )
        termList.emplace_back(term.block(0,i*d,n,d));

      // Ensure the final block extends to column n (i.e. account for index roundoff)
      termList.emplace_back(term.block(0, (threadCount-1)*d, n, n - (threadCount-1)*d) );

      // Define lambda function specifying each threads solver task
      auto lambda = [&termList,&_cholesky](int i) { _cholesky.solveInPlace(termList[i]); };

      // Initialize thread list
      std::vector<std::thread> threadList;

      // Assign tasks to threads
      for ( auto i : boost::irange(0,threadCount) )
        threadList.emplace_back(lambda,i);

      // Join threads
      for ( auto & thread : threadList )
        thread.join();

      // Reassemble blocks from solver threads back into original matrix
      for ( auto i : boost::irange(0,threadCount-1) )
        term.block(0,i*d,n,d) = termList[i];
      term.block(0, (threadCount-1)*d, n, n - (threadCount-1)*d) = termList[threadCount-1];

      // Compute final multiplicative term:  K^-1 - alpha*alpha^T
      term.noalias() -= _alpha*_alpha.transpose();

      end = high_resolution_clock::now();
      time_term += getTime(start, end);

      start = high_resolution_clock::now();
      // Compute gradient for noise term if 'fixedNoise=false'
      int index = 0;
      double noise;
      if (!fixedNoise)
        {
          // Specify gradient of white noise kernel  [ dK_i = params(0)*Matrix::Identity(n,n) ]
          //g(index++) = 0.5 * (term * params(0)).trace() ;
          noise = params(0);
          g(index) = term.trace();
          g(index++) *= 0.5 * noise;
          //g(index++) *= 0.5 * params(0);
        }
      else
        noise = noiseLevel;

      if (!fixedScaling)
        {
          if (!fixedNoise)
            {
              double trace = 0.0;
              for ( auto j : boost::irange(0, n) )
                trace -= _alpha(j)*obsY(j);
              trace += n;
              trace *= 0.5;
              // Add  1/2 * trace[(noise * term)]  (i.e. the noise gradient)
              trace -= g(index-1);
              g(index++) = trace;
            }
          else
            {
              double trace = 0.0;
              Matrix dK = K - noise*Matrix::Identity(n,n);
              for ( auto j : boost::irange(0, n) )
                trace += term.row(j)*dK.col(j);
              g(index++) = 0.5*trace;
            }
          //
          //  NOTE: The following implementation does not account for noise term (!)
          //
          //  Ah, it's so close though...
          //
          //  The adjusted covariance matrix is:  K'  =  s * K  +  noise * I
          //
          //  But if we had   K' = s*K   and set   t = log(s) ~ s = exp(t) :
          //
          //  ( so that  dK'/dt  =  d/dt[ s*K ]  =  d/ds[ s*K ] * ds/dt  =  s * K  =  K'  )
          //
          //  then it would give us the reduction:
          //
          //  d/dt -log p(y|X,t)  =  -1/2 * trace[ ( (K'^-1 y)(K'^-1 y)^T - K'^-1 ) dK'/dt  ]
          //
          //   =  -1/2 * trace[ ( K'^-1 y y^T K'^-1 - K'^-1 ) dK'/dt  ]
          //
          //   =  -1/2 * trace[ ( K'^-1 y y^T K'^1 - K'^-1 ) K'  ]
          //
          //   =  -1/2 * trace[  (K'^-1 y) y^T - I  ]
          //
          //   =  -1/2 * trace[  alpha * y^T - I  ]
          //
          //
          //  but...    we do have:
          //
          //   dK'/dt  =  d/dt[s*K]  =  s * K  =  K' - noise * I
          //
          //  so that the (corected) calculation above still yields:
          //
          //   =  -1/2 * trace[  (alpha * y^T - I)  -  noise * term  ]
          //
          //  and the trace of "term" has already been calculated...
          //
          //
          //double trace = 0.0;
          //for ( auto i : boost::irange(0,n) )
          //  trace -= _alpha(i)*obsY(i);
          //trace += n;
          //g(index++) = 0.5 * trace;
        }

      // Specify gradient w.r.t. the kernel scaling parameter
      //if (!fixedScaling)
      //  gradList.insert(gradList.begin(), K - noise*Matrix::Identity(n,n));

      // Compute gradients with respect to kernel hyperparameters
      for (auto dK_i = gradList.begin(); dK_i != gradList.end(); ++dK_i)
        {
          // Compute trace of full matrix
          //g(index++) = 0.5 * (term * (*dK_i) ).trace() ;

          // POSSIBLE MULTI-THREADED IMPLEMENTATION
          // Construct zero initialized vector of partial trace values
          Matrix traceVals = Eigen::MatrixXd::Zero(threadCount,1);

          std::vector<int> startVals;
          for ( auto i : boost::irange(0,threadCount) )
            startVals.emplace_back(i*d);

          std::vector<int> endVals;
          for ( auto i : boost::irange(1,threadCount) )
            endVals.emplace_back(i*d);
          endVals.emplace_back(n);

          // Define lambda function specifying each threads solver task
          auto lambda = [&term,&dK_i,&traceVals](int i, int startInd, int endInd) {
                          for ( auto j : boost::irange(startInd, endInd) )
                            traceVals(i) += term.row(j)*(*dK_i).col(j);
                        };

          // Initialize thread list
          std::vector<std::thread> traceThreadList;

          // Assign tasks to threads
          for ( auto i : boost::irange(0,threadCount) )
            traceThreadList.emplace_back(lambda,i,startVals[i],endVals[i]);

          // Join threads
          for ( auto & thread : traceThreadList )
            thread.join();

          // Compute final trace value for derivative calculation
          g(index++) = 0.5*(traceVals.sum());

        }
      end = high_resolution_clock::now();
      time_grad += getTime(start, end);

      // Update gradient evaluation count
      gradientEvals += 1;

    }

  time EVAL_end = high_resolution_clock::now();
  time_evaluation += getTime(EVAL_start, EVAL_end);
  return NLML_value;

}

// Define simplified interface for evaluating NLML without gradient calculation
double GP::GaussianProcess::evalNLML(const Vector & p)
{
  Vector nullGrad(0);
  return evalNLML(p,nullGrad,false);
}

int GP::GaussianProcess::getAugParamCount(int count)
{
  if (!fixedNoise)
    {
      if (!fixedScaling)
        return count + 2;
      else
        return count + 1;
    }
  else
    {
      if (!fixedScaling)
        return count + 1;
      else
        return count;
    }
}

// Fit model hyperparameters
void GP::GaussianProcess::fitModel()
{

  // Get combined parameter/noise vector size
  paramCount = (*kernel).getParamCount();
  //augParamCount = (fixedNoise) ? static_cast<int>(paramCount) : static_cast<int>(paramCount) + 1 ;
  augParamCount = getAugParamCount(paramCount);

  // Pass noise level to kernel when 'fixedNoise=true'
  if ( fixedNoise )
    (*kernel).setNoise(noiseLevel);
  if ( fixedScaling )
    (*kernel).setScaling(scalingLevel);

  // Declare vector for storing gradient calculations
  Vector g(augParamCount);

  // Initialize gradient list with identity matrices
  //for ( auto i : boost::irange(0,augParamCount) )
  //int gradientCount = paramCount;
  //if (!fixedScaling)
  //  gradientCount += 1;
  //for ( auto i : boost::irange(0,gradientCount) )

  for ( auto i : boost::irange(0,paramCount) )
    {
      // Avoid compiler warning for unused variable
      (void)i;
      gradList.push_back(Matrix::Identity(static_cast<int>(obsY.size()),static_cast<int>(obsY.size())));
    }

  // Convert hyperparameter bounds to log-scale
  Vector lbs, ubs;
  parseBounds(lbs, ubs, augParamCount);

  // Initialize optimal hyperparamter values
  Vector optParams = Eigen::MatrixXd::Zero(augParamCount,1);

  // Define restart count for optimizer
  int restartCount = solverRestarts;

  // Declare variables to store optimization loop results
  double currentVal;
  double optVal = 1e9;
  Vector theta(augParamCount);

  // Define low-precision solver for restart loop
  LBFGSpp::LBFGSParam<double> param;
  param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE;
  param.m = 10;
  param.epsilon = 1e-6;
  param.max_iterations = 10;
  param.max_linesearch = 5;
  param.delta = 1e-4;

  int niter;

  // Evaluate optimizer with various different initializations
  for ( auto i : boost::irange(0,restartCount) )
    {
      if ( i == 0 )
        {
          // Set initial guess (should make this user specifiable...)
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
        }
      else
        {
          // Sample initial hyperparameter vector
          theta = sampleUnifVector(lbs, ubs);
        }

      // Create solver and function object
      LBFGSpp::LBFGSSolver<double> solver(param);
      niter = solver.minimize(*this, theta, currentVal);

      // Compute current NLML and store parameters if optimal
      if ( currentVal < optVal ) { optVal = currentVal; optParams = theta; }

    }

  // Perform one last optimization starting from best parameters so far
  //if ( restartCount == 0 )
  //  optParams = sampleUnifVector(lbs, ubs);

  LBFGSpp::LBFGSParam<double> finalparam;

  // Line Search Options
  //param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_ARMIJO;
  //param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_WOLFE;
  //param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE;

  // TRY MODELLING SCIPY fmin_l_bfgs_b PARAMETERS
  finalparam.m = 10;
  finalparam.epsilon = 1e-5;
  finalparam.max_linesearch = 20;
  double eps = 2.220446049250313e-16;
  double factr = solverPrecision;
  finalparam.past = 1;
  //finalparam.ftol = factr*eps;
  finalparam.delta = factr*eps;
  finalparam.max_iterations = 100;

  // Create solver and function object
  LBFGSpp::LBFGSSolver<double> finalsolver(finalparam);
  niter = finalsolver.minimize(*this, optParams, optVal);

  if ( VERBOSE )
    {
      std::cout << "\n[*] Solver Iterations = " << niter <<std::endl;
      std::cout << "\n[*] Function Evaluations = " << gradientEvals <<std::endl;
    }

  // ASSUME OPTIMIZATION OVER LOG VALUES
  optParams = optParams.array().exp().matrix();

  ///* [ This is included in the SciKit Learn model.fit() call as well ]

  // Recompute covariance and Cholesky factor
  auto n = static_cast<int>(obsX.rows());
  Matrix K(n,n);
  (*kernel).computeCov(K, obsX, optParams, gradList, jitter, false);
  cholesky = K.llt();
  alpha.noalias() = cholesky.solve(obsY);

  // Assign tuned parameters to model
  if (!fixedNoise)
    {
      noiseLevel = optParams[0];
      if (!fixedScaling)
          scalingLevel = optParams[1];
    }
  else
    {
      if (!fixedScaling)
        scalingLevel = optParams[0];
    }

  optParams = static_cast<Vector>(optParams.tail(paramCount));
  (*kernel).setParams(optParams);

  // DISPLAY TIMING INFORMATION
  if ( VERBOSE )
    {
      std::cout << "\n Time Diagnostics |\n";
      std::cout << "------------------\n";
      std::cout << "computeCov():\t  " << time_computecov/gradientEvals  << std::endl;
      std::cout << "cholesky.llt():\t  " << time_cholesky_llt/gradientEvals  << std::endl;
      std::cout << "_alpha term:\t  " << time_alpha/gradientEvals  << std::endl;
      std::cout << "NLML:\t  \t  " << time_NLML/gradientEvals  << std::endl;
      std::cout << "Grad term:\t  " << time_term/gradientEvals  << std::endl;
      std::cout << "Gradient:\t  " << time_grad/gradientEvals  << std::endl;
      std::cout << "\nEvaluation:\t  " << time_evaluation/gradientEvals  << std::endl;
    }

};

// Compute predicted values
void GP::GaussianProcess::predict()
{
  // Get matrix input observation count
  auto n = static_cast<int>(obsX.rows());
  auto m = static_cast<int>(predX.rows());

  // Get optimized kernel hyperparameters
  Vector params = (*kernel).getParams();

  // Compute cross covariance for test points
  Matrix kstar_and_v;
  kstar_and_v.resize(n,m);
  (*kernel).computeCrossCov(kstar_and_v, obsX, predX, params);
  kstar_and_v *= scalingLevel;

  // Compute covariance matrix for test points
  Matrix kstarmat;
  kstarmat.resize(m,m);
  (*kernel).computeCrossCov(kstarmat, predX, predX, params);
  kstarmat *= scalingLevel;

  // Set predictive means/variances and compute negative log marginal likelihood
  Matrix cholMat(cholesky.matrixL());
  //predMean.noalias() = kstar_and_v.transpose() * _alpha;
  predMean.noalias() = kstar_and_v.transpose() * alpha;
  cholMat.triangularView<Eigen::Lower>().solveInPlace(kstar_and_v);  // kstar_and_v is now 'v'
  predCov.noalias() = kstarmat - kstar_and_v.transpose() * kstar_and_v;

}

// Draw sample paths from posterior distribution
Matrix GP::GaussianProcess::getSamples(int count)
{
  // Get number of target points
  auto n = static_cast<int>(predX.rows());

  // Construct simple random generator engine from a time-based seed
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::normal_distribution<double> normal (0.0,1.0);

  // Assign i.i.d. random normal values to uVals
  Matrix uVals(n,count);
  for (auto i : boost::irange(0,n))
    {
      for (auto j : boost::irange(0,count))
          uVals(i,j) = normal(generator);
    }

  // Compute Cholesky factor L
  Matrix L = ( predCov + (noiseLevel+jitter)*Matrix::Identity(static_cast<int>(predCov.cols()), static_cast<int>(predCov.cols())) ).llt().matrixL();

  // Draw samples using the formula:  y = m + L*u
  Matrix samples = predMean.replicate(1,count) + L*uVals;

  return samples;
}

// Evaluate NLML [public interface]
double GP::GaussianProcess::computeNLML(const Vector & p)
{
  // Compute log-hyperparameters
  Vector logparams(augParamCount);

  int index = 0;
  logparams(index++) = std::log(noiseLevel);
  logparams(index++) = std::log(scalingLevel);
  for ( auto i : boost::irange(index,augParamCount) )
    logparams(i) = std::log(p(i-index));

  // Evaluate NLML using log-hyperparameters
  return evalNLML(logparams);
}

// Evaluate NLML with default noise level [public interface]
double GP::GaussianProcess::computeNLML()
{
  auto params = (*kernel).getParams();
  return computeNLML(params);
}

// Define function for uniform sampling
Matrix GP::sampleUnif(double a, double b, int N, int dim)
{
  //return (b-a)*(Eigen::MatrixXd::Random(N,1) * 0.5 + 0.5*Eigen::MatrixXd::Ones(N,1)) + a*Eigen::MatrixXd::Ones(N,1);
  return (b-a)*(Eigen::MatrixXd::Random(N,dim) * 0.5 + 0.5*Eigen::MatrixXd::Ones(N,dim) ) + a*Eigen::MatrixXd::Ones(N,dim);
}

// Define function for uniform sampling [Vectors]
Vector GP::sampleUnifVector(Vector lbs, Vector ubs)
{
  auto n = static_cast<int>(lbs.rows());

  Vector sampleVector = ((ubs-lbs).array()*( 0.5*Eigen::MatrixXd::Random(n,1) + 0.5*Eigen::MatrixXd::Ones(n,1) ).array()).matrix() + (lbs.array()*Eigen::MatrixXd::Ones(n,1).array()).matrix();

  return sampleVector;
}

// Define function for sampling from standard normal distribution
Matrix GP::sampleNormal(int N)
{
  // Note: Boost random is currently throwing deprecated header warnings...
  //boost::random::mt19937 rng;
  //boost::random::normal_distribution<> normalDist;

  /*
  // [ NOTE: .noalias() is 100% necessary with "std::default_random_engine" ]
  std::default_random_engine rng;
  std::normal_distribution<double> normalDist(0.0,1.0);
  Matrix sampleVals(N,1);
  for ( auto i : boost::irange(0,N) )
    sampleVals(i) = normalDist(rng);
  */

  //
  //          Crude implementation of Box-Muller Transform
  // ( see https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform )
  //
  static const double epsilon = std::numeric_limits<double>::min();
  Matrix U1(N,1);
  Matrix U2(N,1);

  // Ensure log operand is not too small
  double scaledLimit = 2.0*epsilon - 1.0;
  do
    {
      U1 = Eigen::MatrixXd::Random(N,1);
      U2 = Eigen::MatrixXd::Random(N,1);
    }
  while ( U1.minCoeff() <= scaledLimit );

  // Rescale and shift Unif(-1,1) values to the interval [0,1]
  U1 =  0.5*(U1 + Eigen::MatrixXd::Ones(N,1));
  U2 =  0.5*(U2 + Eigen::MatrixXd::Ones(N,1));

  // Compute transformed values with .noalias() to ensure evaluation
  Matrix sampleVals;
  sampleVals.noalias() = ((-2.0*(U1.array().log()).matrix()).array().sqrt() * (2*PI*U2).array().cos()).matrix();

  return sampleVals;
}

// Generate equally spaced points on an interval or square region
Matrix GP::linspace(double a, double b, int N, int dim)
{

  Matrix linspaceVals;
  if ( dim == 1 )
    {
      linspaceVals.resize(N,1);
      linspaceVals = Eigen::Array<double, Eigen::Dynamic, 1>::LinSpaced(N, a, b);
    }
  else if ( dim == 2 )
    {
      linspaceVals.resize(N*N,2);
      Matrix linspaceVals1D = Eigen::Array<double, Eigen::Dynamic, 1>::LinSpaced(N, a, b);
      int k = 0;
      for ( auto i : boost::irange(0,N) )
        {
          for ( auto j : boost::irange(0,N) )
            {
              linspaceVals(k,0) = linspaceVals1D(i);
              linspaceVals(k,1) = linspaceVals1D(j);
              k++;
            }
        }
    }
  else
      std::cout << "[*] GP::linspace has not been implemented for dim > 2\n";

  return linspaceVals;
}

// Define function for retrieving time from chrono
float GP::getTime(std::chrono::high_resolution_clock::time_point start, std::chrono::high_resolution_clock::time_point end)
{
  return static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>( end - start ).count() / 1000000.0);
};

// Define utility function for formatting hyperparameter bounds
void GP::GaussianProcess::parseBounds(Vector & lbs, Vector & ubs, int augParamCount)
{
  lbs.resize(augParamCount);
  ubs.resize(augParamCount);

  double defaultLowerBound = 0.01;
  double defaultUpperBound = 2.0;

  if ( fixedBounds )
    {
      // Check if bounds for noise parameter were provided
      if ( lowerBounds.size() < augParamCount )
        {
          // Set noise bounds to defaults
          lbs(0) = std::log( defaultLowerBound );
          ubs(0) = std::log( defaultUpperBound );

          // Convert specified bounds to log-scale
          for ( auto bi : boost::irange(1,augParamCount) )
            {
              lbs(bi) = std::log(lowerBounds(bi-1));
              ubs(bi) = std::log(upperBounds(bi-1));
            }
        }
      else
        {
          // Convert specified bounds to log-scale
          lbs = (lowerBounds.array().log()).matrix();
          ubs = (upperBounds.array().log()).matrix();
        }
    }
  else
    {
      // Set noise and hyperparameter bounds to defaults
      lbs = ( defaultLowerBound * Eigen::MatrixXd::Ones(augParamCount,1) ).array().log().matrix();
      ubs = ( defaultUpperBound * Eigen::MatrixXd::Ones(augParamCount,1) ).array().log().matrix();
    }

}

/*
//  POSSIBLY UNNEEDED POINTWISE KERNEL DEFINITIONS

// Define kernel function for RBF
double GP::RBF::evalKernel(Matrix & x, Matrix & y, Vector & params, int n)
{
  switch (n)
    {
    case 0: return std::exp( -(x-y).squaredNorm() / (2.0*std::pow(params(0),2)));
    case 1: return (x-y).squaredNorm() / std::pow(params(0),3) * std::exp( -(x-y).squaredNorm() / (2.0*std::pow(params(0),2)));
    default: std::cout << "\n[*] UNDEFINED DERIVATIVE\n"; return 0.0;
    }
};

*/

/*
// POTENTIAL PARALLEL IMPLEMENTATION OF SQUARE FORM; SPEED-UP APPEARS NEGLIGIBLE
// Re-assemble pairwise distances into a dense matrix
void GP::squareFormParallel(Matrix & D, Matrix & Dv, int n, double diagVal)
{

  D.resize(n,n);
  int i;
  int j;
#pragma omp parallel for private(i,j) shared(D,Dv,n)
  for ( i = 0 ; i<n-1; i++ )
    {
      for ( j = i+1 ; j<n ; j++ )
        {
          D(i,j) = D(j,i) = Dv( static_cast<int>(i*n - (i*(i+1))/2 + j - i -1) ,0);
        }
    }
  D.diagonal() = diagVal * Eigen::MatrixXd::Ones(n,1);
}
*/

//
//   SECOND MINIMIZATION IMPLEMENTATION USING CPPOPTLIB
//

// Fit model hyperparameters
//void GP::GaussianProcess::fitModel()

// Initialize gradient vector size
//cppOptLibgrad.resize(augParamCount);

/*
this->setLowerBound(lbs);
this->setUpperBound(ubs);
cppoptlib::LbfgsbSolver<GaussianProcess> solver;

// Specify stopping criteria
cppoptlib::Criteria<double> crit = cppoptlib::Criteria<double>::defaults();
//crit.iterations = 10; //!< Maximum number of iterations
crit.gradNorm = 1e-6;   //!< Minimum norm of gradient vector
//crit.fDelta = 7.5e-5;   //!< Minimum [relative] change in cost function
crit.iterations = solverIterations;
crit.fDelta = solverPrecision;
solver.setStopCriteria(crit);
solver.setHistorySize(10);

solver.minimize(*this, optParams);

*/

// Display final solver criteria values
/*
std::cout << "\nSolver Criteria |";
std::cout << "\n----------------\n" << solver.criteria() << std::endl;
std::cout << "gradEvals =\t" << gradientEvals <<std::endl;
*/

//
//   ORIGINAL MINIMIZATION IMPLEMENTATION USING RASMUSSEN'S CODE
//

//
//    NOTE:
//
//    Remember to include "utils/minimize.h" and derive the
//   'GaussianProcess' class from the 'GradientObj' class:
//
//    i.e.  class GaussianProcess : public minimize::GradientObj
//

/*
void minimize(...)
{

  //
  // Specify the parameters for the minimization algorithm
  //

  //   HIGH ACCURACY SETTINGS   //
  // max of MAX function evaluations per line search
  //int MAX = 30;
  // max number of line searches = length
  //int length = 20;
  // don't reevaluate within INT of the limit of the current bracket
  //double INT = 0.00001;
  // SIG is a constant controlling the Wolfe-Powell conditions
  //double SIG = 0.9;
  // extrapolate maximum EXT times the current step-size
  //double EXT = 5.0;

  //  EFFICIENT SETTINGS  //

  int MAX = 15;
  int length = 10;
  double INT = 0.00001;
  double SIG = 0.9;
  double EXT = 5.0;

  // Define number of exploratory NLML evaluations for specifying
  // a reasonable initial value for the optimization algorithm
  int initParamSearchCount = 30;

  // Define restart count for optimizer
  int restartCount = 0;

  // Convert hyperparameter bounds to log-scale
  Vector lbs, ubs;
  parseBounds(lbs, ubs, augParamCount);

  // Declare variables to store optimization loop results
  double currentVal;
  double optVal = 1e9;
  Vector theta(augParamCount);
  Vector optParams(augParamCount);

  //time start = high_resolution_clock::now();
  // First explore hyperparameter space to get a reasonable initializer for optimization
  for ( auto i : boost::irange(0,initParamSearchCount) )
    {
      if ( i == 0 )
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
      else
          theta = sampleUnifVector(lbs, ubs);

      // Compute current NLML and store parameters if optimal
      currentVal = evalNLML(theta);
      if ( currentVal < optVal )
        {
          optVal = currentVal;
          optParams = theta;
        }
      //std::cout << "Theta Search:  " << theta.transpose() << "  [ NLML = " << currentVal << " ]"<< std::endl;
    }
  //time end = high_resolution_clock::now();
  //time_paramsearch += getTime(start, end);

  // NOTE: THIS NEEDS TO BE RE-WRITTEN TO USE THE PRELIMINARY PARAMETER SEARCH RESULTS
  // Evaluate optimizer with various different initializations
  for ( auto i : boost::irange(0,restartCount) )
    {
      // Avoid compiler warning for unused variable
      //(void)i;

      if ( i == 0 )
        {
          // Set initial guess (should make this user specifiable...)
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
        }
      else
        {
          // Sample initial hyperparameter vector
          theta = sampleUnifVector(lbs, ubs);
        }

      // Optimize hyperparameters
      minimize::cg_minimize(theta, this, g, length, SIG, EXT, INT, MAX);

      // Compute current NLML and store parameters if optimal
      currentVal = evalNLML(theta);
      if ( currentVal < optVal )
        {
          optVal = currentVal;
          optParams = theta;
        }
    }

  // Perform one last optimization starting from best parameters so far
  if ( ( initParamSearchCount == 0 ) && ( restartCount == 0 ) )
    optParams = sampleUnifVector(lbs, ubs);
  //std::cout << "\n[*] FINAL - Initial Values (log):  " << optParams.transpose() << std::endl;
  //std::cout << "[*] FINAL - Initial Values (std):  " << optParams.transpose().array().exp().matrix() << std::endl;
  //start = high_resolution_clock::now();
  minimize::cg_minimize(optParams, this, g, length, SIG, EXT, INT, MAX);
  //end = high_resolution_clock::now();
  //time_minimize += getTime(start, end);

}
*/


/**
 * @file stabrk3_main.cc
 * @brief NPDE homework StabRK3 code
 * @author Oliver Rietmann, Philippe Peter
 * @date 13.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iomanip>
#include <iostream>
#include <vector>

#include "stabrk3.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  // aproximate reference solution used in the convergence study
  double T = 1.0;
  Eigen::Vector2d y0(100.0, T);
  Eigen::Vector2d yT_reference = StabRK3::PredPrey(y0, T, std::pow(2, 14));
  std::cout << "Solution Computed by PredPrey(): "
            << yT_reference.transpose().format(CSVFormat) << std::endl;

  // Convergence study
  StabRK3::SimulatePredPrey();

  return 0;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2013 by Tatsuya Sakashita <t-sakashita@issp.u-tokyo.ac.jp>,
*                            Synge Todo <wistaria@comp-phys.org>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <iostream>
#include <fstream>
#include <vector>
#include <boost/tuple/tuple.hpp>

#include <rokko/utility/xyz_hamiltonian.hpp>
#include <rokko/localized_matrix.hpp>
#include <rokko/localized_vector.hpp>

int main(int argc, char *argv[])
{
  if (argc <= 1) {
    std::cerr << "error: " << argv[0] << " xyz.ip" << std::endl;
    exit(1);
  }

  std::ifstream ifs(argv[1]);
  if (!ifs) {
    std::cout << "can't open file" << std::endl;
    exit(2);
  }

  int L, num_bonds;
  std::vector<std::pair<int, int> > lattice;
  std::vector<boost::tuple<double, double, double> > coupling;
  ifs >> L >> num_bonds;
  for (int i=0; i<num_bonds; ++i) {
    int j, k;
    ifs >> j >> k;
    lattice.push_back(std::make_pair(j, k));
  }

  for (int i=0; i<num_bonds; ++i) {
    double jx, jy, jz;
    ifs >> jx >> jy >> jz;
    coupling.push_back(boost::make_tuple(jx, jy, jz));
  }

  std::cout << "L=" << L << " num_bonds=" << num_bonds << std::endl;
  for (int i=0; i<num_bonds; ++i) {
    std::cout << lattice[i].first << " " << lattice[i].second << " " << coupling[i].get<0>() << " " << coupling[i].get<1>() << " " << coupling[i].get<2>() << std::endl;
  }
  int dim = 1 << L;
  int N = dim;
  std::cout << "dim=" << dim << std::endl;

  rokko::localized_matrix<rokko::matrix_col_major> mat1(N, N);
  std::cout << "multiply:" << std::endl;
  for (int i=0; i<N; ++i) {
    std::vector<double> v, w;
    v.assign(N, 0);
    v[i] = 1;
    w.assign(N, 0);
    rokko::xyz_hamiltonian::multiply(L, lattice, coupling, v, w);
    for (int j=0; j<N; ++j) {
      mat1(j,i) = w[j];
      std::cout << w[j] << " ";
    }
    std::cout << std::endl;
  }

  std::cout << "fill_diagonal:" << std::endl;
  rokko::localized_vector diagonal(N);
  std::vector<double> v(N);
  rokko::xyz_hamiltonian::fill_diagonal(L, lattice, coupling, v);
  for (int j=0; j<N; ++j) {
    diagonal(j) = v[j];
    std::cout << v[j] << " ";
  }
  std::cout << std::endl;

  std::cout << "fill_matrix:" << std::endl;
  rokko::localized_matrix<rokko::matrix_col_major> mat2(N, N);
  rokko::xyz_hamiltonian::generate(L, lattice, coupling, mat2);
  for (int i=0; i<N; ++i) {
    for (int j=0; j<N; ++j) {
      std::cout << mat2(i,j) << " ";
    }
    std::cout << std::endl;
  }

  if (mat1 == mat2) {
    std::cout << "OK: matrix by 'multiply' equals to a matrix by 'generate'." << std::endl;
  } else {
    std::cout << "ERROR: matrix by 'multiply' is differnet from a matrix by 'generate'."<< std::endl;
    exit(1);
  }

  if (diagonal == mat2.diagonal()) {
    std::cout << "OK: diagonal by 'fill_diagonal' equals to diagonal elementas of a matrix by 'genertate'."<< std::endl;
  } else {
    std::cout << "ERROR: diagonal by 'fill_diagonal' is differnet from diagonal elementas of a matrix by 'genertate'."<< std::endl;
    exit(1);
  }

}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <string>
#include <utility>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/splitting.h"
#include "vlasovpp/lagrange5.h"
#include "vlasovpp/config.h"
#include "vlasovpp/signal_handler.h"
#include "vlasovpp/iteration.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

auto
error_H ( std::size_t Nx , std::size_t Nv , double dt )
{
  const double Tf = 6.5;

  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

  const double Kx = 0.5;
  fh.range.v_min = -12.; fh.range.v_max = 12.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();
  fh.compute_steps();

  ublas::vector<double> v(Nv,0.);
  std::generate( v.begin() , v.end() , [&,k=0]() mutable {return (k++)*fh.step.dv+fh.range.v_min;} );

  ublas::vector<double> kx(Nx); // beware, Nx need to be odd
  {
    double l = fh.range.len_x();
    for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
    for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  const double alpha = 0.2 , ui = 3.4;
  auto tb_M1 = maxwellian( 0.5*alpha , ui , 1. ) , tb_M2 = maxwellian( 0.5*alpha , -ui , 1. );
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      hfh[k][i] = 0.;
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(fh[k].begin(),fh[k].end(),hfh[k].begin());
  }

  std::size_t iter = 0;
  double current_time = 0.;

  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);

  std::vector<double> H;
  std::vector<double> ee;
  std::vector<double> t;

  const double rho_c = 1.-alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);

  // init E with Poisson solver, init also ee, Emax, H and times
  {
    poisson<double> poisson_solver(Nx,fh.range.len_x());
    ublas::vector<double> rho(Nx,0.); rho = fh.density(); // compute density from init hot data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += (1.-alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    double total_energy = energy(fh,E);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
  }
  t.push_back(current_time);

  // initialize memory for all temporary variables
  splitting<double,1> Lie( fh , fh.range.len_x() , rho_c );

  ublas::vector<double> uc1(Nx) , uc2(Nx) , uc3(Nx) , uc4(Nx) , ucn(Nx),
                        E1 (Nx) , E2 (Nx) , E3 (Nx) , E4 (Nx) , En (Nx);
  complex_field<double,1> hfh1(boost::extents[Nv][Nx]) , hfh2(boost::extents[Nv][Nx]) ,
                          hfh3(boost::extents[Nv][Nx]) , hfh4(boost::extents[Nv][Nx]) ,
                          hfhn(boost::extents[Nv][Nx]) ;

  const double alpha1=1./(4.-std::cbrt(4.)), alpha2=alpha1, alpha3=1./(1.-SQ(std::cbrt(4.)));

  const double g1 = alpha1, g2 = alpha1+alpha2;
  const double w1 = (g2*(1.-g2))/(g1*(g1-1.)-g2*(g2-1.)) , w2 = 1.-w1 , w3 = w2 , w4 = w1;

  while (  current_time < Tf ) {
  /*std::cout << *std::max_element( hfh[100].begin() , hfh[100].end() , [](auto a,auto b){
    return std::abs(a) < std::abs(b);
  }) << " " << std::endl;*/

///////////////////////////////////////////////////////////////////////////////
// Suzuki /////////////////////////////////////////////////////////////////////

    /*
    Lie.phi_a(0.5*dt,uc,E,hfh);
    Lie.phi_c(0.5*dt,uc,E,hfh);
    Lie.phi_b(dt,uc,E,hfh);
    Lie.phi_c(0.5*dt,uc,E,hfh);
    Lie.phi_a(0.5*dt,uc,E,hfh);
    */

    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);

    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);

    // Strang alpha3
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha3*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);

    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);

    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);

///////////////////////////////////////////////////////////////////////////////
// MONITORING /////////////////////////////////////////////////////////////////

    for ( auto k=0 ; k<Nv ; ++k ) {
      fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin());
    }

    double total_energy = kinetic_energy(fh);
    for ( const auto ei : E ) {
      total_energy += ei*ei*fh.step.dx;
    }
    {
      auto rhoh = fh.density();
      fft::spectrum_ hrhoh(Nx); hrhoh.fft(&rhoh[0]);
      fft::spectrum_ hE(Nx); hE.fft(&E[0]);
      fft::spectrum_ hrhoc(Nx);
      hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
      for ( auto i=1 ; i<Nx ; ++i ) {
        hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
      }
      ublas::vector<double> rhoc (Nx,0.); hrhoc.ifft(&rhoc[0]);

      for ( auto i=0 ; i<Nx ; ++i ) {
        total_energy += rhoc[i]*uc[i]*uc[i]*fh.step.dx;
      }
    }
    H.push_back( total_energy );

    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );

    // increment time
    current_time += dt;
    t.push_back(current_time);

    ++iter;
    //if ( current_time+dt > Tf ) { dt = Tf - current_time; }
  } // while (  current_time < Tf ) // end of time loop

  std::ofstream of("H.dat");
  std::transform( H.cbegin() , H.cend() , std::ostream_iterator<std::string>(of,"\n") ,
    [&,count=0] ( const double h ) mutable {
      std::stringstream ss;
      ss << t[count++] << " " << h << " " << std::abs(H[0]-h)/std::abs(H[0]);
      return ss.str();
  });
  of.close();

  of.open("ee.dat");
  std::transform( ee.cbegin() , ee.cend() , std::ostream_iterator<std::string>(of,"\n") ,
    [&,count=0] ( const double e ) mutable {
      std::stringstream ss;
      ss << t[count++] << " " << e;
      return ss.str();
  });
  of.close();

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
  fh.write("vp.dat");

  double h_max = std::abs(*std::max_element( H.begin() , H.end() ,
    [&] ( double a , double b ) {
      return std::abs(a-H[0])/std::abs(H[0]) < std::abs(b-H[0])/std::abs(H[0]);
    }
  ));

  double h_last = H.back();
  //return std::make_pair(std::abs(H[0]-h_last)/std::abs(H[0]),std::abs(H[0]-h_max)/std::abs(H[0]));
  double h = std::abs(*std::max_element( H.begin() , H.end() , [&](double a,double b){return ( std::abs((a-H[0])/std::abs(H[0])) < std::abs((b-H[0])/std::abs(H[0])) );} ));
  return std::abs((h-H[0])/std::abs(H[0]));
}

int
main ( int argc , char const * argv[] )
{
  const std::size_t Nx = 75 , Nv = 1024;
  const double dt_max = 1.;//3.*16./Nv;

  for ( auto i=1 ; i<10 ; ++i ) {
    double dt = dt_max/double(i);
    std::cout << dt << " " << std::flush;
    double h = error_H(Nx,Nv,dt);
    std::cout << h << std::endl;
  }
  return 0;
}


#include <cassert>
#include <cmath>
#include <cstdint>
#include <chrono>
#include <iostream>
#include <tuple>

#include <boost/optional.hpp>

#define MEASURE(CODE_TO_MEASURE) \
    { \
    auto start = std::chrono::system_clock::now(); \
    CODE_TO_MEASURE \
    auto end = std::chrono::system_clock::now(); \
    std::chrono::duration<double> difference = end - start; \
    std::cout << difference.count(); \
    }

enum class ECode : uint64_t {
    OK = 0xFFF0'0000'0000'0001,
    ERROR,
    INPUT_IS_NAN,
    INPUT_IS_INFINITE,
    INPUT_IS_NEGATIVE,
};

union Result_or_code
{
    double result;
    ECode code;
    Result_or_code(double x) {result = x;}
    Result_or_code(const ECode& c) {code = c;}
    operator double() {return result;}
    operator ECode() {return code;}
};

Result_or_code sqrt_or_not(double x) {
    if (std::isnan(x))
        return ECode::INPUT_IS_NAN;
    if (std::isinf(x))
        return ECode::INPUT_IS_INFINITE;
    if(x < 0)
        return ECode::INPUT_IS_NEGATIVE;
    return std::sqrt(x);
}

boost::optional<double> optional_sqrt(double x) {
    if (std::isnan(x) || std::isinf(x) || x < 0)
        return boost::optional<double>();
    return boost::optional<double>(std::sqrt(x));
}

double sqrt_or_throw(double x) {
    if (std::isnan(x))
        throw std::invalid_argument("");
    if (std::isinf(x))
        throw std::invalid_argument("");
    if(x < 0)
        throw std::domain_error("");
    return std::sqrt(x);
}

using CodeAndResult = std::tuple<ECode, double>;
CodeAndResult code_and_sqrt(double x) {
    if (std::isnan(x))
        return std::make_tuple(ECode::INPUT_IS_NAN, 0.);
    if (std::isinf(x))
        return std::make_tuple(ECode::INPUT_IS_INFINITE, 0.);
    if(x < 0)
        return std::make_tuple(ECode::INPUT_IS_NEGATIVE, 0.);
    return std::make_tuple(ECode::OK, std::sqrt(x));
}

void measure_3_comparisons() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "3 comparisons: ";
    MEASURE(
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            auto root = sqrt_or_not(x);
            if(root == ECode::INPUT_IS_NAN || root == ECode::INPUT_IS_INFINITE || root == ECode::INPUT_IS_NEGATIVE)
                ++errors;
            else {
                ++results;
                total += root;
            }
        }
    );
    std::cout << "\nsize of union: " << sizeof(Result_or_code);
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_isnan() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "is nan: ";
    MEASURE(
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            auto root = sqrt_or_not(x);
            if(std::isnan(root))
                ++errors;
            else {
                ++results;
                total += root;
            }
        }
    );
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_non_less_than_error() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "no less than error: ";
    MEASURE(
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            auto root = sqrt_or_not(x);
            if(root >= ECode::ERROR)
                ++errors;
            else {
                ++results;
                total += root;
            }
        }
    );
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_non_less_than_error_storing() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "no less than error: ";
    MEASURE(
        std::vector<Result_or_code> results_or_codes;
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            results_or_codes.push_back(sqrt_or_not(x));
            if(results_or_codes.back() >= ECode::ERROR)
                ++errors;
            else {
                ++results;
                total += results_or_codes.back();
            }
        }
    );
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_tuple() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "tuple: ";
    MEASURE(
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            auto code_and_root = code_and_sqrt(x);
            if(std::get<0>(code_and_root) != ECode::OK)
                ++errors;
            else {
                ++results;
                total += std::get<1>(code_and_root);
            }
        }
    );
    std::cout << "\nsize of tuple: " << sizeof(std::tuple<ECode, double>);
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_tuple_storing() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "tuple: ";
    MEASURE(
        std::vector<CodeAndResult> results_and_codes;
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            results_and_codes.push_back(code_and_sqrt(x));
            if(std::get<0>(results_and_codes.back()) != ECode::OK)
                ++errors;
            else {
                ++results;
                total += std::get<1>(results_and_codes.back());
            }
        }
    );
    std::cout << "\nsize of tuple: " << sizeof(std::tuple<ECode, double>);
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_optional() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "optional: ";
    MEASURE(
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            auto optional_root = optional_sqrt(x);
            if(!optional_root)
                ++errors;
            else {
                ++results;
                total += *optional_root;
            }
        }
    );
    std::cout << "\nsize of optional: " << sizeof(boost::optional<double>);
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_optional_storing() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "optional: ";
    MEASURE(
        std::vector<boost::optional<double>> optional_results;
        for(double x = -1024.; x <= 1024.; x += 1./65536.) {
            optional_results.push_back(optional_sqrt(x));
            if(!optional_results.back())
                ++errors;
            else {
                ++results;
                total += *optional_results.back();
            }
        }
    );
    std::cout << "\nsize of optional: " << sizeof(boost::optional<double>);
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

void measure_throw() {
    size_t errors = 0;
    size_t results = 0;
    double total = 0.;
    std::cout << "throw (512 less experiments!): ";
    MEASURE(
        for(double x = -2.; x <= 2.; x += 1./65536.) {
            try {
                auto root = sqrt_or_throw(x);
                total += root;
                ++results;
            } catch (const std::logic_error& e) {
                ++errors;
            }
        }
    );
    std::cout << "\nsanity check: " << ((errors == results - 1) == 1. ? "passed" : "not passed!") << "; sum: " << total << "\n" << std::endl;
}

int main(void) {
    measure_3_comparisons();
    measure_isnan();
    measure_non_less_than_error();
    measure_non_less_than_error_storing();
    measure_tuple();
    measure_tuple_storing();
    measure_optional();
    measure_optional_storing();
    measure_throw();
}