/**
 * @file GaussianHyperparameters.cpp
 * @author Jan Nguyen
 * @date 11.05.20
 */

#include "GaussianHyperparameters.h"

#include <Eigen/Cholesky>

#include "GaussianProcess.h"

void autopas::GaussianHyperparameters::precalculate(double sigma, const std::vector<Eigen::VectorXd> &inputs,
                                                    const Eigen::VectorXd &outputs) {
  size_t size = outputs.size();
  // mean of output shifted to zero
  Eigen::VectorXd outputCentered = outputs - mean * Eigen::VectorXd::Ones(size);

  Eigen::MatrixXd covMat(size, size);
  // calculate covariance matrix
  for (size_t i = 0; i < size; ++i) {
    covMat(i, i) = GaussianProcess::kernel(inputs[i], inputs[i], theta, dimScales) + sigma;
    for (size_t j = i + 1; j < size; ++j) {
      covMat(i, j) = covMat(j, i) = GaussianProcess::kernel(inputs[i], inputs[j], theta, dimScales);
    }
  }

  // cholesky decomposition
  Eigen::LLT<Eigen::MatrixXd> llt = covMat.llt();
  Eigen::MatrixXd l = llt.matrixL();

  // precalculate inverse of covMat and weights for predictions
  covMatInv = llt.solve(Eigen::MatrixXd::Identity(size, size));
  weights = covMatInv * outputCentered;

  // likelihood of evidence given parameters
  score = std::exp(-0.5 * outputCentered.dot(weights)) / l.diagonal().prod();

  if (std::isnan(score)) {
    // error score calculation failed
    utils::ExceptionHandler::exception("GaussianProcess: invalid score ", score);
  }
}


/*
 * KolmogorovComputer.cpp
 *
 *  Created on: Jan 28, 2014
 *      Author: jan
 *
 *      This class handles everything that deals with the Kolmogorov distance or Kolmogorov function. In particular this class provides the number of needed
 *      simulations S for each provided tolerance epsilon, and the other way around. It also computes the distance between a computed trajectory and the original data.
 *      The parameters beta  comes from: d(Y_S, X_M) < epsilon ==> P(d(Y, X) > epsilon) <= beta for further details see Lillacci & Khammash 2013
 *      The parameter kappa is the computed tolerance corresponding to the number of samples in the original dataset
 */

#include "KolmogorovComputer.h"

#include <cmath>
#include <iostream>
#include <cstdlib>

#include <Eigen/Dense>
#include <boost/math/tools/roots.hpp>

#include "IllegalArgumentException.h"

namespace INSIGHTv3 {

// Constructor for approximated kappa
KolmogorovComputer::KolmogorovComputer(
		const std::vector<EiVector>& original_data, double beta,
		double tolerance) {

	_kappa = getKappaForMApprox(original_data[0].size(), beta);
	_beta = beta;
	_tolerance = tolerance;
	_init(original_data);
}

// Constructor for exact kappa
KolmogorovComputer::KolmogorovComputer(double kappa_tolerance,
		const std::vector<EiVector>& original_data, double beta,
		double tolerance) {

	std::cout
			<< "Exact kappa for the KolmovorovComputer is being computed. M is "
			<< original_data[0].size()
			<< ". The tolerance for the kolmogorov function root solver is "
			<< kappa_tolerance
			<< ". This may take a long time. Consider using one of the other constructors that use a precomputed kappa, or compute kappa only approximately"
			<< std::endl;
	_kappa = getKappaForM(original_data[0].size(), beta, kappa_tolerance);
	_beta = beta;
	_tolerance = tolerance;
	_init(original_data);
}

// Constructor for precomputed kappa
KolmogorovComputer::KolmogorovComputer(
		const std::vector<EiVector>& original_data, double beta,
		double tolerance, double kappa) {

	_kappa = kappa;
	_beta = beta;
	_tolerance = tolerance;
	_init(original_data);
}

KolmogorovComputer::~KolmogorovComputer() {
}

void KolmogorovComputer::_init(const std::vector<EiVector>& original_data) {
	for (size_t column = 0; column < original_data.size(); column++) {
		EiVector single_column = original_data[column];
		std::sort(single_column.data(),
				single_column.data() + single_column.size());
		_sorted_Data.push_back(single_column);
	}

}

double KolmogorovComputer::distTwoSample(const EiVector& first_sample,
		const size_t column_of_second_sample) {

	size_t j1 = 0, j2 = 0;
	double distance = 0.0, d1, d2, dt, fn1 = 0.0, n1 =
			(double) first_sample.size(), fn2 = 0.0, n2 =
			(_sorted_Data[column_of_second_sample]).size();

	EiVector first_sample_sorted = EiVector(first_sample);
	std::sort(first_sample_sorted.data(),
			first_sample_sorted.data() + first_sample_sorted.size());

	EiVector* second_sample_sorted = (&_sorted_Data[column_of_second_sample]);

	while (j1 < n1 && j2 < n2) {
		if ((d1 = first_sample_sorted(j1))
				<= (d2 = (*second_sample_sorted)(j2))) {

			EiVectorRef first_sample_tail = first_sample_sorted.tail(
					first_sample_sorted.size() - j1);

			if (d1 == d2) {
				j1 = j1 + _findLargestIndex(d2, first_sample_tail) + 1;
			} else {
				j1 = j1 + _findSmallestPredecesorIndex(d2, first_sample_tail)
						+ 1;
			}
		}
		if (d2 <= d1) {
			EiVectorRef second_sample_tail = second_sample_sorted->tail(
					second_sample_sorted->size() - j2);
			if (d1 == d2) {
				j2 = j2 + _findLargestIndex(d1, second_sample_tail) + 1;
			} else {
				j2 = j2 + _findSmallestPredecesorIndex(d1, second_sample_tail)
						+ 1;
			}
		}
		fn2 = (j2) / n2;
		fn1 = (j1) / n1;
		if ((dt = fabs((double) fn2 - fn1)) > distance)
			distance = dt;
	}

	return distance;
}

size_t KolmogorovComputer::_findSmallestPredecesorIndex(const double value,
		EiVectorRef& _vector) {
	if (_vector.size() == 1) {
		return 0;
	} else {
		size_t median_index = floor(_vector.size() / 2.0);
		double median = _vector(median_index);
		if (value > median) {

			EiVectorRef upper_sub_vector = _vector.tail(
					_vector.size() - median_index);

			return median_index
					+ _findSmallestPredecesorIndex(value, upper_sub_vector);
		} else {
			EiVectorRef lower_sub_vector = _vector.head(median_index);
			return _findSmallestPredecesorIndex(value, lower_sub_vector);
		}
	}

}

size_t KolmogorovComputer::_findLargestIndex(const double value,
		EiVectorRef& _vector) {
	if (_vector.size() == 1) {
		return 0;
	} else {
		size_t median_index = floor(_vector.size() / 2.0);
		double median = _vector(median_index);
		if (value >= median) {
			EiVectorRef upper_sub_vector = _vector.tail(
					_vector.size() - median_index);

			return median_index + _findLargestIndex(value, upper_sub_vector);
		} else {
			EiVectorRef lower_sub_vector = _vector.head(median_index);
			return _findLargestIndex(value, lower_sub_vector);
		}
	}
}

double KolmogorovComputer::getKappaForMApprox(int M, double beta) {
	double alpha = 1 - sqrt(1.0 - beta);
	return sqrt(-(1 / (2.0 * M)) * log(alpha / 2.0));
}

double KolmogorovComputer::getKappaForM(int M, double beta, double tolerance) {
	return kolmogorovCdfInverse(M, 1 - (1 - sqrt(1 - beta)), tolerance);
}

double KolmogorovComputer::getThresholdForSApprox(int s, double beta, int M) {
	double kappa = getKappaForMApprox(M, beta);
	return getThresholdForSApprox(s, beta, kappa);
}

double KolmogorovComputer::getThresholdForSApprox(int s, double beta,
		double kappa) {
	double alpha = 1 - sqrt(1.0 - beta);
	return sqrt(-(1 / (2.0 * s)) * log(alpha / 2.0)) + kappa;
}

double KolmogorovComputer::getThresholdForS(int s, double beta, double kappa,
		double tolerance) {
	return kolmogorovCdfInverse(s, 1 - (1 - sqrt(beta)), tolerance) + kappa;
}

int KolmogorovComputer::getSForThresholdApprox(double threshold, double beta,
		int M) {
	double kappa = getKappaForMApprox(M, beta);
	return getSForThresholdApprox(threshold, beta, kappa);
}

int KolmogorovComputer::getSForThresholdApprox(double threshold, double beta,
		double kappa) {
	double alpha = 1 - sqrt(1.0 - beta);
	int s = ceil(-log(alpha / 2.0) / (2.0 * pow(threshold - kappa, 2.0)));
	return s;
}

int KolmogorovComputer::getSForThreshold(double threshold, double beta,
		double kappa, double tolerance) {
	int s = 1;
	double tol = kolmogorovCdfInverse(s, 1 - (1 - sqrt(beta)), tolerance);
	while (tol < threshold - kappa) {
		s++;
		tol = kolmogorovCdfInverse(s, 1 - (1 - sqrt(beta)), tolerance);
	}
	return s;
}

double KolmogorovComputer::getThresholdForSApprox(int s) {
	double alpha = 1 - sqrt(1.0 - _beta);
	return sqrt(-(1 / (2.0 * s)) * log(alpha / 2.0)) + _kappa;
}

double KolmogorovComputer::getThresholdForS(int s) {
	return kolmogorovCdfInverse(s) + _kappa;
}

int KolmogorovComputer::getSForThresholdApprox(double threshold) {

	double alpha = 1 - sqrt(1.0 - _beta);
	int s = ceil(-log(alpha / 2.0) / (2.0 * pow(threshold - _kappa, 2.0)));
	return s;
}

int KolmogorovComputer::getSForThreshold(double threshold) {
	int s = 1;
	double tol = kolmogorovCdfInverse(s);
	while (tol < threshold - _kappa) {
		s++;
		tol = kolmogorovCdfInverse(s);
	}
	return s;
}

#ifdef __cplusplus
extern "C" {
#endif
/**
 Supporting function for kolmogorov_cdf_marsaglia, not to be used directly.
 */
void mMultiply(double *A, double *B, double *C, int m) {
	int i, j, k;
	double s;
	for (i = 0; i < m; i++)
		for (j = 0; j < m; j++) {
			s = 0.;
			for (k = 0; k < m; k++)
				s += A[i * m + k] * B[k * m + j];
			C[i * m + j] = s;
		}
}

/**
 Supporting function for kolmogorov_cdf_marsaglia, not to be used directly.
 */
void mPower(double *A, int eA, double *V, int *eV, int m, int n) {
	double *B;
	int eB, i;
	if (n == 1) {
		for (i = 0; i < m * m; i++)
			V[i] = A[i];
		*eV = eA;
		return;
	}
	mPower(A, eA, V, eV, m, n / 2);
	B = (double*) malloc((m * m) * sizeof(double));
	mMultiply(V, V, B, m);
	eB = 2 * (*eV);
	if (n % 2 == 0) {
		for (i = 0; i < m * m; i++)
			V[i] = B[i];
		*eV = eB;
	} else {
		mMultiply(A, B, V, m);
		*eV = eA + eB;
	}
	if (V[(m / 2) * m + (m / 2)] > 1e140) {
		for (i = 0; i < m * m; i++)
			V[i] = V[i] * 1e-140;
		*eV += 140;
	}
	free(B);
}

double kolmogorov_cdf_marsaglia(int n, double d) {
	int k, m, i, j, g, eH, eQ;
	double h, s, *H, *Q;

	//OMIT NEXT LINE IF YOU REQUIRE >7 DIGIT ACCURACY IN THE RIGHT TAIL
	s = d * d * n;
	if (s > 7.24 || (s > 3.76 && n > 99))
		return 1 - 2 * exp(-(2.000071 + .331 / sqrt(n) + 1.409 / n) * s);

	k = (int) (n * d) + 1;
	m = 2 * k - 1;
	h = k - n * d;
	H = (double*) malloc((m * m) * sizeof(double));
	Q = (double*) malloc((m * m) * sizeof(double));
	for (i = 0; i < m; i++)
		for (j = 0; j < m; j++)
			if (i - j + 1 < 0)
				H[i * m + j] = 0;
			else
				H[i * m + j] = 1;
	for (i = 0; i < m; i++) {
		H[i * m] -= pow(h, i + 1);
		H[(m - 1) * m + i] -= pow(h, (m - i));
	}
	H[(m - 1) * m] += (2 * h - 1 > 0 ? pow(2 * h - 1, m) : 0);
	for (i = 0; i < m; i++)
		for (j = 0; j < m; j++)
			if (i - j + 1 > 0)
				for (g = 1; g <= i - j + 1; g++)
					H[i * m + j] /= g;
	eH = 0;
	mPower(H, eH, Q, &eQ, m, n);
	s = Q[(k - 1) * m + k - 1];
	for (i = 1; i <= n; i++) {
		s = s * i / n;
		if (s < 1e-140) {
			s *= 1e140;
			eQ -= 140;
		}
	}
	s *= pow(10., eQ);
	free(H);
	free(Q);
	return s;
}

#ifdef __cplusplus
}
#endif

// q = 1- alpha or, in terms of beta: q = 1 - (1 - sqrt(beta))
double KolmogorovComputer::kolmogorovCdfInverse(int S, double q,
		double tolerance) {
	double x_lo = 0.0;

	// Set initial guesses using the DKW bounds
	double x_hi = sqrt(-1 / (2 * (double) S) * log((1 - q) / 2));

	KolmogorovCdfRoots kolmogorov_roots(S, q);
	KolmogorovTol kolmogorov_tol(tolerance);

	std::pair<double, double> result = boost::math::tools::bisect<
			KolmogorovCdfRoots, double, KolmogorovTol>(kolmogorov_roots, x_lo,
			x_hi, kolmogorov_tol);

	return (result.first + result.second) / 2.0;
}

double KolmogorovComputer::kolmogorovCdfInverse(int S) {
	double q = 1 - (1 - sqrt(1 - _beta));

	return kolmogorovCdfInverse(S, q, _tolerance);
}

} /* namespace INSIGHTv3 */


#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);
}


/**
 * @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;
}


/* 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


#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 );
}


/**
 * @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;
}


//
//  main.cpp
//  using_eigen
//
//  Created by on 2018/11/6.
//  Copyright @2018 kouui. All rights reserved.
//

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

int main()
{
    // Dynamic
    // X : unknown size, resizable
    // d : double
    Eigen::MatrixXd m;

    // fixed Sized Matrix
    Eigen::Matrix3d f;

    f << 1, 2, 3,
        4, 5, 6,
        7, 8, 9;

    f = Eigen::Matrix3d::Constant(1.0);

    m = Eigen::MatrixXd::Constant(5, 5, 1.0);

    std::cout << m << "\n";
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <complex>
#include <tuple>

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

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

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

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

#define ping(X) std::cerr << __LINE__ << " " << #X << ":" << X << std::endl
int debug = 0;

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

int main(int,char**)
{
  std::size_t Nx = 135, Nv = 256;

  // $(u_c,E,\hat{f}_h)$ and $f_h$
  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,-1.76);
  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

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

  // compute dx, dv
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/Nv;
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/Nx;

  const double dt = 0.1;//1.*fh.step.dv;
  double Tf = 60.*dt;

  // velocity and frequency
  ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  const double l = fh.range.x_max-fh.range.x_min;
  ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  // initial condition
  double ui=2., alpha=0.2;
  auto tb_M1 = maxwellian(0.5*alpha,ui,1.) , tb_M2 = maxwellian(0.5*alpha,-ui,1.);
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] = std::cos(2.*math::pi<double>()/16.*(Vk(k)-0.5));
    }
    fft::fft(&(fh[k][0]),&(fh[k][Nx-1])+1,&(hfh[k][0]));
  }
  fh.write("vphl/split/init.dat");

  splitting<double,1> Lie( fh , l , 1. );

  std::vector<double> times;

  std::size_t i_t = 0;
  double current_time = 0.;
  while ( i_t < 60 ) {
    std::cout << " [" << std::setw(5) << i_t << "] " << i_t*dt << "\r" << std::flush;

    Lie.phi_b(dt,uc,E,hfh);

    current_time += dt;
    ++i_t;
    times.push_back( current_time );
  } // while current_time < Tf
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<< "    "<<std::endl;

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][Nx-1])+1,&(fh[k][0])); }
  fh.write("vphl/split/vp.dat");

  for ( auto k=0 ; k<fh.size(0) ; ++k ) {
    for ( auto i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] -= std::cos(2.*math::pi<double>()/16.*( Vk(k)-0.5-E(i)*Tf ));
    }
  }

  fh.write("vphl/split/diff.dat");

  for ( auto ei : E ) {
    std::cout << ei << " , ";
  }
  std::cout << std::endl;
  return 0;
}


#include <NTL/vec_RR.h>

NTL_START_IMPL

void InnerProduct(RR& xx, const vec_RR& a, const vec_RR& b)
{
   RR t1, x;

   long n = min(a.length(), b.length());
   long i;

   clear(x);
   for (i = 1; i <= n; i++) {
      mul(t1, a(i), b(i));
      add(x, x, t1);
   }

   xx = x;
}

void mul(vec_RR& x, const vec_RR& a, const RR& b_in)
{
   RR b = b_in;
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void mul(vec_RR& x, const vec_RR& a, double b_in)
{
   NTL_THREAD_LOCAL static RR b;
   conv(b, b_in);
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void add(vec_RR& x, const vec_RR& a, const vec_RR& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector add: dimension mismatch");

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      add(x[i], a[i], b[i]);
}

void sub(vec_RR& x, const vec_RR& a, const vec_RR& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector sub: dimension mismatch");
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      sub(x[i], a[i], b[i]);
}

void clear(vec_RR& x)
{
   long n = x.length();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

void negate(vec_RR& x, const vec_RR& a)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      negate(x[i], a[i]);
}

long IsZero(const vec_RR& a)
{
   long n = a.length();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

vec_RR operator+(const vec_RR& a, const vec_RR& b)
{
   vec_RR res;
   add(res, a, b);
   NTL_OPT_RETURN(vec_RR, res);
}

vec_RR operator-(const vec_RR& a, const vec_RR& b)
{
   vec_RR res;
   sub(res, a, b);
   NTL_OPT_RETURN(vec_RR, res);
}

vec_RR operator-(const vec_RR& a)
{
   vec_RR res;
   negate(res, a);
   NTL_OPT_RETURN(vec_RR, res);
}

RR operator*(const vec_RR& a, const vec_RR& b)
{
   RR res;
   InnerProduct(res, a, b);
   return res;
}

void VectorCopy(vec_RR& x, const vec_RR& a, long n)
{
   if (n < 0) LogicError("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) ResourceError("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   long i;

   for (i = 0; i < m; i++)
      x[i] = a[i];

   for (i = m; i < n; i++)
      clear(x[i]);
}

NTL_END_IMPL


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
typedef Matrix<double, Dynamic, Dynamic> TempMatrixImg;
typedef Vector<u_char,Dynamic> ImgLine;
typedef Vector<double,Dynamic> TempImgLine;
//typedef Matrix<ImgLine,Dynamic,Dynamic> FlattenedImages;

unsigned char max(u_char a, u_char b){
    if (a<b) return b;
    else return a;
}

unsigned char min(u_char a, u_char b){
    if (a>b) return b;
    else return a;
}

double max(double a, double b){
    if (a<b) return b;
    else return a;
}

double min(double a, double b){
    if (a>b) return b;
    else return a;
}

int max(int a, int b){
    if (a<b) return b;
    else return a;
}

int min(int a, int b){
    if (a>b) return b;
    else return a;
}

// auto max(auto a, auto b){
//     if (a<b) return b;
//     else return a;
// }

// auto min(auto a, auto b){
//     if (a>b) return b;
//     else return a;
// }

vector<double> projectOnEigenSpace(vector<TempImgLine> eigenfaces, TempImgLine imToProj,int K){
    vector<double> res = vector<double>();
    for (int i=0;i<K;i++){
        res.push_back(eigenfaces[i].dot(imToProj));
    }
    return res;
}

ImgLine octToVec(OCTET* im, int nH, int nW){
    ImgLine res(nH*nW);
    for (int i=0; i<nH*nW;i++){
        res(i)=im[i];
    }
    return res;
}

double eigenProjsDistance(vector<double> proj1, vector<double> proj2){

    double res =0.0;
    for (int i=0; i<min(proj1.size(),proj2.size());i++){
        res+=(proj1[i]-proj2[i])*(proj1[i]-proj2[i]);
    }
    return res;
}

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

    //DB file :
    ofstream outFile;
    outFile.open ("DBEigen.txt");

    //recup eigenfaces

    if (argc<3){
        cout<<"usage : eigenfaces dir0 ... dirn\n"<<endl;
    }
    vector<string> directories;
    for (int i=0; i<argc-2;i++){
        string s = string(argv[i+2]);
        int found = s.find_last_of('/');

        directories.push_back(s.substr(found+1));
    }

    vector<TempImgLine> eigenfaces;
    int K =42; int nH;int nW;

    for (int i=0; i<K;i++){
        OCTET* im;
        char name[100];

        sprintf(name,"/im%d.pgm",i);
        string eigenFolder = string(string(argv[1])+string(name));

        lire_nb_lignes_colonnes_image_pgm(eigenFolder.c_str(),&nH,&nW);
        allocation_tableau(im,OCTET,nH*nW);
        lire_image_pgm(eigenFolder.c_str(),im,nH*nW);
        TempImgLine eigenFace(nH*nW) ;
        double sum=0.0;
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=im[j]-127;
            sum+=(double)(im[j]-127)*(double)(im[j]-127);
        }
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=eigenFace(j)/sqrt(sum);
            }
        eigenfaces.push_back(eigenFace);
        free(im);
    }

    //vector<vector<vector<double>>> registre = vector<vector<vector<double>>>();

    int countDirs =0;
    for (int i=0; i<argc-2;i++){
        int countFile=0;

        outFile<<"!"<<directories[i]<<endl;
        for (auto& file : std::filesystem::directory_iterator(argv[i+2])){
            //registre.push_back(vector<vector<double>>());
            OCTET* img;
            lire_nb_lignes_colonnes_image_pgm(file.path().c_str(),&nH,&nW);
            TempImgLine imgLine(nH*nW);
            allocation_tableau(img,OCTET, nH*nW);
            lire_image_pgm(file.path().c_str(),img,nH*nW);
            double sumIm=0.0;
            for (int ip=0;ip<nH*nW;ip++){sumIm+=(double)(img[ip]-127)*(double)(img[ip]-127);}
            for (int ip=0; ip<nH*nW;ip++){imgLine(ip)=(double)(img[ip]-127)/sumIm;}

            //registre[countDirs].push_back(projectOnEigenSpace(eigenfaces,imgLine,K));
            vector<double> testPrint = projectOnEigenSpace(eigenfaces,imgLine,K);
            for (int d=0; d<testPrint.size();d++){outFile<<testPrint[d]<<" ";}
            outFile<<endl;

            countFile++;
        }
        outFile<<endl;
        countDirs++;
    }

    outFile.close();
}


#ifndef UTILS_HPP
#define UTILS_HPP

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

template <typename qp_t>
void print_qp(qp_t qp)
{
    Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", ",", "[", "]", "[", "]");
    std::cout << "P = " << qp.P.format(fmt) << '\n';
    std::cout << "q = " << qp.q.transpose().format(fmt) << '\n';
    std::cout << "A = " << qp.A.format(fmt) << '\n';
    std::cout << "l = " << qp.l.transpose().format(fmt) << '\n';
    std::cout << "u = " << qp.u.transpose().format(fmt) << '\n';
}

template <typename Mat>
bool is_psd(Mat &h)
{
    Eigen::EigenSolver<Mat> eigensolver(h);
    for (int i = 0; i < eigensolver.eigenvalues().RowsAtCompileTime; i++) {
        double v = eigensolver.eigenvalues()(i).real();
        if (v <= 0) {
            return false;
        }
    }
    return true;
}

#endif /* UTILS_HPP */


/**
 * \file dcs/math/stats/distribution/weibull.hpp
 *
 * \brief The Weibull probability distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_WEIBULL_HPP
#define DCS_MATH_STATS_DISTRIBUTION_WEIBULL_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/weibull.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/random/uniform_01_adaptor.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

/**
 * \brief The Weibull distribution with shape parameter \f$k\f$ and scale
 *  parameter \f$\lambda\f$.
 *
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|k,\lambda)=\begin{cases} \frac{k}{\lambda}\left(\frac{x}{\lambda}\right)^{k-1}e^{-(x/\lambda)^{k}} & x\geq0\\ 0 & x<0\end{cases}
 * \f]
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class weibull_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit weibull_distribution(support_type shape, support_type scale=1)
		: dist_(shape, scale)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a random number distributed according to this
	 * weibull distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this weibull
	 * distribution.
	 *
	 * A \c weibull random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|k,\lambda)=\begin{cases} \frac{k}{\lambda}\left(\frac{x}{\lambda}\right)^{k-1}e^{-(x/\lambda)^{k}} & x\geq0\\ 0 & x<0\end{cases}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&, support_type> eng(rng);

		return dist_.scale()*(::std::pow(-::std::log(eng()),value_type(1)/dist_.shape()));
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * weibull distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * weibull distribution.
	 *
	 * A \c weibull random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\lambda) = \lambda e^{-\lambda x}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n)
	{
		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(rand(rng));
		}

		return rnds;
	}

	public: support_type shape() const
	{
		return dist_.shape();
	}

	public: support_type scale() const
	{
		return dist_.scale();
	}

	public: support_type location() const
	{
		return support_type(0);
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	private: ::boost::math::weibull_distribution<value_type,policy_type> dist_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, weibull_distribution<RealT,PolicyT> const& dist)
{
	return os << "Weibull("
			  << "shape=" <<  dist.shape()
			  << ", scale=" <<  dist.scale()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_WEIBULL_HPP


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>

#include "sys.h"
#include <time.h>
#include "grid.h"
#include <armadillo>

using namespace std;

double compute_convolution_normal(System *sys, double *pNoise, Node *C) {
    double sum = 0;

    for (int i = 1; i < 2 * sys->len; i++) {
        int samp_y = (C[i].x + C[i - 1].x) * 0.5;
        int samp_x = (C[i].y + C[i - 1].y) * 0.5;

        if (samp_x < 0)
            samp_x = 0;
        if (samp_y < 0)
            samp_y = 0;

        if (samp_x > sys->NGrid)
            samp_x = sys->NGrid - 1;
        if (samp_y > sys->NGrid)
            samp_y = sys->NGrid - 1;

        int pic_loc = samp_y * sys->NGrid + samp_x;

        sum = sum + pNoise[pic_loc];
    }

    sum = sum / (2 * sys->len + 1);

    return sum;
}

double compute_convolution_high(System *sys, double *pNoise, Node *C) {
    double sum = 0;

    double filter[3][3] = {
        0, 1 / 2, 0,
        1 / 2, 1.5, 1 / 2,
        0, 1 / 2, 0
    };

    for (int i = 1; i < 2 * sys->len; i++) {
        int samp_x = (C[i].x + C[i - 1].x) * 0.5;
        int samp_y = (C[i].y + C[i - 1].y) * 0.5;

        if (samp_x < 0)
            samp_x = 0;
        if (samp_y < 0)
            samp_y = 0;

        if (samp_x > sys->NGrid)
            samp_x = sys->NGrid - 1;
        if (samp_y > sys->NGrid)
            samp_y = sys->NGrid - 1;

        for (int r = 0; r < 3; r++) {
            for (int s = 0; s < 3; s++) {
                int n = (samp_x - 3 / 2 + r + sys->NGrid) % sys->NGrid;
                int m = (samp_y - 3 / 2 + s + sys->NGrid) % sys->NGrid;
                int pic_loc = n * sys->NGrid + m;
                sum = sum + (pNoise[pic_loc] * filter[r][s]);
            }
        }
    }

    sum = sum / (2 * sys->len + 1);

    return sum;
}

void synthesize_vectors(System *sys, Grid *grid, double **vect_x, double **vect_y) {
    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            vect_x[j][i] = grid->Hy(j,i);
            vect_y[j][i] = grid->Hz(j,i);
        }
    }
}

void normalize_vectors(System *sys, double **vect_x, double **vect_y) {
    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            double mag = sqrtf(vect_x[j][i] * vect_x[j][i] + vect_y[j][i] * vect_y[j][i]);

            if (mag > 0.0) {
                vect_x[j][i] = vect_x[j][i] / mag;
                vect_y[j][i] = vect_y[j][i] / mag;
            }
            else {
                vect_x[j][i] = 0;
                vect_y[j][i] = 0;
            }
        }
    }
}

void white_noise(System *sys, double *pNoise) {
    int pic_loc;

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            int r = rand();
            pic_loc = j * sys->NGrid + i;

            r = ((r & 255) + ((r & 255) >> 8)) & 255;
            pNoise[pic_loc] = (unsigned char) r;
        }
    }
}

void compute_integral_curve(System *sys, Node *p, Node *C, double **vect_x, double **vect_y) {
    double x = p->x + 0.5;
    double y = p->y + 0.5;

    int s;
    int index = 0;

    C[index].x = x;
    C[index].y = y;

    double segLen = 0;
    double vctr_x = 0;
    double vctr_y = 0;

    // positive calculations
    for (s = 0; s < sys->len; s++) {
        if (x < 0)
            x = 0;
        if (y < 0)
            y = 0;

        if (x > sys->NGrid)
            x = sys->NGrid - 1;
        if (y > sys->NGrid)
            y = sys->NGrid - 1;

        vctr_x = vect_x[(int) x][(int) y];
        vctr_y = vect_y[(int) x][(int) y];

        segLen += 0.1;

        x = x + segLen * vctr_x;
        y = y + segLen * vctr_y;

        C[index].x = x;
        C[index].y = y;

        index++;
    }

    x = p->x + 0.5;
    y = p->y + 0.5;

    segLen = 0;

    // negative calculations
    for (s = 0; s < sys->len; s++) {
        if (x < 0)
            x = 0;
        if (y < 0)
            y = 0;

        if (x > sys->NGrid)
            x = sys->NGrid - 1;
        if (y > sys->NGrid)
            y = sys->NGrid - 1;

        vctr_x = vect_x[(int) x][(int) y];
        vctr_y = vect_y[(int) x][(int) y];

        segLen += 0.1;

        x = x - segLen * vctr_x;
        y = y - segLen * vctr_y;

        C[index].x = x;
        C[index].y = y;

        index++;
    }
}

void lic(System *sys, Grid *grid) {
    grid->ix.reshape(sys->NGrid, sys->NGrid);
    grid->iy.reshape(sys->NGrid, sys->NGrid);
    grid->iz.reshape(sys->NGrid, sys->NGrid);

    sys->len = 20;

    std::cout << "\n--- LIC algorithm started ---" << std::endl;

    clock_t begin, end;
    double time_spent;

    begin = clock();

    Node *p = (Node *) calloc(1, sizeof(Node));
    Node *C = (Node *) calloc(2 * sys->len, sizeof(Node));

    double *pNoise = (double *) malloc(sizeof(double) * sys->NGrid * sys->NGrid);
    double *pNoise_double = (double *) malloc(sizeof(double) * sys->NGrid * sys->NGrid);

    double **vect_x = (double **) calloc(sys->NGrid, sizeof(double *));
    double **vect_y = (double **) calloc(sys->NGrid, sizeof(double *));

    for (int i = 0; i < sys->NGrid; i++)
        vect_x[i] = (double *) calloc(sys->NGrid, sizeof(double));
    for (int i = 0; i < sys->NGrid; i++)
        vect_y[i] = (double *) calloc(sys->NGrid, sizeof(double));

    synthesize_vectors(sys, grid, vect_x, vect_y);
    normalize_vectors(sys, vect_x, vect_y);
    white_noise(sys, pNoise);

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            p->x = j;
            p->y = i;

            compute_integral_curve(sys, p, C, vect_x, vect_y);
            double sum = compute_convolution_normal(sys, pNoise, C);

            if (sum < 100)
                sum = 0;
            if (sum > 150)
                sum = 255;

            pNoise_double[j * sys->NGrid + i] = sum;
        }
    }

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            p->x = j;
            p->y = i;

            compute_integral_curve(sys, p, C, vect_x, vect_y);
            double sum = compute_convolution_high(sys, pNoise_double, C);

            if (sum < 0)
                sum = 0;
            if (sum > 255)
                sum = 255;

            grid->ix(j,i) = sum;
        }
    }

    arma::mat magVect(sys->NGrid, sys->NGrid);

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            double mag_x = grid->Hx(j,i) * grid->Hx(j,i);
            double mag_y = grid->Hy(j,i) * grid->Hy(j,i);
            double mag_z = grid->Hz(j,i) * grid->Hz(j,i);

            double mag = sqrtf(mag_x + mag_y + mag_z);

            if (mag > 0)
                magVect(j,i) = mag;
        }
    }

    for (int j = 0; j < sys->NGrid; j++)
        for (int i = 0; i < sys->NGrid; i++)
            grid->iy(j,i) = magVect(j,i);

    end = clock();
    time_spent = (double) (end - begin) / CLOCKS_PER_SEC;

    printf("LIC End\n");
    printf("Time Spend: %lf\n", time_spent);
}