#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#include <adept_source.h>
#include <adept.h>
using adept::adouble;

#define SINCOSN 10000000
static
double sincos_real(double x) {
  double sum = 0;
  for(int i=1; i<=SINCOSN; i++) {
    sum += pow(x, i) / i;
  }
  return sum;
}

static void sincos_real_tapenade(double x, double *xb, double sincos_realb) {
    double sum = 0;
    double sumb = 0.0;
    double sincos_real;
    sumb = sincos_realb;
    for (int i = SINCOSN; i > 0; --i)
        if (!(x<=0.0&&(i==0.0||i!=(int)i)))
            *xb = *xb + pow(x, (i-1))*sumb;
}

static
adouble sincos(adouble x) {
  adouble sum = 0;
  for(int i=1; i<=SINCOSN; i++) {
    sum += pow(x, i) / i;
  }
  return sum;
}

static
double sincos_and_gradient(double xin, double& xgrad) {
    adept::Stack stack;
    adouble x = xin;
    stack.new_recording();
    adouble y = sincos(x);
    y.set_gradient(1.0);
    stack.compute_adjoint();
    xgrad = x.get_gradient();
    return y.value();
}

static void adept_sincos(double inp) {
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = sincos_real(inp);

  gettimeofday(&end, NULL);
  printf("%0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  adept::Stack stack;
 // stack.new_recording();
  adouble resa = sincos(inp);
  double res = resa.value();

  gettimeofday(&end, NULL);
  printf("%0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = 0;
  sincos_and_gradient(inp, res2);

  gettimeofday(&end, NULL);
  printf("%0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void tapenade_sincos(double inp) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = sincos_real(inp);

  gettimeofday(&end, NULL);
  printf("tapenade %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = sincos_real(inp);

  gettimeofday(&end, NULL);
  printf("tapenade %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double res2 = 0;

  sincos_real_tapenade(inp, &res2, 1.0);

  gettimeofday(&end, NULL);
  printf("tapendade %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void enzyme_sincos(double inp) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = sincos_real(inp);

  gettimeofday(&end, NULL);
  printf("%0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = sincos_real(inp);

  gettimeofday(&end, NULL);
  printf("%0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double res2;

  res2 = __enzyme_autodiff<double>(sincos_real, inp);

  gettimeofday(&end, NULL);
  printf("%0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

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

  double inp = atof(argv[1]) ;
  printf("adept\n");
  adept_sincos(inp);
  printf("tapenade\n");
  tapenade_sincos(inp);
  printf("enzyme\n");
  enzyme_sincos(inp);
}


#define CATCH_CONFIG_MAIN
#include <Eigen/Dense>
#include <catch.hpp>
#include <random>

#include "EDP/ConstructSparseMat.hpp"
#include "EDP/LocalHamiltonian.hpp"

#include "yavque/Circuit.hpp"
#include "yavque/operators.hpp"
#include "yavque/utils.hpp"

#include "common.hpp"

tbb::global_control gc(tbb::global_control::max_allowed_parallelism, 2);

yavque::Circuit construct_diagonal_tfi(const uint32_t N)
{
	yavque::Circuit circ(1 << N);
	Eigen::VectorXd zz_all(1 << N);

	for(uint32_t n = 0; n < (1u << N); ++n)
	{
		int elt = 0;
		for(uint32_t k = 0; k < N; ++k)
		{
			int z0 = 1 - 2 * ((n >> k) & 1);
			int z1 = 1 - 2 * ((n >> ((k + 1) % N)) & 1);
			elt += z0 * z1;
		}
		zz_all(n) = elt;
	}

	auto zz_all_ham = yavque::DiagonalOperator(zz_all, "zz all");
	auto x_all_ham
		= yavque::SumLocalHam(N, yavque::pauli_x().cast<yavque::cx_double>(), "x all");

	for(uint32_t p = 0; p < 4; ++p)
	{
		circ.add_op_right<yavque::DiagonalHamEvol>(zz_all_ham);
		circ.add_op_right<yavque::SumLocalHamEvol>(x_all_ham);
	}
	circ.add_op_right<yavque::DiagonalHamEvol>(zz_all_ham);
	return circ;
}

auto construct_bare_tfi(const uint32_t N)
{
	yavque::Circuit circ(1U << N);
	std::vector<yavque::Variable> variables(9);

	std::vector<yavque::Hamiltonian> zz_hams;

	for(uint32_t k = 0; k < N; ++k)
	{
		edp::LocalHamiltonian<double> lh(N, 2);
		lh.addTwoSiteTerm({k, (k + 1) % N}, yavque::pauli_zz());
		zz_hams.emplace_back(edp::constructSparseMat<yavque::cx_double>(1 << N, lh));
	}
	auto x_all_ham
		= yavque::SumLocalHam(N, yavque::pauli_x().cast<yavque::cx_double>(), "x all");

	for(uint32_t p = 0; p < 4; ++p)
	{
		for(uint32_t k = 0; k < N; k++)
		{
			circ.add_op_right<yavque::HamEvol>(zz_hams[k], variables[2 * p + 0]);
		}
		circ.add_op_right<yavque::SumLocalHamEvol>(x_all_ham, variables[2 * p + 1]);
	}
	for(uint32_t k = 0; k < N; k++)
	{
		circ.add_op_right<yavque::HamEvol>(zz_hams[k], variables[8]);
	}

	return std::make_pair(std::move(circ), std::move(variables));
}

Eigen::VectorXcd analytic_twoqubit(double theta, double phi)
{
	Eigen::VectorXcd res(4);
	constexpr yavque::cx_double I(0., 1.);

	res(0) = cos(2 * phi) * exp(-I * theta) - I * sin(2 * phi) * exp(I * theta);
	res(1) = cos(2 * phi) * exp(I * theta) - I * sin(2 * phi) * exp(-I * theta);
	res(2) = cos(2 * phi) * exp(I * theta) - I * sin(2 * phi) * exp(-I * theta);
	res(3) = cos(2 * phi) * exp(-I * theta) - I * sin(2 * phi) * exp(I * theta);

	return res / 2;
}

Eigen::MatrixXcd rot_x(double phi)
{
	constexpr yavque::cx_double I(0., 1.);
	Eigen::MatrixXcd rot_x(2, 2);

	rot_x << cos(phi), -I * sin(phi), -I * sin(phi), cos(phi);

	return rot_x;
}

Eigen::MatrixXcd kron_n(const Eigen::MatrixXcd& m, uint32_t n)
{
	Eigen::MatrixXcd res(m);
	for(uint32_t k = 1; k < n; ++k)
	{
		res = Eigen::kroneckerProduct(res, m).eval();
	}
	return res;
}

Eigen::VectorXcd product_fourqubit(double theta1, double phi1, double theta2, double phi2)
{
	Eigen::VectorXcd res(16);
	constexpr yavque::cx_double I(0., 1.);

	Eigen::VectorXd zz(16);
	zz << 4, 0, 0, 0, 0, -4, 0, 0, 0, 0, -4, 0, 0, 0, 0, 4;

	Eigen::VectorXcd v = Eigen::VectorXcd::Ones(16);
	v /= 4.0;

	v.array() *= (-I * theta1 * zz.array()).exp();
	v = kron_n(rot_x(phi1), 4) * v;
	v.array() *= (-I * theta2 * zz.array()).exp();
	v = kron_n(rot_x(phi2), 4) * v;

	return v;
}

TEST_CASE("test two qubit", "[tfi-twoqubit]")
{
	using namespace yavque;
	using namespace Eigen;

	std::random_device rd;
	std::default_random_engine re{rd()};

	std::normal_distribution<> ndist(0., 1.);

	constexpr unsigned int N = 2;
	const double eps = 1e-6;

	Eigen::VectorXcd zz(1 << N);

	for(uint32_t n = 0; n < (1u << N); ++n)
	{
		int z0 = 1 - 2 * ((n >> 0) & 1);
		int z1 = 1 - 2 * ((n >> 1) & 1);
		zz(n) = z0 * z1;
	}

	auto zz_all_ham = yavque::DiagonalOperator(zz, "zz all");
	auto x_all_ham
		= yavque::SumLocalHam(N, yavque::pauli_x().cast<yavque::cx_double>(), "x all");

	auto circ = yavque::Circuit(1 << N);

	for(uint32_t p = 0; p < 1; ++p)
	{
		circ.add_op_right<yavque::DiagonalHamEvol>(zz_all_ham);
		circ.add_op_right<yavque::SumLocalHamEvol>(x_all_ham);
	}

	Eigen::MatrixXd ham(4, 4);
	ham << -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1;

	auto variables = circ.variables();

	Eigen::VectorXcd ini = Eigen::VectorXcd::Ones(4);
	ini /= sqrt(4.0);

	circ.set_input(ini);

	for(uint32_t _instance = 0; _instance < 10; ++_instance)
	{
		double theta = ndist(re);
		double phi = ndist(re);
		variables[0] = theta;
		variables[1] = phi;

		for(uint32_t epoch = 0; epoch < 100; ++epoch)
		{ // learning loop
			theta = variables[0].value();
			phi = variables[1].value();

			circ.clear_evaluated();
			Eigen::VectorXcd output = *circ.output();
			Eigen::VectorXcd analytic = analytic_twoqubit(theta, phi);

			REQUIRE((output - analytic).norm() < 1e-6);

			// test grad
			for(auto& p : variables)
			{
				p.zero_grad();
			}
			circ.derivs();
			Eigen::MatrixXcd grads(1 << N, 2);
			grads.col(0) = *variables[0].grad();
			grads.col(1) = *variables[1].grad();
			Eigen::VectorXd egrad_circ = 2 * (output.adjoint() * ham * grads).real();

			Eigen::VectorXcd v1 = analytic_twoqubit(theta + eps, phi);
			Eigen::VectorXcd v2 = analytic_twoqubit(theta - eps, phi);

			Eigen::VectorXd egrad_num(2);
			egrad_num.coeffRef(0) = real(cx_double(v1.adjoint() * ham * v1)
			                             - cx_double(v2.adjoint() * ham * v2))
			                        / (2 * eps);

			v1 = analytic_twoqubit(theta, phi + eps);
			v2 = analytic_twoqubit(theta, phi - eps);

			egrad_num.coeffRef(1) = real(cx_double(v1.adjoint() * ham * v1)
			                             - cx_double(v2.adjoint() * ham * v2))
			                        / (2 * eps);

			REQUIRE((egrad_circ - egrad_num).norm() < 1e-6);

			for(uint32_t k = 0; k < variables.size(); ++k)
			{
				variables[k] -= 0.02 * egrad_circ(k);
			}

			std::cout << real(cx_double(output.adjoint() * ham * output)) << std::endl;
		}
	}
}

TEST_CASE("test four qubit", "[tfi-fourqubit]")
{
	using namespace yavque;
	using namespace Eigen;

	std::random_device rd;
	std::default_random_engine re{rd()};

	std::normal_distribution<> ndist(0., 1.);

	constexpr unsigned int N = 4;
	const double eps = 1e-6;

	Eigen::VectorXd zz_all(1 << N);

	for(uint32_t n = 0; n < (1u << N); ++n)
	{
		int elt = 0;
		for(uint32_t k = 0; k < N; ++k)
		{
			int z0 = 1 - 2 * ((n >> k) & 1);
			int z1 = 1 - 2 * ((n >> ((k + 1) % N)) & 1);
			elt += z0 * z1;
		}
		zz_all(n) = elt;
	}

	auto zz_all_ham = yavque::DiagonalOperator(zz_all, "zz all");
	auto x_all_ham
		= yavque::SumLocalHam(N, yavque::pauli_x().cast<yavque::cx_double>(), "x all");

	auto circ = yavque::Circuit(1 << N);

	for(uint32_t p = 0; p < 2; ++p)
	{
		circ.add_op_right<yavque::DiagonalHamEvol>(zz_all_ham);
		circ.add_op_right<yavque::SumLocalHamEvol>(x_all_ham);
	}

	Eigen::MatrixXd ham = zz_all.asDiagonal();

	auto variables = circ.variables();

	Eigen::VectorXcd ini = Eigen::VectorXcd::Ones(16);
	ini /= 4.0;

	circ.set_input(ini);

	for(uint32_t _instance = 0; _instance < 10; ++_instance)
	{
		double theta1 = ndist(re);
		double theta2 = ndist(re);
		double phi1 = ndist(re);
		double phi2 = ndist(re);

		variables[0] = theta1;
		variables[1] = phi1;
		variables[2] = theta2;
		variables[3] = phi2;

		for(uint32_t epoch = 0; epoch < 100; ++epoch)
		{ // learning loop
			theta1 = variables[0].value();
			phi1 = variables[1].value();
			theta2 = variables[2].value();
			phi2 = variables[3].value();

			circ.clear_evaluated();
			Eigen::VectorXcd output = *circ.output();
			Eigen::VectorXcd analytic = product_fourqubit(theta1, phi1, theta2, phi2);

			REQUIRE((output - analytic).norm() < 1e-6);

			// test grad
			for(auto& p : variables)
			{
				p.zero_grad();
			}
			circ.derivs();
			Eigen::MatrixXcd grads(1 << N, 4);
			for(uint32_t k = 0; k < 4; k++)
			{
				grads.col(k) = *variables[k].grad();
			}
			Eigen::VectorXd egrad_circ = 2 * (output.adjoint() * ham * grads).real();

			Eigen::VectorXcd v1 = product_fourqubit(theta1 + eps, phi1, theta2, phi2);
			Eigen::VectorXcd v2 = product_fourqubit(theta1 - eps, phi1, theta2, phi2);

			Eigen::VectorXd egrad_num(4);
			egrad_num.coeffRef(0) = real(cx_double(v1.adjoint() * ham * v1)
			                             - cx_double(v2.adjoint() * ham * v2))
			                        / (2 * eps);

			v1 = product_fourqubit(theta1, phi1 + eps, theta2, phi2);
			v2 = product_fourqubit(theta1, phi1 - eps, theta2, phi2);
			egrad_num.coeffRef(1) = real(cx_double(v1.adjoint() * ham * v1)
			                             - cx_double(v2.adjoint() * ham * v2))
			                        / (2 * eps);

			v1 = product_fourqubit(theta1, phi1, theta2 + eps, phi2);
			v2 = product_fourqubit(theta1, phi1, theta2 - eps, phi2);
			egrad_num.coeffRef(2) = real(cx_double(v1.adjoint() * ham * v1)
			                             - cx_double(v2.adjoint() * ham * v2))
			                        / (2 * eps);

			v1 = product_fourqubit(theta1, phi1, theta2, phi2 + eps);
			v2 = product_fourqubit(theta1, phi1, theta2, phi2 - eps);
			egrad_num.coeffRef(3) = real(cx_double(v1.adjoint() * ham * v1)
			                             - cx_double(v2.adjoint() * ham * v2))
			                        / (2 * eps);

			REQUIRE((egrad_circ - egrad_num).norm() < 1e-6);

			for(uint32_t k = 0; k < variables.size(); ++k)
			{
				variables[k] -= 0.02 * egrad_circ(k);
			}

			std::cout << real(cx_double(output.adjoint() * ham * output)) << std::endl;
		}
	}
}

TEST_CASE("test tfi", "[tfi]")
{
	using namespace yavque;
	using namespace Eigen;

	constexpr unsigned int N = 10;
	constexpr cx_double I(0., 1.);

	std::random_device rd;
	std::default_random_engine re{rd()};
	std::normal_distribution<> nd;

	auto circ1 = construct_diagonal_tfi(N);
	auto variables1 = circ1.variables();

	auto [circ2, variables2] = construct_bare_tfi(N);

	Eigen::VectorXcd ini = Eigen::VectorXcd::Ones(1 << N);
	ini /= sqrt(1 << N);
	circ1.set_input(ini);
	circ2.set_input(ini);

	for(uint32_t k = 0; k < 9; ++k)
	{
		double v = nd(re);
		variables1[k] = v;
		variables2[k] = v;
	}

	for(uint32_t _instance = 0; _instance < 100; ++_instance)
	{ // instance loop
		circ1.clear_evaluated();
		circ2.clear_evaluated();

		for(uint32_t k = 0; k < 9; ++k)
		{
			variables1[k].zero_grad();
			variables2[k].zero_grad();
		}

		Eigen::VectorXcd output1 = *circ1.output();
		Eigen::VectorXcd output2 = *circ2.output();

		REQUIRE((output1 - output2).norm() < 1e-6);

		circ1.derivs();
		circ2.derivs();

		for(uint32_t k = 0; k < 9; ++k)
		{
			Eigen::VectorXcd grad1 = *variables1[k].grad();
			Eigen::VectorXcd grad2 = *variables1[k].grad();
			REQUIRE((grad1 - grad2).norm() < 1e-6);
		}

		for(uint32_t k = 0; k < 9; ++k)
		{
			variables1[k] -= 0.01;
			variables2[k] -= 0.01;
		}
	}
}


/**
 * @file Projector.hpp
 * @author Takashi Michikawa <michikawa@acm.org>
 */

#ifndef MI_PROJECTOR_HPP
#define MI_PROJECTOR_HPP 1
#include <array>
#include <memory>

#include <Eigen/Dense>

namespace mi4
{
        class Projector
        {
        private:
                using map_mat = Eigen::Map< Eigen::Matrix4d >;
                Projector (const Projector& that) = delete;
                Projector (Projector&& that) = delete;
                void operator = (const Projector& that) = delete;
                void operator = (Projector&& that) = delete;
        public:
                explicit Projector (double modelview[16], double projection[16], int vp[4]) : _matrix(map_mat(projection) * map_mat(modelview)), _inv_matrix((map_mat(projection) * map_mat(modelview)).inverse()), _viewport({vp[0], vp[1], vp[2], vp[3]}) {}
                explicit Projector (const Eigen::Matrix4d& modelview, const Eigen::Matrix4d& projection, const std::array< int, 4 >& vp) : _matrix(projection * modelview), _inv_matrix((projection * modelview).inverse()), _viewport(vp) {}
                ~Projector ( void ) = default;

                Eigen::Vector2d project ( const Eigen::Vector3d& p, double* depth )
                {
                        const auto& vp = this->_viewport;
                        const auto p0 = this->_matrix * p.homogeneous();

                        if ( p0.w() == 0 ) {
                                return Eigen::Vector2d(0, 0);
                        }

                        *depth = (1.0 + p0.z() / p0.w()) * 0.5;
                        return Eigen::Vector2d(vp[0] + (1 + p0.x() / p0.w()) * vp[2] * 0.5, vp[1] + (1 + p0.y() / p0.w()) * vp[3] * 0.5);
                }

                Eigen::Vector3d unproject (const Eigen::Vector2d& wp, double depth) const
                {
                        const auto& inv_matrix = this->_inv_matrix;
                        const auto& vp = this->_viewport;
                        const Eigen::Vector4d p0((wp.x() - vp[0]) * 2 / vp[2] - 1.0, (wp.y() - vp[1]) * 2 / vp[3] - 1.0, 2.0 * depth - 1.0, 1.0);
                        const Eigen::Vector4d p1 = inv_matrix * p0;
                        return (p1.w() != 0.0) ? p1.hnormalized() : Eigen::Vector3d(0, 0, 0);
                }
        private:
                const Eigen::Matrix4d _matrix;
                const Eigen::Matrix4d _inv_matrix;
                const std::array< int, 4 > _viewport;
        };
}

#endif// MI_PROJECTOR_HPP


// Filename: matrix_free_cg.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	mtl::vampir_trace<9901> tracer;
	assert(int(size(v)) == m * n);
	Vector w(m * n);

	for (int i= 0; i < m; i++)
	    for (int j= 0; j < n; j++) {
		int k= i * n + j; // offset
		w[k]= 4 * v[k];
		if (i > 0) w[k]-= v[k-n];   // upper neighbor
		if (i < m-1) w[k]-= v[k+n]; // lower neighbor
		if (j > 0) w[k]-= v[k-1];   // left neighbor
		if (j < n-1) w[k]-= v[k+1]; // right neighbor
	    }
	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

int main(int, char**)
{
    // For a more realistic example set size to 1000 or larger
    const int size = 1000, N = size * size;

    typedef ::poisson2D_dirichlet             matrix_type;
    matrix_type                               A(size, size);
    itl::pc::identity<matrix_type>            P(A);

    mtl::dense_vector<double>                 x(N, 1.0), b(N);

    b = A * x;
    x= 0;
    itl::cyclic_iteration<double>             iter(b, 10, 1.e-11, 0.0, 5);
    cg(A, x, b, P, iter);

    return 0;
}


// Copyright Yahoo. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "functiontablefactory.h"
#include <vespa/vespalib/locale/c.h>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <cmath>
#include <vespa/log/log.h>
LOG_SETUP(".fef.functiontablefactory");

namespace {

void logArgumentWarning(const vespalib::string & name, size_t exp, size_t act)
{
    LOG(warning, "Cannot create table for function '%s'. Wrong number of arguments: expected %zu to %zu, but got %zu",
        name.c_str(), exp, exp + 1, act);
}

}

namespace search::fef {

bool
FunctionTableFactory::checkArgs(const std::vector<vespalib::string> & args, size_t exp, size_t & tableSize) const
{
    if (exp <= args.size() && args.size() <= (exp + 1)) {
        if (args.size() == (exp + 1)) {
            tableSize = atoi(args.back().c_str());
        } else {
            tableSize = _defaultTableSize;
        }
        return true;
    }
    return false;
}

bool
FunctionTableFactory::isSupported(const vespalib::string & type) const
{
    return (isExpDecay(type) || isLogGrowth(type) || isLinear(type));
}

Table::SP
FunctionTableFactory::createExpDecay(double w, double t, size_t len) const
{
    Table::SP table(new Table());
    for (size_t x = 0; x < len; ++x) {
        table->add(w * std::exp(-(x / t)));
    }
    return table;
}

Table::SP
FunctionTableFactory::createLogGrowth(double w, double t, double s, size_t len) const
{
    Table::SP table(new Table());
    for (size_t x = 0; x < len; ++x) {
        table->add(w * (std::log(1 + (x / s))) + t);
    }
    return table;
}

Table::SP
FunctionTableFactory::createLinear(double w, double t, size_t len) const
{
    Table::SP table(new Table());
    for (size_t x = 0; x < len; ++x) {
        table->add(w * x + t);
    }
    return table;
}

FunctionTableFactory::FunctionTableFactory(size_t defaultTableSize) :
    _defaultTableSize(defaultTableSize)
{
}

Table::SP
FunctionTableFactory::createTable(const vespalib::string & name) const
{
    ParsedName p;
    if (parseFunctionName(name, p)) {
        if (isSupported(p.type)) {
            size_t tableSize = _defaultTableSize;
            if (isExpDecay(p.type)) {
                if (checkArgs(p.args, 2, tableSize)) {
                    return createExpDecay(vespalib::locale::c::atof(p.args[0].c_str()), vespalib::locale::c::atof(p.args[1].c_str()), tableSize);
                }
                logArgumentWarning(name, 2, p.args.size());
            } else if (isLogGrowth(p.type)) {
                if (checkArgs(p.args, 3, tableSize)) {
                    return createLogGrowth(vespalib::locale::c::atof(p.args[0].c_str()), vespalib::locale::c::atof(p.args[1].c_str()), vespalib::locale::c::atof(p.args[2].c_str()), tableSize);
                }
                logArgumentWarning(name, 3, p.args.size());
            } else if (isLinear(p.type)) {
                if (checkArgs(p.args, 2, tableSize)) {
                    return createLinear(vespalib::locale::c::atof(p.args[0].c_str()), vespalib::locale::c::atof(p.args[1].c_str()), tableSize);
                }
                logArgumentWarning(name, 2, p.args.size());
            }
        } else {
            LOG(warning, "Cannot create table for function '%s'. Function type '%s' is not supported",
                name.c_str(), p.type.c_str());
        }
    } else {
        LOG(warning, "Cannot create table for function '%s'. Could not be parsed.", name.c_str());
    }
    return Table::SP(NULL);
}

bool
FunctionTableFactory::parseFunctionName(const vespalib::string & name, ParsedName & parsed)
{
    size_t ps = name.find('(');
    size_t pe = name.find(')');
    if (ps == vespalib::string::npos || pe == vespalib::string::npos) {
        LOG(warning, "Parse error: Did not find '(' and ')' in function name '%s'", name.c_str());
        return false;
    }
    if (ps >= pe) {
        LOG(warning, "Parse error: Found ')' before '(' in function name '%s'", name.c_str());
        return false;
    }
    parsed.type = name.substr(0, ps);
    vespalib::string args = name.substr(ps + 1, pe - ps - 1);
    if (!args.empty()) {
        boost::split(parsed.args, args, boost::is_any_of(","));
    }
    return true;
}

}


/*Deep Euler implementation of a sonochemical bubble model*/

//#define EIGEN_NO_DEBUG
#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <string>
#include <chrono>

#include <torch/script.h>
#include <Eigen/Core>
#include <boost/numeric/odeint.hpp>

const double rho_L = 9.970639504998557e+02;
const double p_inf = 1.0e+5;
const double sigma = 0.071977583160056;
const double gamma = 1.33;
const double c_L = 1.497251785455527e+03; // water 25 Celsius
const double mu_L = 8.902125058209557e-04; //25 Celsius
const double lambda = 0.6084; //water 25 Celsius
const double T_inf = 298.15; // 25 Celsius

const double R_E = 10e-6; //1...10u
const double p_A = 0.5e5; //0.5..2 bar
const double f = 100e3; //20 kHz... 2 MHz

using namespace std;

const int N = 16;
const int N_z = N / 2 - 1;

const int nn_inputs = 2 + 3 + N_z;
const int nn_outputs = N_z;
const int system_order = 4+N_z;
c10::TensorOptions global_tensor_op;

//Modify these to load the correct model
string file_name = "../simulations/bub_hybrid_test.txt";
string model_file = "../../../training/traced_model_bub0.5_hybrid_e51_2112021549.pt";
string scaler_file = "../../../training/scaler_bub0.5_hybrid_2112021549.psca";

typedef double value_type;
typedef vector<value_type> state_type;
typedef Eigen::Matrix<value_type, N / 2, N / 2> matrix_type;

struct std_scaler {
	torch::Tensor mean;
	torch::Tensor scale;

	torch::Tensor operator()(torch::Tensor tensor) {
		return (tensor - mean) / scale;
	}
	torch::Tensor inverse_transform(torch::Tensor tensor) {
		return tensor * scale + mean;
	}
	void parse(istream& is, int numel) {
		mean = torch::ones({ 1, numel });
		is.get();
		double temp = 0.0;
		for (int i = 0; i < numel; i++) {
			is >> temp;
			mean[0][i] = temp;
		}
		is.get();
		is.get();
		scale = torch::ones({ 1, numel });
		is.get();
		for (int i = 0; i < numel; i++) {
			is >> temp;
			scale[0][i] = temp;
		}
		is.get();
		is.get();
	}
};

struct norm_scaler {
	torch::Tensor data_min;
	torch::Tensor data_max;
	double min = 0;
	double max = 0;

	torch::Tensor operator()(torch::Tensor tensor) {
		torch::Tensor X_std = (tensor - data_min) / (data_max - data_min);
		return X_std * (max - min) + min;
	}
	torch::Tensor inverse_transform(torch::Tensor tensor) {
		torch::Tensor Y_std = (tensor - min) / (max - min);
		return Y_std * (data_max - data_min) + data_min;
	}
	void parse(istream& is) {
		data_min = torch::ones({ 1,nn_outputs });
		is.get();
		double temp = 0.0;
		for (int i = 0; i < nn_outputs; i++) {
			is >> temp;
			data_min[0][i] = temp;
		}
		is.get();
		is.get();
		data_max = torch::ones({ 1, nn_outputs });
		is.get();
		for (int i = 0; i < nn_outputs; i++) {
			is >> temp;
			data_max[0][i] = temp;
		}
		is.get(); //']'
		is.get(); //'\n'
		is >> min;
		is >> max;
	}
};

//ode function of Van der Pol equation
class BubDyn {
	double mu = 1.5;
	std::vector<torch::jit::IValue> inps; //reused neural network input vector
	torch::Tensor inputs; //reused tensor of inputs
public:
	torch::jit::script::Module model; //the neural network
	std_scaler in_transf;
	std_scaler out_transf;

	std::array<double, 5> stage_times;
	std::vector<value_type> C; //constants of the right hand side
	Eigen::Matrix<value_type, N / 2, 1> y; //collocation points (half)
	Eigen::Matrix<value_type, N / 2, 1> y_sq; //same, every entry squared
	matrix_type D_E; //Derivative matrix for even functions
	matrix_type D_O; //Derivative matrix for odd functions

	BubDyn() {
		inps = std::vector<torch::jit::IValue>(1);
		inputs = torch::ones({ 5, nn_inputs }, global_tensor_op);

		//----------------------------------------------------------
		//bubblemodel initializations
		//----------------------------------------------------------
		const double omega = 2 * M_PI * f;
		value_type pi2wRE = 2 * M_PI / (omega * R_E);

		//constants
		C = std::vector<value_type>(13);
		C[0] = omega * R_E / (2 * M_PI * c_L);
		C[1] = 4 * mu_L / (c_L * rho_L * R_E);
		C[2] = 4 * mu_L / (rho_L * R_E) * pi2wRE;
		C[3] = 2 * sigma * pi2wRE * pi2wRE / (rho_L * R_E);
		C[4] = p_inf / rho_L * pi2wRE * pi2wRE;
		C[5] = p_A / rho_L * pi2wRE * pi2wRE;
		C[6] = pi2wRE * p_inf / (c_L * rho_L);
		C[7] = pi2wRE * p_A / (c_L * rho_L);
		C[8] = 2 * M_PI * pi2wRE * p_A / (c_L * rho_L);
		C[9] = lambda * (gamma - 1) / gamma * pi2wRE / R_E * T_inf / p_inf;
		C[10] = lambda * (gamma - 1) * pi2wRE / R_E * T_inf / p_inf;
		C[11] = (gamma - 1) / gamma;
		C[12] = 1.0 / (3 * gamma);

		//Derivative matrices
		Eigen::Matrix<value_type, N, 1> y_full(N);
		value_type rec_cpn = 1.0 / (N - 1);
		for (int i = 0; i < N; i++) {
			y_full[i] = cos(M_PI * i * rec_cpn);
			//std::cout << y_full[i] << std::endl;
		}
		Eigen::Matrix<value_type, N, N> D(N, N);
		for (int i = 0; i < N; i++) {
			for (int j = 0; j < N; j++) {
				if (i == j) {
					if (i == N - 1) {
						D(N - 1, N - 1) = -(1 + 2 * (N - 1) * (N - 1)) / 6.0;
					}
					else if (i == 0) {
						D(0, 0) = (1 + 2 * (N - 1) * (N - 1)) / 6.0;
					}
					else {
						D(i, i) = -y_full[i] / (2.0 * (1.0 - y_full[i] * y_full[i]));
					}
				}
				else {
					D(i, j) = std::pow(-1, i + j) * (i == 0 || i == N - 1 ? 2.0 : 1.0)
						/ ((j == 0 || j == N - 1 ? 2.0 : 1.0) * (y_full[i] - y_full[j]));
				}
			}
		}
		D_E = matrix_type(N / 2, N / 2);
		D_O = matrix_type(N / 2, N / 2);
		for (int i = 0; i < N / 2; i++) {
			for (int j = 0; j < N / 2; j++) {
				D_E(i, j) = D(i, j) + D(i, N - 1 - j);
			}
		}
		for (int i = 0; i < N / 2; i++) {
			for (int j = 0; j < N / 2; j++) {
				D_O(i, j) = D(i, j) - D(i, N - 1 - j);
			}
		}
		y = y_full.head<N / 2>();
		y_sq = y.cwiseProduct(y);

		//---------------------------------------------------------
		//neural network initializations
		//---------------------------------------------------------
		torch::Tensor inputs = torch::ones({ 1, nn_inputs }, global_tensor_op);

		try {
			model = torch::jit::load(model_file);
			std::vector<torch::jit::IValue> inp;
			inp.push_back(torch::ones({ 1, nn_inputs }, global_tensor_op));
			std::cout << inp << endl;
			// Execute the model and turn its output into a tensor.
			at::Tensor output = model.forward(inp).toTensor().detach();
			std::cout << output << endl;
		}
		catch (const c10::Error& e) {
			std::cerr << "Error loading the model: " << e.what() << endl;
			exit(-1);
		}
		ifstream in(scaler_file);
		if (!in) {
			std::cerr << "Error loading the scalers." << endl;
			exit(-1);
		}
		out_transf.parse(in, nn_outputs);
		in_transf.parse(in, nn_inputs);
		in.close();
	}

	//Rewrites the errors array with the predicted local truncation errors
	torch::Tensor local_error(double t, double dt, const double * x, const double* z) {
		double sinpi = sin(2 * M_PI * t);

		for (int jj = 0; jj < 5; jj++) {
			inputs[jj][1] = x[0];
			inputs[jj][2] = x[1];
			inputs[jj][3] = x[2];
			inputs[jj][nn_inputs - 1] = sinpi;
			//timestep
			inputs[jj][0] = stage_times[jj] * dt;
			//temperature
			for (int i = 0; i < N_z; i++) {
				inputs[jj][i + 4] = z[i+1];
			}
			//scaling
			inputs.index_put_({jj, torch::indexing::Slice()}, in_transf(inputs.index({ jj, torch::indexing::Slice() })));
		}
		inps[0] = inputs;
		//evaluating
		torch::Tensor loc_trun_err = model.forward(inps).toTensor().detach();
		for (int i = 0; i < 5; i++) {
			loc_trun_err.index_put_({ i, torch::indexing::Slice() }, out_transf.inverse_transform(loc_trun_err.index({ i, torch::indexing::Slice() })));
		}
		return loc_trun_err;
	}
	//ODE function of discretized temperature (z)
	void temperature(double t, const double * x, const double* z, double* dzdt) {
		Eigen::Map<const Eigen::Matrix<value_type, N / 2, 1>> z_vector(z);
		Eigen::Map<Eigen::Matrix<value_type, N / 2, 1>> dzdt_vector(dzdt);

		Eigen::Matrix<value_type, N / 2, 1> De_x = D_E * z_vector; //derivative of z (dimless temperature)

		value_type rec_xR = 1.0 / x[0];
		value_type rec_xp = 1.0 / x[2];
		value_type dxdt2 = 3 * rec_xR * (C[10] * rec_xR * De_x[0] - gamma * x[1] * x[2]);

		//discretized PDE of bubble temperature
		dzdt_vector = De_x.cwiseProduct(x[1] * rec_xR * y - C[9] * rec_xR * rec_xR * rec_xp * De_x //this might show error, but it will not fail at compile time, valid syntax
			+ C[12] * rec_xp * dxdt2 * y)
			+ C[11] * rec_xp * dxdt2 * z_vector + C[9] * rec_xp * rec_xR * rec_xR * z_vector
			.cwiseProduct(y_sq.cwiseInverse()).cwiseProduct(D_O * (y_sq.cwiseProduct(De_x)));
		dzdt_vector[0] = 0.0; //Boundary condition

		return;
	}

	//ODE function of bubbledynamics (x). In the pointer x the values are rewritten with the computed slopes
	void operator()(double t, const double* x, const double * z, double* dxdt) {
		Eigen::Map<const Eigen::Matrix<value_type, N / 2, 1>> z_vector(z);
		value_type rec_xR = 1.0 / x[0];
		value_type rec_xp = 1.0 / x[2];

		//bubble pressure evolution
		dxdt[2] = 3 * rec_xR * (C[10] * rec_xR * (D_E * z_vector)[0] - gamma * x[1] * x[2]);

		//Keller-Miksis equation
		dxdt[0] = x[1];
		value_type sin2pit = sin(2 * M_PI * t);
		value_type den = x[0] - C[0] * x[0] * x[1] + C[1];
		value_type num = 0.5 * C[0] * x[1] * x[1] * x[1] - 1.5 * x[1] * x[1] - C[2] * x[1] * rec_xR - C[3] * rec_xR
			+ C[4] * x[2] - C[4] - C[5] * sin2pit + C[6] * x[1] * x[2] - C[6] * x[1] - C[7] * x[1] * sin2pit
			- C[8] * x[0] * cos(2 * M_PI * t) + C[6] * x[0] * dxdt[2];
		dxdt[1] = num / den;
	}
};

class BubbleSolver
{
public:

	BubbleSolver(int temperature_size) :z_size(temperature_size) {};

	bool setInitialConditions(const double* conds_x, const double * conds_z, const double at) {
		begin_t = at;
		x_init = (double*)malloc(sizeof(double) * x_size);
		for (int u = 0; u < x_size; u++) {
			x_init[u] = conds_x[u];
		}
		z_init = (double*)malloc(sizeof(double) * z_size);
		for (int u = 0; u < z_size; u++) {
			z_init[u] = conds_z[u];
		}
		return true;
	}
	void setMaxTime(double max_t) {
		t_max = max_t;
	}
	void setTolerances(double rel, double abs) {
		abs_tol = abs;
		rel_tol = rel;
	}

	void solve(BubDyn& sys, ostream& os) {

		double* x = (double*)malloc(sizeof(double) * x_size);
		for (int u = 0; u < x_size; u++) {
			x[u] = x_init[u];
		}
		double* z = (double*)malloc(sizeof(double) * z_size);
		for (int u = 0; u < z_size; u++) {
			z[u] = z_init[u];
		}

		//preparations
		double* z_stage = (double*)malloc(sizeof(double) * z_size);
		double* z_next = (double*)malloc(sizeof(double) * z_size);
		double* dzdt = (double*)malloc(sizeof(double) * z_size);
		double* x_stage = (double*)malloc(sizeof(double) * x_size);
		double* x_tmp = (double*)malloc(sizeof(double) * x_size);
		double* k1 = (double*)malloc(sizeof(double) * x_size);
		double* k2 = (double*)malloc(sizeof(double) * x_size);
		double* k3 = (double*)malloc(sizeof(double) * x_size);
		double* k4 = (double*)malloc(sizeof(double) * x_size);
		double* k5 = (double*)malloc(sizeof(double) * x_size);
		double t = begin_t;
		int l = 0;
		torch::Tensor errors;
		bool accept = true;
		bool nan_detect = false;
		value_type rel_err = 0.0;
		value_type coeff = 1.0;
		sys.stage_times = { 0.2, 0.3, 0.6, 1.0, 7.0 / 8.0 };
		z_stage[0] = 1.0; //fixed, boundary condition
		z_next[0] = 1.0; //same

		os << t;
		for (int i = 0; i < x_size; i++) {
			os << " " << x[i];
		}
		for (int i = 0; i < z_size; i++) {
			os << " " << z[i];
		}
		os << endl;

		if (t_max - t < delta_t) delta_t = t_max - t;

		while (t < t_max) {

			//DEM for temperature----------------------------------------------
			errors = sys.local_error(t, delta_t, x, z); //neural network
			sys.temperature(t, x, z, dzdt);
			//DOPRI for bubbleradius-------------------------------------------
			sys(t, x, z, k1);

			//k2
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 0.2 * delta_t * dzdt[j] +0.04 * delta_t * delta_t * errors[0][j - 1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + 0.2 * delta_t * k1[j];
			}
			sys(t + 0.2 * delta_t, x_stage, z_stage,  k2);

			//k3
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 0.3 * delta_t * dzdt[j] + 0.09 * delta_t * delta_t * errors[1][j-1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + 3.0 / 40.0 * delta_t * k1[j] + 9.0 / 40.0 * delta_t * k2[j];
			}
			sys(t + 0.3 * delta_t, x_stage, z_stage, k3); //k3

			//k4
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 0.6 * delta_t * dzdt[j] + 0.36 * delta_t * delta_t * errors[2][j-1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (0.3 * k1[j] - 0.9 * k2[j] + 6.0 / 5.0 * k3[j]);
			}
			sys(t + 0.6 * delta_t, x_stage, z_stage, k4);

			//k5
			for (int j = 1; j < z_size; j++) {
				z_next[j] = z[j] + delta_t * dzdt[j] + delta_t * delta_t * errors[3][j-1].item<double>();
				//this is the DEM solution
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (-11.0 / 54.0 * k1[j] + 5.0 / 2.0 * k2[j] - 70.0 / 27.0 * k3[j] + 35.0 / 27.0 * k4[j]);
			}
			sys(t + delta_t, x_stage, z_next, k5);

			//k6
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 7.0/8.0 * delta_t * dzdt[j] + 49.0/64.0 * delta_t * delta_t * errors[4][j-1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (1631.0 / 55296.0 * k1[j] + 175.0 / 512.0 * k2[j] + 575.0 / 13824.0 * k3[j] + 44275.0 / 110592.0 * k4[j] + 253.0 / 4096.0 * k5[j]);
			}
			sys(t + 7.0 / 8.0 * delta_t, x_stage, z_stage, k2); //k6

			//solution--------------------------------------------
			for (int j = 0; j < x_size; j++) {
				//Main solution:
				x_tmp[j] = x[j] + delta_t * (37.0 / 378.0 * k1[j] + 250.0 / 621.0 * k3[j] + 125.0 / 594.0 * k4[j] + 512.0 / 1771.0 * k2[j]); //k2=k6
				//main solution end
			}
			//secondary solution
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (2825.0 / 27648.0 * k1[j] + 18575.0 / 48384.0 * k3[j] + 13525.0 / 55296.0 * k4[j] + 277.0 / 14336.0 * k5[j] + 0.25 * k2[j]); //k2=k6
			}

			//error control---------------------------------------
			accept = true;
			nan_detect = false;
			rel_err = 0;
			for (int j = 0; j < x_size; j++) {
				if (!std::isfinite(x_tmp[j]) || !isfinite(x_stage[j])) {
					accept = false;
					std::cout << "NaN detected!" << std::endl;
					nan_detect = true;
					break;
				}
				k2[j] = distance(x_tmp[j], x_stage[j]); // local error
				k4[j] = abs_tol + std::fmax(std::fabs(x_tmp[j]), std::fabs(x[j])) * rel_tol; //tolerance
				if (k2[j] > k4[j]) {
					accept = false;
				}
				rel_err = std::fmax(rel_err, k2[j] / k4[j]);
			}
			/*if (std::isfinite(rel_err))
				for (int j = 1; j < z_size; j++) {
					if ( !std::isfinite(z_next[j]) ) {
						accept = false;
						std::cout << "NaN detected!" << std::endl;
						nan_detect = true;
						break;
					}
				}*/

			if (!accept) {
				//1/(q+1) = 1/5 = 0.2;
				coeff = safety_factor * std::pow(1.0 / rel_err, 0.2);
				if (!std::isfinite(coeff) || nan_detect) coeff = 0.1;
				delta_t = coeff * delta_t;
				//std::cout << "Not good\n";
				continue;
				//redo this step
			}

			//save------------------------------------------------
			t += delta_t;
			l++;
			coeff = safety_factor * std::pow(1.0 / rel_err, 0.2);
			if (!std::isfinite(coeff)) coeff = 0.1;
			else if (coeff < 0.1) coeff = 0.1;
			else if (coeff > 5.0) coeff = 5.0;

			if (t + coeff * delta_t > t_max)delta_t = t_max - t;
			else delta_t = coeff * delta_t;

			os << t;
			for (int i = 0; i < x_size; i++) {
				x[i] = x_tmp[i];
				os << " " << x[i];
			}
			for (int i = 0; i < z_size; i++) {
				z[i] = z_next[i];
				os << " " << z[i];
			}
			os << endl;
		}
		free(x); free(z);
		free(x_stage); free(x_tmp);
		free(dzdt);
		free(z_stage); free(z_next);
		free(k1); free(k2); free(k3); free(k4); free(k5);
	}

	~BubbleSolver() {
		free(x_init);
		free(z_init);
	}
private:
	const int x_size = 3;
	int z_size = 8;
	double* x_init = 0;
	double* z_init = 0;
	double begin_t = 0;
	double delta_t = 1e-4;
	double abs_tol = 1e-6;
	double rel_tol = 1e-6;
	const double safety_factor = 0.8;
	int t_max = 10;
	double distance(double a, double b) {
		if (a < b)return b - a;
		else return a - b;
	}
};

int main() {
	global_tensor_op = torch::TensorOptions().dtype(torch::kFloat64);
	std::cout << "BubbleDynamics with DEM started\n" << setprecision(17) << endl;

	ofstream ofs(file_name);
	if (!ofs.is_open()) {
		std::cout << "File could not be opened: " << file_name << endl;
		exit(-1);
	}
	ofs.precision(17);
	ofs.flags(ios::scientific);
	std::cout << "Writing file: " << file_name << endl;

	//initial conditions
	double* x = new double[3] {1.0, 0.0, 1.0 + 2.0 * sigma / (R_E * p_inf)};
	double* z = new double[N/2] {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
	double t_start = 0.0;
	std::cout << "Rarrrr" << endl;
	BubDyn bubi;

	BubbleSolver solver(N/2);
	solver.setInitialConditions(x, z, 0.0);
	solver.setTolerances(1e-8, 1e-8);
	solver.setMaxTime(5.0);

	cout << "Solving..." << endl;
	auto t1 = chrono::high_resolution_clock::now();
	solver.solve(bubi, ofs);
	auto t2 = chrono::high_resolution_clock::now();
	//Not valid measurement of DEM computational time. Just a slight indicator
	cout << "Time (ms):" << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << endl;

	ofs.flush();
	ofs.close();

	cout << "Ready" << endl;
	return 0;
}


#ifndef STOCHASTICCOLLOCATIONS_HPP_
#define STOCHASTICCOLLOCATIONS_HPP_

#include <mpi.h>
#include <boost/random.hpp>
#include <iostream>
#include <numeric>
#include <cmath>
#include <cstring>
#include <fstream>

 class StochasticCollocations
 {
 private:
    /* the Raynolds number will be of the form mean + stddev*sigma, where
    sigma is ~N(0,1) or ~U(0,1) */
    double u_gauss, s_gauss; // u_gauss = 0, s_gauss = 1;
    int u_uniform , s_uniform; // u_uniform = 0; s_uniform = 1;

    /* mersenne twister random number generator */
    boost::mt19937 rng;
    /* normal(Gaussian) distribution */
    boost::normal_distribution<> normal_distr;
    /* uniform distribution */
    boost::uniform_int<> uniform_distr;

    boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<> >* var_normal;

    boost::variate_generator<boost::mt19937&,
    boost::uniform_int<> >* var_uniform;

public:
    StochasticCollocations(double u_gauss, double s_gauss);
    StochasticCollocations(int u_uniform, int s_uniform);

    /** Uncertainty (i.e. Random variables) related methods **/

    /* generate nsamples samples of normal distributed random variables */
    std::vector<double> generate_nd_samples(double mean_nd, double sttdev_nd, int nsamples);
    /* generate nsamples samples of uniformly distributed random variables */
    std::vector<double> generate_ud_samples(double mean_ud, double sttdev_ud, int nsamples);

    /* compute the first two statistical moments */
    double compute_mean(const std::vector<double> &v) const;
    double compute_variance(const std::vector<double> &v, double mean) const;

    /* get a normal and uniform distributed RV */
    double get_normal() const;
    double get_uniform() const;

    double hermite_poly(int degree, double &var);
    void gauss_hermite_quad(int quad_degree, std::vector<double> &nodes, std::vector<double> &weights);
    std::vector<double> get_coefficiants(int quad_degree, int no_coeff, double mean, double stddev, std::vector<double> &nodes, std::vector<double> &weights);

    /***********************************************************/

    /** Parallelization related methods **/

    /* data decomposition among processes*/
    void data_decomposition(int* ncoeff, int* nprocs, int* coeff_per_proc);

    /* call the NS solver for each generated sample */
    void get_NS_solution(int* coeff_per_proc, const std::vector<double> &nodes, int rv_flag, int imax, int jmax);

    /* get the QoI (Quantities of interest - the desired output parameters, from a UQ point of view) */
    void get_QoI(int* coeff_per_proc, std::vector<double> &coeff);

    /***********************************************************/

    /* destructor */
    ~StochasticCollocations();
};

#endif /* MONTECARLO_HPP_ */


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "parametric_iir_coefficient_calculator.hpp"

#include "biquad_coefficient.hpp"
#include "parametric_iir_coefficient.hpp"

#include <libefl/db_linear_conversion.hpp>

#include <boost/math/constants/constants.hpp>

#include <cmath>

namespace visr
{
namespace rbbl
{

template< typename CoefficientType >
BiquadCoefficient<CoefficientType>
ParametricIirCoefficientCalculator::
calculateIirCoefficients( ParametricIirCoefficient< CoefficientType> const & param,
                                                    CoefficientType samplingFrequency )
{
  BiquadCoefficient<CoefficientType> res;
  calculateIirCoefficients( param, res, samplingFrequency );
  // Return value optimization avoids copy operation (normally)
  return res;
}

// Explicit instantiations
// Note: This code needs to be excluded from Doxygen documentation generation to avoid
// warnings about non-matching class members.
/// @cond NEVER
template VISR_RBBL_LIBRARY_SYMBOL
BiquadCoefficient<float> ParametricIirCoefficientCalculator::
calculateIirCoefficients<float>(ParametricIirCoefficient<float> const &, float);
template VISR_RBBL_LIBRARY_SYMBOL
BiquadCoefficient<double> ParametricIirCoefficientCalculator::
calculateIirCoefficients<double>(ParametricIirCoefficient<double> const &, double);
/// @endcond NEVER

template< typename T >
void ParametricIirCoefficientCalculator::
calculateIirCoefficients( ParametricIirCoefficient<T> const & param,
                          BiquadCoefficient<T> & coeffs,
                          T samplingFrequency )
{
  T const w0 = static_cast<T>(2.0) * boost::math::constants::pi<T>()*param.frequency() / samplingFrequency;
  T const alpha = std::sin( w0 ) / (static_cast<T>(2.0) * param.quality() );
  T const cw0 = std::cos( w0 );

  switch( param.type() )
  {
    case ParametricIirCoefficientBase::Type::lowpass:
    {
      // b0 = (1 - cos( w0 )) / 2
      // b1 = 1 - cos( w0 )
      // b2 = (1 - cos( w0 )) / 2
      // a0 = 1 + alpha
      // a1 = -2 * cos( w0 )
      // a2 = 1 - alpha
      T const a0 = static_cast<T>(1.0) + alpha;
      coeffs.b0() = (static_cast<T>(1.0) - cw0) / (static_cast<T>(2.0)*a0);
      coeffs.b1() = (static_cast<T>(1.0) - cw0) / a0;
      coeffs.b2() = coeffs.b0();
      coeffs.a1() = (static_cast<T>(-2.0)*cw0) / a0;
      coeffs.a2() = (static_cast<T>(1.0) - alpha) / a0;
      break;
    }
    case ParametricIirCoefficientBase::Type::highpass:
    {
      //b0 = (1 + cos( w0 )) / 2
      //b1 = -(1 + cos( w0 ))
      //b2 = (1 + cos( w0 )) / 2
      //a0 = 1 + alpha
      //a1 = -2 * cos( w0 )
      //a2 = 1 - alpha
      T const a0 = static_cast<T>(1.0) + alpha;
      coeffs.b0() = (static_cast<T>(1.0) + cw0) / (static_cast<T>(2.0)*a0);
      coeffs.b1() = -(static_cast<T>(1.0) + cw0) / a0;
      coeffs.b2() = coeffs.b0();
      coeffs.a1() = (static_cast<T>(-2.0)*cw0) / a0;
      coeffs.a2() = (static_cast<T>(1.0) - alpha) / a0;
      break;
    }
    case ParametricIirCoefficientBase::Type::bandpass:
    {
      // "Constant 0 dB gain" variant.
      // b0 = alpha
      //  b1 = 0
      //  b2 = -alpha
      //  a0 = 1 + alpha
      //  a1 = -2 * cos( w0 )
      //  a2 = 1 - alpha
      T const a0 = static_cast<T>(1.0) + alpha;
      coeffs.b0() = alpha / a0;
      coeffs.b1() = static_cast<T>(0.0);
      coeffs.b2() = -coeffs.b0();
      coeffs.a1() = (static_cast<T>(-2.0)*cw0) / a0;
      coeffs.a2() = (static_cast<T>(1.0) - alpha) / a0;
      break;
    }
    case ParametricIirCoefficientBase::Type::bandstop:
    {
      // b0 = 1
      //  b1 = -2 * cos( w0 )
      //  b2 = 1
      //  a0 = 1 + alpha
      //  a1 = -2 * cos( w0 )
      //  a2 = 1 - alpha
      T const a0 = static_cast<T>(1.0) + alpha;
      coeffs.b0() = static_cast<T>(1.0) / a0;
      coeffs.b1() = (static_cast<T>(-2.0) * cw0) / a0;
      coeffs.b2() = coeffs.b0();
      coeffs.a1() = (static_cast<T>(-2.0)*cw0) / a0;
      coeffs.a2() = (static_cast<T>(1.0) - alpha) / a0;
      break;
    }
    case ParametricIirCoefficientBase::Type::allpass:
    {
      // b0 = 1 - alpha
      // b1 = -2 * cos( w0 )
      // b2 = 1 + alpha
      // a0 = 1 + alpha
      // a1 = -2 * cos( w0 )
      // a2 = 1 - alpha
      T const a0 = static_cast<T>(1.0) + alpha;
      coeffs.b0() = (static_cast<T>(1.0) -alpha) / a0;
      coeffs.b1() = (static_cast<T>(-2.0) * cw0) / a0;
      coeffs.b2() = static_cast<T>(1.0); // the unnormalised b0 is the same as a0
      coeffs.a1() = coeffs.b1();
      coeffs.a2() = coeffs.b0();
      break;
    }
    case ParametricIirCoefficientBase::Type::peak:
    {
      // b0 = 1 + alpha*A
      // b1 = -2 * cos( w0 )
      // b2 = 1 - alpha*A
      // a0 = 1 + alpha / A
      // a1 = -2 * cos( w0 )
      // a2 = 1 - alpha / A
      T const A = std::sqrt( efl::dB2linear( param.gain() ) );
      T const a0 = static_cast<T>(1.0) + alpha/A;
      coeffs.b0() = (static_cast<T>(1.0) + A *alpha) / a0;
      coeffs.b1() = (static_cast<T>(-2.0)*cw0)/a0;
      coeffs.b2() = (static_cast<T>(1.0) - A *alpha) / a0;
      coeffs.a1() = coeffs.b1();
      coeffs.a2() = (static_cast<T>(1.0) - alpha/A) / a0;
      break;
    }
    case ParametricIirCoefficientBase::Type::lowshelf:
    {
      // b0 = A*((A + 1) - (A - 1)*cos( w0 ) + 2 * sqrt( A )*alpha)
      //  b1 = 2 * A*((A - 1) - (A + 1)*cos( w0 ))
      //  b2 = A*((A + 1) - (A - 1)*cos( w0 ) - 2 * sqrt( A )*alpha)
      //  a0 = (A + 1) + (A - 1)*cos( w0 ) + 2 * sqrt( A )*alpha
      //  a1 = -2 * ((A - 1) + (A + 1)*cos( w0 ))
      //  a2 = (A + 1) + (A - 1)*cos( w0 ) - 2 * sqrt( A )*alpha
      T const A = std::sqrt( efl::dB2linear( param.gain() ) );
      T const Asqrt = sqrt( A );
      T const a0 = (A + static_cast<T>(1.0)) + (A - static_cast<T>(1.0))*cw0 + static_cast<T>(2.0) * Asqrt*alpha;
      coeffs.b0() = A*((A + static_cast<T>(1.0)) - (A - static_cast<T>(1.0))*cw0 + static_cast<T>(2.0) * Asqrt*alpha)/a0 ;
      coeffs.b1() = (static_cast<T>(2.0) * A * ((A - static_cast<T>(1.0)) - (A + static_cast<T>(1.0)) * cw0)) / a0;
      coeffs.b2() = A*((A + static_cast<T>(1.0)) - (A - static_cast<T>(1.0))*cw0 - static_cast<T>(2.0) * Asqrt*alpha) / a0;
      coeffs.a1() = (static_cast<T>(-2.0) * ((A - static_cast<T>(1.0)) + (A + static_cast<T>(1.0)) * cw0)) / a0;
      coeffs.a2() = ((A + static_cast<T>(1.0)) + (A - static_cast<T>(1.0))*cw0 - static_cast<T>(2.0) * Asqrt*alpha) / a0;
      break;
    }
    case ParametricIirCoefficientBase::Type::highshelf:
    {
      //  b0 = A*((A + 1) + (A - 1)*cos( w0 ) + 2 * sqrt( A )*alpha)
      //  b1 = -2 * A*((A - 1) + (A + 1)*cos( w0 ))
      //  b2 = A*((A + 1) + (A - 1)*cos( w0 ) - 2 * sqrt( A )*alpha)
      //  a0 = (A + 1) - (A - 1)*cos( w0 ) + 2 * sqrt( A )*alpha
      //  a1 = 2 * ((A - 1) - (A + 1)*cos( w0 ))
      //  a2 = (A + 1) - (A - 1)*cos( w0 ) - 2 * sqrt( A )*alpha
      T const A = std::sqrt( efl::dB2linear( param.gain() ) );
      T const Asqrt = sqrt( A );
      T const a0 = (A + static_cast<T>(1.0)) - (A - static_cast<T>(1.0))*cw0 + static_cast<T>(2.0) * Asqrt*alpha;
      coeffs.b0() = A*((A + static_cast<T>(1.0)) + (A - static_cast<T>(1.0))*cw0 + static_cast<T>(2.0) * Asqrt*alpha) / a0;
      coeffs.b1() = (static_cast<T>(-2.0) * A * ((A - static_cast<T>(1.0)) + (A + static_cast<T>(1.0)) * cw0)) / a0;
      coeffs.b2() = A*((A + static_cast<T>(1.0)) + (A - static_cast<T>(1.0))*cw0 - static_cast<T>(2.0) * Asqrt*alpha) / a0;
      coeffs.a1() = (static_cast<T>(2.0) * ((A - static_cast<T>(1.0)) - (A + static_cast<T>(1.0)) * cw0)) / a0;
      coeffs.a2() = ((A + static_cast<T>(1.0)) - (A - static_cast<T>(1.0))*cw0 - static_cast<T>(2.0) * Asqrt*alpha) / a0;
      break;
    }
  }
}

// Explicit instantiations
// Note: This code needs to be excluded from Doxygen documentation generation to avoid
// warnings about non-matching class members.
/// @cond NEVER
template  VISR_RBBL_LIBRARY_SYMBOL
void ParametricIirCoefficientCalculator::
calculateIirCoefficients<float>( ParametricIirCoefficient<float> const &,
                                 BiquadCoefficient<float> &, float );
template  VISR_RBBL_LIBRARY_SYMBOL
void ParametricIirCoefficientCalculator::
calculateIirCoefficients<double>( ParametricIirCoefficient<double> const &,
                                  BiquadCoefficient<double> &, double );
/// @endcond NEVER

template< typename CoefficientType >
void ParametricIirCoefficientCalculator::calculateIirCoefficients( ParametricIirCoefficientList<CoefficientType> const & params,
                                                                   BiquadCoefficientList<CoefficientType> & coeffs,
                                                                   CoefficientType samplingFrequency )
{
  if( params.size() > coeffs.size() )
  {
    throw std::invalid_argument( "calculateIirCoefficients(): The output argument list \"coeffs\" holds less elements than the input list \"params\"." );
  }
  typename BiquadCoefficientList<CoefficientType>::iterator it = std::transform( params.begin(), params.end(), coeffs.begin(),
     [samplingFrequency]( ParametricIirCoefficient<CoefficientType> const & params ) { return calculateIirCoefficients<CoefficientType>( params, samplingFrequency ); } );
  // Fill the remaining entries in coeffs with default (flat) biquad parameters.
  std::fill( it, coeffs.end(), BiquadCoefficient<CoefficientType>() );
}

// Note: This code needs to be excluded from Doxygen documentation generation to avoid
// warnings about non-matching class members.
/// @cond NEVER
template VISR_RBBL_LIBRARY_SYMBOL
void ParametricIirCoefficientCalculator::
calculateIirCoefficients<float>( ParametricIirCoefficientList<float> const &, BiquadCoefficientList<float> &, float );
template VISR_RBBL_LIBRARY_SYMBOL
void ParametricIirCoefficientCalculator::
calculateIirCoefficients<double>( ParametricIirCoefficientList<double> const &, BiquadCoefficientList<double> &, double );
/// @endcond NEVER

} // namespace rbbl
} // namespace visr


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

using namespace std;
using namespace boost;
typedef property<edge_weight_t, int> EdgeWeightProperty;
typedef boost::adjacency_list<listS, vecS, undirectedS, no_property, EdgeWeightProperty> Graph;
typedef Graph::vertex_descriptor Vertex;
typedef Graph::edge_descriptor Edge;

int main() {
  Graph g;
  Vertex u = add_vertex(g);
  Vertex v = add_vertex(g);
  Vertex w = add_vertex(g);
  Vertex x = add_vertex(g);
  add_edge(u, v, 10, g);
  add_edge(u, w,  5, g);
  add_edge(u, x,  3, g);
  add_edge(v, w,  1, g);
  add_edge(v, x,  3, g);
  add_edge(w, x,  7, g);
  cout << "Number of edges: " << num_edges(g) << "\n";
  cout << "Number of vertices: " << num_vertices(g) << "\n";
  list<Edge> spanning_tree;
  kruskal_minimum_spanning_tree(g, back_inserter(spanning_tree));
  for (list<Edge>::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei) {
    cout << *ei << " ";
  }
  cout << "\n";
  return 0;
}


#include <vector>
#include <list>
#include <map>
#include <set>

#include <queue>
#include <deque>
#include <stack>
#include <bitset>
#include <algorithm>
#include <functional>
#include <numeric>
#include <utility>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <cstdio>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <cstring>
#include <assert.h>
#include <boost/lexical_cast.hpp>

#define INF 1023123123
#define EPS 1e-11
#define LSOne(S) (S & (-S))

#define M_PI           3.14159265358979323846  /* pi */

#define FORN(X,Y) for (int (X) = 0;(X) < (Y);++(X))
#define FORB(X,Y) for (int (X) = (Y);(X) >= 0;--(X))
#define REP(X,Y,Z) for (int (X) = (Y);(X) < (Z);++(X))
#define REPB(X,Y,Z) for (int (X) = (Y);(X) >= (Z);--(X))

#define SZ(Z) ((int)(Z).size())
#define ALL(W) (W).begin(), (W).end()
#define PB push_back

#define MP make_pair
#define A first
#define B second

#define FORIT(X,Y) for(typeof((Y).begin()) X = (Y).begin();X!=(Y).end();X++)

using namespace std;

typedef long long ll;
typedef double db;
typedef vector<int> vi;
typedef pair<int, int> ii;
typedef vector<ii> vii;

void print_case(int i)
{
   cout << "Case #" << i + 1 << ": ";
}

bool almost_equal(double a, double b)
{
   return abs(a - b) < EPS;
}

double calc_failure(vector<double> vc)
{
   double prob = 1.0;
   FORN(i, vc.size())
   {
      prob *= vc[i];
   }
   return prob;
}

vi failureTable(51);

int targetNumSuccesses = 0;
vector<double> successChances(50);
int numEvents = 50;
int totalNumEvents = 50;

void resetFailureTable()
{
   FORN(i, failureTable.size())
      failureTable[i] = 0;
}

void reverseSubList(vi& list, int front, int back)
{
   if (front > back)
      swap(front, back);
   reverse(list.begin() + front, list.begin() + back + 1);
}

void calcu()
{
   double numEventProbability = 0;
   double totalProbability = 0;
   bool exhausted = false;

   for (numEvents = 1; numEvents <= (totalNumEvents - targetNumSuccesses); numEvents++)
   {
      resetFailureTable();
      FORN(i, numEvents)
         failureTable[i] = 1;

      while (!exhausted)
      {
         double eventChainProbability = 1;
         REP(subEvent, 1, numEvents + 1)
         {
            if (failureTable[subEvent - 1])
               eventChainProbability = eventChainProbability*(1 - successChances[subEvent - 1]);
            else
               eventChainProbability = eventChainProbability*(successChances[subEvent - 1]);
         }
         numEventProbability = numEventProbability + eventChainProbability;

         for (int i = totalNumEvents - 1; i >= 0; i--)
         {
            if (failureTable[i] && !failureTable[i + 1])
            {
               int rightmost;
               for (int j = totalNumEvents - 1; j >= 0; j--) {
                  if (!failureTable[j]) {
                     rightmost = j; break;
                  }
               }

               failureTable[i] = false;
               failureTable[rightmost] = true;
               reverseSubList(failureTable, i + 1, totalNumEvents - 1);
               break;
            }
            else if (i == 1)
               exhausted = true;
         }
      }

      exhausted = false;
      printf("Odds of being exactly %d failures: %.2f", numEvents, numEventProbability);
      totalProbability = totalProbability + numEventProbability;
      numEventProbability = 0;
   }

   printf("Odds of at least %d successes: %f", targetNumSuccesses, 1 - totalProbability);
}

int main()
{
   /*numEvents = 2;
   totalNumEvents = 2;
   targetNumSuccesses = 1;
   successChances[0] = 0.4000;
   successChances[1] = 0.6000;
   calcu();*/

   cout.precision(12);
   int ntc;
   cin >> ntc;

   FORN(kk, ntc)
   {
      print_case(kk);
      int n, k;
      cin >> n >> k;
      double units;
      cin >> units;
      vector<double> cores(n + 1);
      cores[n] = 1.0;
      FORN(i, n)
      {
         cin >> cores[i];
      }

      sort(cores.begin(), cores.end());

      FORN(i, n)
      {
         int nrcoresequal = 1;
         REP(j, 1, n)
         {
            if (!almost_equal(cores[i], cores[j]))
            {
               break;
            }
            nrcoresequal++;
         }
         double nextcore = cores[nrcoresequal];
         double diff = nextcore - cores[0];
         double tospend = min(units, nrcoresequal * diff);
         FORN(j, nrcoresequal)
         {
            cores[j] += tospend / nrcoresequal;
         }
         units -= tospend;
      }

      cout << calc_failure(cores) << endl;
   }
}


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

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

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

// Optimizaiton
#include <ml/optimization/optimization_interface.hpp>
#include <ml/optimization/regularizer_interface.hpp>

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

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup regularizers Regularizers
 * \{
 */

/**
 * Interface for the regularizer (Scaled L2-norm)
 *
 *      f(x) = \sum_{i} lambda_i * x_i^2
 *
 */
class l2_norm : public smooth_regularizer_interface {

  protected:

    DenseVector lambda;                     /**< Penalty on the regularizer */
    size_t variables;                       /**< # Variables in the problem */

  public:

  /**
   * Default constructor.
   */
  l2_norm(const DenseVector& _lambda){
    lambda= _lambda;
    variables = _lambda.size();
  }

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

  /**
   * Compute the hessian of the regularizer at a given point.
   * \param[in]      point   Point at which we are computing the gradient.
   * \param[in,out]  hessian Diagonal matrix as the hessian gradient.
   *
   */
  inline void compute_hessian(const DenseVector &point, DiagonalMatrix
      &hessian) const {
    hessian = 2 * lambda.asDiagonal();
  }

  /**
   * Compute the function value of the regularizer at a given point.
   * \param[in]  point   Point at which we are computing the gradient.
   *
   */
  inline double compute_function_value(const DenseVector &point) const{
    DASSERT_EQ(variables, point.size());
    return lambda.dot(point.cwiseAbs2());
  }

  /**
   * Compute the gradient (or subgradient) at the given point.
   *
   * \param[in]  point    Point at which we are computing the gradient.
   * \param[out] gradient Dense gradient
   *
   */
  inline void compute_gradient(const DenseVector &point, DenseVector& gradient)
    const{
    DASSERT_EQ(variables, point.size());
    gradient = 2 * lambda.cwiseProduct(point);
  }

  /**
   * Compute the proximal operator for the l2-regularizer
   *
   * \param[in,out]  point      Point at which we are computing the gradient.
   * \param[in]      penalty    Penalty
   *
   * \note The proximal operator for lambda * ||x||^2 at the point v is
   * given by
   *                  v/(1 + 2*lambda*penalty)
   *
   */
  inline void apply_proximal_operator(DenseVector &point, const double&
      _penalty=0)const{
    DASSERT_EQ(variables, point.size());
    for(size_t i = 0; i < variables; i++)
      point[i] = point[i] / (1 + 2*_penalty*lambda[i]);
  }

};

/**
 * Interface for the regularizer (Scaled L1-norm)
 *
 *      f(x) = \sum_{i} lambda_i * |x_i|
 *
 */
class l1_norm : public regularizer_interface {

  protected:

    DenseVector lambda;                     /**< Penalty on the regularizer */
    size_t variables;                       /**< # Variables in the problem */

  public:

  /**
   * Default constructor.
   */
  l1_norm(const DenseVector& _lambda){
    lambda= _lambda;
    variables = _lambda.size();
  }

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

  /**
   * Compute the function value of the regularizer at a given point.
   * \param[in]  point   Point at which we are computing the gradient.
   *
   */
  inline double compute_function_value(const DenseVector &point) const{
    DASSERT_EQ(variables, point.size());
    return lambda.dot(point.cwiseAbs());
  }

  /**
   * Compute the subgradient at the given point.
   *
   * \param[in]  point    Point at which we are computing the gradient.
   * \param[out] gradient Dense sub-gradient
   *
   */
  inline void compute_gradient(const DenseVector &point, DenseVector& gradient) const{
    DASSERT_EQ(variables, point.size());
    gradient.setZero();
    for(size_t i = 0; i < variables; i++)
      if (gradient[i] > OPTIMIZATION_ZERO)
        gradient[i] = lambda[i];
      else if (gradient[i] < - OPTIMIZATION_ZERO)
        gradient[i] = - lambda[i];
  }

  /**
   * Compute the proximal operator for the l2-regularizer
   *
   * \param[in,out]  point      Point at which we are computing the gradient.
   * \param[in]      penalty    Penalty
   *
   * \note The proximal operator for lambda * ||x||_1 at the point v is
   * given by
   *        soft(x, lambda) = (x - lambda)_+ - (-x - lambda)_+
   *
   */
  inline void apply_proximal_operator(DenseVector &point, const double&
      _penalty=0)const{
    DASSERT_EQ(variables, point.size());
    for(size_t i = 0; i < variables; i++)
      point[i] = std::max(point[i] - _penalty*lambda[i], 0.0) -
                            std::max(-point[i] - _penalty*lambda[i], 0.0);
  }

};

/**
 * Interface for the elastic net regularizer (Scaled L1-norm)
 *
 *      f(x) = \sum_{i} alpha_i * |x_i| + \sum_{i} beta_i * x_i^2
 *
 */
class elastic_net : public regularizer_interface {

  protected:

    DenseVector alpha;                     /**< Penalty on the l1-regularizer */
    DenseVector beta;                      /**< Penalty on the l2-regularizer */
    size_t variables;                      /**< # Variables in the problem */

  public:

  /**
   * Default constructor.
   */
  elastic_net(const DenseVector& _alpha, const DenseVector& _beta){
    DASSERT_EQ(_alpha.size(), _beta.size());
    alpha = _alpha;
    beta = _beta;
    variables = _alpha.size();
  }

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

  /**
   * Compute the function value of the regularizer at a given point.
   * \param[in]  point   Point at which we are computing the gradient.
   *
   */
  inline double compute_function_value(const DenseVector &point) const{
    DASSERT_EQ(variables, point.size());
    return alpha.dot(point.cwiseAbs()) + beta.dot(point.cwiseAbs2());
  }

  /**
   * Compute the subgradient at the given point.
   *
   * \param[in]  point    Point at which we are computing the gradient.
   * \param[out] gradient Dense sub-gradient
   *
   */
  inline void compute_gradient(const DenseVector &point, DenseVector& gradient) const{
    DASSERT_EQ(variables, point.size());
    gradient = 2 * beta.cwiseProduct(point);
    for(size_t i = 0; i < variables; i++)
      if (gradient[i] > OPTIMIZATION_ZERO)
        gradient[i] += alpha[i];
      else if (gradient[i] < - OPTIMIZATION_ZERO)
        gradient[i] += -alpha[i];
  }

  /**
   * Compute the proximal operator for the elastic-regularizer
   *
   * \param[in,out]  point      Point at which we are computing the gradient.
   * \param[in]      penalty    Penalty
   *
   * \note The proximal operator for alpha||x||_1 + beta||x||_2^2 at
   *        y = soft(x, alpha) = (x - alpha)_+ - (-x - alpha)_+
   *        x = y / (1 + 2 beta)
   *
   * \note Do not swap the order.
   *
   */
  inline void apply_proximal_operator(DenseVector &point, const double&
      _penalty=0)const{
    DASSERT_EQ(variables, point.size());
    for(size_t i = 0; i < variables; i++){
      point[i] = std::max(point[i] - _penalty*alpha[i], 0.0) -
                            std::max(-point[i] - _penalty*alpha[i], 0.0);
      point[i] = point[i] / (1 + 2*_penalty*beta[i]);
    }
  }

};

/// \}

} // optimization
} // turicreate

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SCALAR_ELLINT_1_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SCALAR_ELLINT_1_HPP_INCLUDED
#include <nt2/toolbox/elliptic/functions/ellint_1.hpp>
#include <boost/math/special_functions.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/include/constants/infinites.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/sdk/error/policies.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is fundamental_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellint_1_, tag::cpu_
                            , (A0)
                            , (scalar_< fundamental_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type x = a0;
      if (x > One<result_type>()) return Nan<result_type>();
      if (x == One<result_type>())  return Inf<result_type>();
      if (is_eqz(x))      return Pio_2<result_type>();
      return boost::math::ellint_1(x, nt2_policy());
    }
  };
} }

#endif


#include "helper/bearing_vector.h"

#include "openvslam/type.h"
#include "openvslam/solver/essential_solver.h"
#include "openvslam/util/random_array.h"

#include <Eigen/Geometry>

#include <gtest/gtest.h>

using namespace openvslam;

TEST(essential_solver, check_E_solution_1) {
    // 3次元点を作成
    const unsigned int num_landmarks = 1000;
    const auto landmarks = create_random_landmarks(num_landmarks);

    // 姿勢を作成
    const Mat33_t rot_1 = Eigen::AngleAxisd(54.0 * M_PI / 180, Vec3_t(5, 3, -2).normalized()).toRotationMatrix();
    const Vec3_t trans_1 = Vec3_t(10.3, -1.6, 8.4);
    const Mat33_t rot_2 = Eigen::AngleAxisd(-21.0 * M_PI / 180, Vec3_t(-2, -5, 6).normalized()).toRotationMatrix();
    const Vec3_t trans_2 = Vec3_t(-5.4, 11.5, -24.6);

    // bearing vectorsを作成
    Mat33_t true_E_21;
    eigen_alloc_vector<Vec3_t> bearings_1;
    eigen_alloc_vector<Vec3_t> bearings_2;
    create_bearing_vectors(rot_1, trans_1, rot_2, trans_2, landmarks, true_E_21, bearings_1, bearings_2);

    // matchesを作成
    std::vector<std::pair<int, int>> matches_12(num_landmarks);
    for (unsigned int i = 0; i < num_landmarks; ++i) {
        matches_12.at(i).first = i;
        matches_12.at(i).second = i;
    }

    // E行列を求める
    solver::essential_solver solver(bearings_1, bearings_2, matches_12);
    solver.find_via_ransac(100);
    Mat33_t E_21 = solver.get_best_E_21();

    // スケールと正負を合わせる
    true_E_21 /= true_E_21.norm();
    E_21 /= E_21.norm();
    if (true_E_21.mean() * E_21.mean() < 0) {
        true_E_21 *= -1.0;
    }

    EXPECT_LT((true_E_21 - E_21).norm(), 1e-4);
}

TEST(essential_solver, check_E_solution_2) {
    // 3次元点を作成
    const unsigned int num_landmarks = 1000;
    const auto landmarks = create_random_landmarks(num_landmarks);

    // 姿勢を作成
    const Mat33_t rot_1 = Eigen::AngleAxisd(54.0 * M_PI / 180, Vec3_t(5, 3, -2).normalized()).toRotationMatrix();
    const Vec3_t trans_1 = Vec3_t(10.3, -1.6, 8.4);
    const Mat33_t rot_2 = Eigen::AngleAxisd(-21.0 * M_PI / 180, Vec3_t(-2, -5, 6).normalized()).toRotationMatrix();
    const Vec3_t trans_2 = Vec3_t(-5.4, 11.5, -24.6);

    // bearing vectorsを作成
    Mat33_t true_E_21;
    eigen_alloc_vector<Vec3_t> bearings_1;
    eigen_alloc_vector<Vec3_t> bearings_2;
    create_bearing_vectors(rot_1, trans_1, rot_2, trans_2, landmarks, true_E_21, bearings_1, bearings_2, 0.01);

    // matchesを作成
    std::vector<std::pair<int, int>> matches_12(num_landmarks);
    for (unsigned int i = 0; i < num_landmarks; ++i) {
        matches_12.at(i).first = i;
        matches_12.at(i).second = i;
    }

    // E行列を求める
    solver::essential_solver solver(bearings_1, bearings_2, matches_12);
    solver.find_via_ransac(100);
    Mat33_t E_21 = solver.get_best_E_21();

    // スケールと正負を合わせる
    true_E_21 /= true_E_21.norm();
    E_21 /= E_21.norm();
    if (true_E_21.mean() * E_21.mean() < 0) {
        true_E_21 *= -1.0;
    }

    EXPECT_LT((true_E_21 - E_21).norm(), 1e-1);
}


#include <iostream>
#include <ostream>
#include <fstream>
#include <igl/readOFF.h>
#include <Eigen/Core>
#include <vector>
#include <array>
#include "../external/dkm/include/dkm.hpp"
using namespace std;
using namespace Eigen;

MatrixXd V;
MatrixXi F;

#define N_VIEWS 102

void eigen2std(const MatrixXd &mat, vector<array<double, 3>> &vec)
{
    for (size_t i = 0; i < mat.rows(); i++)
    {
        array<double, 3> line;
        for (size_t j = 0; j < mat.cols(); j++)
        {
            line[j] = mat(i, j);
        }
        vec.push_back(line);
    }
}

int main(int argc, char *argv[])
{
    igl::readOFF("../sphere.off", V, F);

    int nPoints = V.rows(), dim = V.cols();
    vector<array<double,3> > points;
    eigen2std(V, points);
    auto res = dkm::kmeans_lloyd(points, N_VIEWS);
    auto centroids = get<0>(res);

    ofstream cameraFile;
    cameraFile.open("../camera_position.xy");
    for (size_t i = 0; i < N_VIEWS; i++)
    {
        for (size_t j = 0; j < dim; j++)
        {
            cameraFile << centroids[i][j] << " ";
        }
        cameraFile << endl;
    }
    cameraFile.close();
    return 0;
}


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

#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

typedef boost::array< double , 3 > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

void write_lorenz( const state_type &x , const double t )
{
    cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << x[2] << endl;
}

int main(int argc, char **argv)
{
    state_type x = {{ 10.0 , 1.0 , 1.0 }}; // initial conditions
    integrate( lorenz , x , 0.0 , 25.0 , 0.1 , write_lorenz );
}


#include <vector>
#include <tuple>
#include <cmath>
#include <numeric>
#include <Eigen/Dense>
#include "../happly.h"
#include "../nanoflann.hpp"
#include "utils.hpp"

float icp::mse_cost(std::vector<std::tuple<float, int, int>>& distances, float xi)
{
    size_t Np = distances.size();
    size_t Npo = xi*Np;

    float Sts = std::accumulate(distances.begin(), distances.begin() + Npo, 0, tupleAccumulateOP);
    xi = (float)std::pow(xi, 3);
    float e = Sts/Npo;

    return e/xi;
}

bool icp::comparePairs(const std::tuple<float, int, int>& lhs, const std::tuple<float, int, int>& rhs)
{
    return std::get<0>(lhs) < std::get<0>(rhs);
}

float icp::tupleAccumulateOP(const float &a, const std::tuple<float, int, int> &b)
{
    return a+std::get<0>(b);
}

icp::PointCloud icp::readPointCloud(const std::string filename)
{
    //"../data/fountain_a.ply"
    happly::PLYData plyData(filename);
    // auto propsNames = plyData.getElement("vertex").getPropertyNames();
    std::vector<std::array<double, 3>> vPos = plyData.getVertexPositions();
    size_t dim = 3;
    size_t N = vPos.size();
    icp::PointCloud mat(N, dim);

    mat.resize(N, dim);
    for (size_t i = 0; i < N; i++)
        for (size_t d = 0; d < dim; d++)
            mat(i, d) = vPos[i][d];
    // for(const auto& prop: propsNames)
    //     std::cout<<prop<<std::endl;
    return mat;
}

icp::PointCloud icp::addNoise(icp::PointCloud M)
{
    // get dimentions
    size_t rows = M.rows();
    size_t cols = M.cols();

    return icp::PointCloud::Random(rows, cols);
}

// rotation
Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> icp::rotate(icp::PointCloud& M, float deg, Eigen::Vector3f axis)
{
    // float deg = 10.f;
    float rad = deg * M_PI / 180;
    // Eigen::Vector3f axis(0,1,1);
    Eigen::Transform<float, 3, Eigen::Affine> t;
    Eigen::AngleAxis<float> rot(rad, axis);
    t = rot;

    // std::cout<<t.linear()<<std::endl;
    // std::cout<<t.linear().inverse()<<std::endl;
    // std::cout<<t.linear().inverse().matrix()<<std::endl;
    M = (t.linear() * M.transpose()).transpose();
    return t.linear().matrix();
}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/pybind11.h>

#include <boost/geometry/geometries/register/point.hpp>

#include "pyinterp/detail/geometry/point.hpp"
#include "pyinterp/geodetic/algorithm.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp::geodetic {

// Handle a point in a equatorial spherical coordinates system in degrees.
class Point : public detail::geometry::GeographicPoint2D<double> {
 public:
  /// Default constructor
  Point() noexcept = default;

  /// Build a new point with the coordinates provided.
  Point(const double lon, const double lat)
      : detail::geometry::GeographicPoint2D<double>(lon, lat) {}

  /// Get longitude value in degrees
  [[nodiscard]] inline auto lon() const -> double { return this->get<0>(); }

  /// Get latitude value in degrees
  [[nodiscard]] inline auto lat() const -> double { return this->get<1>(); }

  /// Set longitude value in degrees
  inline auto lon(double const v) -> void { this->set<0>(v); }

  /// Set latitude value in degrees
  inline auto lat(double const v) -> void { this->set<1>(v); }

  /// Calculate the distance between the two points
  [[nodiscard]] auto distance(const Point& other,
                              const DistanceStrategy strategy,
                              const std::optional<System>& wgs) const
      -> double {
    return geodetic::distance(*this, other, strategy, wgs);
  }

  /// Get a tuple that fully encodes the state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    return pybind11::make_tuple(lon(), lat());
  }

  /// Create a new instance from a registered state of an instance of this
  /// object.
  static auto setstate(const pybind11::tuple& state) -> Point {
    if (state.size() != 2) {
      throw std::runtime_error("invalid state");
    }
    return {state[0].cast<double>(), state[1].cast<double>()};
  }

  /// Converts a Point into a string with the same meaning as that of this
  /// instance.
  [[nodiscard]] auto to_string() const -> std::string {
    std::stringstream ss;
    ss << boost::geometry::dsv(*this);
    return ss.str();
  }

  /// Returns true if the tow points are equal.
  auto operator==(const Point& other) const -> bool {
    return boost::geometry::equals(*this, other);
  }
};

}  // namespace pyinterp::geodetic

// BOOST specialization to accept pyinterp::geodectic::Point as a geometry
// entity
namespace boost::geometry::traits {

namespace pg = pyinterp::geodetic;

/// Coordinate tag
template <>
struct tag<pg::Point> {
  /// Typedef for type
  using type = point_tag;
};

/// Coordinate type
template <>
struct coordinate_type<pg::Point> {
  /// Typedef for type
  using type = double;
};

/// Coordinate system
template <>
struct coordinate_system<pg::Point> {
  /// Typedef for type
  using type = cs::geographic<degree>;
};

template <>
struct dimension<pg::Point> : boost::mpl::int_<2> {};

/// access struct defining with Cartesian
template <std::size_t I>
struct access<pg::Point, I> {
  /// Accessor to pointer.
  static auto get(pg::Point const& p) -> double { return p.template get<I>(); }

  /// Pointer setter.
  static void set(pg::Point& p, double const& v) {  // NOLINT
    p.template set<I>(v);
  }
};

}  // namespace boost::geometry::traits


/**
 * @date Fri Jan 27 14:10:23 2012 +0100
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <algorithm>
#include <boost/shared_array.hpp>

#include <bob.math/lu.h>

#include <bob.core/assert.h>
#include <bob.core/array_copy.h>

// Declaration of the external LAPACK functions
// LU decomposition of a general matrix (dgetrf)
extern "C" void dgetrf_( const int *M, const int *N, double *A,
  const int *lda, int *ipiv, int *info);
// Cholesky decomposition of a real symmetric definite-positive matrix (dpotrf)
extern "C" void dpotrf_( const char *uplo, const int *N, double *A,
  const int *lda, int *info);

void bob::math::lu(const blitz::Array<double,2>& A, blitz::Array<double,2>& L,
  blitz::Array<double,2>& U, blitz::Array<double,2>& P)
{
  // Size variable
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int minMN = std::min(M,N);

  // Check
  const blitz::TinyVector<int,2> shapeL(M,minMN);
  const blitz::TinyVector<int,2> shapeU(minMN,N);
  const blitz::TinyVector<int,2> shapeP(minMN,minMN);

  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(L);
  bob::core::array::assertZeroBase(U);
  bob::core::array::assertZeroBase(P);

  bob::core::array::assertSameShape(L,shapeL);
  bob::core::array::assertSameShape(U,shapeU);
  bob::core::array::assertSameShape(P,shapeP);

  bob::math::lu_(A, L, U, P);
}

void bob::math::lu_(const blitz::Array<double,2>& A, blitz::Array<double,2>& L,
  blitz::Array<double,2>& U, blitz::Array<double,2>& P)
{
  // Size variable
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int minMN = std::min(M,N);

  // Prepares to call LAPACK function

  // Initialises LAPACK variables
  int info = 0;
  const int lda = M;

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double *A_lapack = A_blitz_lapack.data();
  boost::shared_array<int> ipiv(new int[minMN]);

  // Calls the LAPACK function
  dgetrf_( &M, &N, A_lapack, &lda, ipiv.get(), &info);

  // Checks info variable
  // If U is greater than zero, this means that the U matrix is equal to zero.
  if (info < 0)
    throw std::runtime_error("The LAPACK dgetrf function returned a negative value.");

  // Copy result back to L and U
  blitz::firstIndex bi;
  blitz::secondIndex bj;
  blitz::Array<double,2> A_blitz_lapack_t = A_blitz_lapack.transpose(1,0);
  blitz::Range rall = blitz::Range::all();
  L = blitz::where(bi>bj, A_blitz_lapack_t(rall,blitz::Range(0,minMN-1)), 0.);
  L = blitz::where(bi==bj, 1., L);
  U = blitz::where(bi<=bj, A_blitz_lapack_t(blitz::Range(0,minMN-1),rall), 0.);

  // Converts weird permutation format returned by LAPACK into a permutation
  // function
  blitz::Array<int,1> Pp(minMN);
  Pp = bi;
  int temp;
  for (int i=0; i<minMN-1; ++i)
  {
    temp = Pp(ipiv[i]-1);
    Pp(ipiv[i]-1) = Pp(i);
    Pp(i) = temp;
  }
  // Updates P
  P = 0.;
  for (int j = 0; j<minMN; ++j)
    P(j,Pp(j)) = 1.;
}

void bob::math::chol(const blitz::Array<double,2>& A,
  blitz::Array<double,2>& L)
{
  // Size variable
  const int M = A.extent(0);
  const int N = A.extent(1);

  // Check
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(L);
  bob::core::array::assertSameDimensionLength(M,N);
  bob::core::array::assertSameShape(A,L);

  bob::math::chol_(A, L);
}

void bob::math::chol_(const blitz::Array<double,2>& A,
  blitz::Array<double,2>& L)
{
  // Size variable
  const int N = A.extent(0);

  // Prepares to call LAPACK function
  // Initialises LAPACK variables
  int info = 0;
  const int lda = N;
  const char uplo = 'L';

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack;
  // Tries to use V directly
  blitz::Array<double,2> Lt = L.transpose(1,0);
  const bool Lt_direct_use = bob::core::array::isCZeroBaseContiguous(Lt);
  if (Lt_direct_use)
  {
    A_blitz_lapack.reference(Lt);
    A_blitz_lapack = A;
  }
  else
    A_blitz_lapack.reference(bob::core::array::ccopy(A));
  double *A_lapack = A_blitz_lapack.data();

  // Calls the LAPACK function
  dpotrf_( &uplo, &N, A_lapack, &lda, &info);

  // Checks info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK dpotrf function returned a non-zero value.");

  // Copy result back to L if required
  if (!Lt_direct_use)
    Lt = A_blitz_lapack;

  // Sets strictly upper triangular part to 0
  blitz::firstIndex i;
  blitz::secondIndex j;
  L = blitz::where(i < j, 0, L);
}


#include <iostream>
#include <cmath>
#include <complex>
#include <type_traits>

#include <boost/numeric/mtl/mtl.hpp>

namespace tst {

    template <typename T>
    struct is_matrix
      : std::false_type
    {};

    template <typename Value, typename Para>
    struct is_matrix<mtl::dense2D<Value, Para> >
      : std::true_type
    {};

    template <typename T>
    struct is_vector
      : std::false_type
    {};

    template <typename Value, typename Para>
    struct is_vector<mtl::dense_vector<Value, Para> >
      : std::true_type
    {};

    template <typename T>
    struct Magnitude
    {
	using type= T;
    };

    template <typename T>
    struct Magnitude<std::complex<T> >
    {
	using type= T;
    };

    template <typename T, typename Para>
    struct Magnitude<mtl::dense_vector<T, Para> >
    {
	using type= typename Magnitude<T>::type;
    };

    template <typename T, typename Para>
    struct Magnitude<mtl::dense2D<T, Para> >
    {
	using type= typename Magnitude<T>::type;
    };

    template <typename T>
    using Magnitude_t= typename Magnitude<T>::type;

    template <bool Cond, typename T= void>
    using enable_if_t= typename std::enable_if<Cond, T>::type;

    template <typename T>
    enable_if_t<is_matrix<T>::value, Magnitude_t<T>>
    inline one_norm(const T& A)
    {
	using std::abs;
	Magnitude_t<T> max{0};
	for (unsigned c= 0; c < num_cols(A); c++) {
	    Magnitude_t<T> sum{0};
	    for (unsigned r= 0; r < num_cols(A); r++)
		sum+= abs(A[r][c]);
	    max= max < sum ? sum : max;
	}
	return max;
    }

    template <typename T>
    enable_if_t<is_vector<T>::value, Magnitude_t<T>>
    inline one_norm(const T& v)
    {
	using std::abs;
	Magnitude_t<T> sum{0};
	for (unsigned r= 0; r < size(v); r++)
	    sum+= abs(v[r]);
	return sum;
    }

}

int main (int argc, char* argv[])
{
    mtl::dense2D<float> A= {{2, 3, 4},
			    {5, 6, 7},
			    {8, 9, 10}};
    mtl::dense_vector<float> v{3, 4, 5};

    std::cout << "one_norm(A) is " << tst::one_norm(A) << "\n";
    std::cout << "one_norm(v) is " << tst::one_norm(v) << "\n";

    // std::cout << "one_norm(3.5) is " << tst::one_norm(3.5) << "\n";

    return 0 ;

}


#include <Eigen/Core>
#include <iostream>
#include "timer.h"

#include <Spectra/SymEigsSolver.h>
#include <Spectra/GenEigsSolver.h>

using namespace Spectra;

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::MatrixXcd;
using Eigen::VectorXcd;

void eigs_sym_Cpp(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err, int &nops)
{
    double start, end;
    start = get_wall_time();

    DenseSymMatProd<double> op(M);
    SymEigsSolver<double, LARGEST_MAGN, DenseSymMatProd<double> > eigs(&op, k, m);
    eigs.init(init_resid.data());

    int nconv = eigs.compute();
    int niter = eigs.num_iterations();
    nops = eigs.num_operations();

    VectorXd evals = eigs.eigenvalues();
    MatrixXd evecs = eigs.eigenvectors();

    /* std::cout << "computed eigenvalues D = \n" << evals.transpose() << std::endl;
    std::cout << "first 5 rows of computed eigenvectors U = \n" << evecs.topRows<5>() << std::endl;
    std::cout << "nconv = " << nconv << std::endl;
    std::cout << "niter = " << niter << std::endl;
    std::cout << "nops = " << nops << std::endl; */

    end = get_wall_time();
    time_used = (end - start) * 1000;

    MatrixXd err = M * evecs - evecs * evals.asDiagonal();
    prec_err = err.cwiseAbs().maxCoeff();
}

void eigs_gen_Cpp(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err, int &nops)
{
    double start, end;
    start = get_wall_time();

    DenseGenMatProd<double> op(M);
    GenEigsSolver<double, LARGEST_MAGN, DenseGenMatProd<double> > eigs(&op, k, m);
    eigs.init(init_resid.data());

    int nconv = eigs.compute();
    int niter = eigs.num_iterations();
    nops = eigs.num_operations();

    VectorXcd evals = eigs.eigenvalues();
    MatrixXcd evecs = eigs.eigenvectors();

    /* std::cout << "computed eigenvalues D = \n" << evals.transpose() << std::endl;
    std::cout << "first 5 rows of computed eigenvectors U = \n" << evecs.topRows<5>() << std::endl;
    std::cout << "nconv = " << nconv << std::endl;
    std::cout << "niter = " << niter << std::endl;
    std::cout << "nops = " << nops << std::endl;

    MatrixXcd err = M * evecs - evecs * evals.asDiagonal();
    std::cout << "||AU - UD||_inf = " << err.array().abs().maxCoeff() << std::endl; */

    end = get_wall_time();
    time_used = (end - start) * 1000;

    MatrixXcd err = M * evecs - evecs * evals.asDiagonal();
    prec_err = err.cwiseAbs().maxCoeff();
}


/*
 * main.cpp
 *
 *  Created on: Dec 10, 2021
 *      Author: tiba
 */

//

#define _USE_MATH_DEFINES

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <vector>
#include <Eigen/StdVector>
#include <fstream>
#include <iterator>
#include "STRUC.h"
#include "Mesh.h"
#include "Fluid.h"
#include "FSI.h"
#include "config.h"

using namespace Eigen;
using namespace std;

properties load_ppts()
{
	properties ppts;
	float a, b, c, interm;

	ppts.Coeff = coeff; // Fraction of natural structural period, giving the total period of simulation

	ppts.L_0 = 1; // Initial Gas Chamber Length (not including the initial displacement)
	ppts.A = 1;	  // Section

	ppts.U_0 = .2; // Initial displacement
	ppts.L_t = ppts.L_0 + ppts.U_0;

	ppts.gam = 1.4; // the specific heat ratio of the gas
	ppts.gamm1 = ppts.gam - 1.;
	ppts.R = 287;					// the individual gas constant
	ppts.C_v = ppts.R / ppts.gamm1; // the specific heat capacity of the gas

	ppts.pres_init0 = 1E5;														// initial pressure for chamber length = L0
	ppts.temp_init0 = 300;														// initial temperature
	ppts.rho_init0 = ppts.pres_init0 / ppts.gamm1 / ppts.C_v / ppts.temp_init0; // initial volumic mass

	ppts.pres_init = ppts.pres_init0 * pow((ppts.L_0 / ppts.L_t), ppts.gam);

	ppts.rho_init = ppts.rho_init0 * pow((ppts.pres_init / ppts.pres_init0), (1. / ppts.gam));
	ppts.temp_init = ppts.pres_init / ppts.rho_init / ppts.gamm1 / ppts.C_v;
	ppts.p_ext = 0 * ppts.pres_init0; // pressure on the right of the piston

	// we set the initial fluid velocity and the initial total fluid energy
	ppts.u_init = 0.;
	ppts.e_init = ppts.pres_init / ppts.gamm1 / ppts.rho_init + 0.5 * pow(ppts.u_init, 2.);

	ppts.vprel.push_back(1e7);	// Spring rigidity
	ppts.vprel.push_back(mass); // Spring mass
	ppts.spring_model = "linear";
	ppts.nln_order = 3;

	ppts.Lsp0 = 1.2; // Unstretched spring length
	if (ppts.spring_model == "nonlinear")
	{
		ppts.umax = 0.2; // Maximum spring displacements for linear spring model ('C' Model)
		ppts.mu = mu_coeff * ppts.vprel[0] / ppts.umax;
		if (ppts.nln_order == 2)
		{
			ppts.u0 = (-ppts.vprel[0] + sqrt(pow(ppts.vprel[0], 2) + 4 * ppts.mu * ppts.A * ppts.pres_init0)) / (-2 * ppts.mu);
		}
		else
		{
			a = ppts.vprel[0];
			b = ppts.mu;
			c = ppts.A * ppts.pres_init0;
			interm = pow((((std::sqrt((27 * b * pow(c, 2) + 4 * pow(a, 3)) / b)) / (b * 2 * pow(3, (3. / 2.)))) - c / (2 * b)), (1. / 3.));
			ppts.u0 = interm - a / (3 * b * interm);
		}
		ppts.Lspe = ppts.Lsp0 + ppts.u0;
	}
	else
	{
		ppts.Lspe = ppts.Lsp0 - (ppts.pres_init0 - ppts.p_ext) * ppts.A / ppts.vprel[0]; // initial spring length
	}

	return ppts;
}

int main()
{

	// Geometrical and physical properties
	properties ppts;
	ppts = load_ppts();

	// Create the mesh
	int nnt = nmesh;
	Mesh mesh_n;
	mesh_n.load(nnt, ppts.L_t);

	// Create the fluid FEM model
	Fluid fluid_model(ppts);
	fluid_model.initialize(mesh_n);

	// Create the structure FEM model
	STRUC structure_model(ppts);
	structure_model.initialize(fluid_model.get_vpres()(nnt - 1));

	// Create the fluid-strucure interaction coupling
	FSI fsi_piston(structure_model.T0);

	// Solve the problem
	fsi_piston.solve(structure_model, fluid_model);

	// Export the results into .txt files
	fsi_piston.export_results();

	return 0;
}


#include <cstdio>

#include <cctbx/sgtbx/direct_space_asu/proto/direct_space_asu.h>
#include <boost/shared_ptr.hpp> // necessary for pair_tables.h
#include <cctbx/crystal/pair_tables.h>

// @@@@@@@@
// This file is a translation of find_distances_using_cpp_objects.py into C++
// @@@@@@@@

namespace cc {
  using namespace cctbx::sgtbx;
  using namespace cctbx::uctbx;
  using namespace cctbx::sgtbx::asu;
  using namespace cctbx::crystal::direct_space_asu;
  using cctbx::crystal::pair_asu_table;
  using cctbx::crystal::pair_sym_table;
  using cctbx::crystal::pair_sym_dict;
  using cctbx::crystal::pair_sym_ops;
  typedef scitbx::vec3<double> double3;
};

void find_distances(
  const cc::unit_cell &unit_cell,
  const cc::space_group &space_group,
  const scitbx::af::const_ref< cc::double3 > &sites_frac,
  double distance_cutoff)
{
  cc::direct_space_asu metric_free_asu( space_group.type() );
  cc::float_asu<> float_asu = metric_free_asu.as_float_asu(unit_cell, 1.0E-6);
  boost::shared_ptr< cc::asu_mappings<> > asu_mappings(
    new cc::asu_mappings<> (space_group, float_asu, distance_cutoff) );
  asu_mappings->process_sites_frac(sites_frac, 0.5);
  cc::pair_asu_table<> pair_asu_table(asu_mappings);
  pair_asu_table.add_all_pairs(distance_cutoff);
  cc::pair_sym_table pair_sym_table = pair_asu_table.extract_pair_sym_table();
  for(unsigned i=0; i<pair_sym_table.size(); ++i) // af::shared  array
  {
    std::printf("i: %d\n", i);
    const cctbx::fractional<> frac_i = sites_frac[i];
    const cc::pair_sym_dict pair_sym_dict = pair_sym_table[i];
    for(cc::pair_sym_dict::const_iterator pair=pair_sym_dict.begin();
      pair!=pair_sym_dict.end(); ++pair ) // std::map
    {
      const int j = pair->first;
      std::printf("  j: %d\n", j);
      const cctbx::fractional<> frac_j = sites_frac[j];
      const cc::pair_sym_ops sym_ops = pair->second;
      for(cc::pair_sym_ops::const_iterator sym_op = sym_ops.begin();
        sym_op!=sym_ops.end(); ++sym_op) // std::vector
      {
        const cctbx::fractional<> frac_ji = (*sym_op) * frac_j;
          std::printf("    %-20s %8.3f\n", sym_op->as_xyz().c_str(),
            unit_cell.distance(frac_i, frac_ji));
      }
    }
  }
}

int main(int argc, char *[])
{
  CCTBX_ASSERT( argc==1 );
  scitbx::af::double6 cell(5.01, 5.01, 5.47, 90.0, 90.0, 120.0);
  cc::unit_cell unit_cell(cell);
  cc::space_group space_group(cctbx::sgtbx::space_group_symbols("P6222"));
  scitbx::af::shared< cc::double3 > sites_frac;
  sites_frac.push_back( cc::double3(0.5, 0.5, 1.0/3.0) );
  sites_frac.push_back( cc::double3(0.197, -0.197, 0.83333) );
  find_distances(unit_cell, space_group, sites_frac.const_ref(), 5.0);
  return 0;
}