#include "image_processing.h"
#include <numeric>
#include <cfloat>
#include <Eigen/LU>
#include <QImage>

namespace ImageProcessing
{

template <typename Scalar>
Scalar crop(Scalar x, Scalar min_x, Scalar max_x)
{
    return std::max(std::min(x, max_x), min_x);
}

void Image::force_unity()
{
    const double sum = std::accumulate(pixels.begin(), pixels.end(), 0.0);
    assert(sum > 1e-16);
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        set_pixel(x, y, get_pixel(x, y) / sum);
    }
}

void Image::scale_to_unit()
{
    double max_value = - DBL_MAX;
    double min_value = + DBL_MAX;

    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const double value = get_pixel(x, y);
        max_value = std::max(max_value, value);
        min_value = std::min(min_value, value);
    }
    assert(max_value - min_value > 0.0);
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        set_pixel(x, y, (get_pixel(x, y) - min_value) / (max_value - min_value));
    }
}

void AbstractImage::save(const std::string &file_path) const
{
    QImage q_image(width(), height(), QImage::Format_ARGB32);
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const IntColor color = get_color(x, y);
        q_image.setPixelColor(x, y, QColor(color(0), color(1), color(2), color(3)));
    }
    q_image.save(QString::fromStdString(file_path));
}

AbstractImage::IntColor Image::get_color(int x, int y) const
{
    const double value = crop(get_pixel(x, y), 0.0, 1.0);
    return IntColor(value * 255, value * 255, value * 255, 255);
}

AbstractImage::IntColor ColorImage::get_color(int x, int y) const
{
    Eigen::Vector4d color = get_rgba(x, y);
    for (int i : { 0, 1, 2, 3 }) color(i) = crop(color(i), 0.0, 1.0);
    return IntColor(color(0) * 255, color(1) * 255, color(2) * 255, color(3) * 255);
}

ColorImage::ColorImage(const std::string &file_path)
{
    QImage q_image(QString::fromStdString(file_path));
    width_  = q_image.width();
    height_ = q_image.height();

    assert(width() > 0 && height() > 0);

    rgba_ = std::vector<Image>(4, Image(width(), height()));
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const QColor q_color = q_image.pixelColor(x, y);
        rgba_[0].set_pixel(x, y, q_color.redF());
        rgba_[1].set_pixel(x, y, q_color.greenF());
        rgba_[2].set_pixel(x, y, q_color.blueF());
        rgba_[3].set_pixel(x, y, q_color.alphaF());
    }
}

Image ColorImage::get_luminance() const
{
    Image new_image(width(), height());
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const double r = rgba_[0].get_pixel(x, y);
        const double g = rgba_[1].get_pixel(x, y);
        const double b = rgba_[2].get_pixel(x, y);

        // https://en.wikipedia.org/wiki/Relative_luminance
        const double luminance = 0.2126 * r + 0.7152 * g + 0.0722 * b;

        new_image.set_pixel(x, y, luminance);
    }
    return new_image;
}

///////////////////////////////////////////////////////////////////////////////////////

Image apply_convolution(const Image& image, const Eigen::MatrixXd &kernel)
{
    const int w = image.width();
    const int h = image.height();

    const int kernel_size = kernel.rows();

    assert(kernel_size % 2 == 1);
    assert(kernel_size == kernel.cols());

    Image new_image(w, h);

    for (int x = 0; x < w; ++ x) for (int y = 0; y < h; ++ y)
    {
        double value = 0.0;
        for (int kernel_x = 0; kernel_x < kernel_size; ++ kernel_x)
        {
            for (int kernel_y = 0; kernel_y < kernel_size; ++ kernel_y)
            {
                const int original_image_x = crop(x + kernel_x - ((kernel_size - 1) / 2), 0, w - 1);
                const int original_image_y = crop(y + kernel_y - ((kernel_size - 1) / 2), 0, h - 1);

                value += kernel(kernel_x, kernel_y) * image.get_pixel(original_image_x, original_image_y);
            }
        }
        new_image.set_pixel(x, y, value);
    }
    return new_image;
}

Image calculate_guided_filter_kernel(const Image& image, int center_x, int center_y, int radius, double epsilon, bool force_positive)
{
    const int width  = image.width();
    const int height = image.height();

    const Image mean_I = ImageProcessing::apply_box_filter(image, radius);
    const Image corr_I = ImageProcessing::apply_box_filter(ImageProcessing::square(image), radius);
    const Image var_I  = ImageProcessing::subtract(corr_I, ImageProcessing::square(mean_I));
    const double I_seed = image.get_pixel(center_x, center_y);

    Image weight_map(width, height, 0.0);
    for (int x = center_x - radius; x <= center_x + radius; ++ x)
    {
        for (int y = center_y - radius; y <= center_y + radius; ++ y)
        {
            if (x < 0 || x >= width || y < 0 || y >= height) continue;

            const double I_j = image.get_pixel(x, y);
            double weight = 0.0;
            for (int k_x = 0; k_x < width; ++ k_x)
            {
                for (int k_y = 0; k_y < height; ++ k_y)
                {
                    bool range_j    = (k_x >= x - radius && k_x <= x + radius) && (k_y >= y - radius && k_y <= y + radius);
                    bool range_seed = (k_x >= center_x - radius && k_x <= center_x + radius) && (k_y >= center_y - radius && k_y <= center_y + radius);
                    if (!range_j || !range_seed) continue;

                    const double mu_k  = mean_I.get_pixel(k_x, k_y);
                    const double var_k = var_I.get_pixel(k_x, k_y);
                    weight += 1.0 + ((I_seed - mu_k) * (I_j - mu_k)) / (epsilon + var_k);
                }
            }
            if (force_positive) weight = std::max(0.0, weight);
            weight_map.set_pixel(x, y, weight);
        }
    }
    weight_map.force_unity();
    return weight_map;
}

Image calculate_gradient_magnitude(const Image& image)
{
    const int width  = image.width();
    const int height = image.height();
    const Image sobel_x = ImageProcessing::apply_sobel_filter_x(image);
    const Image sobel_y = ImageProcessing::apply_sobel_filter_y(image);

    Image gradient_magnitude = Image(width, height);
    for (int x = 0; x < width; ++ x)
    {
        for (int y = 0; y < height; ++ y)
        {
            const double g_x = sobel_x.get_pixel(x, y);
            const double g_y = sobel_y.get_pixel(x, y);
            const double value = std::sqrt(g_x * g_x + g_y * g_y);
            gradient_magnitude.set_pixel(x, y, value);
        }
    }
    return gradient_magnitude;
}

Image apply_guided_filter(const Image& input_image, const ColorImage& guidance_image, int radius, double epsilon)
{
    const int width  = input_image.width();
    const int height = input_image.height();

    assert(width == guidance_image.width());
    assert(height == guidance_image.height());

    const Image I_r = guidance_image.get_r();
    const Image I_g = guidance_image.get_g();
    const Image I_b = guidance_image.get_b();

    const Image mean_I_r = apply_box_filter(I_r, radius);
    const Image mean_I_g = apply_box_filter(I_g, radius);
    const Image mean_I_b = apply_box_filter(I_b, radius);

    const Image mean_p = apply_box_filter(input_image, radius);

    const Image mean_Ip_r = apply_box_filter(multiply(I_r, input_image), radius);
    const Image mean_Ip_g = apply_box_filter(multiply(I_g, input_image), radius);
    const Image mean_Ip_b = apply_box_filter(multiply(I_b, input_image), radius);

    const Image cov_Ip_r = subtract(mean_Ip_r, multiply(mean_I_r, mean_p));
    const Image cov_Ip_g = subtract(mean_Ip_g, multiply(mean_I_g, mean_p));
    const Image cov_Ip_b = subtract(mean_Ip_b, multiply(mean_I_b, mean_p));

    const Image var_I_rr = subtract(apply_box_filter(multiply(I_r, I_r), radius), multiply(mean_I_r, mean_I_r));
    const Image var_I_rg = subtract(apply_box_filter(multiply(I_r, I_g), radius), multiply(mean_I_r, mean_I_g));
    const Image var_I_rb = subtract(apply_box_filter(multiply(I_r, I_b), radius), multiply(mean_I_r, mean_I_b));
    const Image var_I_gg = subtract(apply_box_filter(multiply(I_g, I_g), radius), multiply(mean_I_g, mean_I_g));
    const Image var_I_gb = subtract(apply_box_filter(multiply(I_g, I_b), radius), multiply(mean_I_g, mean_I_b));
    const Image var_I_bb = subtract(apply_box_filter(multiply(I_b, I_b), radius), multiply(mean_I_b, mean_I_b));

    Image a_r(width, height);
    Image a_g(width, height);
    Image a_b(width, height);
    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
    {
        Eigen::Matrix3d sigma;
        sigma << var_I_rr.get_pixel(x, y), var_I_rg.get_pixel(x, y), var_I_rb.get_pixel(x, y),
                 var_I_rg.get_pixel(x, y), var_I_gg.get_pixel(x, y), var_I_gb.get_pixel(x, y),
                 var_I_rb.get_pixel(x, y), var_I_gb.get_pixel(x, y), var_I_bb.get_pixel(x, y);

        const Eigen::Vector3d cov_Ip = { cov_Ip_r.get_pixel(x, y), cov_Ip_g.get_pixel(x, y), cov_Ip_b.get_pixel(x, y) };
        const Eigen::Vector3d a_xy = (sigma + epsilon * Eigen::Matrix3d::Identity()).inverse() * cov_Ip;

        a_r.set_pixel(x, y, a_xy(0));
        a_g.set_pixel(x, y, a_xy(1));
        a_b.set_pixel(x, y, a_xy(2));
    }

    const Image b = subtract(subtract(subtract(mean_p, multiply(a_r, mean_I_r)), multiply(a_g, mean_I_g)), multiply(a_b, mean_I_b));

    Image q = apply_box_filter(b, radius);
    q = add(q, multiply(apply_box_filter(a_r, radius), I_r));
    q = add(q, multiply(apply_box_filter(a_g, radius), I_g));
    q = add(q, multiply(apply_box_filter(a_b, radius), I_b));

    return q;
}

}


// practice 5-8

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/size.hpp>

using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

struct fibo_tag {};
struct fibo_series
{
	typedef fibo_tag tag;
	typedef fibo_series type;
};

template <int Value, int Pos>
struct fibo_iterator
{
	typedef fibo_tag tag;
	typedef typename fibo_iterator type;
	static const int value = Value;
	static const int pos = Pos;
};

template<int N>
struct fibo_val
{
	static const int value =
		fibo_val<N-1>::value +
		fibo_val<N-2>::value;
};

template<>
struct fibo_val<0> : mpl::int_<0>
{};

template<>
struct fibo_val<1> : mpl::int_<1>
{};

namespace boost { namespace mpl {
	template<>
	struct begin_impl< fibo_tag >
	{
		template<class S>
		struct apply
		{
			typedef typename fibo_iterator<0,0> type;
		};
	};
}}

namespace boost { namespace mpl {
	template<>
	struct advance_impl< fibo_tag >
	{
		template<class It, class N>
		struct apply
		{
			typedef typename fibo_iterator<
				fibo_val< It::pos + N::value >::value
				, It::pos + N::value > type;
		};
	};
}}

namespace boost { namespace mpl {
	template<int Value, int PosValue>
	struct deref< fibo_iterator<Value, PosValue> >
	{
		typedef typename fibo_iterator<Value, PosValue> type;
	};
}}

// 0 1 1 2 3 5 8 13 21..
//fibonacci_series
// typedef mpl::advance_c< mpl::begin< fibon >::type, 6 >::type i;
// mpl::deref<i>:;type::value == 8

// typedef mpl::advance_c< i, 4 >::type j;
// mpl::deref<j>::type::value == 55

typedef mpl::advance_c<
	mpl::begin< fibo_series >::type, 6
	>::type i;
BOOST_STATIC_ASSERT( mpl::deref<i>::type::value == 8 );

typedef mpl::advance_c< i, 4>::type j;
BOOST_STATIC_ASSERT( mpl::deref<j>::type::value == 55 );

void main()
{
	int n = mpl::deref<i>::type::value;

}


#include <cstddef>
#include <iostream>
#include <iomanip>

#include <eigen-checks/gtest.h>
#include <gtest/gtest.h>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <cholmod.h>
#include <SuiteSparseQR.hpp>

#include "truncated-svd-solver/tsvd-solver.h"
#include "truncated-svd-solver/linear-algebra-helpers.h"
#include "truncated-svd-solver/timing.h"

void evaluateSVDSPQRSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b,
    const Eigen::VectorXd& x, double tol = 1e-9) {
  cholmod_common cholmod;
  cholmod_l_start(&cholmod);
  cholmod_sparse* A_CS = truncated_svd_solver::eigenDenseToCholmodSparseCopy(A,
    &cholmod);
  cholmod_dense b_CD;
  truncated_svd_solver::eigenDenseToCholmodDenseView(b, &b_CD);
  Eigen::VectorXd x_est;
  truncated_svd_solver::TruncatedSvdSolver linearSolver;
  for (std::ptrdiff_t i = 1; i < A.cols(); ++i) {
    linearSolver.solve(A_CS, &b_CD, i, x_est);
    double error = (b - A * x_est).norm();
    ASSERT_NEAR(error, 0, tol);
    linearSolver.getOptions().columnScaling = true;
    linearSolver.solve(A_CS, &b_CD, i, x_est);
    error = (b - A * x_est).norm();
    linearSolver.getOptions().columnScaling = false;
    ASSERT_NEAR(error, 0, tol);
  }
  cholmod_l_free_sparse(&A_CS, &cholmod);
  cholmod_l_finish(&cholmod);

  EXPECT_TRUE(EIGEN_MATRIX_NEAR(x_est, x, 1e-8));
}

void evaluateSVDSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b,
    const Eigen::VectorXd& x) {
  const Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,
    Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::VectorXd x_est = svd.solve(b);

  EXPECT_TRUE(EIGEN_MATRIX_NEAR(x_est, x, 1e-8));
}

void evaluateSPQRSolver(const Eigen::MatrixXd& A,
                                   const Eigen::VectorXd& b,
                                   const Eigen::VectorXd& x) {
  cholmod_common cholmod;
  cholmod_l_start(&cholmod);
  cholmod_sparse* A_CS =
      truncated_svd_solver::eigenDenseToCholmodSparseCopy(A, &cholmod);
  cholmod_dense b_CD;
  truncated_svd_solver::eigenDenseToCholmodDenseView(b, &b_CD);
  Eigen::VectorXd x_est;
  SuiteSparseQR_factorization<double>* factor = SuiteSparseQR_factorize<double>(
    SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, A_CS, &cholmod);
  cholmod_dense* Qtb = SuiteSparseQR_qmult<double>(SPQR_QTX, factor, &b_CD,
    &cholmod);
  cholmod_dense* x_est_cd = SuiteSparseQR_solve<double>(SPQR_RETX_EQUALS_B,
    factor, Qtb, &cholmod);
  cholmod_l_free_dense(&Qtb, &cholmod);
  truncated_svd_solver::cholmodDenseToEigenDenseCopy(x_est_cd, x_est);
  cholmod_l_free_dense(&x_est_cd, &cholmod);
  SuiteSparseQR_free(&factor, &cholmod);
  cholmod_l_free_sparse(&A_CS, &cholmod);
  cholmod_l_finish(&cholmod);

  EXPECT_TRUE(EIGEN_MATRIX_NEAR(x_est, x, 1e-8));
}

void evaluateSPQRSolverDeterminedSystem(
    const truncated_svd_solver::TruncatedSvdSolverOptions& options) {
  // Create the system.
  constexpr size_t kNumVariables = 10u;
  constexpr double kXResult = 2.0;

  Eigen::VectorXd Adiag(kNumVariables);
  for(size_t i = 0; i < kNumVariables; ++i){
    Adiag(i) =  kNumVariables - i;
  }

  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(kNumVariables, kNumVariables);
  A.diagonal() = Adiag;

  Eigen::VectorXd b = A * Eigen::VectorXd::Constant(kNumVariables, kXResult);

  // Convert to cholmod types.
  cholmod_common_struct cholmod;
  cholmod_l_start(&cholmod);
  cholmod_sparse* A_cm =
      truncated_svd_solver::eigenDenseToCholmodSparseCopy(A, &cholmod);

  cholmod_dense b_cm;
  truncated_svd_solver::eigenDenseToCholmodDenseView(b, &b_cm);

  // Solve this system and check the results.

  truncated_svd_solver::TruncatedSvdSolver solver(options);

  for (std::ptrdiff_t i = 0; i <= A.cols(); ++i) {
    const size_t num_calib_vars = kNumVariables - i;

    Eigen::VectorXd x;
    solver.clear();
    EXPECT_EQ(solver.getSVDRank(), -1);
    EXPECT_EQ(solver.getSVDRankDeficiency(), -1);
    EXPECT_EQ(solver.getQRRank(), 0);
    EXPECT_EQ(solver.getQRRankDeficiency(), 0);
    EXPECT_EQ(solver.getNullSpace().size(), 0);

    solver.analyzeMarginal(A_cm, i);
    EXPECT_EQ(solver.getSVDRank(), num_calib_vars);
    EXPECT_EQ(solver.getQRRank(), i);
    EXPECT_EQ(solver.getQRRankDeficiency(), 0);
    EXPECT_EQ(solver.getSVDRankDeficiency(), 0);
    EXPECT_EQ(solver.getNullSpace().size(), 0);

    solver.clear();
    EXPECT_EQ(solver.getSVDRank(), -1);
    EXPECT_EQ(solver.getSVDRankDeficiency(), -1);
    EXPECT_EQ(solver.getQRRank(), 0);
    EXPECT_EQ(solver.getQRRankDeficiency(), 0);
    EXPECT_EQ(solver.getNullSpace().size(), 0);

    solver.solve(A_cm, &b_cm, i, x);
    EXPECT_EQ(solver.getSVDRank(), num_calib_vars);
    EXPECT_EQ(solver.getQRRank(), i);
    EXPECT_EQ(solver.getQRRankDeficiency(), 0);
    EXPECT_EQ(solver.getSVDRankDeficiency(), 0);
    EXPECT_EQ(solver.getNullSpace().size(), 0);

    Eigen::VectorXd expectedSingularValues;
    if(options.columnScaling){
      expectedSingularValues = Eigen::VectorXd::Ones(num_calib_vars);
    }else{
      expectedSingularValues =
          Adiag.tail(num_calib_vars).array() * Adiag.tail(num_calib_vars).array();
    }

    EXPECT_TRUE(
        EIGEN_MATRIX_NEAR(solver.getSingularValues(),
                          expectedSingularValues, 1e-8));
    EXPECT_TRUE(
        EIGEN_MATRIX_NEAR(x, Eigen::VectorXd::Constant(kNumVariables,
                                                       kXResult),
                          1e-8));
  }

  cholmod_l_free_sparse(&A_cm, &cholmod);
}

TEST(TruncatedSvdSolver, DeterminedSystemWithoutColumnScaling) {
  truncated_svd_solver::TruncatedSvdSolverOptions options;
  options.columnScaling = false;
  evaluateSPQRSolverDeterminedSystem(options);
}

TEST(TruncatedSvdSolver, DeterminedSystemWithColumnScaling) {
  truncated_svd_solver::TruncatedSvdSolverOptions options;
  options.columnScaling = true;
  evaluateSPQRSolverDeterminedSystem(options);
}

TEST(TruncatedSvdSolver, OverdeterminedSystem) {
  Eigen::MatrixXd A = Eigen::MatrixXd::Random(100, 30);
  const Eigen::VectorXd x = Eigen::VectorXd::Random(30);
  Eigen::VectorXd b = A * x;

  std::cout << "-------------------------------------------------" << std::endl;
  std::cout << "|                  Standard case                |" << std::endl;
  std::cout << "-------------------------------------------------" << std::endl;
  evaluateSVDSPQRSolver(A, b, x);
  evaluateSVDSolver(A, b, x);
  evaluateSPQRSolver(A, b, x);

  std::cout << "-------------------------------------------------" << std::endl;
  std::cout << "|                 Badly scaled case             |" << std::endl;
  std::cout << "-------------------------------------------------" << std::endl;
  A.col(2) = 1e6 * A.col(2);
  A.col(28) = 1e6 * A.col(28);
  b = A * x;
  evaluateSVDSPQRSolver(A, b, x, 1e-3);
  evaluateSVDSolver(A, b, x);
  evaluateSPQRSolver(A, b, x);

//  std::cout << "-------------------------------------------------" << std::endl;
//  std::cout << "|                 Rank-deficient case 1         |" << std::endl;
//  std::cout << "-------------------------------------------------" << std::endl;
//  A = Eigen::MatrixXd::Random(100, 30);
//  A.col(10) = Eigen::VectorXd::Zero(A.rows());
//  b = A * x;
//  evaluateSVDSPQRSolver(A, b, x, 1e-3);
//  evaluateSVDSolver(A, b, x);
//  evaluateSPQRSolver(A, b, x);

//  std::cout << "-------------------------------------------------" << std::endl;
//  std::cout << "|                 Rank-deficient case 2         |" << std::endl;
//  std::cout << "-------------------------------------------------" << std::endl;
//  A = Eigen::MatrixXd::Random(100, 30);
//  A.col(10) = 2 * A.col(1) + 5 * A.col(20);
//  b = A * x;
//  evaluateSVDSPQRSolver(A, b, x, 1e-3);
//  evaluateSVDSolver(A, b, x);
//  evaluateSPQRSolver(A, b, x);

//  std::cout << "-------------------------------------------------" << std::endl;
//  std::cout << "|                 Near rank-deficient case 1    |" << std::endl;
//  std::cout << "-------------------------------------------------" << std::endl;
//  A = Eigen::MatrixXd::Random(100, 30);
//  A.col(10) = Eigen::VectorXd::Zero(A.rows());
//  b = A * x;
//  A.col(10) = truncated_svd_solver::NormalDistribution<100>(
//    Eigen::VectorXd::Zero(A.rows()),
//    1e-6 * Eigen::MatrixXd::Identity(A.rows(), A.rows())).getSample();
//  evaluateSVDSPQRSolver(A, b, x, 1e-3);
//  evaluateSVDSolver(A, b, x);
//  evaluateSPQRSolver(A, b, x);

//  std::cout << "-------------------------------------------------" << std::endl;
//  std::cout << "|                 Near rank-deficient case 2    |" << std::endl;
//  std::cout << "-------------------------------------------------" << std::endl;
//  A = Eigen::MatrixXd::Random(100, 30);
//  A.col(10) = 2 * A.col(1) + 5 * A.col(20);
//  b = A * x;
//  A.col(10) = truncated_svd_solver::NormalDistribution<100>(A.col(10),
//    1e-20 * Eigen::MatrixXd::Identity(A.rows(), A.rows())).getSample();
//  evaluateSVDSPQRSolver(A, b, x, 1e-3);
//  evaluateSVDSolver(A, b, x);
//  evaluateSPQRSolver(A, b, x);
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  google::InitGoogleLogging(argv[0]);
  google::InstallFailureSignalHandler();\
  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
  FLAGS_alsologtostderr = true;
  FLAGS_colorlogtostderr = true;
  return RUN_ALL_TESTS();
}


/**
 * @file matrix_geometry.hpp
 * @author Maximilien Naveau (maximilien.naveau@gmail.com)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck
 * Gesellschaft.
 * @date 2019-05-22
 */

#ifndef MATRIX_GEOMETRY_HH
#define MATRIX_GEOMETRY_HH

#include <dynamic-graph/eigen-io.h>
#include <dynamic-graph/linear-algebra.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#define MRAWDATA(x) x.data()

namespace dynamic_graph_manager
{
typedef Eigen::Transform<double, 3, Eigen::Affine> MatrixHomogeneous;
typedef Eigen::Matrix<double, 3, 3> MatrixRotation;
typedef Eigen::AngleAxis<double> VectorUTheta;
typedef Eigen::Quaternion<double> VectorQuaternion;
typedef Eigen::Vector3d VectorRotation;
typedef Eigen::Vector3d VectorRollPitchYaw;
typedef Eigen::Matrix<double, 6, 6> MatrixForce;
typedef Eigen::Matrix<double, 6, 6> MatrixTwist;

inline void buildFrom(const MatrixHomogeneous& MH, MatrixTwist& MT)
{
    Eigen::Vector3d _t = MH.translation();
    MatrixRotation R(MH.linear());
    Eigen::Matrix3d Tx;
    Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
    Eigen::Matrix3d sk;
    sk = Tx * R;

    MT.block<3, 3>(0, 0) = R;
    MT.block<3, 3>(0, 3) = sk;
    MT.block<3, 3>(3, 0).setZero();
    MT.block<3, 3>(3, 3) = R;
}
}  // namespace dynamic_graph_manager

#endif  // MATRIX_GEOMETRY_HH


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE HandEyeCalibration_test
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <random>

#include "camodocal/calib/HandEyeCalibration.h"
#include "camodocal/EigenUtils.h"

BOOST_AUTO_TEST_SUITE(HandEyeCalibration_test)

BOOST_AUTO_TEST_CASE(FullMotion)
{
    camodocal::HandEyeCalibration::setVerbose(false);

    Eigen::Matrix4d H_12_expected = Eigen::Matrix4d::Identity();
    H_12_expected.block<3,3>(0,0) = Eigen::AngleAxisd(0.4, Eigen::Vector3d(0.1, 0.2, 0.3).normalized()).toRotationMatrix();
    H_12_expected.block<3,1>(0,3) << 0.5, 0.6, 0.7;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > rvecs1, tvecs1, rvecs2, tvecs2;
    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_int_distribution<> dis(-10.0, 10.0);
    std::uniform_int_distribution<> dis1(-1.0, 1.0);

    int motionCount = 2;
    for (int i = 0; i < motionCount; ++i)
    {

        double droll = boost::math::constants::radian<double>()*dis(gen);
        double dpitch = boost::math::constants::radian<double>()*dis(gen);
        double dyaw = boost::math::constants::radian<double>()*dis(gen);
        double dx = dis1(gen);
        double dy = dis1(gen);
        double dz = dis1(gen);

        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(dyaw, Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(dpitch, Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(droll, Eigen::Vector3d::UnitX());

        Eigen::Matrix4d H = Eigen::Matrix4d::Identity();
        H.block<3,3>(0,0) = R;
        H.block<3,1>(0,3) << dx, dy, dz;

        Eigen::Matrix4d H_ = H.inverse();
        H = H_;

        Eigen::Vector3d rvec1, tvec1, rvec2, tvec2;

        Eigen::AngleAxisd angleAxis1((H_12_expected * H * H_12_expected.inverse()).block<3,3>(0,0));
        rvec1 = angleAxis1.angle() * angleAxis1.axis();

        tvec1 = (H_12_expected * H * H_12_expected.inverse()).block<3,1>(0,3);

        Eigen::AngleAxisd angleAxis2(H.block<3,3>(0,0));
        rvec2 = angleAxis2.angle() * angleAxis2.axis();

        tvec2 = H.block<3,1>(0,3);

        rvecs1.push_back(rvec1);
        tvecs1.push_back(tvec1);
        rvecs2.push_back(rvec2);
        tvecs2.push_back(tvec2);
    }

    Eigen::Matrix4d H_12;
    camodocal::HandEyeCalibration::estimateHandEyeScrew(rvecs1, tvecs1, rvecs2, tvecs2, H_12);

    for (int i = 0; i < 4; ++i)
    {
        for (int j = 0; j < 4; ++j)
        {
            BOOST_REQUIRE_CLOSE(H_12_expected(i,j),H_12(i,j),0.0000000001);        }
    }
}

BOOST_AUTO_TEST_CASE(PlanarMotion)
{
    camodocal::HandEyeCalibration::setVerbose(false);

    Eigen::Matrix4d H_12_expected = Eigen::Matrix4d::Identity();
    H_12_expected.block<3,3>(0,0) = Eigen::AngleAxisd(0.4, Eigen::Vector3d(0.1, 0.2, 0.3).normalized()).toRotationMatrix();
    H_12_expected.block<3,1>(0,3) << 0.5, 0.6, 0.7;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > rvecs1, tvecs1, rvecs2, tvecs2;
    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_int_distribution<> dis10(-10.0, 10.0);
    std::uniform_int_distribution<> dis1(-1.0, 1.0);

    int motionCount = 2;
    for (int i = 0; i < motionCount; ++i)
    {
        double droll = boost::math::constants::radian<double>()*dis10(gen);
        droll = 0;
        double dpitch = boost::math::constants::radian<double>()*dis10(gen);
        dpitch = 0;
        double dyaw = boost::math::constants::radian<double>()*dis10(gen);
        double dx = dis1(gen);
        double dy = dis1(gen);
        double dz = dis1(gen);
        dz = 0;

        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(dyaw, Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(dpitch, Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(droll, Eigen::Vector3d::UnitX());

        Eigen::Matrix4d H = Eigen::Matrix4d::Identity();
        H.block<3,3>(0,0) = R;
        H.block<3,1>(0,3) << dx, dy, dz;

        Eigen::Matrix4d H_ = H.inverse();
        H = H_;

        Eigen::Vector3d rvec1, tvec1, rvec2, tvec2;

        Eigen::AngleAxisd angleAxis1(H.block<3,3>(0,0));
        rvec1 = angleAxis1.angle() * angleAxis1.axis();

        tvec1 = H.block<3,1>(0,3);

        Eigen::AngleAxisd angleAxis2((H_12_expected.inverse() * H * H_12_expected).block<3,3>(0,0));
        rvec2 = angleAxis2.angle() * angleAxis2.axis();

        tvec2 = (H_12_expected.inverse() * H * H_12_expected).block<3,1>(0,3);

        rvecs1.push_back(rvec1);
        tvecs1.push_back(tvec1);
        rvecs2.push_back(rvec2);
        tvecs2.push_back(tvec2);
    }

    Eigen::Matrix4d H_12;
    camodocal::HandEyeCalibration::estimateHandEyeScrew(rvecs1, tvecs1, rvecs2, tvecs2, H_12, true);

    for (int i = 0; i < 4; ++i)
    {
        for (int j = 0; j < 4; ++j)
        {
            if (i == 2 && j == 3) continue;
            BOOST_REQUIRE_CLOSE(H_12_expected(i,j),H_12(i,j),0.0000000001);
        }
    }
}

BOOST_AUTO_TEST_CASE(PlanarMotionWithNoise)
{
    camodocal::HandEyeCalibration::setVerbose(true);

    Eigen::Matrix4d H_12_expected = Eigen::Matrix4d::Identity();
    H_12_expected.block<3,3>(0,0) = Eigen::AngleAxisd(0.4, Eigen::Vector3d(0.1, 0.2, 0.3).normalized()).toRotationMatrix();
    H_12_expected.block<3,1>(0,3) << 0.5, 0.6, 0.7;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > rvecs1, tvecs1, rvecs2, tvecs2;

    double scale = 1.5;
    int motionCount = 200;
    double sigma = 0.0005;

    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_int_distribution<> dis10(-10.0, 10.0);
    std::uniform_int_distribution<> dis100(-100.0, 100.0);
    std::normal_distribution<> gaussian(0,sigma);
    for (int i = 0; i < motionCount; ++i)
    {
        double droll = boost::math::constants::radian<double>()*dis10(gen);
        droll = 0;
        double dpitch =  boost::math::constants::radian<double>()*dis10(gen);
        dpitch = 0;
        double dyaw =  boost::math::constants::radian<double>()*dis100(gen);
        double dx = dis10(gen);
        double dy = dis10(gen);
        double dz = dis10(gen);
        dz = 0;

        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(dyaw, Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(dpitch, Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(droll, Eigen::Vector3d::UnitX());

        Eigen::Matrix4d H = Eigen::Matrix4d::Identity();
        H.block<3,3>(0,0) = R;
        H.block<3,1>(0,3) << dx, dy, dz;

        Eigen::Matrix4d H_ = H.inverse();
        H = H_;

        Eigen::Vector3d rvec1, tvec1, rvec2, tvec2;

        Eigen::AngleAxisd angleAxis1(H.block<3,3>(0,0));
        rvec1 = angleAxis1.angle() * angleAxis1.axis();

        tvec1 = H.block<3,1>(0,3);

        Eigen::AngleAxisd angleAxis2((H_12_expected.inverse() * H * H_12_expected).block<3,3>(0,0));
        rvec2 = angleAxis2.angle() * angleAxis2.axis();
        double roll, pitch, yaw;
        camodocal::mat2RPY(angleAxis2.toRotationMatrix(), roll, pitch, yaw);

        roll += gaussian(gen);
        pitch += gaussian(gen);
        yaw += gaussian(gen);

        angleAxis2.fromRotationMatrix(camodocal::RPY2mat(roll, pitch, yaw));
        rvec2 = angleAxis2.angle() * angleAxis2.axis();

        tvec2 = (H_12_expected.inverse() * H * H_12_expected).block<3,1>(0,3);

        rvecs1.push_back(rvec1);
        tvecs1.push_back(tvec1);
        rvecs2.push_back(rvec2);
        tvecs2.push_back(tvec2);
    }

    Eigen::Matrix4d H_12;
    camodocal::HandEyeCalibration::estimateHandEyeScrew(rvecs1, tvecs1, rvecs2, tvecs2, H_12, true);
    Eigen::Matrix4d H1 = H_12_expected;
    Eigen::Matrix4d H2 = H_12;
    std::cout << "# INFO: H_12_expected = " << std::endl;
    std::cout << H_12_expected << std::endl; ;
//    std::cout << H1 << std::endl << H2 << std::endl;

//    for (int i = 0; i < 4; ++i)
//    {
//        for (int j = 0; j < 4; ++j)
//        {
//            if (i == 2 && j == 3) continue;
//            EXPECT_NEAR(H1(i,j),H2(i,j),0.0000000001) << "Elements differ at (" << i << "," << j << ")";
//        }
//    }
}

/*
TEST(HandEyeCalibration, EstimateWithUnitTranslation)
{
    Eigen::Matrix4d H_12_expected = Eigen::Matrix4d::Identity();
//    H_12_expected.block<3,3>(0,0) = Eigen::AngleAxisd(0.4, Eigen::Vector3d(0.0, 0.0, 0.3).normalized()).toRotationMatrix();
//    H_12_expected.block<3,3>(0,0) = Eigen::AngleAxisd(0.4, Eigen::Vector3d(1, 1, 0.3).normalized()).toRotationMatrix();
    H_12_expected.block<3,1>(0,3) << 0.5, 0.6, 0.7;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > rvecs1, tvecs1, rvecs2, tvecs2;

    int motionCount = 2;
    for (int i = 0; i < motionCount; ++i)
    {
        double droll = boost::math::constants::radian<double>()*dis10(gen));
        double dpitch =  boost::math::constants::radian<double>()*dis10(gen));
        double dyaw =  boost::math::constants::radian<double>()*dis10(gen));
        double dx = dis1(gen);
        double dy = dis1(gen);
        double dz = dis1(gen);

        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(dyaw, Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(dpitch, Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(droll, Eigen::Vector3d::UnitX());

        Eigen::Matrix4d H = Eigen::Matrix4d::Identity();
        H.block<3,3>(0,0) = R;
        H.block<3,1>(0,3) << dx, dy, dz;

        Eigen::Matrix4d H_ = H.inverse();
        H = H_;

        Eigen::Vector3d rvec1, tvec1, rvec2, tvec2;

        Eigen::AngleAxisd angleAxis1((H_12_expected * H * H_12_expected.inverse()).block<3,3>(0,0));
        rvec1 = angleAxis1.angle() * angleAxis1.axis();

        tvec1 = (H_12_expected * H * H_12_expected.inverse()).block<3,1>(0,3);
        tvec1.normalize();

        Eigen::AngleAxisd angleAxis2(H.block<3,3>(0,0));
        rvec2 = angleAxis2.angle() * angleAxis2.axis();

        tvec2 = H.block<3,1>(0,3);

        rvecs1.push_back(rvec1);
        tvecs1.push_back(tvec1);
        rvecs2.push_back(rvec2);
        tvecs2.push_back(tvec2);
    }

    Eigen::Matrix4d H_12;
    camodocal::HandEyeCalibration::estimateHandEyeScrew(rvecs1, tvecs1, rvecs2, tvecs2, H_12, true);

    for (int i = 0; i < 4; ++i)
    {
        for (int j = 0; j < 4; ++j)
        {
            EXPECT_NEAR(H_12_expected(i,j),H_12(i,j),0.0000000001) << "Elements differ at (" << i << "," << j << ")";
        }
    }
}
*/
}


#include "Camera.h"
#include "Utilities.h"

#include <Eigen/Geometry>

namespace sol {

Camera::Camera(const Eigen::Vector3f &o, const Eigen::Vector3f &lookat, const Eigen::Vector3f &up, float fov, float aspect, float aperture, float focusDist)
: origin(o) {
    lensRadius = aperture / 2.0;
    const auto theta = fov * M_PI/180;
    const auto halfHeight = tan(theta/2);
    const auto halfWidth = aspect * halfHeight;
    w = (origin - lookat).normalized();
    u = up.cross(w).normalized();
    v = w.cross(u).normalized();
    lowerLeftCorner = origin - halfWidth * focusDist * u - halfHeight * focusDist * v - focusDist * w;
    horizontal = 2 * halfWidth * focusDist * u;
    vertical = 2 * halfHeight * focusDist * v;
}

Ray Camera::getRay(float s, float t) const {
    const auto rd = lensRadius * randomInUnitSphere();
    const auto offset = u * rd.x() + v * rd.y();
    const auto o = origin + offset;
    return Ray(o, lowerLeftCorner + s * horizontal + t * vertical - o);
}

}


///////////////////////////////////////////////////////////////////////////////
// ars::detail::tangent_intersection.hpp                                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_DETAIL_TANGENT_INTERSECTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_DETAIL_TANGENT_INTERSECTION_HPP_ER_2009
#include <boost/ars/constant.hpp>
#include <boost/ars/point.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

        //    x1  x2  x3            point abscissae
        //   / \ / \ / \
        //  z0  z1  z2  z3 = zn     tangent abscissae
        //
        // z0 = x_min
        // zn = x_max
        // Let tang[i](x) = y[i] + dy[i](x-x[i])
        // z[i] solves : tang[i+1](z[i]) == tang[i](z[i])
        // t[i] = tang[i](z[i])

template<typename T>
struct tangent_intersection{
    typedef constant<T> const_;

    tangent_intersection()
    :z_(const_::zero_),
    t_(const_::zero_),
    cum_sum_(const_::zero_){}

    tangent_intersection(const point<T>& a,const point<T>& b){
        T eps = exp(const_::lmin_); //TODO numeric_smallest()?
        if(fabs(a.y()-b.y()) <= eps){
            z_ = (a.x() + b.x()) / const_::two_;
            t_ = (a.y() + b.y()) / const_::two_;
        }else{
            T ddy = b.dy() - a.dy();
            // For greater precision :
            if(fabs(a.dy())<fabs(b.dy())){
                z_ = b.x() + (a.y() - b.y() + a.dy() * (b.x() - a.x()))/ddy;
                t_ = a.y() + a.dy() * (z_ - a.x());
            }else{
                z_ = a.x() + (a.y() - b.y() + b.dy() * (b.x() - a.x()))/ddy;
                t_ = b.y() +  b.dy() * (z_ - b.x());
            }
        }
    }

    tangent_intersection(T z,T t)
    :z_(z), t_(t),cum_sum_(const_::zero_){}

    const T& z()const{ return z_; }
    const T& t()const{ return t_; }

    private:
    T z_; //abscissa
    T t_; //ordinate
    public:
    T cum_sum_; //area under exp tangent up to z_
};

template <typename T>
std::ostream&
operator<<(std::ostream& out, const tangent_intersection<T> &ti)
{
    out << '(' << ti.z() << ',' << ti.t() << ',' << ti.cum_sum_ << ')';
    return out;
}

template <typename T>
bool operator<(
    const tangent_intersection<T> &a,
    const tangent_intersection<T> &b
){
    return (a.cum_sum_ < b.cum_sum_);
}

}// ars
}// detail
}// statistics
}// boost

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// Triangulation Example - computes a delaunay triangulation and draws the
// triangulation and the corresponding finite voronoi edges

#include <fstream>
#include <random>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/extensions/triangulation/triangulation.hpp>
#include <boost/geometry/extensions/triangulation/geometries/voronoi_adaptor.hpp>

const int samples = 100;

int main()
{
    namespace bg = boost::geometry;
    typedef bg::model::point<double, 2, bg::cs::cartesian> point;
    typedef bg::model::triangulation<point> triangulation;

    std::default_random_engine gen(1);
    std::uniform_real_distribution<> dist(0.0, 1.0);
    std::vector<point> in;
    for (int i = 0 ; i < samples ; ++i)
    {
        in.push_back(point(dist(gen), dist(gen)));
    }
    triangulation out(samples);
    bg::delaunay_triangulation(in, out);

    std::ofstream svg("triangulation.svg");
    bg::svg_mapper<point> mapper(svg, 720, 720);
    mapper.add(point(0, 0));
    mapper.add(point(1, 1));

    for (auto const& f : bg::face_range(out))
    {
        mapper.map(f, "fill-opacity:0.3;fill:rgb(102,102,201);stroke:rgb(51,51,152);");
    }

    typedef bg::model::voronoi_face_view<triangulation> voronoi_face_view;
    for (auto it = out.vertices_begin() ; it != out.vertices_end() ; ++it)
    {
        voronoi_face_view vfv(out, it);
        mapper.map(voronoi_face_view(out, it), "opacity:1.0;fill:none;stroke:rgb(255,0,0);stroke-width:1");
    }

    return 0;
}


#include "Expression.h"

#include <iostream>

#include <Eigen/Dense>

#include <gmpxx.h>

typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> MatrixXq;

static std::ostream& operator<<(std::ostream & stream, mpq_class const & rop) {
  stream << rop.get_str();
  return stream;
}

Expression Expression::ApplyGaugeSymmetry(Expression const & delta) const {
  Expression gauge_term (*this);
  gauge_term.MultiplyOther(delta);

  gauge_term.SortMonomials();

  gauge_term.EliminateEpsilonEpsilonI();
  gauge_term.EliminateEpsilonI();

  gauge_term.SortMonomials();
  gauge_term.EliminateEtaEta();

  gauge_term.SortMonomials();
  gauge_term.EliminateDelta();

  gauge_term.SortMonomials();
  gauge_term.EliminateEpsilon();

  gauge_term.ApplyMonomialSymmetries();

  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();

  gauge_term.ApplyMonomialSymmetriesToContractions();

  gauge_term.ApplyMonomialSymmetries();
  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();

  gauge_term.EliminateEtaPartial();
  gauge_term.EliminateEpsilon();

  gauge_term.ApplyMonomialSymmetries();
  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();

  gauge_term.EliminateEtaRankOne();
  gauge_term.RenameDummies();

  gauge_term.ApplyMonomialSymmetries();
  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();
  gauge_term.SortSummandsByPrefactors();

  std::cout << "Gauge transform:" << std::endl;
  std::cout << delta.GetLatexString() << std::endl;

  std::cout << "The gauge term reads:" << std::endl;
  std::cout << gauge_term.GetLatexString() << std::endl;

  std::cout << "It has to vanish identically for each possible xi." << std::endl;

  std::set<ScalarSum> sum_set;
  std::set<size_t> coefficient_set;

  std::for_each(gauge_term.summands->begin(), gauge_term.summands->end(),
    [&coefficient_set,&sum_set] (auto const & a) {
      coefficient_set.merge(a.second->CoefficientSet());
      sum_set.insert(*a.second);
    });

  std::map<size_t, size_t> coefficient_map = GetCoefficientMap();

  std::cout << "number of different (not necessarily linear independent) equations : " << sum_set.size() << std::endl;
  std::cout << "number of coefficients (e_.) in these equations                    : " << coefficient_map.size() << std::endl;

  std::vector<std::vector<Rational>> matrix;
  std::for_each(sum_set.begin(), sum_set.end(), [&coefficient_map, &matrix](auto & a) {
    matrix.push_back(a.CoefficientVector(coefficient_map));
    });

  MatrixXq mq(matrix.size(), coefficient_map.size());

  for (size_t row_counter = 0; row_counter < matrix.size(); ++row_counter) {
    for (size_t column_counter = 0; column_counter < coefficient_map.size(); ++column_counter) {
      Fraction frac = matrix[row_counter][column_counter].get_fraction();
      mq(row_counter, column_counter) = mpq_class(frac.first, frac.second);
    }
  }

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "the rank of the matrix is : " << lu_decompq.rank() << std::endl;
  std::cout << "null space basis: " << std::endl;

  MatrixXq kq = lu_decompq.kernel();

  std::cout << kq << std::endl;

  std::map<size_t, size_t> coefficient_rmap;
  std::for_each(coefficient_map.begin(), coefficient_map.end(), [&coefficient_rmap](auto a) { coefficient_rmap.insert(std::make_pair(a.second, a.first)); });

  std::map<size_t, ScalarSum> replace_map;

  std::vector<std::vector<mpq_class>> kq_vec (kq.rows());
  std::generate(kq_vec.begin(), kq_vec.end(),
    [n=0,&kq] () mutable {
      std::vector<mpq_class> vec_ret (kq.cols());
      std::generate(vec_ret.begin(), vec_ret.end(),
      [m=0,&kq,_n=n++] () mutable {
        return kq(_n,m++);
      });
      return vec_ret;
    });

  std::for_each(kq_vec.cbegin(), kq_vec.cend(),
    [&replace_map,coefficient_rmap,n=0] (auto const & a) mutable {
      ScalarSum scalar_sum;
      std::for_each(a.cbegin(), a.cend(),
        [&scalar_sum, m=0] (auto const & b) mutable {
          scalar_sum.AddScalar(Scalar(Rational(b), ++m));
        });
      replace_map[coefficient_rmap[n++]] = scalar_sum;
    });

  std::cout << "Thus the " << coefficient_map.size() << " original constants can be parameterized as:" << std::endl;
  std::for_each(replace_map.begin(), replace_map.end(),
    [](auto const & a) {
      std::cout << "e_" << a.first << " --> " << a.second.ToString() << std::endl;
    });

  std::cout << "With " << kq.cols() << " remaining constant" << (kq.cols() == 1 ? "" : "(s)") << "." << std::endl;

  Expression expression_copy = *this;

  expression_copy.SubstituteVariables(replace_map);

  expression_copy.CanonicalisePrefactors();
  expression_copy.EliminateZeros();

  return Expression(expression_copy);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REMAINDER_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REMAINDER_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing remainder capabilities

    Computes the remainder of division.
    The return value is x-n*y, where n is the value x/y,
    rounded to the nearest integer (using round2even).

    @par semantic:
    For any given value @c x, @c y of type @c T:

    @code
    T r = remainder(x, y);
    @endcode

    For floating point values the code is similar to:

    @code
    T r = x-divround2even(x, y)*y;
    @endcode

    @par Note:

    As r can be negative, @c remainder is not defined for unsigned types.

    For floating entries:
       -  if x is +/-inf , Nan is returned
       -  If y is +/-0   , Nan is returned
       -  If either argument is NaN, Nan is returned
       -  If the returned value is 0, it will have the same sign as x.

    If correct values for these limit cases do not matter for you, using the fast_ decorator
    can gain some cycles.

    @see mod, rem, modulo

    @par Alias

    @c drem

    @par Decorators

    fast_ for floating entries

  **/
  const boost::dispatch::functor<tag::remainder_> remainder = {};
} }
#endif

#include <boost/simd/function/scalar/remainder.hpp>
#include <boost/simd/function/simd/remainder.hpp>

#endif


//
// Created by michel on 27-04-21.
//

#include "test-helper.h"
#include <boost/math/special_functions/relative_difference.hpp>
#include <cmath>
#include "org-simple/util/dsp/iir-butterworth.h"
#include <vector>

#include "util/text/iir-coefficients-test-helper.h"

using namespace org::simple::util::dsp;

struct FilterGainScenario : public FilterScenario {
  size_t signal_period;
  size_t filter_period;

  const char *typeOfScenario() const override { return "FilterGainScenario"; }
  void parameters(std::ostream &out) const override {
    out << "; signal_period=" << signal_period
        << "; filter_period=" << filter_period;
  }

  FilterGainScenario(size_t sp, size_t fp, FilterType t, unsigned o)
      : FilterScenario(t, o), signal_period(sp), filter_period(fp) {}
};

const std::vector<FilterGainScenario> getFilterGainScenarios() {
  std::vector<FilterGainScenario> result;
  for (size_t signal_period : get_test_periods()) {
    for (size_t filter_period : get_test_periods()) {
      result.emplace_back(FilterGainScenario(signal_period, filter_period,
                                             FilterType::LOW_PASS, 1));
      result.emplace_back(FilterGainScenario(signal_period, filter_period,
                                             FilterType::HIGH_PASS, 1));
      result.emplace_back(FilterGainScenario(signal_period, filter_period,
                                             FilterType::LOW_PASS, 2));
      result.emplace_back(FilterGainScenario(signal_period, filter_period,
                                             FilterType::HIGH_PASS, 2));
      result.emplace_back(FilterGainScenario(signal_period, filter_period,
                                             FilterType::LOW_PASS, 4));
      result.emplace_back(FilterGainScenario(signal_period, filter_period,
                                             FilterType::HIGH_PASS, 4));
    }
  }
  return result;
}

template <size_t ORDER>
bool verifyButterworthGain(size_t signal_period, size_t filter_period, FilterType type,
                double &measured, double &calculated) {
  double filterRelativeFrequency =
      filter_period > 2 ? 1.0 / filter_period : 0.45;
  double signalRelativeFrequency = 1.0 / signal_period;
  size_t min_period = std::min(signal_period, filter_period);
  double error = 1.0 / (1.0 - 1.0 / min_period);
  FixedOrderCoefficients<double, ORDER> filter;
  Butterworth::create(filter, filterRelativeFrequency, type, 1.0);
  size_t filter_length;
  measured = measureFilterGain(filter, signal_period, filter_length);
  double gainSquared = measured * measured;
  calculated = Butterworth::getGain(type, ORDER,
                           signalRelativeFrequency / filterRelativeFrequency);
  double minGain = std::min(measured, calculated) * error;
  if (gainSquared < minGain && calculated < minGain) {
    return true;
  }
  return false;
}

static double reference_high_pass_gain(size_t order, double rel) {
  double alpha = pow(fabs(rel), order);
  return alpha / sqrt(1.0 + alpha * alpha);
}

static double reference_low_pass_gain(size_t order, double rel) {
  double alpha2 = pow(fabs(rel), order * 2);
  return 1.0 / sqrt(1.0 + alpha2);
}

struct GainScenario : public FilterScenario {
  double relative;
  double expected_gain;
  bool ref;

  double actual() const {
    if (!ref) {
      switch (type) {
      case FilterType::LOW_PASS:
        return Butterworth::getLowPassGain(order, relative, false);
      case FilterType::HIGH_PASS:
        return Butterworth::getHighPassGain(order, relative, false);
      default:
        return std::numeric_limits<double>::quiet_NaN();
      }
    } else {
      switch (type) {
      case FilterType::LOW_PASS:
        return reference_low_pass_gain(order, relative);
      case FilterType::HIGH_PASS:
        return reference_high_pass_gain(order, relative);
      default:
        return std::numeric_limits<double>::quiet_NaN();
      }
    }
  }

  GainScenario(FilterType t, unsigned o, double rel, double expGain, bool isRef = false)
      : FilterScenario(t, o), relative(rel), expected_gain(expGain),
        ref(isRef) {}

  const char *typeOfScenario() const override { return "GainCalculationScenario"; }
  void parameters(std::ostream &out) const override {
    out << "; relative-frequency=" << relative
        << "; expected gain=" << expected_gain
        << "; reference=" << (ref ? "true" : "false");
  }
};

// static std::ostream &operator<<(std::ostream &out,
//                                 const GainScenario &scenario) {
//   out << "GainScenario(type=";
//   switch (scenario.type) {
//   case FilterType::LOW_PASS:
//     out << "low-pass";
//     break;
//   case FilterType::HIGH_PASS:
//     out << "high-pass";
//     break;
//   default:
//     out << "invalid";
//   }
//   out << "; order=" << scenario.order;
//   out << "; w/w0=" << scenario.relative;
//   out << "; gain=" << scenario.expected_gain;
//   out << "; reference=" << scenario.ref;
//
//   return out;
// }

std::vector<GainScenario> createTestScenarios() {
  std::vector<GainScenario> scenarios;

  scenarios.emplace_back(GainScenario(FilterType::LOW_PASS, 1, 0, 1, true));
  scenarios.emplace_back(GainScenario(FilterType::HIGH_PASS, 1, 0, 0, true));
  scenarios.emplace_back(GainScenario(FilterType::LOW_PASS, 1,
                                      std::numeric_limits<double>::epsilon(), 1,
                                      true));
  scenarios.emplace_back(
      GainScenario(FilterType::HIGH_PASS, 1,
                   1.0 / std::numeric_limits<double>::epsilon(), 1, true));

  for (unsigned order = 1; Butterworth::isValidOrder(order); order++) {
    scenarios.emplace_back(
        GainScenario(FilterType::LOW_PASS, order, 1.0, M_SQRT1_2, true));
    scenarios.emplace_back(
        GainScenario(FilterType::HIGH_PASS, order, 1.0, M_SQRT1_2, true));
  }

  for (unsigned order = 1; Butterworth::isValidOrder(order); order++) {
    for (double relative = 0.125; relative <= 8; relative *= 2) {
      scenarios.emplace_back(
          GainScenario(FilterType::LOW_PASS, order, relative,
                       reference_low_pass_gain(order, relative)));
      scenarios.emplace_back(
          GainScenario(FilterType::HIGH_PASS, order, relative,
                       reference_high_pass_gain(order, relative)));
    }
  }

  return scenarios;
}

static bool same(double x, double y) {
  return boost::math::relative_difference(x, y) <= 1e-11;
}

BOOST_AUTO_TEST_SUITE(org_simple_dsp_iir_butterworth_tests)

BOOST_DATA_TEST_CASE(sample, createTestScenarios()) {
  if (!same(sample.expected_gain, sample.actual())) {
    BOOST_CHECK_EQUAL(sample.expected_gain, sample.actual());
    double g1 = Butterworth::getLowPassGain(sample.order, sample.relative, false);
    double g2 = reference_low_pass_gain(sample.order, sample.relative);
    BOOST_CHECK_EQUAL(g1, g2);
  } else {
    BOOST_CHECK(same(sample.expected_gain, sample.actual()));
  }
}

BOOST_AUTO_TEST_CASE(testSupportedFilterTypes) {
  BOOST_CHECK(!Butterworth::isValidFilterType(FilterType::ALL_PASS));
  BOOST_CHECK(Butterworth::isValidFilterType(FilterType::LOW_PASS));
  BOOST_CHECK(!Butterworth::isValidFilterType(FilterType::LOW_SHELVE));
  BOOST_CHECK(!Butterworth::isValidFilterType(FilterType::BAND_PASS));
  BOOST_CHECK(!Butterworth::isValidFilterType(FilterType::PARAMETRIC));
  BOOST_CHECK(!Butterworth::isValidFilterType(FilterType::HIGH_SHELVE));
  BOOST_CHECK(Butterworth::isValidFilterType(FilterType::HIGH_PASS));
}

BOOST_DATA_TEST_CASE(testFirstOrderHighPass, getFilterGainScenarios()) {
  double actualGain = 0.0;
  double calculatedGain = 0.0;
  if (verifyButterworthGain<1>(sample.signal_period, sample.filter_period,
                               sample.type, actualGain, calculatedGain)) {
    BOOST_CHECK(true);
  } else {
    BOOST_CHECK_EQUAL(actualGain, calculatedGain);
  }
}

#ifdef ORG_SIMPLE_IIR_BUTTERWORTH_PRINT_FILTER_LENGTHS

template <unsigned ORDER>
void printFilterLength(double hz, unsigned short bits, FilterType type) {
  static constexpr double rate = 96000;
  static constexpr size_t MAX_LENGTH = 1048576;
  FixedOrderCoefficients<double, ORDER> coefficients;
  double f = hz / rate;
  double err = pow(0.5, bits);
  Butterworth::create(coefficients, f, FilterType::LOW_PASS, 1.0);
  size_t length = effectiveIRLength(coefficients, MAX_LENGTH, err);
  std::cout << "Impulse response length of low pass butterworth (order" << ORDER
            << ") @ " << hz << " Hz. (" << f << ") for " << bits
            << " bits accuracy is " << length << " samples \t"
            << (1.0 * length / rate) << " seconds \t" << (f * length)
            << " periods." << std::endl;
}

BOOST_AUTO_TEST_CASE(estimateFilterLength) {
  FilterType types[2] = { FilterType::LOW_PASS, FilterType::HIGH_PASS };

  for (FilterType type : types) {
    printFilterLength<1>(0, 24, type);
    printFilterLength<2>(0, 24, type);
    printFilterLength<4>(0, 24, type);
    for (double hz = 40; hz < 12000; hz *= 2) {
      printFilterLength<1>(hz, 24, type);
      printFilterLength<2>(hz, 24, type);
      printFilterLength<4>(hz, 24, type);
    }
    printFilterLength<2>(80, 15, type);
    printFilterLength<2>(80, 23, type);
    printFilterLength<2>(80, 31, type);
  }
}

#endif

BOOST_AUTO_TEST_SUITE_END()


/*
 Copyright 2011 Mario Mulansky
 Copyright 2012 Karsten Ahnert

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

/*
 * Example of a 2D simulation of nonlinearly coupled oscillators.
 * Program just prints final energy which should be close to the initial energy (1.0).
 * No parallelization is employed here.
 * Run time on a 2.3GHz Intel Core-i5: about 10 seconds for 100 steps.
 * Compile simply via bjam or directly:
 * g++ -O3 -I${BOOST_ROOT} -I../../../../.. spreading.cpp
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <cstdlib>
#include <sys/time.h>

#include <boost/ref.hpp>
#include <boost/numeric/odeint/stepper/symplectic_rkn_sb3a_mclachlan.hpp>

// we use a vector< vector< double > > as state type,
// for that some functionality has to be added for odeint to work
#include "nested_range_algebra.hpp"
#include "vector_vector_resize.hpp"

// defines the rhs of our dynamical equation
#include "lattice2d.hpp"
/* dynamical equations (Hamiltonian structure):
dqdt_{i,j} = p_{i,j}
dpdt_{i,j} = - omega_{i,j}*q_{i,j} - \beta*[ (q_{i,j} - q_{i,j-1})^3
                                            +(q_{i,j} - q_{i,j+1})^3
                                            +(q_{i,j} - q_{i-1,j})^3
                                            +(q_{i,j} - q_{i+1,j})^3 ]
*/

using namespace std;

static const int MAX_N = 1024;//2048;

static const size_t KAPPA = 2;
static const size_t LAMBDA = 4;
static const double W = 1.0;
static const double gap = 0.0;
static const size_t steps = 100;
static const double dt = 0.1;

double initial_e = 1.0;
double beta = 1.0;
int realization_index = 0;

//the state type
typedef vector< vector< double > > state_type;

//the stepper, choose a symplectic one to account for hamiltonian structure
//use nested_range_algebra for calculations on vector< vector< ... > >
typedef boost::numeric::odeint::symplectic_rkn_sb3a_mclachlan<
    state_type , state_type , double , state_type , state_type , double ,
    nested_range_algebra< boost::numeric::odeint::range_algebra > ,
    boost::numeric::odeint::default_operations > stepper_type;

double time_diff_in_ms( timeval &t1 , timeval &t2 )
{ return (t2.tv_sec - t1.tv_sec)*1000.0 + (t2.tv_usec - t1.tv_usec)/1000.0 + 0.5; }

int main( int argc, const char* argv[] ) {

    srand( time(NULL) );

    lattice2d< KAPPA , LAMBDA > lattice( beta );

    lattice.generate_pot( W , gap , MAX_N );

    state_type q( MAX_N , vector< double >( MAX_N , 0.0 ) );

    state_type p( q );

    state_type energy( q );

    p[MAX_N/2][MAX_N/2] = sqrt( 0.5*initial_e );
    p[MAX_N/2+1][MAX_N/2] = sqrt( 0.5*initial_e );
    p[MAX_N/2][MAX_N/2+1] = sqrt( 0.5*initial_e );
    p[MAX_N/2+1][MAX_N/2+1] = sqrt( 0.5*initial_e );

    cout.precision(10);

    lattice.local_energy( q , p , energy );
    double e=0.0;
    for( size_t i=0 ; i<energy.size() ; ++i )
        for( size_t j=0 ; j<energy[i].size() ; ++j )
        {
            e += energy[i][j];
        }

    cout << "initial energy: " << lattice.energy( q , p ) << endl;

    timeval elapsed_time_start , elapsed_time_end;
    gettimeofday(&elapsed_time_start , NULL);

    stepper_type stepper;

    for( size_t step=0 ; step<=steps ; ++step )
    {
        stepper.do_step( boost::ref( lattice ) ,
                         make_pair( boost::ref( q ) , boost::ref( p ) ) ,
                         0.0 , 0.1 );
    }

    gettimeofday(&elapsed_time_end , NULL);
    double elapsed_time = time_diff_in_ms( elapsed_time_start , elapsed_time_end );
    cout << steps << " steps in " << elapsed_time/1000 << " s (energy: " << lattice.energy( q , p ) << ")" << endl;
}


#define BOOST_TEST_MODULE test_regularize
#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mave/mave.hpp>
#include <tests/generate_random_matrices.hpp>
#include <tests/tolerance.hpp>

typedef boost::mpl::list<
    mave::vector<double, 3>, mave::vector<float, 3>
    > test_targets;

constexpr std::size_t N = 12000;

template<typename T, std::size_t N>
T length_ref(const mave::vector<T, N>& v)
{
    T retval(0);
    for(std::size_t i=0; i<v.size(); ++i)
    {
        retval += v[i] * v[i];
    }
    return std::sqrt(retval);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(regularize_1arg, T, test_targets)
{
    using real = typename T::value_type;
    std::mt19937 mt(123456789);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto& v = vectors.at(i);

        const auto ref = length_ref(v);
        const auto val = mave::regularize(v);

        BOOST_TEST(ref == std::get<1>(val), mave::test::tolerance<real>());
        BOOST_TEST(length_ref(std::get<0>(val)) == real(1.0),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val)[0] == v[0] / std::get<1>(val),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val)[1] == v[1] / std::get<1>(val),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val)[2] == v[2] / std::get<1>(val),
                   mave::test::tolerance<real>());

        BOOST_TEST(v.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(regularize_2arg, T, test_targets)
{
    using real = typename T::value_type;
    std::mt19937 mt(123456789);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=2)
    {
        const auto& v1 = vectors.at(i);
        const auto& v2 = vectors.at(i+1);

        const auto ref1 = length_ref(v1);
        const auto ref2 = length_ref(v2);

        const auto val = mave::regularize(v1, v2);
        const auto& val1 = std::get<0>(val);
        const auto& val2 = std::get<1>(val);

        BOOST_TEST(ref1 == std::get<1>(val1), mave::test::tolerance<real>());
        BOOST_TEST(ref2 == std::get<1>(val2), mave::test::tolerance<real>());

        BOOST_TEST(length_ref(std::get<0>(val1)) == real(1.0),
                   mave::test::tolerance<real>());
        BOOST_TEST(length_ref(std::get<0>(val2)) == real(1.0),
                   mave::test::tolerance<real>());

        BOOST_TEST(std::get<0>(val1)[0] == v1[0] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val1)[1] == v1[1] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val1)[2] == v1[2] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[0] == v2[0] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[1] == v2[1] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[2] == v2[2] / std::get<1>(val2),
                   mave::test::tolerance<real>());

        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(regularize_3arg, T, test_targets)
{
    using real = typename T::value_type;
    std::mt19937 mt(123456789);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=3)
    {
        const auto& v1 = vectors.at(i);
        const auto& v2 = vectors.at(i+1);
        const auto& v3 = vectors.at(i+2);

        const auto ref1 = length_ref(v1);
        const auto ref2 = length_ref(v2);
        const auto ref3 = length_ref(v3);

        const auto val = mave::regularize(v1, v2, v3);
        const auto& val1 = std::get<0>(val);
        const auto& val2 = std::get<1>(val);
        const auto& val3 = std::get<2>(val);

        BOOST_TEST(ref1 == std::get<1>(val1), mave::test::tolerance<real>());
        BOOST_TEST(ref2 == std::get<1>(val2), mave::test::tolerance<real>());
        BOOST_TEST(ref3 == std::get<1>(val3), mave::test::tolerance<real>());

        BOOST_TEST(length_ref(std::get<0>(val1)) == real(1.0),
                   mave::test::tolerance<real>());
        BOOST_TEST(length_ref(std::get<0>(val2)) == real(1.0),
                   mave::test::tolerance<real>());
        BOOST_TEST(length_ref(std::get<0>(val3)) == real(1.0),
                   mave::test::tolerance<real>());

        BOOST_TEST(std::get<0>(val1)[0] == v1[0] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val1)[1] == v1[1] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val1)[2] == v1[2] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[0] == v2[0] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[1] == v2[1] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[2] == v2[2] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val3)[0] == v3[0] / std::get<1>(val3),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val3)[1] == v3[1] / std::get<1>(val3),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val3)[2] == v3[2] / std::get<1>(val3),
                   mave::test::tolerance<real>());

        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
        BOOST_TEST(v3.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(regularize_4arg, T, test_targets)
{
    using real = typename T::value_type;
    std::mt19937 mt(123456789);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=4)
    {
        const auto& v1 = vectors.at(i);
        const auto& v2 = vectors.at(i+1);
        const auto& v3 = vectors.at(i+2);
        const auto& v4 = vectors.at(i+3);

        const auto ref1 = length_ref(v1);
        const auto ref2 = length_ref(v2);
        const auto ref3 = length_ref(v3);
        const auto ref4 = length_ref(v4);

        const auto val = mave::regularize(v1, v2, v3, v4);
        const auto& val1 = std::get<0>(val);
        const auto& val2 = std::get<1>(val);
        const auto& val3 = std::get<2>(val);
        const auto& val4 = std::get<3>(val);

        BOOST_TEST(ref1 == std::get<1>(val1), mave::test::tolerance<real>());
        BOOST_TEST(ref2 == std::get<1>(val2), mave::test::tolerance<real>());
        BOOST_TEST(ref3 == std::get<1>(val3), mave::test::tolerance<real>());
        BOOST_TEST(ref4 == std::get<1>(val4), mave::test::tolerance<real>());

        BOOST_TEST(length_ref(std::get<0>(val1)) == real(1.0),
                   mave::test::tolerance<real>());
        BOOST_TEST(length_ref(std::get<0>(val2)) == real(1.0),
                   mave::test::tolerance<real>());
        BOOST_TEST(length_ref(std::get<0>(val3)) == real(1.0),
                   mave::test::tolerance<real>());
        BOOST_TEST(length_ref(std::get<0>(val4)) == real(1.0),
                   mave::test::tolerance<real>());

        BOOST_TEST(std::get<0>(val1)[0] == v1[0] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val1)[1] == v1[1] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val1)[2] == v1[2] / std::get<1>(val1),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[0] == v2[0] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[1] == v2[1] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val2)[2] == v2[2] / std::get<1>(val2),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val3)[0] == v3[0] / std::get<1>(val3),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val3)[1] == v3[1] / std::get<1>(val3),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val3)[2] == v3[2] / std::get<1>(val3),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val4)[0] == v4[0] / std::get<1>(val4),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val4)[1] == v4[1] / std::get<1>(val4),
                   mave::test::tolerance<real>());
        BOOST_TEST(std::get<0>(val4)[2] == v4[2] / std::get<1>(val4),
                   mave::test::tolerance<real>());

        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
        BOOST_TEST(v3.diagnosis());
        BOOST_TEST(v4.diagnosis());
    }
}


#include "lm/interpolate/tune_weights.hh"

#include "lm/interpolate/tune_derivatives.hh"
#include "lm/interpolate/tune_instances.hh"

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpragmas" // Older gcc doesn't have "-Wunused-local-typedefs" and complains.
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <Eigen/Dense>
#pragma GCC diagnostic pop
#include <boost/program_options.hpp>

#include <iostream>

namespace lm { namespace interpolate {
void TuneWeights(int tune_file, const std::vector<StringPiece> &model_names, const InstancesConfig &config, std::vector<float> &weights_out) {
  Instances instances(tune_file, model_names, config);
  Vector weights = Vector::Constant(model_names.size(), 1.0 / model_names.size());
  Vector gradient;
  Matrix hessian;
  for (std::size_t iteration = 0; iteration < 10 /*TODO fancy stopping criteria */; ++iteration) {
    std::cerr << "Iteration " << iteration << ": weights =";
    for (Vector::Index i = 0; i < weights.rows(); ++i) {
      std::cerr << ' ' << weights(i);
    }
    std::cerr << std::endl;
    std::cerr << "Perplexity = " << Derivatives(instances, weights, gradient, hessian) << std::endl;
    // TODO: 1.0 step size was too big and it kept getting unstable.  More math.
    weights -= 0.7 * hessian.inverse() * gradient;
  }
  weights_out.assign(weights.data(), weights.data() + weights.size());
}
}} // namespaces


#pragma once

#include <Eigen/Dense>

#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>

namespace percepto
{

// For initializing vectors to random in a range
template <typename Derived>
void randomize_vector( Eigen::DenseBase<Derived>& mat,
                       double minRange = -1.0, double maxRange = 1.0 )
{
	if( minRange == maxRange )
	{
		mat.setConstant( minRange );
		return;
	}
	mat.setConstant( minRange );

	boost::random::mt19937 generator;
	boost::random::random_device rng;
	generator.seed( rng );
	boost::random::uniform_real_distribution<> xDist( minRange, maxRange );

	for( unsigned int i = 0; i < mat.rows(); ++i )
	{
		for( unsigned int j = 0; j < mat.cols(); ++j )
		{
			mat(i,j) = xDist( generator );
		}
	}
}

}


#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Cholesky>
#include <Eigen/Sparse>
#include <limits>
using namespace Rcpp;
using namespace RcppEigen;
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;

// [[Rcpp::depends(RcppEigen)]]

void updateS(Eigen::MatrixXd &S, Eigen::MatrixXd &S2, Eigen::MatrixXd &S0, Eigen::MatrixXd &M, Eigen::MatrixXd &delta0, Eigen::MatrixXd &delta1, const Eigen::VectorXd &x, int iter){
  delta0 = x - M; // delta new point wrt old mean
  M+= delta0/(double)iter;     // sample mean
  delta1= x - M;      // delta new point wrt new mean
  if (iter>1){
    S2 +=(iter-1)/(double)(iter*iter)*(delta0*delta0.transpose())+(delta1*delta1.transpose());
    S0=S;
    S = S2/(double)(iter-1);           // sample covariance
  }
}

// [[Rcpp::interfaces(r,cpp)]]

//' Complex Polytope Gibbs Sampling
//' This function draw uniform samples in a convex polytope with inequality constraints
//'
//' @param N the number of samples to generate
//' @param A a matrix
//' @param b a vector of length equals to nrow(A)
//' @param x0 a vector of length equals to nrcol(A) that should be in the polytope, for example returned by \code{\link{chebycenter}}
//' @param thin thinning interval
//'
//' @section Details:
//' This function is based on an initial matlab code developped called CPRND
//' (https://ch.mathworks.com/matlabcentral/fileexchange/34208-uniform-distribution-over-a-convex-polytope)
//' It generates samples within the complex polytope defined by \eqn{A \cdot x \leqslant   b}
//'
//' @return a matrix with one row per sample and one column per parameter
//' @examples
//' n <- 20
//' A1 <- -diag(n)
//' b1 <- as.matrix(rep(0,n))
//' A2 <- diag(n)
//' b2 <- as.matrix(rep(1,n))
//' A <- rbind(A1,A2)
//' b <- rbind(b1,b2)
//' X0 <- chebycenter(A,b)
//' x <- cpgs(1000,A,b,X0)
//' @export
//' @useDynLib cpgsR
// [[Rcpp::export]]

Eigen::MatrixXd cpgs(const int N, const Eigen::MatrixXd &A ,const Eigen::VectorXd &b,const Eigen::VectorXd &x0, const int thin=1) {
  int p=A.cols();
  int m=A.rows();
  double inf = std::numeric_limits<double>::max();

  // Check input arguments
  if (m < (p+1) || b.size()!=m || x0.size()!=p){
    throw std::range_error("dimensions mismatch");
  }
  // Initialisation
  Eigen::MatrixXd X(N,p);
  Eigen::MatrixXd x(p,1);
  Eigen::MatrixXd y(p,1);
  x=x0;

  // Initialize variables for keeping track of sample mean, covariance
  // and isotropic transform.
  Eigen::MatrixXd M(p,1);
  M.setZero();
  Eigen::MatrixXd S2(p,p);
  S2.setZero();

  // outer products.
  Eigen::MatrixXd S(p,p);
  Eigen::MatrixXd S0(p,p);
  S.setIdentity();

  IntegerVector index=Rcpp::seq(0,p-1);

  Eigen::MatrixXd T1(p,p);
  Eigen::MatrixXd T2(p,p);
  T1.setIdentity();
  Eigen::MatrixXd W(m,p);

  W = A;
  bool adapt=true;
  bool updatingS=true;
  Eigen::MatrixXd d(m,1);
  Eigen::MatrixXd d2(m,1);
  Eigen::MatrixXd delta0(p,1);
  Eigen::MatrixXd delta1(p,1);
  Eigen::MatrixXd z(m,1);
  Eigen::MatrixXd L(p,p);
  Eigen::MatrixXd D(p,p);
  Eigen::VectorXd Dtmp(p);
  Eigen::VectorXd Dzero=VectorXd::Constant(p,1.0e-16);
  Eigen::LDLT<MatrixXd> ldltOfS(S.cols());
  int runup=0; //number of adapt
  int discard=0; //number of discard
  int isample=0; //number of sample
  int n=0; //total number of iterations
  int stage=0; //0 adapting phase, 1 discarding phase, 2 sampling
  int runupmax= 10*p*(p+1);
  int sampleit=0; //total number of iteration during sampling, useful for thin
  int discardmax=runupmax;
  double crit=0;
  while (isample<N){               //sampling loop
    //std::random_shuffle(index.begin(), index.end()); //we change the order to
    //limit the influence of initial ordering
    index=sample(index,p,false);
    y=x;
    NumericVector alea2=runif(p);
    // compute approximate stochastic transformation
    if ((stage==1 && discard==0) || (stage>0 && updatingS==true)){ //first true
      //sample, we now make the isotropic transformation
      ldltOfS.compute(S.transpose());
      D=ldltOfS.vectorD().cwiseMax(Dzero).asDiagonal();
      L=ldltOfS.matrixL();
      T1=ldltOfS.transpositionsP().transpose()*L*D.sqrt();
      T2=T1.inverse();
      W = A*T1;
    }
    if (stage>0) y=T2*y; //otherwise y=I^-1 * y=y

    // choose p new components
    for (int ip=0;ip<p;++ip){
      int i=index[ip];
      //Find points where the line with the (p-1) components x_i
      //fixed intersects the bounding polytope.
      z = W.col(i); //prevent any divisions by 0
      if (ip==0)
        d2=(b - W*y);
      d=d2.cwiseQuotient(z);
      double tmin=-inf;
      double tmax=inf;
      for (int j=0;j<m;++j){
        if (z(j)<0 && tmin<d(j)) tmin=d(j);
        if (z(j)>0 && tmax>d(j)) tmax=d(j);
     }
      tmin=std::min(0.0, tmin);
      tmax=std::max(0.0, tmax);

      double delta = -y(i);
      y(i) += (tmin+(tmax-tmin)*alea2(i));
      y(i)=std::min(std::max(y(i),-inf),inf);
      //Rcout<<tmin<<" "<<tmax<<" "<<y(i)<<std::endl;
      delta += y(i);
      d2 =d2- W.col(i)*delta; //we do this to avoid making a matrix
      //multiplication for each parameter (we just update the value of the
      //constraint with the delta of parameter)
}
    x=T1*y;

    if (stage==0){//still in updating phase
      ++runup;
      if (updatingS) updateS(S, S2, S0, M, delta0, delta1, x, runup);
      crit=(S0-S).norm()/S0.norm();
      if(runup>p & crit<0.5){
        stage=1;
        updatingS=false;
        Rcout<<"########adapation successful after "<<runup<<" iterations"<<std::endl;
        discardmax=runup;
      } else if (runup==runupmax){
        stage=1;
        M.setZero();
        S2.setZero();
        S.setIdentity();
        Rcout<<"########adapation unsuccessful after "<<runup<<" iterations"<<std::endl;
      }
    } else if (stage==1){ //we are in adapting phase
      ++discard;
      if (updatingS) updateS(S, S2, S0, M, delta0, delta1, x, discard);
      crit=(S0-S).norm()/S0.norm();
      if (crit < 0.5) {
        if (updatingS) Rcout<<"##stop updating S during discarding phase"<<std::endl;
        updatingS=false;
      }
      if (discard==discardmax){
        stage=2;
        M.setZero();
        S.setIdentity();
        S2.setZero();
        Rcout<<"#######end of discarding phase"<<std::endl;
        if (updatingS) Rcout<<"S still updated"<<std::endl;
      }
    } else{ //we are in sampling phase
      if ((sampleit % thin) == 0){
        X.row(isample)=x.col(0);
        ++isample;
      }
      if (updatingS) updateS(S, S2, S0, M, delta0, delta1, x, isample);
      double crit=(S0-S).norm()/S0.norm();
      if (crit < 0.5) {
        if (updatingS) Rcout<<"##stop updating S during sampling phase"<<std::endl;
        updatingS=false;
      }
      ++sampleit;
    }
    if (n % 100 == 0) Rcout<<"##iteration "<<n<<" stage "<<stage<<" crit "<<crit<<" S0.norm "<<S0.norm()<<" S-S0 "<<(S-S0).norm()<<std::endl;
    ++n;
  }
  return X;
}

using Eigen::FullPivLU;

//' Complex Polytope Gibbs Sampling
//' This function draw uniform samples in a convex polytope with both equality and inequality constraints
//'
//' @param N the number of samples to generate
//' @param A a matrix of coefficients of inequality constants A.x<=b
//' @param b a vector of length equals to nrow(A)
//' @param C a matrix of coefficients of inequality constants C.x=v
//' @param v a vector of length equals to nrow(C)
//' @param x0 a vector of length equals to ncol(A) that should be in the polytope, for example returned by \code{\link{chebycenter}}
//' @param thin the thinning interval
//'
//' @section Details:
//' This function is based on an initial matlab code developped called CPRND
//' (https://ch.mathworks.com/matlabcentral/fileexchange/34208-uniform-distribution-over-a-convex-polytope)
//' It generates samples within the complex polytope defined by \eqn{A \cdot x \leqslant   b}
//'
//' @return a matrix with one row per sample and one column per parameter
//' @examples
//' n <- 20
//' A1 <- -diag(n)
//' b1 <- as.matrix(rep(0,n))
//' A2 <- diag(n)
//' b2 <- as.matrix(rep(1,n))
//' A <- rbind(A1,A2)
//' b <- rbind(b1,b2)
//' C <- rbind(c(1,1,rep(0,n-2)),c(0,0,1,1,rep(0,n-4)))
//' v <- matrix(rep(0.2,2),2)
//' X0 <- rep(0.1,n)
//' x <- cpgsEquality(1000,A,b,C,v,X0)
//' @export
//' @useDynLib cpgsR
// [[Rcpp::export]]

Eigen::MatrixXd cpgsEquality(const int N, const Eigen::MatrixXd &A,
                             const Eigen::VectorXd &b, const Eigen::MatrixXd &C,
                             const Eigen::VectorXd &v, const Eigen::VectorXd &x0,
                             const int thin=1){
  int p=A.cols();
  int m=A.rows();
  int p2=C.cols();
  int m2=C.rows();

  MatrixXd X(N,p);

  // Check input arguments
  if (m < (p+1) || b.size()!=m || x0.size()!=p){
    throw std::range_error("dimensions mismatch");
  }
  if (v.size()!=m2 || x0.size()!=p2){
    throw std::range_error("dimensions mismatch");
  }
  // Initialisation
  FullPivLU<MatrixXd> lu(C);
  MatrixXd Nt = lu.kernel();
  MatrixXd Abis=A*Nt;
  VectorXd bbis=b-A*x0;

  VectorXd x0bis=VectorXd::Zero(Nt.cols());
  MatrixXd x=cpgs(N, Abis, bbis, x0bis, thin);
  for(int i=0;i<N;++i) {
    X.row(i)=Nt*x.row(i).transpose()+x0;
  }
  return X;
}


#include <complex>
#include <limits>

#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <rsvd/ErrorEstimators.hpp>

using Rsvd::relativeFrobeniusNormError;

template <typename T> struct RelativeFrobeniusNormError : public ::testing::Test {
  using RealType = typename Eigen::NumTraits<T>::Real;
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

  const RealType macheps = std::numeric_limits<RealType>::epsilon();
};

using NumericalTypes = ::testing::Types<float, double, std::complex<float>, std::complex<double>>;

TYPED_TEST_CASE(RelativeFrobeniusNormError, NumericalTypes, );

/// \brief Relative error between equal matrices must be zero.
TYPED_TEST(RelativeFrobeniusNormError, SameMatrix) {
  using MatrixType = typename TestFixture::MatrixType;

  const MatrixType a = MatrixType::Identity(4, 4);
  ASSERT_NEAR(relativeFrobeniusNormError(a, a), 0, TestFixture::macheps);
}

TYPED_TEST(RelativeFrobeniusNormError, DifferentMatrices) {
  using MatrixType = typename TestFixture::MatrixType;

  MatrixType reference = MatrixType::Zero(3, 4);
  reference(0, 0) = 1;

  MatrixType approx = MatrixType::Zero(3, 4);
  approx(0, 0) = 1;
  approx(2, 3) = 100;

  const auto relErr = relativeFrobeniusNormError(reference, approx);
  ASSERT_NEAR(relErr, 100, TestFixture::macheps);
}

TYPED_TEST(RelativeFrobeniusNormError, NonCommutativity) {
  using MatrixType = typename TestFixture::MatrixType;

  MatrixType a = MatrixType::Zero(2, 2);
  a << 1, 2, 3, 4;

  MatrixType b = MatrixType::Zero(2, 2);
  b << 5, 6, 7, 8;

  const auto relativeToA = relativeFrobeniusNormError(a, b);
  const auto relativeToB = relativeFrobeniusNormError(b, a);

  ASSERT_NEAR(relativeToA, 8 / sqrt(30), TestFixture::macheps);
  ASSERT_NEAR(relativeToB, 8 / sqrt(174), TestFixture::macheps);
}

/// \brief Relative error function asserts that the reference matrix has non-zero norm.
TYPED_TEST(RelativeFrobeniusNormError, ZeroReferenceMatrix) {
  using MatrixType = typename TestFixture::MatrixType;

  const MatrixType a = MatrixType::Zero(4, 4);
  ASSERT_DEATH(relativeFrobeniusNormError(a, a), "Assertion `referenceNorm > 0' failed");
}


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/static/StaticDenseRowMajor.h++>

typedef boost::mpl::list<
	float,
	double
> scalar_list;

BOOST_AUTO_TEST_SUITE(test_matrix)

BOOST_AUTO_TEST_CASE_TEMPLATE( Declaration, Scalar, scalar_list )
{
	mla::matrix::StaticDenseRowMajor<Scalar, 3, 3> m;

	BOOST_CHECK_EQUAL(m.rows(), 3);
	BOOST_CHECK_EQUAL(m.columns(), 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Clearing, Scalar, scalar_list )
{
	mla::matrix::StaticDenseRowMajor<Scalar, 3, 3> m;
	m.setZero();

	for(unsigned int i = 0; i < 3; i++)
	{
		for(unsigned int j = 0; j < 3; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), 0.0);
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( Assigning, Scalar, scalar_list )
{
	mla::matrix::StaticDenseRowMajor<Scalar, 3, 3> m;
	m.setZero();

	m(1,1) = 1.0;

	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);

	BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 1), 0.0);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * \file boost/numeric/ublasx/operation/pow2.hpp
 *
 * \brief Apply the \c std::pow2 function to a vector or matrix expression.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_POW2_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_POW2_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>
#include <complex>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_pow2_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_pow2_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

//namespace /*<unnamed>*/ {

/// Auxiliary function used to replace ::std::pow2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T pow2(T x)
{
	return ::std::pow(2,x);
}

/// Auxiliary function used to replace ::std::pow2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
::std::complex<T> pow2(::std::complex<T> x)
{
	// Use the complex exponentiation formula.
	// See:
	// - http://en.wikipedia.org/wiki/Exponentiation#Powers_of_complex_numbers
	// - http://mathworld.wolfram.com/ComplexExponentiation.html

	T c0 = ::std::pow(2,x.real());
	T c1 = x.imag()*::std::log(2);
	return ::std::complex<T>(c0*::std::cos(c1),c0*::std::sin(c1));
}

//} // Namespace <unnamed>

} // Namespace detail

/**
 * \brief Applies the \c std::pow2 function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::pow2 to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_pow2_functor_traits<VectorExprT>::result_type pow2(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_pow2_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_pow2_functor_traits<VectorExprT>::signature_argument_type signature_argument_type;
	typedef typename detail::vector_pow2_functor_traits<VectorExprT>::signature_result_type signature_result_type;

//	return expression_type(ve(), detail::pow2<signature_result_type>);
//	signature_result_type (*)(ptr_pow2_fun)(signature_argument_type)(BOOST_NUMERIC_UBLASX_OPERATION_POW2_NS_::pow2);
	typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
	fun_ptr_type ptr_pow2_fun(&detail::pow2);
	return expression_type(ve(), ptr_pow2_fun);
}

/**
 * \brief Applies the \c std::pow2 function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::pow2 to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_pow2_functor_traits<MatrixExprT>::result_type pow2(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_pow2_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_pow2_functor_traits<MatrixExprT>::signature_argument_type signature_argument_type;
	typedef typename detail::matrix_pow2_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

//	return expression_type(me(), detail::pow2<signature_result_type>(signature_argument_type));
	typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
	fun_ptr_type ptr_pow2_fun(&detail::pow2);
	return expression_type(me(), ptr_pow2_fun);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_POW2_HPP


///////////////////////////////////////////////////////////////////////////////
// integrated_acf.hpp                                                        //
//                                                                           //
//  Copyright 2008 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_INTEGRATED_ACF_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_INTEGRATED_ACF_HPP_ER_2008_04
#include <cmath>
#include <vector>
#include <algorithm>
#include <boost/mpl/int.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/call_traits.hpp>
#include <boost/array.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

#include <boost/accumulators/statistics/acv0.hpp>
#include <boost/accumulators/statistics/integrated_acvf.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // integrated_acf_impl
    template<typename T,typename I>
    class integrated_acf_impl
      : public accumulator_base
    {
    public:
        typedef T                              result_type;

        integrated_acf_impl(dont_care):val(static_cast<T>(0)){}

        template<typename Args>
        void operator ()(Args const &args)
        {
            T iacvf = integrated_acvf<I>(args[accumulator]);
            T acv0_val = acv0<I>(args[accumulator]);
            if(acv0_val>static_cast<T>(0)){
                val = iacvf/acv0_val;
            }else{
                val = static_cast<T>(0);
            }
		}

        result_type result(dont_care) const{
            return val;
        }

    private:
	   T  val;

    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::integrated_acf
//

namespace tag
{

    template <typename I = default_delay_discriminator>
    struct integrated_acf
      : depends_on<acv0<I>,integrated_acvf<I> >
    {
        /// INTERNAL ONLY
      typedef
        accumulators::impl::integrated_acf_impl<mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::integrated_acf
//

namespace extract
{

  template<typename I,typename AccumulatorSet>
  typename mpl::apply<
    AccumulatorSet,tag::integrated_acf<I> >::type::result_type
  integrated_acf(AccumulatorSet const& acc){
    typedef tag::integrated_acf<I> the_tag;
    return extract_result<the_tag>(acc);
  }

}

using extract::integrated_acf;

}} // namespace boost::accumulators

#endif


/*
 * Simulation of an ensemble of Roessler attractors using NT2 SIMD library
 * This requires the SIMD library headers.
 *
 * Copyright 2014 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#include <iostream>
#include <vector>
#include <random>

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

#include <boost/numeric/odeint.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/multiplies.hpp>

namespace odeint = boost::numeric::odeint;
namespace simd = boost::simd;

typedef boost::timer timer_type;

static const size_t dim = 3;  // roessler is 3D

typedef double fp_type;
//typedef float fp_type;

typedef simd::pack<fp_type> simd_pack;
typedef boost::array<simd_pack, dim> state_type;
// use the simd allocator to get properly aligned memory
typedef std::vector< state_type, simd::allocator< state_type > > state_vec;

static const size_t pack_size = simd_pack::static_size;

//---------------------------------------------------------------------------
struct roessler_system {
    const fp_type m_a, m_b, m_c;

    roessler_system(const fp_type a, const fp_type b, const fp_type c)
        : m_a(a), m_b(b), m_c(c)
    {}

    void operator()(const state_type &x, state_type &dxdt, const fp_type t) const
    {
        dxdt[0] = -1.0*x[1] - x[2];
        dxdt[1] = x[0] + m_a * x[1];
        dxdt[2] = m_b + x[2] * (x[0] - m_c);
    }
};

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
if(argc<3)
{
    std::cerr << "Expected size and steps as parameter" << std::endl;
    exit(1);
}
const size_t n = atoi(argv[1]);
const size_t steps = atoi(argv[2]);

const fp_type dt = 0.01;

const fp_type a = 0.2;
const fp_type b = 1.0;
const fp_type c = 9.0;

// random initial conditions on the device
std::vector<fp_type> x(n), y(n), z(n);
std::default_random_engine generator;
std::uniform_real_distribution<fp_type> distribution_xy(-8.0, 8.0);
std::uniform_real_distribution<fp_type> distribution_z(0.0, 20.0);
auto rand_xy = std::bind(distribution_xy, std::ref(generator));
auto rand_z = std::bind(distribution_z, std::ref(generator));
std::generate(x.begin(), x.end(), rand_xy);
std::generate(y.begin(), y.end(), rand_xy);
std::generate(z.begin(), z.end(), rand_z);

state_vec state(n/pack_size);
for(size_t i=0; i<n/pack_size; ++i)
{
    for(size_t p=0; p<pack_size; ++p)
    {
        state[i][0][p] = x[i*pack_size+p];
        state[i][1][p] = y[i*pack_size+p];
        state[i][2][p] = z[i*pack_size+p];
    }
}

std::cout << "Systems: " << n << std::endl;
std::cout << "Steps: " << steps << std::endl;
std::cout << "SIMD pack size: " << pack_size << std::endl;

std::cout << state[0][0] << std::endl;

// Stepper type
odeint::runge_kutta4_classic<state_type, fp_type, state_type, fp_type,
                             odeint::array_algebra, odeint::default_operations,
                             odeint::never_resizer> stepper;

roessler_system sys(a, b, c);

timer_type timer;

fp_type t = 0.0;

for(int step = 0; step < steps; step++)
{
    for(size_t i = 0; i < n/pack_size; ++i)
    {
        stepper.do_step(sys, state[i], t, dt);
    }
    t += dt;
}

std::cout.precision(16);

std::cout << "Integration finished, runtime for " << steps << " steps: ";
std::cout << timer.elapsed() << " s" << std::endl;

// compute some accumulation to make sure all results have been computed
simd_pack s_pack = 0.0;
for(size_t i = 0; i < n/pack_size; ++i)
{
    s_pack += state[i][0];
}

fp_type s = 0.0;
for(size_t p=0; p<pack_size; ++p)
{
    s += s_pack[p];
}

std::cout << state[0][0] << std::endl;
std::cout << s/n << std::endl;

}


#include "socpInterface.hpp"

#include <array>
#include <chrono>
#include <fmt/format.h>

#include <Eigen/Dense>

// This example solves the portfolio optimization problem

int main()
{
    std::vector<double> solve_times;

    // assets, factors pair
    std::vector<std::tuple<size_t, size_t, size_t>> sets = {{100, 5, 10000},
                                                            {300, 10, 5000},
                                                            {500, 20, 1000},
                                                            {1000, 30, 100},
                                                            {2000, 40, 100},
                                                            {4000, 50, 100},
                                                            {7500, 60, 50},
                                                            {10000, 70, 25},
                                                            {20000, 80, 10}};
    for (auto [n, m, repetitions] : sets)
    {
        fmt::print("Running with assets: {}, factors: {}\n", n, m);

        // Set up problem data.
        double gamma = 0.5;      // risk aversion parameter
        Eigen::VectorXd mu(n);   // vector of expected returns
        Eigen::MatrixXd F(n, m); // factor-loading matrix
        Eigen::VectorXd D(n);    // diagonal of idiosyncratic risk

        mu.setRandom();
        F.setRandom();
        D.setRandom();
        mu = mu.cwiseAbs();
        F = F.cwiseAbs().transpose();
        D = D.cwiseAbs().cwiseSqrt();

        // Formulate SOCP.
        auto t0 = std::chrono::high_resolution_clock::now();

        op::SecondOrderConeProgram socp;

        op::Variable x = socp.createVariable("x", n);
        op::Variable t = socp.createVariable("t");
        op::Variable s = socp.createVariable("s");
        op::Variable u = socp.createVariable("u");
        op::Variable v = socp.createVariable("v");

        socp.addConstraint(x >= 0.);
        socp.addConstraint(op::sum(x) == op::Parameter(1.));
        socp.addConstraint(op::norm2(op::Parameter(&D).cwiseProduct(x)) <= u);
        socp.addConstraint(op::norm2(op::Parameter(&F) * x) <= v);
        socp.addConstraint(op::norm2(op::vstack({op::Parameter(1.) + -t, op::Parameter(2.) * u})) <= op::Parameter(1.) + t);
        socp.addConstraint(op::norm2(op::vstack({op::Parameter(1.) + -s, op::Parameter(2.) * v})) <= op::Parameter(1.) + s);

        socp.addMinimizationTerm(-op::Parameter(&mu).transpose() * x);
        socp.addMinimizationTerm(op::Parameter(gamma) * (t + s));

        // Create and initialize the solver instance.
        op::Solver solver(socp);
        solver.initialize();

        // Solve the problem and show solver output.
        double total_time = 0.;
        for (size_t rep = 0; rep < repetitions; rep++)
        {
            fmt::print("Repetition {}/{}\n", rep + 1, repetitions);
            mu.setRandom();
            F.setRandom();
            D.setRandom();
            mu = mu.cwiseAbs();
            F = F.cwiseAbs().transpose();
            D = D.cwiseAbs().cwiseSqrt();

            t0 = std::chrono::high_resolution_clock::now();
            const bool success = solver.solveProblem(false);
            auto t1 = std::chrono::high_resolution_clock::now();
            total_time += std::chrono::duration<double>(t1 - t0).count();
        }
        solve_times.push_back(total_time / repetitions);
    }

    fmt::print("\nAverage times:\n");
    for (size_t i = 0; i < sets.size(); i++)
    {
        fmt::print("{},\n", solve_times[i]);
    }
}