//
//  meanie3D-satconv
//

#include <string>
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/program_options/parsers.hpp>

using namespace std;
using namespace boost;

#pragma mark -
#pragma mark Definitions

typedef enum {
    RadicanceToTemperature,
    TemperatureToRadiance
} ConversionType;

// Radiation constant #1
const double c1 = 1.19104e-05;
// Radiation constant #2
const double c2 = 1.43877;

// Names of the variables available for conversion
const string variables[8] = {
    "msevi_l15_ir_039",
    "msevi_l15_ir_087",
    "msevi_l15_ir_097",
    "msevi_l15_ir_108",
    "msevi_l15_ir_120",
    "msevi_l15_ir_134",
    "msevi_l15_wv_062",
    "msevi_l15_wv_073"
};

// Calibration constant alpha
const double alpha[8] = {
    0.9954,
    0.9996,
    0.9999,
    0.9983,
    0.9988,
    0.9981,
    0.9963,
    0.9991
};

// Calibration constant beta
const double beta[8] = {
    3.438,
    0.179,
    0.056,
    0.64,
    0.408,
    0.561,
    2.185,
    0.47
};

// Wavenumber
const double wavenum[8] = {
    2568.832,
    1148.62,
    1035.289,
    931.7,
    836.445,
    751.792,
    1600.548,
    1360.33
};

#pragma mark -
#pragma mark Command line parsing

void parse_commmandline(program_options::variables_map vm,
        int& variableIndex,
        double &value,
        ConversionType &conversionType)
{
    if (vm.count("temperature") != 0) {
        conversionType = RadicanceToTemperature;
    } else if (vm.count("radiance") != 0) {
        conversionType = TemperatureToRadiance;
    } else {
        cerr << "Specify -r or -t" << endl;
        exit(EXIT_FAILURE);
    }

    if (vm.count("value") == 0) {
        cerr << "Missing --value argument" << endl;
        exit(EXIT_FAILURE);
    }
    value = vm["value"].as<double>();

    if (vm.count("variable") == 0) {
        cerr << "Missing --variable argument" << endl;
        exit(EXIT_FAILURE);
    }
    string variable = vm["variable"].as<string>();

    variableIndex = -1;
    for (int i=0; i<8; i++) {
        if (variables[i] == variable) {
            variableIndex = i;
            break;
        }
    }
    if (variableIndex < 0) {
        cerr << "Illegal value for --variable" << endl;
        exit(EXIT_FAILURE);
    }

}

#pragma mark -
#pragma mark Helper Methods

/** Calculates the brightness temperature in degree centigrade
* from the given spectral radiance.
 *
* @param index of the variable to be converted
* @param radiance value for the given channel
* @return equivalent brightness temperature in [degree C]
*/
double brightness_temperature(const int var_index, const double &radiance) {
   double nu = wavenum[var_index];
   double Tbb = c2 * nu / log(1 + nu * nu * nu * c1 / radiance);
   double Tb = (Tbb - beta[var_index]) / alpha[var_index];
   return Tb - 273.15;
}

/** Inverse calculation. Spectral radiance from temperature
* in degree centigrade
 *
* @param index of the variable to be converted
* @param brightness temperature in [degree C]
* @return radiance value for the given channel
*/
double spectral_radiance(const size_t var_index, const double &temperature) {
   double nu = wavenum[var_index];
   double Tbb = (temperature + 273.15) * alpha[var_index] + beta[var_index];
   return nu * nu * nu * c1 / (exp(c2 * nu / Tbb) - 1);
}

#pragma mark -
#pragma mark MAIN

int main(int argc, char** argv)
{
    // Declare the supported options.
    namespace po = boost::program_options;

    po::positional_options_description p;
    p.add("value", -1);

    po::options_description desc("Converts spectral radiance to equivalent brightness temperature or vice versa.");
    desc.add_options()
            ("temperature,t", "radiance to temperature")
            ("radiance,r", "temperature to radiance")
            ("value", po::value<double>(), "value to convert")
            ("variable,v", po::value<string>(), "One of msevi_l15_ir_039, msevi_l15_ir_087, msevi_l15_ir_097, msevi_l15_ir_108, msevi_l15_ir_120, msevi_l15_ir_134, msevi_l15_wv_062,msevi_l15_wv_073")
            ;

    if (argc < 2) {
        cout << desc << "\n";
        exit(EXIT_SUCCESS);
    }

    po::variables_map vm;
    try
    {
        po::store(parse_command_line(argc, argv, desc, po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
        po::notify(vm);
    } catch (std::exception &e) {
        cerr << "FATAL:parsing command line caused exception: " << e.what()
                << ": check meanie3D-satconv --help for command line options"
                << endl;
        exit(EXIT_FAILURE);
    }

    // Evaluate user input
    double value;
    int variableIndex;
    ConversionType type;
    try {
        parse_commmandline(vm,variableIndex,value,type);
    } catch (const std::exception &e) {
        cerr << "ERROR:exception " << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    switch (type) {
        case RadicanceToTemperature:
            cout << brightness_temperature(variableIndex,value) << endl;
            break;
        case TemperatureToRadiance:
            cout << spectral_radiance(variableIndex,value) << endl;
            break;
    }

    return EXIT_SUCCESS;
};


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_CONSTRAINTS_H_
#define TURI_CONSTRAINTS_H_

#include <string>
#include <core/data/flexible_type/flexible_type.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/SparseCore>

// Optimizaiton
#include <ml/optimization/utils.hpp>
#include <ml/optimization/optimization_interface.hpp>
#include <ml/optimization/constraint_interface.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
//

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup optimization_constraints Optimization Constraints
 * \{
 */

/**
 * Interface for non-negative constriants.
 *   x >= 0
 */
class non_negative_orthant : public constraint_interface {

  protected:

    size_t variables;                       /**< # Variables in the problem */

  public:

  /**
   * Default constructor.
   */
  non_negative_orthant(const size_t& _variables){
    variables = _variables;
  }

  /**
   * Default desctuctor. Do nothing.
   */
  ~non_negative_orthant(){
  }

  /**
   * Project a dense point into the constraint space.
   * \param[in,out]  point   Point (Dense Vector)
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project(DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    point = point.cwiseMax(0);
  }

  /**
   * Project a block of a dense point into the constraint space.
   *
   * \param[in,out]  point        A block project the point.
   * \param[in]      block_start  Start index of the block
   * \param[in]      block_size   Size of the block
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project_block(DenseVector &point, const size_t block_start, const
      size_t block_size) const {
    DASSERT_LE(variables, block_start + block_size);
    DASSERT_EQ(block_size, point.size());
    point = point.cwiseMax(0);
  }

  /**
   * Boolean function to deterstd::mine if a dense point is present in a constraint
   * space.
   * \param[in]  point   Point which we are querying.
   *
   */
  inline bool is_satisfied(const DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    for(size_t i=0; i < size_t(point.size()); i++){
      if( point(i) <= -OPTIMIZATION_ZERO)
        return false;
    }
    return true;
  }

  /**
   * A measure of the first order optimality conditions.
   *
   * \param[in]  point    Point which we are querying.
   * \param[in]  gradient Gradient at that point for a given function
   *
   * Use the Cauchy point as a measure of optimality. See Pg 486 of
   * Nocedal and Wright (Edition 2)
   *
   */
  inline double first_order_optimality_conditions(const DenseVector &point,
                                        const DenseVector& gradient) const{
    DASSERT_TRUE(is_satisfied(point));
    DenseVector proj_gradient = gradient;
    for(size_t i=0; i < size_t(point.size()); i++){
      if(point(i) <= OPTIMIZATION_ZERO){
        proj_gradient(i) = std::min(0.0, gradient(i));
      } else {
        proj_gradient(i) = gradient(i);
      }
    }
    return compute_residual(proj_gradient);
  }
};

/**
 * Interface for box-constraints on variables.
 *   lb <= x <= ub
 */
class box_constraints: public constraint_interface {

  protected:

    DenseVector lb;                  /**< # Upper bound */
    DenseVector ub;                  /**< # Lower bound */
    size_t variables;                /**< # Variables in the problem */

  public:

  /**
   * Default constructor.
   * \param[in]  _variables Number of variables
   * \param[in]  _lb Lower bound
   * \param[in]  _ub Upper bound
   */
  box_constraints(const double& _lb, const double& _ub, const size_t&
      _variables){
    variables = _variables;
    lb.resize(variables);
    lb.setConstant(_lb);
    ub.resize(variables);
    ub.setConstant(_ub);
  }

  /**
   * Default constructor.
   * \param[in]  _variables Number of variables
   * \param[in]  _lb Lower bound
   * \param[in]  _ub Upper bound
   */
  box_constraints(const DenseVector& _lb, const
      DenseVector& _ub){
    variables = _lb.size();
    DASSERT_EQ(variables, _ub.size());
    lb = _lb;
    ub = _ub;
  }

  /**
   * Default desctuctor. Do nothing.
   */
  ~box_constraints(){
  }

  /**
   * Project a dense point into the constraint space.
   * \param[in,out]  point   Point (Dense Vector)
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project(DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    for(size_t i=0; i < size_t(point.size()); i++){
      point(i) = std::min(std::max(point(i), lb(i)), ub(i));
    }
  }

  /**
   * Project a block of a dense point into the constraint space.
   *
   * \param[in,out]  point        A block project the point.
   * \param[in]      block_start  Start index of the block
   * \param[in]      block_size   Size of the block
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project_block(DenseVector &point, const size_t block_start, const
      size_t block_size) const {
    DASSERT_GE(variables, block_start + block_size);
    DASSERT_EQ(block_size, point.size());
    for(size_t i=0; i < block_size; i++){
      point(i) = std::min(std::max(point(i),
                                  lb(block_start + i)), ub(block_start + i));
    }
  }

  /**
   * Boolean function to determine if a dense point is present in a constraint
   * space.
   * \param[in]  point   Point which we are querying.
   *
   */
  inline bool is_satisfied(const DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    for(size_t i=0; i < size_t(point.size()); i++){
      if( point(i) <= lb(i) - OPTIMIZATION_ZERO ||
          point(i) >= ub(i) + OPTIMIZATION_ZERO)
        return false;
    }
    return true;
  }

  /**
   * A measure of the first order optimality conditions.
   *
   * \param[in]  point    Point which we are querying.
   * \param[in]  gradient Gradient at that point for a given function
   *
   * Use the Cauchy point as a measure of optimality. See Pg 486 of
   * Nocedal and Wright (Edition 2)
   *
   */
  inline double first_order_optimality_conditions(const DenseVector &point,
                                        const DenseVector& gradient) const{
    DASSERT_TRUE(is_satisfied(point));
    DenseVector proj_gradient = gradient;
    for(size_t i=0; i < size_t(point.size()); i++){
      if(point(i) <= OPTIMIZATION_ZERO + lb(i)){
        proj_gradient(i) = std::min(0.0, gradient(i));
      } else if (point(i) >= ub(i) - OPTIMIZATION_ZERO){
        proj_gradient(i) = std::max(0.0, gradient(i));
      } else {
        proj_gradient(i) = gradient(i);
      }
    }
    return compute_residual(proj_gradient);
  }

};

} // optimization
} // turicreate

#endif


#pragma once
#include <cmath>
#include <tuple>
#include <cstddef>
#include <utility>
#include <algorithm>
#include <functional>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "utils/types.hpp"
#include "utils/utils.hpp"
#include "utils/assert.hpp"
#include "utils/comparators.hpp"

namespace ample {

    namespace _impl {

        template<typename T>
        using vector_t = Eigen::Matrix<T, Eigen::Dynamic, 1>;

        template<typename T>
        using matrix_t = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

        template<typename T>
        struct solver_type {

            using type = Eigen::EigenSolver<matrix_t<T>>;

        };

        template<typename T>
        struct solver_type<std::complex<T>> {

            using type = Eigen::ComplexEigenSolver<matrix_t<std::complex<T>>>;

        };

        template<typename T>
        using solver_type_t = typename solver_type<T>::type;

        template<typename T>
        struct pade_coefficients {

            auto operator()(const types::vector1d_t<T>& c, const size_t& n, const size_t& m) {
                vector_t<T> b(m);
                matrix_t<T> A(m, m);

                for (size_t i = 0, ci = n + 1; i < m; ++i, ++ci) {
                    b(i) = -c[ci];

                    for (size_t j = 0; j < std::min(ci, m); ++j)
                        A(i, j) = c[ci - j - 1];
                }

                const vector_t<T> x = A.colPivHouseholderQr().solve(b);

                types::vector1d_t<T> ac(n + 1), bc(m + 1, T(1));
                for (size_t i = 0; i < n + 1; ++i) {
                    ac[i] = c[i];
                    for (size_t j = 0; j < std::min(i, m); ++j)
                        ac[i] += c[i - j - 1] * x(j);
                }

                for (size_t i = 0; i < m; ++i)
                    bc[i + 1] = x(i);

                return std::make_tuple(ac, bc);
            }

        };

        template<typename T>
        struct theta_pade_coefficients {

            T theta{};

            auto operator()(const types::vector1d_t<T>& c, const size_t& n, const size_t& m) {
                utils::dynamic_assert(n == m, "coefficients: n(", n, ") must be equal to m(", m, ") for theta-weighted coefficients");

                pade_coefficients<T> coefficients{};
                const auto [a0, b0] = coefficients(c, n - 1, n);
                      auto [a1, b1] = coefficients(c, n, n);

                const auto theta1 = T(1) - theta;
                for (size_t i = 0; i < a0.size(); ++i)
                    a1[i] = a1[i] * theta + a0[i] * theta1;

                a1.back() *= theta;

                for (size_t i = 0; i < b0.size(); ++i)
                    b1[i] = b1[i] * theta + b0[i] * theta1;

                return std::make_tuple(a1, b1);
            }

        };

        template<typename T>
        struct exp_taylor {

            static constexpr auto on = T(1);
            static constexpr auto tw = T(2);
            static constexpr auto fo = T(4);
            static constexpr auto si = T(6);

            auto operator()(const T& value, const size_t& n) {
                const auto v2 = std::pow(value, 2);

                types::vector1d_t<T> c(n);
                c[0] = on;
                c[1] = value /tw;

                for (size_t i = 2, m = 0; i < n; ++i, ++m) {
                    const auto mm = T(m);
                    c[i] = (v2 * c[m] - (tw + si * mm + fo * mm * mm) * c[m + 1]) / (fo * (on + mm) * (tw + mm));
                }

                return c;
            }

        };

        template<typename T>
        struct root_taylor {

            auto operator()(const T& value, const size_t& n) {
                types::vector1d_t<T> c(n, T(0));
                std::for_each(c.begin() + 1, c.end(),
                    [&value, p=T(1), i=size_t(0), f=T(1)](auto& x) mutable {
                        ++i;
                        x = value * (p *= (T(1.5) - T(i))) / (f *= i);
                    }
                );

                return c;
            }

        };

        template<typename T>
        struct no_transform {

            auto operator()(types::vector1d_t<T>& p, types::vector1d_t<T>& q) {}

        };

        template<typename T>
        struct wampe_transform {

            auto operator()(types::vector1d_t<T>& p, types::vector1d_t<T>& q) {
                for (size_t i = 0; i < p.size(); ++i) {
                    const auto q2 = T(2) * q[i];
                    std::tie(p[i], q[i]) = std::make_tuple(-p[i] - q2, p[i] - q2);
                }

                for (size_t i = p.size(); i < q.size(); ++i) {
                    const auto q2 = T(-2) * q[i];
                    p.push_back(q2);
                    q[i] = q2;
                }
            }

        };

        template<typename T>
        auto get_roots(const types::vector1d_t<T>& c) {
            const auto n = c.size() - 1;
            matrix_t<T> A = matrix_t<T>::Zero(n, n);

            for (size_t i = 0; i < n - 1; ++i)
                A(i + 1, i) = T(1);

            for (size_t i = 0; i < n; ++i)
                A(0, i) = -c[n - i - 1] / c.back();

            solver_type_t<T> solver;
            solver.compute(A);
            const vector_t<T> values = solver.eigenvalues();

            auto result = utils::make_vector_i(n, [&values](const auto& i) { return values(i); });
            std::sort(result.begin(), result.end(), utils::less<T>{});
            return result;
        }

    }// namespace ample::_impl

    template<typename T>
    class coefficients {

    public:

        using taylor_func_t      = std::function<types::vector1d_t<T>(const T&, const size_t&)>;
        using transform_func_t   = std::function<void (types::vector1d_t<T>&, types::vector1d_t<T>&)>;
        using coeficients_func_t = std::function<std::tuple<types::vector1d_t<T>, types::vector1d_t<T>>(const types::vector1d_t<T>&, const size_t&, const size_t&)>;

        coefficients(const size_t& n,
                     taylor_func_t taylor,
                     transform_func_t transform,
                     coeficients_func_t coefficients) : ample::coefficients<T>(n, n, std::move(taylor), std::move(transform), std::move(coefficients)) {}

        coefficients(const size_t& n, const size_t& m,
                     taylor_func_t taylor,
                     transform_func_t transform,
                     coeficients_func_t coefficients) : _n(n), _m(m), _taylor(std::move(taylor)), _transform(std::move(transform)), _coefficients(std::move(coefficients)) {
            utils::dynamic_assert(n > 0,  "coefficients: n(", n, ") must be positive");
            utils::dynamic_assert(n <= m, "coefficients: n(", n, ") must be less or equal to m(", m, ")");
        }

        auto get(const T& value) const {
            const auto tc = _taylor(value, _n + _m + 1);
            auto [np, dp] = _coefficients(tc, _n, _m);

            _transform(np, dp);

            const auto nr = _impl::get_roots<T>(np);
            const auto dr = _impl::get_roots<T>(dp);

            types::vector1d_t<size_t> ix(_m);
            std::iota(ix.begin(), ix.end(), size_t(0));

            auto a = types::vector1d_t<T>(_m);
            for (size_t i = 0; i < _m; ++i) {
                const auto p = std::accumulate(nr.begin(), nr.end(), np.back(), [&x=dr[i]](const auto& a, const auto& x0) { return a * (x - x0); });
                const auto q = std::accumulate(ix.begin(), ix.end(), dp.back(),
                                               [&x=dr[i], &i, &dr](const auto& a, const auto& j) {
                                                   return i == j ? a : a * (x - dr[j]);
                                               }
                );

                a[i] = -p / q / dr[i];
            }

            auto a0 = np.front() / dp.front() - std::accumulate(a.begin(), a.end(), T(0));
            return std::make_tuple(a0, std::move(a), utils::make_vector(dr, [](const auto& x) { return -T(1) / x; }));
        }

        [[nodiscard]] auto nc() const {
            return _m;
        }

    private:

        size_t _n{}, _m{};
        taylor_func_t _taylor;
        transform_func_t _transform;
        coeficients_func_t _coefficients;

    };

    template<typename T>
    auto ssp_coefficients(const size_t& n, const size_t& m) {
        return coefficients<T>(n, m, _impl::exp_taylor<T>{}, _impl::no_transform<T>{}, _impl::pade_coefficients<T>{});
    }

    template<typename T>
    auto ssp_coefficients(const size_t& n) {
        return ssp_coefficients<T>(n, n);
    }

    template<typename T>
    auto theta_ssp_coefficients(const size_t& n, const T& theta) {
        return coefficients<T>(n, n, _impl::exp_taylor<T>{}, _impl::no_transform<T>{}, _impl::theta_pade_coefficients<T>{ theta });
    }

    template<typename T>
    auto wampe_coefficients(const size_t& n, const size_t& m) {
        return coefficients<T>(n, m, _impl::root_taylor<T>{}, _impl::wampe_transform<T>{}, _impl::pade_coefficients<T>{});
    }

    template<typename T>
    auto wampe_coefficients(const size_t& n) {
        return wampe_coefficients<T>(n, n);
    }

    template<typename T>
    auto theta_wampe_coefficients(const size_t& n, const T& theta) {
        return coefficients<T>(n, n, _impl::root_taylor<T>{}, _impl::wampe_transform<T>{}, _impl::theta_pade_coefficients<T>{ theta });
    }

}// namespace ample


#pragma once
#include<complex>
#include<vector>
#include"mkl_lapacke.h"
#include <Eigen/Sparse>
#include <Eigen/Dense>

namespace Many_Body{
  struct TriDiagMat{
    TriDiagMat(const Eigen::VectorXd& diagel, const Eigen::VectorXd& offDiag): diagel(diagel), offDiag(offDiag){}
    Eigen::VectorXd diagel;
    Eigen::VectorXd offDiag;
    auto dData(){return diagel.data();}
      auto offData(){return offDiag.data();}
  };

  void diag(    Many_Body::TriDiagMat& tri, Eigen::MatrixXd& z, Eigen::VectorXd& ev)
  {
    MKL_INT N= ev.size();
        MKL_INT info= LAPACKE_dstedc(LAPACK_COL_MAJOR, 'I', N, tri.dData(), tri.offData(), z.data(), N);
    if(info!=0)
      {
  	std::cout << " diagonalization failed" << '\n';
      }
    ev=tri.diagel;

   }
  template<typename Vector, typename Matrix>
  struct threeLVec
  {
    threeLVec(size_t dim, const Matrix& A): A(A), beta(1), alpha(0) {
      Vector vecOne=Eigen::VectorXcd::Zero(dim);
    Vector vecTwo=Eigen::VectorXcd::Zero(dim);
    Vector vecThree=Eigen::VectorXcd::Zero(dim);
    }
    const Matrix& A;
    double beta{1};
    double alpha{};
    Vector vecOne;
    Vector vecTwo;
    Vector vecThree;

void iterate()
    {
      	vecThree=A*vecTwo;
	//	std::complex<double> c=qk.adjoint()*qMiddle;
	std::complex<double> c=(vecTwo).adjoint()*vecThree;

      	alpha=real(c);

	//	Eigen::VectorXcd  rk=qMiddle - alpha*qk -beta*qkmin;

	Eigen::VectorXcd  rk=vecThree - alpha*vecTwo -beta*vecOne;

     	beta=rk.norm();
	vecOne=vecTwo;

	//qk=rk/rk.norm();
	vecTwo=rk/rk.norm();
    }
    void lastIterate()
    {
      	vecThree=A*vecTwo;

	std::complex<double> c=(vecTwo).adjoint()*vecThree;

      	alpha=real(c);

    }

  };
  template<typename Matrix>
  TriDiagMat Lanczos3Vec(Matrix& A, Eigen::VectorXcd& state, const size_t iterations)
  {
    using namespace Eigen::internal;
    using namespace Eigen;
    	assert(std::abs(state.norm() -1.) < Many_Body::err);

	threeLVec< Eigen::VectorXcd, Matrix>  threeLanczVec(A.rows(), A);
	threeLanczVec.vecTwo=state;
        long double beta=1;

    double alpha(0);
    Eigen::VectorXd bandTdiag(iterations);
        Eigen::VectorXd bandTOff(iterations-1);
    bandTdiag.setZero();
        bandTOff.setZero();

    Eigen::VectorXcd qk=state;
    Eigen::VectorXcd qkmin(A.rows());
    qkmin.setZero();
    threeLanczVec.vecOne=qkmin;
    size_t dim=0;

     for (size_t k = 1; k < iterations; ++k)
       {
	 threeLanczVec.iterate();

      	alpha=threeLanczVec.alpha;
     	beta=threeLanczVec.beta;

	bandTdiag(k-1)=alpha;
     	bandTOff(k-1)=beta;

	  	 if( std::abs(beta)<0.00001)
     	   {

    		  bandTdiag.resize( k);
    		  bandTOff.resize( k-1);

     	    break;
     	   }
    	 dim=k;
    	 }

     {

       threeLanczVec.lastIterate();
       bandTdiag(dim)=threeLanczVec.alpha;

    }

    TriDiagMat T(bandTdiag, bandTOff);
    return T;
  }
    template<typename Matrix>
  TriDiagMat Lanczos(Matrix& A, Eigen::VectorXcd& state, const size_t iterations, Eigen::MatrixXcd& Q)
  {
    using namespace Eigen::internal;
    using namespace Eigen;
    	assert(std::abs(state.norm() -1.) < Many_Body::err);

	Q.setZero();
	Q.col(0)=state;
        long double beta=1;
    double alpha(0);
     Eigen::VectorXd bandTdiag(iterations);
         Eigen::VectorXd bandTOff(iterations-1);
	   bandTdiag.setZero();
        bandTOff.setZero();

    Eigen::VectorXcd qk=state;
    Eigen::VectorXcd qkmin(A.rows());
    qkmin.setZero();
    size_t dim=0;
 for (size_t k = 1; k < iterations; ++k)
        {
	  Eigen::VectorXcd qMiddle=A*qk;

	    	std::complex<double> c=qk.adjoint()*qMiddle;
	     	alpha=real(c);

      	Eigen::VectorXcd  rk=qMiddle - alpha*qk -beta*qkmin;
     	beta=rk.norm();

	bandTdiag(k-1)=alpha;
     	bandTOff(k-1)=beta;

      	qkmin=qk;
	qk=rk/rk.norm();
	     Q.col(k)=qk;

	  	 if( std::abs(beta)<0.0001)
     	   {
	     Eigen::MatrixXcd W=Q;
	           Q.resize(A.rows(), k);
		 for (size_t i = 0; i < k; ++i)
		   {
		     Q.col(i)=W.col(i);

		   }
    		  bandTdiag.resize( k);
    		  bandTOff.resize( k-1);
		  break;
     	   }
	dim=k;
             }
     {
       Eigen::VectorXcd qMiddle=A*qk;
       	std::complex<double> c=qk.adjoint()*qMiddle;
     	alpha=real(c);

       		        bandTdiag(dim)=(alpha);

     }
     //             assert(std::abs( (Q.adjoint()*Q).sum() -(dim+1)) < Many_Body::err);
     std::cout<<"Qdag Q sum "<< (Q.adjoint()*Q).sum()<<std::endl;

     TriDiagMat T(bandTdiag, bandTOff);
    return T;
  }

  template<typename Vector, typename Vector2, typename Matrix>
  auto lanczTrafo(const  Vector & state, const Vector2 & iniState, size_t iteration, const Matrix& A)
    ->Eigen::VectorXcd
  {
    Many_Body::threeLVec< Eigen::VectorXcd, Matrix>  threeLanczVec(A.rows(), A);
    Eigen::VectorXcd newVec=Eigen::VectorXcd::Zero(A.rows());
    threeLanczVec.vecTwo=iniState;
    Eigen::VectorXcd qkmin(A.rows());
    qkmin.setZero();
    threeLanczVec.vecOne=qkmin;
    for(size_t i=0; i<iteration; i++)
      {
	newVec+=threeLanczVec.vecTwo*state(i);
	threeLanczVec.iterate();

      }

    return newVec;
  }

};


#include "decode.hpp"
#include "pauli_product.hpp"
#include <armadillo>
#include <iostream>

using namespace std;
using namespace arma;

double coeff_a1(unsigned int q_idx, cx_dvec &psi)
{
  int d = psi.size();
  double mysum = 0.0;
  for (int y=0; y<d; y++)
    {
      if (!(y & (1<<q_idx)))
	{
	   mysum += std::norm(psi(y));
	}
    }
  return mysum;
}

double coeff_a2(unsigned int q_idx, unsigned int x, cx_dvec &psi)
{
  int d = psi.size();
  double mysum = 0.0;
  for (int y=0; y<d ; y++)
    {
      int temp = 1<< q_idx;
      int temp2 = y^x;
      if (!((y^x) & (1<<q_idx)))
	{
	  mysum += std::norm(psi(y));
	}
      else
	{
	}
    }
  return mysum;

}

double coeff_a3(unsigned int q_idx, unsigned int x, unsigned int z, cx_dvec &psi)
{
  int d = psi.size();
  double mysum = 0.0;
  for (int y=0; y<d; y++)
    {
      unsigned int xz = __builtin_popcount(x&z);
      unsigned int yz = __builtin_popcount(y&z);

      cx_double phase;
      switch((xz + 2*yz)%4)
	{
	case 0:
	  phase = cx_double(1.0, 0.0);
	  break;
	case 1:
	  phase = cx_double(0.0, 1.0);
	  break;
	case 2:
	  phase = cx_double(-1.0, 0.0);
	  break;
	case 3:
	  phase = cx_double(0.0, -1.0);
	  break;
	}
      if (!((y^x) & (1<<q_idx)))
	{
	  //	  cout << y << endl;
	  //	  cout << (y^x) << endl;
	  mysum += (-2) * std::imag(std::conj(psi(y^x)) * psi(y) * phase);
	}
    }
  return mysum;
}

// Given a Pauli string, find the rotation angle with respect to the corresponding PPR that gives the optimal overlap with the |0>_{q_idx} state.
double optimized_theta(unsigned int q_idx, unsigned int x, unsigned int z, cx_dvec &psi)
{
  double a1 = coeff_a1(q_idx, psi);
  double a2 = coeff_a2(q_idx, x, psi);
  double a3 = coeff_a3(q_idx, x, z, psi);

  double theta1, theta2;
  if (a1==a2)
    theta1= 3.14159265358979/2.0;
  else
    theta1 = atan(a3/(a1 - a2))/2.0;

  theta2 = theta1 + 3.14159265358979/2.0;

  double f1 = ((a1-a2) * cos(2*theta1) + a3 * sin(2*theta1))/2.0;
  double f2 = ((a1-a2) * cos(2*theta2) + a3 * sin(2*theta2))/2.0;

  f1 += (a1+a2)/2.0;
  f2 += (a1+a2)/2.0;

  if (f1>f2)
    return theta1;
  else
    return theta2;
}

// Given a Pauli string, find the optimal overlap with the |0>_{q_idx} using
// a PPR.
double optimal_overlap(unsigned int q_idx, unsigned int x, unsigned int z, cx_dvec &psi)
{
  double a1 = coeff_a1(q_idx, psi);
  double a2 = coeff_a2(q_idx, x, psi);
  double a3 = coeff_a3(q_idx, x, z, psi);

  double theta1, theta2;
  if (a1==a2)
    theta1= 3.14159265358979/2.0;
  else
    theta1 = atan(a3/(a1 - a2))/2.0;

  theta2 = theta1 + 3.14159265358979/2.0;
  double f1 = ((a1-a2) * cos(2*theta1) + a3 * sin(2*theta1))/2.0;
  double f2 = ((a1-a2) * cos(2*theta2) + a3 * sin(2*theta2))/2.0;

  f1 += (a1+a2)/2.0;
  f2 += (a1+a2)/2.0;

  if (f1>f2)
    return f1;
  else
    return f2;
}

// Find the optimal Pauli string that maximizes the overlap with |0>_q
// and apply the corresponding PPR to psi. Here the PPR is assumed to be restricted
// to a set of first 'bits' qubits
void optimal_update(unsigned int q_idx, unsigned int setbits, cx_dvec &psi)
{
  int i_best, j_best;
  double f = 0.0;
  double theta = 0.0;
  for (int i=0; i<(1<<setbits); i++)
    {
      for (int j=0; j<(1<<setbits); j++)
	{
	  /*	  cout << "State=" << endl;
	  cout << psi << endl;
	  cout << "i, j=" << i << ", " << j << endl;
	  double a1 = coeff_a1(0, psi);
	  double a2 = coeff_a2(0, i, psi);
	  double a3 = coeff_a3(0, i, j, psi);
	  double f_opt = optimal_overlap(0, i, j, psi);
	  cout << "a1=" << a1 << endl;
	  cout << "a2=" << a2 << endl;
	  cout << "a3=" << a3 << endl;
	  cout << "f_opt=" << f_opt << endl;
	  if ((a1==a2) && (abs(a3)>0.0001))
	  exit (EXIT_FAILURE);*/

	  // Apply optimal update only if the PPR acts nontrivially on q_idx.
	  if ((i & (1<<q_idx)) || (j & (1<<q_idx)))
	    {
	      if (optimal_overlap(q_idx, i, j, psi) > f)
		{
		  f = optimal_overlap(q_idx, i, j, psi);
		  //	      cout << "New record: " << f << endl;
		  i_best = i;
		  j_best = j;
		}
	    }

	}
    }
  theta = optimized_theta(0, i_best, j_best, psi);
  apply_ppr(i_best, j_best, theta, psi);
}

// Skip the given choice of i and j with probability 1-ratio.
void randomized_update(unsigned int q_idx, unsigned int setbits, double ratio, cx_dvec &psi)
{
  int i_best, j_best;
  double f = 0.0;
  double theta = 0.0;
  for (int i=0; i<(1<<setbits); i++)
    {
      for (int j=0; j<(1<<setbits); j++)
	{
	  if ((i & (1<<q_idx)) || (j & (1<<q_idx)))
	    {
	      if ((double)rand() / (double)RAND_MAX < ratio)
		{
		  if (optimal_overlap(q_idx, i, j, psi) > f)
		    {
		      f = optimal_overlap(q_idx, i, j, psi);
		      //	      cout << "New record: " << f << endl;
		      i_best = i;
		      j_best = j;
		    }
		}
	    }

	}
    }
  theta = optimized_theta(0, i_best, j_best, psi);
  apply_ppr(i_best, j_best, theta, psi);
}

// Decode and print the result.
void decode(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double eps)
{
  double f = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      optimal_update(q_idx, setbits, psi);
      f = coeff_a1(0, psi);
      cout << itt << ":" << f << endl;
    }
}

void randomized_decode(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double ratio, double eps)
{
  double f = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      randomized_update(q_idx, setbits, ratio, psi);
      f = coeff_a1(0, psi);
      cout << itt << ":" << f << endl;
    }
}

double decode_cost(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double eps)
{
  double f = 0.0;
  double cost = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      optimal_update(q_idx, setbits, psi);
      f = coeff_a1(0, psi);
      //      cout << itt << ":" << f << endl;
      cost += 1.0;
    }
  return cost;
}

double randomized_decode_cost(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double ratio, double eps)
{
  double f = 0.0;
  double cost = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      randomized_update(q_idx, setbits, ratio, psi);
      f = coeff_a1(0, psi);
      //      cout << itt << ":" << f << endl;
      cost += 1.0;
    }
  return cost;
}


/*
*  @file 		ex8.cpp
*  @details  	This file is the solution to exercise 8.
*  @author    	Alexander Rettkowski
*  @date      	12.07.2017
*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <iostream>
#include <string>
#include <complex>
#include <fstream>
#include <queue>

using namespace boost;

namespace exercise8
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;
	struct edge
	{
		int startNode;
		int endNode;
		int length;
	};
}

BOOST_FUSION_ADAPT_STRUCT(
	exercise8::edge,
	(int, startNode)
	(int, endNode)
	(int, length)
)

namespace exercise8
{
	template <typename Iterator>
	struct line_parser : qi::grammar<Iterator, edge()>
	{
		line_parser() : line_parser::base_type(start)
		{
			using qi::int_;
			start %= int_ >> ' ' >> int_ >> ' ' >> int_;
		}

		qi::rule<Iterator, edge()> start;
	};
}

/**
* The function that calculates the shortest paths using Dijkstra's method.
* @param numberOfNodes Number of nodes in the graph.
* @param graph The graph represented as a vector auf edge-vectors.
* @param startNode The id of the node where the search starts.
*/
std::vector<int> dijkstra(int numberOfNodes, std::vector< std::vector< std::pair<int, int> > > graph, int startNode, std::vector<int>& predecessors)
{
	predecessors.resize(numberOfNodes, -1);
	std::vector<int> distanceTo(numberOfNodes, INT_MAX);
	std::priority_queue< std::pair<int, int>, std::vector< std::pair<int, int> >, std::greater< std::pair<int, int> > > queue;
	queue.push(std::pair<int, int>(startNode, 0));
	distanceTo[startNode] = 0;
	//predecessors[startNode] = -1;

	int currentNode, compareNode, compareNodeDistance, currentNodeDistance;

	while (!queue.empty()) {
		currentNode = queue.top().first;
		currentNodeDistance = queue.top().second;
		queue.pop();

		if (distanceTo[currentNode] < currentNodeDistance) continue;

		for (int i = 0; i < graph[currentNode].size(); i++) {
			compareNode = graph[currentNode][i].first;
			compareNodeDistance = graph[currentNode][i].second;
			if (distanceTo[compareNode] > distanceTo[currentNode] + compareNodeDistance) {
				distanceTo[compareNode] = distanceTo[currentNode] + compareNodeDistance;
				queue.push(std::pair<int, int>(compareNode, distanceTo[compareNode]));
				predecessors[compareNode] = currentNode;
			}
		}
	}
	return distanceTo;
}

/**
* This method checks (in a naive way), if a given number is prime or not.
* @param number The number to check.
* @returns True, if number is prime; false otherwise.
*/
bool isPrime(int number) {
	if (number == 2)
		return true;
	if (number % 2 == 0)
		return false;
	for (int i = 3; (i*i) <= number; i += 2) {
		if (number % i == 0) return false;
	}
	return true;
}

/**
* The main function that reads in a file and processes it.
* @param argc Number of command line arguments.
* @param *argv a pointer to the array of command line arguments.
*/
int main(int argc, char *argv[])
{
	timer::cpu_timer boostTimer;

	using boost::spirit::ascii::space;
	typedef std::string::const_iterator iterator_type;
	typedef exercise8::line_parser<iterator_type> line_parser;
	line_parser parser;
	std::string currentLine;
	std::ifstream file(argv[1]);

	// get number of nodes
	char delimiter = ' ';
	getline(file, currentLine, delimiter);
	const int numberOfNodes = stoi(currentLine);
	getline(file, currentLine);
	int constSub = 1;

	std::vector<std::vector<std::pair<int, int>>> edges(numberOfNodes);
	while (getline(file, currentLine))
	{
		exercise8::edge parsedLine;
		std::string::const_iterator currentPosition = currentLine.begin();
		std::string::const_iterator lineEnd = currentLine.end();
		bool parsingSucceeded = phrase_parse(currentPosition, lineEnd, parser, space, parsedLine);

		if (parsingSucceeded && currentPosition == lineEnd)
		{
			std::pair<int, int> *tempEdge = new std::pair<int, int>();
			tempEdge->first = parsedLine.endNode - constSub;
			tempEdge->second = parsedLine.length;
			edges[parsedLine.startNode - constSub].push_back(*tempEdge);

			std::pair<int, int> *reverseEdge = new std::pair<int, int>();
			reverseEdge->first = parsedLine.startNode - constSub;
			reverseEdge->second = parsedLine.length;
			edges[parsedLine.endNode - constSub].push_back(*reverseEdge);
		}
	}

	file.close();

	std::vector<int> connectedVertices, unconnectedTerminals;
	for (int i = 0; i < numberOfNodes; i++)
	{
		if (isPrime(i))
			unconnectedTerminals.push_back(i);
	}

	connectedVertices.push_back(unconnectedTerminals.back());
	unconnectedTerminals.pop_back();

	std::map<int, std::vector<int>> distancesFromTerminals;
	std::map<int, std::vector<int>> predecessorsFromTerminals;
	for (int i : unconnectedTerminals)
	{
		std::vector<int> predecessors;
		std::vector<int> distancesFromI = dijkstra(numberOfNodes, edges, i, predecessors);
		distancesFromTerminals.insert(std::pair<int, std::vector<int>>(i, distancesFromI));
		predecessorsFromTerminals.insert(std::pair<int, std::vector<int>>(i, predecessors));
	}

	int steinerWeight = 0;

	while (unconnectedTerminals.size() > 0)
	{
		int connectingPointId = -1, connectingPointDistance = INT32_MAX;
		std::vector<int> path;

		int pathFrom = -1, pathTo = -1, mindist = INT32_MAX;
		for (int i = 0; i < unconnectedTerminals.size(); i++)
		{
			int vertexId = unconnectedTerminals[i];
			for (int j : connectedVertices)
			{
				if (distancesFromTerminals[vertexId][j] < mindist)
				{
					mindist = distancesFromTerminals[vertexId][j];
					pathTo = j;
					pathFrom = vertexId;
				}
			}
		}

		path.push_back(pathFrom);
		do
		{
			path.push_back(pathTo);
			pathTo = predecessorsFromTerminals[pathFrom][pathTo];
		} while (pathTo != pathFrom);

		// get path from
		for (int i : path)
		{
			// if the vertex wasn't in the tree before, add
			if (std::find(connectedVertices.begin(), connectedVertices.end(), i) == connectedVertices.end())
			{
				connectedVertices.push_back(i);
			}
			// if it was an unconnected terminal, delete from the list
			auto pos = std::find(unconnectedTerminals.begin(), unconnectedTerminals.end(), i);
			if (pos != unconnectedTerminals.end())
			{
				unconnectedTerminals.erase(pos);
			}
		}

		steinerWeight += mindist;

	}

	std::cout << steinerWeight;

	return 0;
}


/*
 * Copyright 2013-2015 Raphael Bost
 *
 * This file is part of ciphermed.

 *  ciphermed is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  ciphermed is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with ciphermed.  If not, see <http://www.gnu.org/licenses/>. 2
 *
 */

#include <math/num_th_alg.hh>
#include <math/math_util.hh>
#include <math/util_gmp_rand.h>
#include <math/prime_seq.hh>
#include <math/mpz_class.hh>
#include <NTL/ZZ.h>
#include <cassert>

#include <iostream>

/* The algorithms here are from Victor Shoup's Book
 * A Computational Introduction to Number Theory and Algebra
 * or Shoup's NTL library (for Sophie Germain primes)
 */

std::vector<mpz_class> gen_rand_non_increasing_seq(const mpz_class &m, gmp_randstate_t state)
{
    std::vector<mpz_class> seq;

    mpz_class n = m;
    mpz_class new_n;
    do {
        // pick a new n between 1...n
        mpz_urandomm(new_n.get_mpz_t(),state,n.get_mpz_t());
        n = new_n + 1;
        seq.push_back(n);
    } while (n != 1);

    return seq;
}

std::vector<mpz_class> extract_prime_seq(const std::vector<mpz_class> &seq, int reps)
{
    std::vector<mpz_class> primes;
    for (size_t i = 0; i < seq.size(); i++) {
        if (mpz_class_probab_prime_p(seq[i],reps) != 0) {
            primes.push_back(seq[i]);
        }
    }

    return primes;
}

// generates a random integer with its factorization (returns the factorization)
std::vector<mpz_class> gen_rand_number_factorization(const mpz_class &m, mpz_class *result, gmp_randstate_t state, int reps)
{
    for (; ; ) {
        std::vector<mpz_class> seq = gen_rand_non_increasing_seq(m,state);
        std::vector<mpz_class> primes = extract_prime_seq(seq, reps);

        mpz_class y = 1;
        for (size_t i = 0; i < primes.size(); i++) {
            y *= primes[i];
            if (y > m) {
                break;
            }
        }
        if (y > m) {
            continue;
        }

        mpz_class x;
        mpz_urandomm(x.get_mpz_t(),state,m.get_mpz_t());
        x += 1;

        if (x <= y) {
            if (result) {
                *result = y;
            }
            return primes;
        }
    }
}

// generates a random prime p with the factorization of p-1 (returns the factorization)
std::vector<mpz_class> gen_rand_prime_with_factorization(const mpz_class &m, mpz_class *p, gmp_randstate_t state, int reps)
{
    for (; ; ) {
        mpz_class n;
        std::vector<mpz_class> factorization = gen_rand_number_factorization(m,&n,state,reps);

        if (mpz_class_probab_prime_p(n+1, reps) != 0) {
            if (p) {
                *p = n+1;
            }
            return factorization;
        }
    }
}

mpz_class simple_safe_prime_gen(size_t n_bits, gmp_randstate_t state, int reps)
{
    for (size_t count = 1; ; count ++) {
        mpz_class n;
        mpz_urandom_len(n.get_mpz_t(),state,n_bits);

        if (mpz_class_probab_prime_p(n,reps) !=0) {
            if (mpz_class_probab_prime_p(2*n+1,reps) != 0 ) {
                std::cout << count << " iterations needed to generate safe prime" << std::endl;
                return n;
            }
        }
    }
}

static long bit_count(long a)
{
    unsigned long aa;
    if (a < 0)
    aa = - ((unsigned long) a);
    else
    aa = a;

    long k = 0;
    while (aa) {
        k++;
        aa = aa >> 1;
    }

    return k;
}

/* The following code is just NTL's code for generating Germain primes using GMP */

// prime_bound computes a reasonable bound for trial
// division in the Miller-Rabin test.
// It is computed a bit on the "low" side, since being a bit
// low doesn't hurt much, but being too high can hurt a lot.

static
long prime_bound(long bn)
{
    long wn = (bn+NBITS_MAX-1)/NBITS_MAX;

    long fn;

    if (wn <= 36)
    fn = wn/4 + 1;
    else
    fn = long(1.67*sqrt(double(wn)));

    long prime_bnd;

    if (bit_count(bn) + bit_count(fn) > NBITS_MAX)
        prime_bnd = (1L << NBITS_MAX);
    else
        prime_bnd = bn*fn;

    return prime_bnd;
}

static
long ErrBoundTest(long kk, long tt, long nn)

{
    const double fudge = (1.0 + 1024.0/NTL_FDOUBLE_PRECISION);
    const double log2_3 = log2(3.0);
    const double log2_7 = log2(7.0);
    const double log2_20 = log2(20.0);

    double k = kk;
    double t = tt;
    double n = nn;

    if (k < 3 || t < 1) return 0;
    if (n < 1) return 1;

    // the following test is largely academic
    assert(9*t < NTL_FDOUBLE_PRECISION);

    double log2_k = log2(k);

    if ((n + log2_k)*fudge <= 2*t)
    return 1;

    if ((2*log2_k + 4.0 + n)*fudge <= 2*sqrt(k))
    return 2;

    if ((t == 2 && k >= 88) || (3 <= t && 9*t <= k && k >= 21)) {
        if ((1.5*log2_k + t + 4.0 + n)*fudge <= 0.5*log2(t) + 2*(sqrt(t*k)))
        return 3;
    }

    if (k <= 9*t && 4*t <= k && k >= 21) {
        if ( ((log2_3 + log2_7 + log2_k + n)*fudge <= log2_20 + 5*t)  &&
            ((log2_3 + (15.0/4.0)*log2_k + n)*fudge <= log2_7 + k/2 + 2*t) &&
            ((2*log2_3 + 2 + log2_k + n)*fudge <= k/4 + 3*t) )
        return 4;
    }

    if (4*t >= k && k >= 21) {
        if (((15.0/4.0)*log2_k + n)*fudge <= log2_7 + k/2 + 2*t)
        return 5;
    }

    return 0;
}

static long make_odd(mpz_class &n)
{
    long k = 0;

    while (mpz_even_p(n.get_mpz_t())) {
        n >>= 1;
        k++;
    }

    return k;
}

static long is_Miller_witness(const mpz_class& n, const mpz_class& x)
{
    mpz_class m(0), y(0), z(0);
    long j, k;

    if (x == 0) return 0;

    m = n-1;
    k = make_odd(m);

    z = mpz_class_powm(x,m,n);

    if (z == 1) return 0;

    j = 0;
    do {
        y = z;
        z = (y*y) %n;
        j++;
    } while (j != k && z != 1);

    if (z != 1) return 1;
    y = y + 1;
    if (y != n) return 1;
    return 0;
}

void gen_germain_prime(mpz_class& n, long k, gmp_randstate_t state, long err)
{
    assert(k > 1);
    assert(k <= (1L << 20));

    if (err < 1) err = 1;
    if (err > 512) err = 512;

    if (k == 2) {
        if (gmp_urandomm_ui(state,2))
        n = 3;
        else
        n = 2;

        return;
    }

    long prime_bnd = prime_bound(k);

    if (bit_count(prime_bnd) >= k/2)
    prime_bnd = (1L << (k/2-1));

    mpz_class two;
    two = 2;

    mpz_class n1;

    PrimeSeq s;

    mpz_class iter;
    iter = 0;

    for (;;) {
        iter++;

        mpz_urandom_len(n.get_mpz_t(),state,k);

        if (mpz_even_p(n.get_mpz_t())) {
            n = n+1;
        }

        s.reset(3);
        long p;

        long sieve_passed = 1;

        p = s.next();
        while (p && p < prime_bnd) {
            mpz_class r;
            mpz_tdiv_r_ui(r.get_mpz_t(),n.get_mpz_t(),p);

            if (r == 0) {
                sieve_passed = 0;
                break;
            }

            // test if 2*r + 1 = 0 (mod p)
            if (r == p-r-1) {
                sieve_passed = 0;
                break;
            }

            p = s.next();
        }

        if (!sieve_passed) continue;

        if (is_Miller_witness(n, two)) continue;

        n1 = 2*n+1;

        if (is_Miller_witness(n1, two)) continue;

        // now do t M-R iterations...just to make sure

        // First compute the appropriate number of M-R iterations, t
        // The following computes t such that
        //       p(k,t)*8/k <= 2^{-err}/(5*iter^{1.25})
        // which suffices to get an overall error probability of 2^{-err}.
        // Note that this method has the advantage of not requiring
        // any assumptions on the density of Germain primes.
        long iter_n_bits = mpz_sizeinbase(iter.get_mpz_t(),2);
        long err1 = std::max(1L, err + 7 + (5*iter_n_bits + 3)/4 - bit_count(k));
        long t;
        t = 1;
        while (!ErrBoundTest(k, t, err1))
        t++;

        if(mpz_probab_prime_p(n.get_mpz_t(),t))
            break;
    }
}

// Constructs a generator for the cyclic group \Z^*_p where p is a Sophie Germain prime
mpz_class get_generator_for_cyclic_group(const mpz_class &p, gmp_randstate_t state)
{
    mpz_class q = (p >> 1);
    mpz_class g;

    // find a generator for ZZ*_p
    // Shoup's algorithm

    mpz_class alpha, beta;
    do {
        mpz_urandomm(alpha.get_mpz_t(),state,p.get_mpz_t());

        beta = mpz_class_powm(alpha,q,p);
    } while (beta == 1);

    g = beta;

    do {
        mpz_urandomm(alpha.get_mpz_t(),state,p.get_mpz_t());
        beta = (alpha*alpha) %p;

    } while (beta == 1);

    g = (g*beta) %p;

    return g;
}


// Boost.Geometry

// Copyright (c) 2017-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DENSIFY_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DENSIFY_HPP

#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/signed_size_type.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/arithmetic/normalize.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/formulas/spherical.hpp>
#include <boost/geometry/formulas/interpolate_point_spherical.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/srs/sphere.hpp>
#include <boost/geometry/strategies/densify.hpp>
#include <boost/geometry/strategies/spherical/get_radius.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace densify
{

/*!
\brief Densification of spherical segment.
\ingroup strategies
\tparam RadiusTypeOrSphere \tparam_radius_or_sphere
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.densify.densify_4_with_strategy densify (with strategy)]
}
 */
template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
{
public:
    typedef typename strategy_detail::get_radius
        <
            RadiusTypeOrSphere
        >::type radius_type;

    // For consistency with area strategy the radius is set to 1
    inline spherical()
        : m_radius(1.0)
    {}

    template <typename RadiusOrSphere>
    explicit inline spherical(RadiusOrSphere const& radius_or_sphere)
        : m_radius(strategy_detail::get_radius
                    <
                        RadiusOrSphere
                    >::apply(radius_or_sphere))
    {}

    template <typename Point, typename AssignPolicy, typename T>
    inline void apply(Point const& p0, Point const& p1, AssignPolicy & policy, T const& length_threshold) const
    {
        typedef typename AssignPolicy::point_type out_point_t;
        typedef typename select_most_precise
            <
                typename coordinate_type<Point>::type,
                typename coordinate_type<out_point_t>::type,
                CalculationType
            >::type calc_t;

        calc_t angle01;

        formula::interpolate_point_spherical<calc_t> formula;
        formula.compute_angle(p0, p1, angle01);

        BOOST_GEOMETRY_ASSERT(length_threshold > T(0));

        signed_size_type n = signed_size_type(angle01 * m_radius / length_threshold);
        if (n <= 0)
            return;

        formula.compute_axis(p0, angle01);

        calc_t step = angle01 / (n + 1);

        calc_t a = step;
        for (signed_size_type i = 0 ; i < n ; ++i, a += step)
        {
            out_point_t p;
            formula.compute_point(a, p);

            geometry::detail::conversion::point_to_point
                <
                    Point, out_point_t,
                    2, dimension<out_point_t>::value
                >::apply(p0, p);

            policy.apply(p);
        }
    }

    inline radius_type radius() const
    {
        return m_radius;
    }

private:
    radius_type m_radius;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<spherical_equatorial_tag>
{
    typedef strategy::densify::spherical<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::densify

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DENSIFY_HPP


#ifndef SM_NUMERICAL_DIFF_HPP
#define SM_NUMERICAL_DIFF_HPP

#include <Eigen/Core>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <sm/assert_macros.hpp>

namespace sm {
namespace eigen {

template <typename RESULT_VEC_T, typename INPUT_VEC_T, typename JACOBIAN_T = Eigen::MatrixXd>
struct NumericalDiffFunctor {
    typedef RESULT_VEC_T value_t;
    typedef typename value_t::Scalar scalar_t;
    typedef INPUT_VEC_T input_t;
    typedef JACOBIAN_T jacobian_t;

    NumericalDiffFunctor(boost::function<value_t(input_t)> f) : _f(f) {}

    value_t operator()(const input_t& x) { return _f(x); }

    input_t update(const input_t& x, int c, scalar_t delta) {
        input_t xnew = x;
        xnew[c] += delta;
        return xnew;
    }
    boost::function<value_t(input_t)> _f;
};

// A simple implementation of central differences to estimate a Jacobian matrix
template <typename FUNCTOR_T>
struct NumericalDiff {
    typedef FUNCTOR_T functor_t;
    typedef typename functor_t::input_t input_t;
    typedef typename functor_t::value_t value_t;
    typedef typename functor_t::scalar_t scalar_t;
    typedef typename functor_t::jacobian_t jacobian_t;

    NumericalDiff(functor_t f, scalar_t eps = sqrt(std::numeric_limits<scalar_t>::epsilon())) : functor(f), eps(eps) {}

    jacobian_t estimateJacobian(input_t const& x0) {
        // evaluate the function at the operating point:
        value_t fx0 = functor(x0);
        size_t N = x0.size();
        size_t M = fx0.size();

        // std::cout << "Size: " << M << ", " << N << std::endl;
        jacobian_t J;
        J.resize(M, N);

        SM_ASSERT_EQ(std::runtime_error, x0.size(), J.cols(), "Unexpected number of columns for input size");
        SM_ASSERT_EQ(std::runtime_error, fx0.size(), J.rows(), "Unexpected number of columns for output size");

        for (unsigned c = 0; c < N; c++) {
            // Calculate a central difference.
            // This step size was stolen from cminpack: temp = eps * fabs(x[j]);
            scalar_t rcEps = std::max(static_cast<scalar_t>(fabs(x0(c))) * eps, eps);

            value_t fxp = functor(functor.update(x0, c, rcEps));
            value_t fxm = functor(functor.update(x0, c, -rcEps));
            J.block(0, c, M, 1) = (fxp - fxm).template cast<typename jacobian_t::Scalar>() /
                                  (typename jacobian_t::Scalar)(rcEps * (scalar_t)2.0);
        }
        return J;
    }

    functor_t functor;
    scalar_t eps;
};

template <typename ValueType_, typename InputType_>
Eigen::MatrixXd numericalDiff(
    std::function<ValueType_(const InputType_&)> function, InputType_ const& input,
    double eps =
        sqrt(std::numeric_limits<typename NumericalDiffFunctor<ValueType_, InputType_>::scalar_t>::epsilon())) {
    typedef NumericalDiffFunctor<ValueType_, InputType_> Functor;

    NumericalDiff<Functor> numDiff(Functor(function), eps);
    return numDiff.estimateJacobian(input);
}

}  // namespace eigen
}  // namespace sm

#endif /* SM_NUMERICAL_DIFF_HPP */


////////////////////////////////////////////////////////////////////////////////////
// The MIT License (MIT)                                                          //
//                                                                                //
// Copyright (c) 2015 Whit Armstrong                                              //
//                                                                                //
// Permission is hereby granted, free of charge, to any person obtaining a copy   //
// of this software and associated documentation files (the "Software"), to deal  //
// in the Software without restriction, including without limitation the rights   //
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      //
// copies of the Software, and to permit persons to whom the Software is          //
// furnished to do so, subject to the following conditions:                       //
//                                                                                //
// The above copyright notice and this permission notice shall be included in all //
// copies or substantial portions of the Software.                                //
//                                                                                //
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     //
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       //
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    //
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         //
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  //
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE  //
// SOFTWARE.                                                                      //
////////////////////////////////////////////////////////////////////////////////////

#pragma once

#include <armadillo>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/factorials.hpp>

namespace arma {

  // lgamma
  class eop_lgamma : public eop_core<eop_lgamma> {};

  template<> template<typename eT> arma_hot arma_pure arma_inline eT
  eop_core<eop_lgamma>::process(const eT val, const eT  ) {
    return boost::math::lgamma(val);
  }

  // Base
  template<typename T1>
  arma_inline
  const eOp<T1, eop_lgamma> lgamma(const Base<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_lgamma>(A.get_ref());
  }

  // BaseCube
  template<typename T1>
  arma_inline
  const eOpCube<T1, eop_lgamma> lgamma(const BaseCube<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOpCube<T1, eop_lgamma>(A.get_ref());
  }

  // factln
  double factln(const int i) {
    static std::vector<double> factln_table;

    if(i < 0) {
      return -std::numeric_limits<double>::infinity();
    }

    if(i > 100) {
      return boost::math::lgamma(static_cast<double>(i) + 1);
    }

    if(factln_table.size() < static_cast<size_t>(i+1)) {
      for(int j = factln_table.size(); j < (i+1); j++) {
        factln_table.push_back(std::log(boost::math::factorial<double>(static_cast<double>(j))));
      }
    }
    //for(auto v : factln_table) { std::cout << v << "|"; }  std::cout << std::endl;
    return factln_table[i];
  }

  class eop_factln : public eop_core<eop_factln> {};

  template<> template<typename eT> arma_hot arma_pure arma_inline eT
  eop_core<eop_factln>::process(const eT val, const eT  ) {
    return factln(val);
  }

  // Base
  template<typename T1>
  arma_inline
  const eOp<T1, eop_factln> factln(const Base<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_factln>(A.get_ref());
  }

  // BaseCube
  template<typename T1>
  arma_inline
  const eOpCube<T1, eop_factln> factln(const BaseCube<typename T1::elem_type,T1>& A) {
    arma_extra_debug_sigprint();
    return eOpCube<T1, eop_factln>(A.get_ref());
  }

  // cube
  //! element-wise multiplication of BaseCube objects with same element type
  template<typename T1, typename T2>
  arma_inline
  const eGlueCube<T1, T2, eglue_schur>
  schur_prod
  (
   const BaseCube<typename T1::elem_type,T1>& X,
   const BaseCube<typename T1::elem_type,T2>& Y
   )
  {
    arma_extra_debug_sigprint();
    return eGlueCube<T1, T2, eglue_schur>(X.get_ref(), Y.get_ref());
  }

  //! element-wise multiplication of BaseCube objects with different element types
  template<typename T1, typename T2>
  inline
  const mtGlueCube<typename promote_type<typename T1::elem_type, typename T2::elem_type>::result, T1, T2, glue_mixed_schur>
  schur_prod
  (
   const BaseCube< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T1_result, T1>& X,
   const BaseCube< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T2_result, T2>& Y
   )
  {
    arma_extra_debug_sigprint();
    typedef typename T1::elem_type eT1;
    typedef typename T2::elem_type eT2;
    typedef typename promote_type<eT1,eT2>::result out_eT;
    promote_type<eT1,eT2>::check();
    return mtGlueCube<out_eT, T1, T2, glue_mixed_schur>( X.get_ref(), Y.get_ref() );
  }

  // matrix
  template<typename T1, typename T2>
  arma_inline
  const eGlue<T1, T2, eglue_schur>
  schur_prod(const Base<typename T1::elem_type,T1>& X, const Base<typename T1::elem_type,T2>& Y) {
    arma_extra_debug_sigprint();
    return eGlue<T1, T2, eglue_schur>(X.get_ref(), Y.get_ref());
  }

  //! element-wise multiplication of Base objects with different element types
  template<typename T1, typename T2>
  inline
  const mtGlue<typename promote_type<typename T1::elem_type, typename T2::elem_type>::result, T1, T2, glue_mixed_schur>
  schur_prod
  (
   const Base< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T1_result, T1>& X,
   const Base< typename force_different_type<typename T1::elem_type, typename T2::elem_type>::T2_result, T2>& Y
   )
  {
    arma_extra_debug_sigprint();
    typedef typename T1::elem_type eT1;
    typedef typename T2::elem_type eT2;
    typedef typename promote_type<eT1,eT2>::result out_eT;
    promote_type<eT1,eT2>::check();
    return mtGlue<out_eT, T1, T2, glue_mixed_schur>( X.get_ref(), Y.get_ref() );
  }

  //! Base * scalar
  template<typename T1>
  arma_inline
  const eOp<T1, eop_scalar_times>
  schur_prod
  (const Base<typename T1::elem_type,T1>& X, const typename T1::elem_type k)
  {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_scalar_times>(X.get_ref(),k);
  }

  //! scalar * Base
  template<typename T1>
  arma_inline
  const eOp<T1, eop_scalar_times>
  schur_prod
  (const typename T1::elem_type k, const Base<typename T1::elem_type,T1>& X)
  {
    arma_extra_debug_sigprint();
    return eOp<T1, eop_scalar_times>(X.get_ref(),k);  // NOTE: order is swapped
  }

  double schur_prod(const int x, const double y) { return x * y; }
  double schur_prod(const double x, const int y) { return x * y; }
  double schur_prod(const double& x, const double& y) { return x * y; }
  double schur_prod(const int& x, const int& y) { return x * y; }

  // insert an 'any' function for bools into the arma namespace
  bool any(const bool x) {
    return x;
  }

  bool vectorise(bool x) {
    return x;
  }
} // namespace arma


#include <stdexcept>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <Eigen/Core>
#include <spectra/GenEigsSolver.h>
#include "diffusion_map.h"
#include "distance_matrix.h"

#ifdef _OPENMP
#include <omp.h>
#endif

using namespace Spectra;

namespace dmaps
{

    diffusion_map::diffusion_map(const matrix_t& d, const vector_t& w, int num_threads) :
    dint_(d), d_(dint_), w_(w)
    {
        check_params();

        #ifdef _OPENMP
        if(num_threads) omp_set_num_threads(num_threads);
        #endif
    }

    diffusion_map::diffusion_map(const distance_matrix& dm, const vector_t& w, int num_threads) :
    d_(dm.get_distances()), w_(w)
    {
        check_params();

        #ifdef _OPENMP
        if(num_threads) omp_set_num_threads(num_threads);
        #endif
    }

    void diffusion_map::check_params()
    {
        if(d_.cols() != d_.rows())
            throw std::invalid_argument("Distance matrix must be square.");

        if(w_.size() == 0)
            w_ = vector_t::Ones(d_.cols());
        else if(w_.size() != d_.cols())
            throw std::invalid_argument("Weights vector length must match distance matrix size.");
    }

    void diffusion_map::set_kernel_bandwidth(f_type eps)
    {
        if(eps <= 0)
            throw std::invalid_argument("Kernel bandwidth must be positive.");
        eps_ = eps;
    }

    f_type diffusion_map::get_kernel_bandwidth() const
    {
        return eps_;
    }

    f_type diffusion_map::sum_similarity_matrix(f_type eps, f_type alpha) const
    {
        matrix_t wwt = w_*w_.transpose();
        return ((-0.5/eps*d_.array().square().pow(alpha)).exp()*wwt.array()).sum();
    }

    /*
    void diffusion_map::estimate_local_scale(int k)
    {
        // Default choice of k.
        if(k == 0) k = static_cast<int>(std::sqrt(d_.rows()));

        // Set local epsilon scale for each entry.
        #ifdef _OPENMP
        #pragma omp parallel
        #endif
        {
            // Create sort indexer.
            std::vector<size_t> idx(d_.rows());
            std::iota(std::begin(idx), std::end(idx), static_cast<size_t>(0));

            #ifdef _OPENMP
            #pragma for schedule(static)
            #endif
            for(size_t i = 0; i < eps_.size(); ++i)
            {
                const vector_t& dist = d_.row(i);

                // Get indices of sorted distances (ascending).
                std::sort(std::begin(idx), std::end(idx),
                    [&](size_t a, size_t b) { return dist[a] < dist[b]; }
                );

                // Sum and determine weight.
                // We skip the first element which is itself.
                f_type sum = 0.;
                for(size_t j = 1; j < idx.size(); ++j)
                {
                    sum += w_[idx[j]];
                    // Break at k value.
                    if(sum >= k)
                    {
                        eps_[i] = dist[idx[j]];
                        break;
                    }
                }
            }
        }
    }
    */

    void diffusion_map::compute(int n, f_type alpha, f_type beta)
    {
        if(eps_ == 0)
            throw std::runtime_error("Kernel bandwidth must be defined before computing diffusion coordinates.");

        if(alpha <= 0 || alpha > 1)
            throw std::invalid_argument("Distance scaling must be in the interval (0,1].");

        // Compute similarity matrix and row normalize
        // to get right stochastic matrix.
        k_ = -0.5/eps_*d_.array().square().pow(alpha);
        k_.array() = k_.array().exp();
        k_.array() *= (w_*w_.transpose()).array();

        // Density normalization.
        vector_t rsum =  k_.rowwise().sum().array().pow(-beta);
        k_ = rsum.asDiagonal()*k_*rsum.asDiagonal();

        // Right stochastic matrix.
        rsum =  k_.rowwise().sum().array().cwiseInverse();
        k_ = rsum.asDiagonal()*k_;

        // Define eigensolver.
        DenseGenMatProd<f_type> op(k_);
        GenEigsSolver <f_type, LARGEST_MAGN, DenseGenMatProd<f_type>> eigs(&op, n, 2*n);

        // Solve.
        eigs.init();
        eigs.compute();

        if(eigs.info() != SUCCESSFUL)
            throw std::runtime_error("Eigensolver did not converge.");

        dvals_ = eigs.eigenvalues().real();
        dvecs_ = eigs.eigenvectors().real();
    }

    vector_t diffusion_map::nystrom(const vector_t& distances, f_type alpha, f_type beta)
    {
        if(dvals_.size() == 0)
            throw std::runtime_error("Eigenvectors must be computed first.");

        vector_t k = -0.5/eps_*distances.array().square().pow(alpha);
        k.array() = k.array().exp();
        k /= k.sum();

        vector_t point(dvals_.size());

        for(int i = 0; i < dvals_.size(); ++i)
            point[i] = 1./dvals_[i]*(k.array()*dvecs_.col(i).array()).sum();

        return point;
    }

    const matrix_t& diffusion_map::get_eigenvectors() const
    {
        return dvecs_;
    }

    const vector_t& diffusion_map::get_eigenvalues() const
    {
        return dvals_;
    }

    const matrixc_t& diffusion_map::get_kernel_matrix() const
    {
        return k_;
    }
}


#include <iostream>
#include <Eigen/Dense>
#include <manifold/S.h>

int main (int argc, char** argv) {

  S3d q;
  std::cout << q << std::endl;

  std::cout << q.Exp(Eigen::Vector3d(0.,M_PI/2.,0.)).norm() << std::endl;
  std::cout << q.Exp(q.ToAmbient(Eigen::Vector2d(0.,M_PI/2.))).norm() << std::endl;

  S3d mu;
  mu.vector() << 1./sqrt(2),1./sqrt(2), 0.;
  std::cout << mu << std::endl;

  double delta = 0.1;
  double f_prev = 1e99;
  double f = mu.dot(q);
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100; ++it) {
    Eigen::Vector3d J = -2.*(mu.vector() - q.vector()*q.dot(mu));

    q = q.Exp(-delta*J);
//    q = q.RetractOrtho(-delta*J);

    f_prev = f;
    f = mu.dot(q);
    std::cout << "@" << it << ": f=" << f
      << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) > -1e-10) break;
  }
  std::cout << std::endl << mu << std::endl;
  std::cout << std::endl << q << std::endl;
}


#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "flame/constants.h"
#include "flame/moment.h"

#define sqr(x)  ((x)*(x))
#define cube(x) ((x)*(x)*(x))

#ifdef DEFPATH
    #define defpath DEFPATH
#else
    #define defpath "."
#endif

std::map<std::string,boost::shared_ptr<Config> > CurveMap;

// http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
// by LU-decomposition.
void inverse(MomentElementBase::value_t& out, const MomentElementBase::value_t& in)
{
    using boost::numeric::ublas::permutation_matrix;
    using boost::numeric::ublas::lu_factorize;
    using boost::numeric::ublas::lu_substitute;
    using boost::numeric::ublas::identity_matrix;

    MomentElementBase::value_t scratch(in); // copy
    permutation_matrix<size_t> pm(scratch.size1());
    if(lu_factorize(scratch, pm)!=0)
        throw std::runtime_error("Failed to invert matrix");
    out.assign(identity_matrix<double>(scratch.size1()));
    //out = identity_matrix<double>(scratch.size1());
    lu_substitute(scratch, pm, out);
}

void RotMat(const double dx, const double dy,
            const double theta_x, const double theta_y, const double theta_z,
            typename MomentElementBase::value_t &R)
{
    typedef typename MomentElementBase::state_t state_t;

    MomentState::matrix_t T = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    R = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    // Left-handed coordinate system => theta_y -> -theta_y.

    double m11 =  cos(-theta_y)*cos(theta_z),
           m12 =  sin(theta_x)*sin(-theta_y)*cos(theta_z) + cos(theta_x)*sin(theta_z),
           m13 = -cos(theta_x)*sin(-theta_y)*cos(theta_z) + sin(theta_x)*sin(theta_z),

           m21 = -cos(-theta_y)*sin(theta_z),
           m22 = -sin(theta_x)*sin(-theta_y)*sin(theta_z) + cos(theta_x)*cos(theta_z),
           m23 =  cos(theta_x)*sin(-theta_y)*sin(theta_z) + sin(theta_x)*cos(theta_z),

           m31 =  sin(-theta_y),
           m32 = -sin(theta_x)*cos(-theta_y),
           m33 =  cos(theta_x)*cos(-theta_y);

    R(0, 0) = m11, R(0, 2) = m12, R(0, 4) = m13;
    R(2, 0) = m21, R(2, 2) = m22, R(2, 4) = m23;
    R(4, 0) = m31, R(4, 2) = m32, R(4, 4) = m33;

    R(1, 1) = m11, R(1, 3) = m12, R(1, 5) = m13;
    R(3, 1) = m21, R(3, 3) = m22, R(3, 5) = m23;
    R(5, 1) = m31, R(5, 3) = m32, R(5, 5) = m33;

    T(0, 6) = -dx, T(2, 6) = -dy;

    R = prod(R, T);
}

void GetQuadMatrix(const double L, const double K, const unsigned ind, typename MomentElementBase::value_t &M)
{
    // 2D quadrupole transport matrix.
    double sqrtK,
           psi,
           cs,
           sn;

    if (K > 0e0) {
        // Focusing.
        sqrtK = sqrt(K);
        psi = sqrtK*L;
        cs = ::cos(psi);
        sn = ::sin(psi);

        M(ind, ind) = M(ind+1, ind+1) = cs;
        if (sqrtK != 0e0)
            M(ind, ind+1) = sn/sqrtK;
        else
            M(ind, ind+1) = L;
        if (sqrtK != 0e0)
            M(ind+1, ind) = -sqrtK*sn;
        else
            M(ind+1, ind) = 0e0;
    } else {
        // Defocusing.
        sqrtK = sqrt(-K);
        psi = sqrtK*L;
        cs = ::cosh(psi);
        sn = ::sinh(psi);

        M(ind, ind) = M(ind+1, ind+1) = cs;
        if (sqrtK != 0e0)
            M(ind, ind+1) = sn/sqrtK;
        else
            M(ind, ind+1) = L;
        if (sqrtK != 0e0)
            M(ind+1, ind) = sqrtK*sn;
        else
            M(ind+1, ind) = 0e0;
    }
}

void GetSextMatrix(const double L, const double K3, double Dx, double Dy,
                   const double D2x, const double D2y, const double D2xy, const bool thinlens, const bool dstkick, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;
    // 2D sextupole transport matrix.
    double sqrtK, psi, cs, sn, ch, sh,
           dr = sqrt(sqr(Dx)+sqr(Dx));

    if (thinlens) {

        //thin-lens (drift-kick-drift) model

        MomentState::matrix_t T = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);
        MomentState::matrix_t P = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);
        MomentState::matrix_t scratch;

        T(state_t::PS_X, state_t::PS_PX) = L/2e0;
        T(state_t::PS_Y, state_t::PS_PY) = L/2e0;

        P(state_t::PS_PX, state_t::PS_X) = -K3*L*Dx;
        P(state_t::PS_PX, state_t::PS_Y) =  K3*L*Dy;

        P(state_t::PS_PY, state_t::PS_X) = K3*L*Dy;
        P(state_t::PS_PY, state_t::PS_Y) = K3*L*Dx;

        scratch = prod(P,T);
        M = prod(T,scratch);

    } else {

        //thick-lens model

        sqrtK = sqrt(fabs(K3)*dr);
        psi = sqrtK*L;

        // Focusing or Defocusing switch
        Dx *= copysign(1e0,K3);
        Dy *= copysign(1e0,K3);

        cs = ::cos(psi);
        sn = ::sin(psi);
        ch = ::cosh(psi);
        sh = ::sinh(psi);

        if (sqrtK != 0e0) {
            M(state_t::PS_X, state_t::PS_X) = M(state_t::PS_PX, state_t::PS_PX) = ((dr+Dx)*cs+(dr-Dx)*ch)/(2e0*dr);
            M(state_t::PS_X, state_t::PS_PX) = ((dr+Dx)*sn+(dr-Dx)*sh)/(2e0*sqrtK*dr);
            M(state_t::PS_X, state_t::PS_Y)  = M(state_t::PS_PX, state_t::PS_PY) =
            M(state_t::PS_Y, state_t::PS_X)  = M(state_t::PS_PY, state_t::PS_PX) = Dy*(-cs+ch)/(2e0*dr);
            M(state_t::PS_X, state_t::PS_PY) = Dy*(-sn+sh)/(2e0*sqrtK*dr);

            M(state_t::PS_PX, state_t::PS_X) = sqrtK*(-(dr+Dx)*sn+(dr-Dx)*sh)/(2e0*dr);
            M(state_t::PS_PX, state_t::PS_Y) = M(state_t::PS_PY, state_t::PS_X) =  Dy*sqrtK*(sn+sh)/(2e0*dr);

            M(state_t::PS_Y, state_t::PS_PX) = Dy*(-sn+sh)/(2e0*sqrtK*dr);
            M(state_t::PS_Y, state_t::PS_Y) = M(state_t::PS_PY, state_t::PS_PY) = ((dr-Dx)*cs+(dr+Dx)*ch)/(2e0*dr);
            M(state_t::PS_Y, state_t::PS_PY) = ((dr-Dx)*sn+(dr+Dx)*sh)/(2e0*sqrtK*dr);

            M(state_t::PS_PY, state_t::PS_Y) = sqrtK*(-(dr-Dx)*sn+(dr+Dx)*sh)/(2e0*dr);

        } else {
            M(state_t::PS_X, state_t::PS_PX) = L;
            M(state_t::PS_Y, state_t::PS_PY) = L;
        }

    } // option
    if (dstkick){
        M(state_t::PS_PX, 6) = -K3*L*(D2x-D2y);
        M(state_t::PS_PY, 6) =  2e0*K3*L*D2xy;
    }
}

void GetEdgeMatrix(const double rho, const double phi, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    M = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    M(state_t::PS_PX, state_t::PS_X) =  tan(phi)/rho;
    M(state_t::PS_PY, state_t::PS_Y) = -tan(phi)/rho;
}

void GetEEdgeMatrix(const double fringe_x, const double fringe_y, const double kappa, typename MomentElementBase::value_t &M)
{
    // Edge focusing for electrostatic dipole.
    typedef typename MomentElementBase::state_t state_t;

    M = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    M(state_t::PS_PX, state_t::PS_X)  = fringe_x;
    M(state_t::PS_PX, state_t::PS_PX) = sqrt(1e0+kappa);
    M(state_t::PS_PY, state_t::PS_Y)  = fringe_y;
    M(state_t::PS_PS, state_t::PS_PS) = 1e0+kappa;
}

void GetSBendMatrix(const double L, const double phi, const double phi1, const double phi2, const double K,
                    const double IonEs, const double ref_gamma, const double qmrel,
                    const double dip_beta, const double dip_gamma, const double d, const double dip_IonK, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    MomentState::matrix_t edge1, edge2, R;

    double  rho = L/phi,
            Kx  = K + 1e0/sqr(rho),
            Ky  = -K,
            dx  = 0e0,
            sx  = 0e0;

    // Horizontal plane.
    GetQuadMatrix(L, Kx, (unsigned)state_t::PS_X, M);
    // Vertical plane.
    GetQuadMatrix(L, Ky, (unsigned)state_t::PS_Y, M);

    // Include dispersion.
    if (Kx == 0e0) {
        dx = sqr(L)/(2e0*rho);
        sx = L/rho;
    } else if (Kx > 0e0) {
        dx = (1e0-cos(sqrt(Kx)*L))/(rho*Kx);
        sx = sin(sqrt(Kx)*L)/(rho*sqrt(Kx));
    } else {
        dx = (1e0-cosh(sqrt(-Kx)*L))/(rho*Kx);
        sx = sin(sqrt(-Kx)*L)/(rho*sqrt(-Kx));
    }

    M(state_t::PS_X,  state_t::PS_PS) = dx/(sqr(dip_beta)*dip_gamma*IonEs/MeVtoeV);
    M(state_t::PS_PX, state_t::PS_PS) = sx/(sqr(dip_beta)*dip_gamma*IonEs/MeVtoeV);

    M(state_t::PS_S,  state_t::PS_X)  = sx*dip_IonK;
    M(state_t::PS_S,  state_t::PS_PX) = dx*dip_IonK;
    // Low beta approximation.
    M(state_t::PS_S,  state_t::PS_PS) =
            ((L/rho-sx)/(Kx*rho)-L/sqr(ref_gamma))*dip_IonK
            /(sqr(dip_beta)*dip_gamma*IonEs/MeVtoeV);

    // Add dipole terms.
    M(state_t::PS_S,  6) = ((L/rho-sx)/(Kx*rho)*d-L/sqr(ref_gamma)*(d+qmrel))*dip_IonK;
    M(state_t::PS_X,  6) = dx*d;
    M(state_t::PS_PX, 6) = sx*d;

    // Edge focusing.
    GetEdgeMatrix(rho, phi1, edge1);
    GetEdgeMatrix(rho, phi2, edge2);

    M = prod(M, edge1);
    M = prod(edge2, M);

    // Longitudinal plane.
    // For total path length.
    //        M(state_t::PS_S,  state_t::PS_S) = L;
}

void GetSolMatrix(const double L, const double K, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    double C = ::cos(K*L),
           S = ::sin(K*L);

    M(state_t::PS_X, state_t::PS_X)
            = M(state_t::PS_PX, state_t::PS_PX)
            = M(state_t::PS_Y, state_t::PS_Y)
            = M(state_t::PS_PY, state_t::PS_PY)
            = sqr(C);

    if (K != 0e0)
        M(state_t::PS_X, state_t::PS_PX) = S*C/K;
    else
        M(state_t::PS_X, state_t::PS_PX) = L;
    M(state_t::PS_X, state_t::PS_Y) = S*C;
    if (K != 0e0)
        M(state_t::PS_X, state_t::PS_PY) = sqr(S)/K;
    else
        M(state_t::PS_X, state_t::PS_PY) = 0e0;

    M(state_t::PS_PX, state_t::PS_X)  = -K*S*C;
    M(state_t::PS_PX, state_t::PS_Y)  = -K*sqr(S);
    M(state_t::PS_PX, state_t::PS_PY) = S*C;

    M(state_t::PS_Y, state_t::PS_X)   = -S*C;
    if (K != 0e0)
        M(state_t::PS_Y, state_t::PS_PX) = -sqr(S)/K;
    else
        M(state_t::PS_Y, state_t::PS_PX) = 0e0;
    if (K != 0e0)
        M(state_t::PS_Y, state_t::PS_PY) = S*C/K;
    else
        M(state_t::PS_Y, state_t::PS_PY) = L;

    M(state_t::PS_PY, state_t::PS_X)  = K*sqr(S);
    M(state_t::PS_PY, state_t::PS_PX) = -S*C;
    M(state_t::PS_PY, state_t::PS_Y)  = -K*S*C;

    // Longitudinal plane.
    // For total path length.
//        M(state_t::PS_S, state_t::PS_S) = L;
}

void GetEBendMatrix(const double L, const double phi, const double fringe_x, const double fringe_y, const double kappa,
                    const double Kx, const double Ky, const double IonEs, const double real_gamma, const double eta0, const double h,
                    const double delta_K, const double delta_KZ, const double SampleIonK, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    MomentState::matrix_t edge;

    double  rho = L/phi,
            scl = (real_gamma - 1e0)*IonEs/MeVtoeV,
            dx  = 0e0,
            sx  = 0e0;

    // Horizontal plane.
    GetQuadMatrix(L, Kx, (unsigned)state_t::PS_X, M);
    // Vertical plane.
    GetQuadMatrix(L, Ky, (unsigned)state_t::PS_Y, M);

    // Include dispersion.
    if (Kx == 0e0) {
        dx = 0e0;
        sx = 0e0;
    } else if (Kx > 0e0) {
        dx = (1e0-cos(sqrt(Kx)*L))/(rho*Kx);
        sx = sin(sqrt(Kx)*L)/sqrt(Kx);
    } else {
        dx = (1e0-cosh(sqrt(-Kx)*L))/(rho*Kx);
        sx = sin(sqrt(Kx)*L)/sqrt(Kx);
    }

    double Nk   = (sqr(1e0+2e0*eta0)+h)/(2e0*(1e0+eta0)*(1e0+2e0*eta0)),
           Nt   = 1e0 + h/sqr(1e0+2e0*eta0),
           CorT = -real_gamma/(1e0+real_gamma),
           tx   = sx/rho*Nt*CorT,
           txp  = dx*Nt*CorT,
           tzp  = (-L/(2e0*(1e0+eta0)*(1e0+2e0*eta0))+((L-sx)/Kx/sqr(rho))*Nk*Nt)*CorT;

    M(state_t::PS_X,  state_t::PS_PS) = dx*Nk/scl;
    M(state_t::PS_PX, state_t::PS_PS) = sx/rho*Nk/scl;

    M(state_t::PS_S,  state_t::PS_X)  = -tx*SampleIonK;
    M(state_t::PS_S,  state_t::PS_PX) = -txp*SampleIonK;
    // Low beta approximation.
    M(state_t::PS_S,  state_t::PS_PS) = -tzp*SampleIonK/scl;

    // Add dipole terms.
    double Nkz = (1e0+2e0*eta0-h)/(2e0*(1e0+eta0)*(1e0+2e0*eta0));

    M(state_t::PS_X,  6) = dx*(Nk*delta_K+Nkz*delta_KZ);
    M(state_t::PS_PX, 6) = sx/rho*(Nk*delta_K+Nkz*delta_KZ);

    // Edge focusing.
    GetEEdgeMatrix(fringe_x, fringe_y ,kappa, edge);

    M = prod(M, edge);
    M = prod(edge, M);

    // Longitudinal plane.
    // For total path length.
    //        M(state_t::PS_S,  state_t::PS_S) = L;
}

void GetCurveData(const Config &c, const unsigned ncurve, std::vector<double> &Scales,
                  std::vector<std::vector<double> > &Curves)
{
    boost::shared_ptr<Config> conf;

    std::string filename;
    std::vector<double> range;
    bool checker = c.tryGet<std::string>("CurveFile", filename),
         rngchecker = c.tryGet<std::vector<double> >("use_range", range);

    if (checker){
        std::string CurveFile =  c.get<std::string>("Eng_Data_Dir", defpath);
        CurveFile += "/" + filename;
        std::string key(SB()<<CurveFile<<"|"<<boost::filesystem::last_write_time(CurveFile));
        if ( CurveMap.find(key) == CurveMap.end() ) {
            // not found in CurveMap
            try {
                try {
                    GLPSParser P;
                    conf.reset(P.parse_file(CurveFile.c_str(), false));
                }catch(std::exception& e){
                    throw std::runtime_error(SB()<<"Parse error: "<<e.what()<<"\n");
                }

            }catch(std::exception& e){
                throw std::runtime_error(SB()<<"Error: "<<e.what()<<"\n");
            }
            CurveMap.insert(std::make_pair(key, conf));
        } else {
            // found in CurveMap
            conf=CurveMap[key];
        }
    }

    size_t prev_size = 0;
    for (unsigned n=0; n<ncurve; n++) {
        std::string num(boost::lexical_cast<std::string>(n));
        std::vector<double> cv;
        bool cvchecker;
        if (checker){
            cvchecker = conf->tryGet<std::vector<double> >("curve"+num, cv);
        } else {
            cvchecker = c.tryGet<std::vector<double> >("curve"+num, cv);
        }
        if (!cvchecker)throw std::runtime_error(SB()<<"'curve" << num << "' is missing in lattice file.\n");

        if (rngchecker){
            unsigned start, end;
            if (range.size() != 2)
                throw std::runtime_error(SB()<<"Size of 'use_range' must be 2.\n");
            start = static_cast<unsigned>(range[0]);
            end   = static_cast<unsigned>(range[1]);

            if (start > cv.size() or end > cv.size())
                throw std::runtime_error(SB()<<"'use_range' is out of curve size.\n");

            std::vector<double> part(cv.begin()+start, cv.begin()+end);
            Curves.push_back(part);
        } else {
            Curves.push_back(cv);
        }

        if (n != 0 and prev_size != cv.size())
            throw std::runtime_error(SB()<<"Size of 'curve" << n << "' (" << cv.size() <<") and 'curve" << n-1 <<
                                           "' (" << prev_size <<") are inconsistent.  All curves must have the same size.\n");
        prev_size = cv.size();

        Scales.push_back(c.get<double>("scl_fac"+num, 0.0));
    }
}


#ifndef TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP
#define TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>
#include <triumf/math/faddeeva.hpp>
#include <iostream>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed Gaussian distribution of exponentials integral
/// (from 0 to time_p <= time)
template <typename T = double>
T pulsed_gauss_dist_exp_integral(T time, T time_p, T nuclear_lifetime,
                                 T slr_rate, T sigma) {
  // imaginary number i
  std::complex<T> i(0.0, -1.0);
  // common exponential factor
  T expo =
      std::exp(-slr_rate / nuclear_lifetime / sigma / sigma -
               1.0 / 2.0 / sigma / sigma / nuclear_lifetime / nuclear_lifetime -
               slr_rate * slr_rate / 2.0 / sigma / sigma);
  std::cout << "expo = " << expo << "\n";
  // argument for the first error function
  std::complex<T> erf_arg1 =
      (((boost::math::constants::root_two<T>() * i * time -
         boost::math::constants::root_two<T>() * i * time_p) *
            sigma * sigma -
        boost::math::constants::root_two<T>() * i * slr_rate) *
           nuclear_lifetime -
       boost::math::constants::root_two<T>() * i) /
      (2.0 * sigma * nuclear_lifetime);
  std::cout << "erf_arg1 = " << erf_arg1 << "\n";
  // argument for the second error function
  std::complex<T> erf_arg2 =
      ((i * time * sigma * sigma - i * slr_rate) * nuclear_lifetime - i) /
      (boost::math::constants::root_two<T>() * sigma * nuclear_lifetime);
  std::cout << "erf_arg2 = " << erf_arg2 << "\n";
  // evaluate the result
  std::complex<T> result = boost::math::constants::root_pi<T>() * i * expo *
                           (triumf::math::faddeeva::erf(erf_arg1) -
                            triumf::math::faddeeva::erf(erf_arg2)) /
                           (boost::math::constants::root_two<T>() * sigma);
  std::cout << "result = " << result << "\n";
  // return only the real part
  return result.real();
}

/// pulsed Gaussian distribution of exponentials
template <typename T = double>
T pulsed_gauss_dist_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                        T slr_rate, T sigma) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_gauss_dist_exp_integral(time, time, nuclear_lifetime,
                                          slr_rate, sigma) /
           normalization(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_gauss_dist_exp_integral(pulse_length, pulse_length,
                                           nuclear_lifetime, slr_rate, sigma) /
            normalization(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed Gaussian distribution of exponentials (ROOT)
template <typename T = double>
T pulsed_gauss_dist_exp(const T *x, const T *par) {
  return pulsed_gauss_dist_exp<T>(*x, par[0], par[1], par[2], par[3], par[4]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP


#include <Eigen/Dense>
#include <iostream>

#include <ancse/config.hpp>
#include <ancse/polynomial_basis.hpp>
#include <ancse/dg_handler.hpp>
#include <ancse/dg_limiting.hpp>
#include <ancse/cfl_condition.hpp>
#include <ancse/dg_rate_of_change.hpp>
#include <ancse/snapshot_writer.hpp>
#include <ancse/time_loop.hpp>

static int n_vars = 3;

template<class F>
Eigen::MatrixXd ic(const F &f,
                   const Grid &grid,
                   const PolynomialBasis &poly_basis,
                   const DGHandler &dg_handler)
{
    int n_coeff = 1 + poly_basis.get_degree();
    Eigen::MatrixXd u0 = Eigen::MatrixXd::Zero (n_vars*n_coeff,
                                                grid.n_cells);

    auto [quad_points, quad_weights] = dg_handler.get_quadrature();
    int n_quad = static_cast<int>(quad_points.size());

    /// eval basis and its derivate for all quadrature points
    Eigen::MatrixXd basis(n_coeff, n_quad);
    for (int k = 0; k < n_quad; k++) {
        basis.col(k) = poly_basis(quad_points(k));
    }

    // L2-projection
    for (int j = 0; j < grid.n_cells; ++j)
    {
        for (int k = 0; k < quad_points.size(); k++)
        {
            auto fVal = f(cell_point(grid, j, quad_points(k)));
            for (int i = 0; i < n_vars; i++) {
                u0.col(j).segment(i*n_coeff, n_coeff)
                        += quad_weights(k)*fVal(i)*basis.col(k);
            }
        }
    }

    return u0*grid.dx;
}

TimeLoop make_dg(const nlohmann::json &config,
                 const Grid &grid,
                 const PolynomialBasis &poly_basis,
                 const DGHandler &dg_handler,
                 std::shared_ptr<Model> &model)
{
    double t_end = config["t_end"];
    double cfl_number = config["cfl_number"];

    auto n_ghost = grid.n_ghost;
    auto n_cells = grid.n_cells;

    auto n_vars = model->get_nvars();
    int n_coeff = 1 + poly_basis.get_degree();

    auto simulation_time = std::make_shared<SimulationTime>(t_end);
    auto boundary_condition
            = make_boundary_condition(n_ghost,
                                      config["boundary_condition"]);
    auto dg_rate_of_change
            = make_dg_rate_of_change(config, grid, model,
                                     poly_basis, dg_handler,
                                     simulation_time);
    auto dg_limiting
            = make_dg_limiting(config, grid, dg_handler);
    auto time_integrator = make_runge_kutta(config,
                                            dg_rate_of_change,
                                            boundary_condition,
                                            dg_limiting,
                                            n_vars*n_coeff, n_cells);
    auto cfl_condition
            = make_cfl_condition(grid, model, dg_handler, cfl_number);
    auto snapshot_writer = std::make_shared<JSONSnapshotWriter <DG>>
            (grid, model, dg_handler, simulation_time,
             std::string(config["output_dir"]),
             std::string(config["output_file"]));

    return TimeLoop(simulation_time, time_integrator,
                    cfl_condition, snapshot_writer);
}

void sod_shock_tube_test(const nlohmann::json &config)
{
    int deg = int(config["degree"]);

    double gamma = 7./5.;
    std::shared_ptr<Model> model = std::make_shared<Euler>();
    auto model_euler = dynamic_cast<Euler*>(model.get());
    model_euler->set_gamma(gamma);

    auto fn = [gamma](double x) {
        Eigen::VectorXd u(n_vars);
        if (x <= 0.5) { // left state
            u(0) = 1;
            u(1) = 0;
            u(2) = 1/(gamma-1);
        } else {        // right state
            u(0) = 0.125;
            u(1) = 0;
            u(2) = 0.1/(gamma-1);
        }
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto poly_basis = PolynomialBasis(deg, 1./sqrt(grid.dx));
    auto dg_handler = DGHandler(model, poly_basis);
    auto u0 = ic(fn, grid, poly_basis, dg_handler);

    auto dg = make_dg(config, grid, poly_basis, dg_handler, model);
    dg(u0);
}

int main(int argc, char* const argv[])
{
    nlohmann::json config;
    std::string fileName;
    if (argc == 2) {
        fileName = argv[1];
    } else {
        fileName = "../config.json";
    }
    config = get_config (fileName);

    std::string ic_key = config["initial_conditions"];
    if (ic_key == "sod_shock_tube") {
        sod_shock_tube_test(config);
    }

    return 0;
}