#pragma once
#include <math.h>

#include <boost/logic/tribool.hpp>
#include <boost/numeric/interval.hpp>
#include <boost/numeric/interval/io.hpp>
#include <boost/numeric/interval/rounded_arith.hpp>
#include <cmath>
#include <iostream>
#include <utility>

static const double ulp = ldexpl(1.0, -52);
static const double min_denormal = ldexpl(1.0, -1074);

namespace bn = boost::numeric;
namespace bni = bn::interval_lib;
typedef bni::checking_no_nan<double> checking;
typedef bn::interval<double, bni::policies<bni::save_state<bni::rounded_transc_std<double>>, checking>> Interval;

////////////////////////////////////////////////////////////////////////////////
//
// DUAL NUMBER INTERVAL CLASS
//
////////////////////////////////////////////////////////////////////////////////

class DualInterval {
   public:
    Interval real;
    Interval dual;

    DualInterval(double rl, double ru, double dl, double du) {
        real = Interval(rl, ru);
        dual = Interval(dl, du);
    }

    DualInterval(const Interval& ri, const Interval& di) {
        real = ri;
        dual = di;
    }

    DualInterval(double r) {
        real = Interval(r, r);
        dual = Interval(0, 0);
    }

    DualInterval() {
        real = Interval(0, 0);
        dual = Interval(0, 0);
    }

    DualInterval operator+(const DualInterval& rhs) const;
    DualInterval operator+(const double rhs) const;
    friend DualInterval operator+(const double lhs, const DualInterval& rhs);

    DualInterval operator-(const DualInterval& rhs) const;
    DualInterval operator-(const double rhs) const;
    friend DualInterval operator-(const double lhs, const DualInterval& rhs);
    DualInterval operator-() const;

    DualInterval operator*(const DualInterval& rhs) const;
    DualInterval operator*(const double rhs) const;
    friend DualInterval operator*(const double lhs, const DualInterval& rhs);

    DualInterval operator/(const DualInterval& rhs) const;
    DualInterval operator/(const double rhs) const;

    DualInterval& operator+=(const DualInterval& rhs);
    DualInterval& operator+=(const double rhs);

    DualInterval operator|(const DualInterval& rhs) const;

    friend DualInterval exp(const DualInterval& x);
    friend DualInterval log(const DualInterval& x);
    friend DualInterval sqrt(const DualInterval& x);
    friend DualInterval tanh(const DualInterval& x);
    friend DualInterval atan(const DualInterval& x);
    friend DualInterval logistic(const DualInterval& x);
    friend DualInterval sin(const DualInterval& x);
    friend DualInterval cos(const DualInterval& x);
    friend DualInterval abs(const DualInterval& x);
    friend DualInterval relu(const DualInterval& x);
    friend DualInterval max(const DualInterval& x, const DualInterval& y);
    friend DualInterval min(const DualInterval& x, const DualInterval& y);

    friend std::ostream& operator<<(std::ostream& os, const DualInterval& di);

    bool isEmpty() const;
    void setReal(Interval real_);
    void setReal(double a, double b);
    void setDual(Interval dual_);
    void setDual(double a, double b);
    Interval getReal() const;
    Interval getDual() const;
};

typedef DualInterval DI;

bool DualInterval::isEmpty() const {
    return (empty(real) && empty(dual));
}

void DualInterval::setReal(Interval real_) {
    real = real_;
}

void DualInterval::setReal(double a, double b) {
    real = Interval(a, b);
}

void DualInterval::setDual(Interval dual_) {
    dual = dual_;
}

void DualInterval::setDual(double a, double b) {
    dual = Interval(a, b);
}

Interval DualInterval::getReal() const {
    return real;
}
Interval DualInterval::getDual() const {
    return dual;
}

std::ostream& operator<<(std::ostream& os, const DualInterval& di) {
    os << di.real << " + " << di.dual << "\u03B5";
    return os;
}

// internal function only used within this header
Interval add_intervals(const Interval& a, const Interval& b) {
    Interval i = a + b;

    #ifdef SOUND
        double maxA = fmax(fabs(a.lower()), fabs(a.upper()));
        double maxB = fmax(fabs(b.lower()), fabs(b.upper()));
        Interval tmp = Interval(-maxA * ulp, maxA * ulp) + Interval(-maxB * ulp, maxB * ulp) + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// internal function only used within this header
Interval mul_intervals(const Interval& a, const Interval& b) {
    Interval i = a * b;

    #ifdef SOUND
        double maxB = fmax(fabs(b.lower()), fabs(b.upper()));
        Interval tmp = a * Interval(-maxB * ulp, maxB * ulp) + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// internal function only used within this header
Interval square_interval(const Interval& a) {
    Interval i = square(a);

    #ifdef SOUND
        double maxB = fmax(fabs(a.lower()), fabs(a.upper()));
        Interval tmp = a * Interval(-maxB * ulp, maxB * ulp) + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// internal function only used within this header
Interval div_intervals(const Interval& a, const Interval& b) {
    Interval i = a / b;

    #ifdef SOUND
        double maxA = fmax(fabs(a.lower()), fabs(a.upper()));
        Interval tmp = Interval(-maxA * ulp, maxA * ulp) / b + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// addition
DI DualInterval::operator+(const DI& rhs) const {
    assert(!isEmpty() && !rhs.isEmpty());

    Interval r = add_intervals(real, rhs.real);
    Interval d = add_intervals(dual, rhs.dual);
    return DI(r, d);
}

// addition with a scalar (this automatically casts the scalar to a DualInterval)
DI DualInterval::operator+(const double rhs) const {
    return *this + DI(rhs);
}

DI operator+(const double lhs, const DI& rhs) {
    return DI(lhs) + rhs;
}

// subtraction
DI DualInterval::operator-(const DI& rhs) const {
    return *this + (-rhs);
}

DI DualInterval::operator-(const double rhs) const {
    return *this - DI(rhs);
}

DI operator-(const double lhs, const DI& rhs) {
    return DI(lhs) - rhs;
}

// negation
DI DualInterval::operator-() const {
    return DI(-real.upper(), -real.lower(), -dual.upper(), -dual.lower());
}

// multiplication
DI DualInterval::operator*(const DI& rhs) const {
    assert(!isEmpty() && !rhs.isEmpty());

    Interval r = mul_intervals(real, rhs.real);
    Interval d = add_intervals(mul_intervals(real, rhs.dual), mul_intervals(dual, rhs.real));
    return DI(r, d);
}

DI DualInterval::operator*(const double rhs) const {
    return *this * DI(rhs);
}

DI operator*(const double lhs, const DI& rhs) {
    return DI(lhs) * rhs;
}

// division
DI DualInterval::operator/(const DI& rhs) const {
    assert(!isEmpty() && !rhs.isEmpty());

    Interval r = div_intervals(real, rhs.real);
    Interval d = div_intervals(add_intervals(mul_intervals(dual, rhs.real), -mul_intervals(real, rhs.dual)), square_interval(rhs.real));
    return DI(r, d);
}

DI DualInterval::operator/(const double rhs) const {
    return *this / DI(rhs);
}

// increment
DI& DualInterval::operator+=(const DI& rhs) {
    assert(!isEmpty() && !rhs.isEmpty());

    real = add_intervals(real, rhs.real);
    dual = add_intervals(dual, rhs.dual);
    return *this;
}

DI& DualInterval::operator+=(double rhs) {
    return *this += DI(rhs);
}

// join (union)
DI DualInterval::operator|(const DI& rhs) const {
    if (rhs.isEmpty()) {
        return DI(real.lower(), real.upper(), dual.lower(), dual.upper());
    }

    if (isEmpty()) {
        return DI(rhs.real.lower(), rhs.real.upper(), rhs.dual.lower(), rhs.dual.upper());
    }

    return DI(hull(real, rhs.real), hull(dual, rhs.dual));
}

////////////////////////////////////////////////////////////////////////////////
//
// FUNCTIONS OF SCALAR DUAL INTERVALS
//
////////////////////////////////////////////////////////////////////////////////

DI exp(const DI& x) {
    assert(!x.isEmpty());

    Interval r = exp(x.real);
    Interval d = r * x.dual;
    return DI(r, d);
}

DI log(const DI& x) {
    assert(!x.isEmpty());
    return DI(log(x.real), x.dual / x.real);
}

DI sqrt(const DI& x) {
    assert(!x.isEmpty());

    Interval r = sqrt(x.real);
    Interval d = x.dual / r * 0.5;
    return DI(r, d);
}

DI tanh(const DI& x) {
    assert(!x.isEmpty());

    Interval r = tanh(x.real);
    Interval d = (1. - square(r)) * x.dual;
    return DI(r, d);
}

DI atan(const DI& x) {
    assert(!x.isEmpty());
    return DI(atan(x.real), 1. / (1. + square(x.real)) * x.dual);
}

DI logistic(const DI& x) {
    assert(!x.isEmpty());

    Interval t = tanh(x.real / 2.);
    Interval r = 0.5 * t + 0.5;
    Interval d = 0.25 * (1. - square(t)) * x.dual;
    return DI(r, d);
}

DI sin(const DI& x) {
    assert(!x.isEmpty());
    return DI(sin(x.real), cos(x.real) * x.dual);
}

DI cos(const DI& x) {
    assert(!x.isEmpty());
    return DI(cos(x.real), -sin(x.real) * x.dual);
}

DI abs(const DI& x) {
    assert(!x.isEmpty());

    if (x.real.upper() < 0) {
        return -x;
    } else if (x.real.lower() > 0) {
        return x;
    } else {
        DI positive_branch(0, x.real.upper(), x.dual.lower(), x.dual.upper());
        DI negative_branch(0, -x.real.lower(), -x.dual.upper(), -x.dual.lower());
        return (positive_branch | negative_branch);
    }
}

DI max(const DI& x, const DI& y) {
    assert(!x.isEmpty() && !y.isEmpty());

    if (x.real.lower() > y.real.upper()) {
        return x;
    } else if (x.real.upper() < y.real.lower()) {
        return y;
    }
    return DI(max(x.real, y.real), hull(x.dual, y.dual));
}

DI min(const DI& x, const DI& y) {
    return -max(-x, -y);
}

DI relu(const DI& x) {
    return max(x, 0);
}


/*! \file exactmath.hpp
  \brief Exact math
  \author Elad Steinberg
 */

#ifndef EXACTMATH_HPP
#define EXACTMATH_HPP 1

#include <stdlib.h>
#include <cmath>
#include <vector>
#include <boost/array.hpp>

using std::vector;
using std::min;

/*! \brief Calculates the sum of two numbers
  \param a First number
  \param b Second number
  \param res Output
  \param err Roundoff error
 */
void fastTwoSum(double a, double b, double& res, double& err);

/*! \brief Subtracts two numbers
   \param a First number
   \param b Second number
   \param res Output
   \param err Roundoff error
 */
void fastTwoDiff(double a, double b, double& res, double& err);

/*! \brief Calculates the sum of a and b.
   \param a First number
   \param b Second number
   \param res Result
   \param err Roundoff error
 */
void twoSum(double a, double b, double& res, double& err);

/*! \brief Difference between two numbers
   \param a First number
   \param b Second number
   \param res Result
   \param err Roundoff error
 */
void twoDiff(double a, double b, double& res, double& err);

/*! \brief Splits a given number into two, Used for multiplication.
   \param num Number
   \param high Higher part
   \param low Lower part
 */
void split(double num, double& high, double& low);

/*! \brief Product of two number
   \param a First number
   \param b Second number
   \param res Result
   \param err Error
 */
void twoProduct(double a, double b, double& res, double& err);

/*! \brief Calculates the square of a number.
   \param num Number
   \param res Result
   \param err Roundoff error
 */
void square(double num, double& res, double& err);

/*! \brief Calculates the sum of a two-expansion and a double.
   \param a Two expansion
   \param b A number
   \return Sum
 */
boost::array<double,3> twoOneSum(boost::array<double,2> const& a, double b);

/*! \brief Calculates the difference between a two-expansion and a double.
   \param a Two expansion
   \param b Number
   \return Difference
 */
boost::array<double,3> twoOneDiff(boost::array<double,2> const& a, double b);

/*! \brief Calculates the sum of two two-expansions.
   \param a First two expansion
   \param b Second two expansio
   \return sum
 */
vector<double> twoTwoSum(boost::array<double,2> const& a,boost::array<double,2> const& b);

/*! \brief Calculates the difference between two two-expansions.
   \param a First two expansion
   \param b Second two expansion
   \return Difference
 */
vector<double> twoTwoDiff(boost::array<double,2> const& a, boost::array<double,2> const& b);

/*! \brief Adds a scalar to an existing expansion.
  \param e Expansion
  \param b Scalar
  \return Expansion
 */
vector<double> growExpansionZeroElim(vector<double> const& e, double b);

/*! \brief Adds up two expansions.
   \param e First expansion
   \param f Second expansion
   \return Sum
 */
vector<double> expansionSumZeroElim(vector<double> const& e, vector<double> const& f);

/*! \brief Adds up two expansions.
  \param e First expansion
  \param f Second expansion
  \return Sum
 */
vector<double> fastExpansionSumZeroElim(vector<double> const& e, vector<double> const& f);

/*! \brief Adds up two expansions.
  \param e First expansion
  \param f Second expansion
  \return Expansion
 */
vector<double> linearExpansionSumZeroElim(vector<double> const& e, vector<double> const& f);

/*! \brief Multiplies a scalar by an expansion.
  \param e Expansion
  \param b Scalar
  \param result Result
 */
void scaleExpansionZeroElim(vector<double> const& e, double b,
	vector<double> &result);

/*! \brief Compresses an expansion.
   \param e Expansion
   \return Expansion
 */
vector<double> compress(vector<double> const& e);

/*! \brief Calculate a double precision approximation of the expansion.
  \param e Expansion
  \return A number
 */
double estimate(vector<double> const& e);

#endif //EXACTMATH_HPP


#include <iostream>

#include <Eigen/Sparse>

#include <gtest/gtest.h>

#include "tbb/blocked_range.h"
#include "tbb/parallel_for.h"
#include "tbb/parallel_reduce.h"

#include "Integrator.h"
#include "LevelSet.h"
#include "ScalarGrid.h"
#include "Transform.h"
#include "Utilities.h"
#include "VectorGrid.h"

using namespace FluidSim3D;

class AnalyticalPoissonSolver
{
public:
    AnalyticalPoissonSolver(const Transform& xform, const Vec3i& size)
		: myXform(xform)
	{
		myPoissonGrid = ScalarGrid<float>(myXform, size, 0);
	}

    template <typename RHS, typename Solution>
	double solve(const RHS& rhsFunction, const Solution& solutionFunction);

private:
    Transform myXform;
    ScalarGrid<float> myPoissonGrid;
};

template <typename RHS, typename Solution>
double AnalyticalPoissonSolver::solve(const RHS& rhsFuction, const Solution& solutionFunction)
{
    UniformGrid<int> solvableCells(myPoissonGrid.size(), -1);

    int solutionDOFCount = 0;

    Vec3i gridSize = myPoissonGrid.size();

    forEachVoxelRange(Vec3i::Zero(), gridSize, [&](const Vec3i& cell) { solvableCells(cell) = solutionDOFCount++; });

    std::vector<Eigen::Triplet<double>> sparseMatrixElements;

    VectorXd rhsVector = VectorXd::Zero(solutionDOFCount);

    double dx = myPoissonGrid.dx();
	double coeff = std::pow(dx, 2);

	{
		tbb::enumerable_thread_specific<std::vector<Eigen::Triplet<double>>> parallelSparseElements;
		tbb::parallel_for(tbb::blocked_range<int>(0, solvableCells.voxelCount()), [&](const tbb::blocked_range<int>& range)
		{
        	auto& localSparseElements = parallelSparseElements.local();

			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec3i cell = solvableCells.unflatten(cellIndex);
				int row = solvableCells(cell);

				assert(row >= 0);

				// Build RHS
				Vec3d gridPoint = Vec3d(myPoissonGrid.indexToWorld(cell.cast<double>()));

				rhsVector(row) = -coeff * rhsFuction(gridPoint);

				for (auto axis : {0, 1, 2})
					for (auto direction : {0, 1})
					{
						Vec3i adjacentCell = cellToCell(cell, axis, direction);

						// Bounds check. Use analytical solution for Dirichlet condition.
						if ((direction == 0 && adjacentCell[axis] < 0) || (direction == 1 && adjacentCell[axis] >= gridSize[axis]))
						{
							Vec3d adjacentPoint = Vec3d(myPoissonGrid.indexToWorld(adjacentCell.cast<double>()));
							rhsVector(row) += solutionFunction(adjacentPoint);
						}
						else
						{
							// If neighbouring cell is solvable, it should have an entry in the system
							int adjacentRow = solvableCells(adjacentCell);
							assert(adjacentRow >= 0);

							localSparseElements.emplace_back(row, adjacentRow, -1);
						}
					}
				localSparseElements.emplace_back(row, row, 6);
			}
		});

		mergeLocalThreadVectors(sparseMatrixElements, parallelSparseElements);
	}

    SparseMatrix sparseMatrix(solutionDOFCount, solutionDOFCount);
    sparseMatrix.setFromTriplets(sparseMatrixElements.begin(), sparseMatrixElements.end());

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::Upper | Eigen::Lower> solver;
    solver.compute(sparseMatrix);

    if (solver.info() != Eigen::Success)
    {
        return -1;
    }

    VectorXd solutionVector = solver.solve(rhsVector);

    if (solver.info() != Eigen::Success)
    {
        return -1;
    }

	double error = tbb::parallel_reduce(tbb::blocked_range<int>(0, solvableCells.voxelCount()), double(0),
					[&](const tbb::blocked_range<int>& range, double error) -> double
					{
						for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
						{
							Vec3i cell = solvableCells.unflatten(cellIndex);

							int row = solvableCells(cell);

							assert(row >= 0);

							Vec3d gridPoint = myPoissonGrid.indexToWorld(cell.cast<double>());
							double localError = fabs(solutionVector(row) - solutionFunction(gridPoint));
							error = std::max(error, localError);
						}

						return error;
					},
					[](double a, double b) -> double {
						return std::max(a, b);
					});

    return error;
}

TEST(ANALYTICAL_POISSON_SOLVER_TEST, CONVERGENCE_TEST)
{
	auto rhs = [](const Vec3d& pos) -> double
	{
		return 3. * std::exp(-pos[0] - pos[1] - pos[2]);
	};

	auto solution = [](const Vec3d& pos) -> double
	{
		return std::exp(-pos[0] - pos[1] - pos[2]);
	};

    const int startGrid = 4;
    const int endGrid = startGrid * int(pow(2, 4));

    std::vector<double> errors;
    for (int gridSize = startGrid; gridSize < endGrid; gridSize *= 2)
    {
        double dx = PI / double(gridSize);
		Vec3d origin = Vec3d::Zero();
		Vec3i size = Vec3i::Constant(int(std::round(PI / dx)));
		Transform xform(dx, origin);

		AnalyticalPoissonSolver solver(xform, size);
		double error = solver.solve(rhs, solution);

	    errors.push_back(error);
        EXPECT_GT(error, 0.);
	}

    for (int errorIndex = 1; errorIndex < errors.size(); ++errorIndex)
    {
        double errorRatio = errors[errorIndex - 1] / errors[errorIndex];
        EXPECT_GT(errorRatio, 4.);
    }
}


/*
BSD 3-Clause License

Copyright (c) 2021 Jack Miles Hunt
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#define BOOST_TEST_MODULE HashingTests

#include <utility>

#include <boost/test/included/unit_test.hpp>

#include <CPPUtils/Algorithms/Hashing.hpp>

using namespace CPPUtils::Algorithms;

BOOST_AUTO_TEST_SUITE(HashingTestSuite)

BOOST_AUTO_TEST_CASE(CantorHashTest) {
    BOOST_CHECK_EQUAL(cantorHash(0, 0), 0);
    BOOST_CHECK_EQUAL(cantorHash(0, 1), 2);
    BOOST_CHECK_EQUAL(cantorHash(0, 2), 5);
    BOOST_CHECK_EQUAL(cantorHash(0, 3), 9);

    BOOST_CHECK_EQUAL(cantorHash(1, 0), 1);
    BOOST_CHECK_EQUAL(cantorHash(1, 1), 4);
    BOOST_CHECK_EQUAL(cantorHash(1, 2), 8);
    BOOST_CHECK_EQUAL(cantorHash(1, 3), 13);

    BOOST_CHECK_EQUAL(cantorHash(2, 0), 3);
    BOOST_CHECK_EQUAL(cantorHash(2, 1), 7);
    BOOST_CHECK_EQUAL(cantorHash(2, 2), 12);
    BOOST_CHECK_EQUAL(cantorHash(2, 3), 18);

    BOOST_CHECK_EQUAL(cantorHash(3, 0), 6);
    BOOST_CHECK_EQUAL(cantorHash(3, 1), 11);
    BOOST_CHECK_EQUAL(cantorHash(3, 2), 17);
    BOOST_CHECK_EQUAL(cantorHash(3, 3), 24);
}

BOOST_AUTO_TEST_CASE(InverseCantorHashTest) {
    BOOST_CHECK(inverseCantorHash(0) == std::make_pair<int>(0, 0));
    BOOST_CHECK(inverseCantorHash(2) == std::make_pair<int>(0, 1));
    BOOST_CHECK(inverseCantorHash(5) == std::make_pair<int>(0, 2));
    BOOST_CHECK(inverseCantorHash(9) == std::make_pair<int>(0, 3));

    BOOST_CHECK(inverseCantorHash(1) == std::make_pair<int>(1, 0));
    BOOST_CHECK(inverseCantorHash(4) == std::make_pair<int>(1, 1));
    BOOST_CHECK(inverseCantorHash(8) == std::make_pair<int>(1, 2));
    BOOST_CHECK(inverseCantorHash(13) == std::make_pair<int>(1, 3));

    BOOST_CHECK(inverseCantorHash(3) == std::make_pair<int>(2, 0));
    BOOST_CHECK(inverseCantorHash(7) == std::make_pair<int>(2, 1));
    BOOST_CHECK(inverseCantorHash(12) == std::make_pair<int>(2, 2));
    BOOST_CHECK(inverseCantorHash(18) == std::make_pair<int>(2, 3));

    BOOST_CHECK(inverseCantorHash(6) == std::make_pair<int>(3, 0));
    BOOST_CHECK(inverseCantorHash(11) == std::make_pair<int>(3, 1));
    BOOST_CHECK(inverseCantorHash(17) == std::make_pair<int>(3, 2));
    BOOST_CHECK(inverseCantorHash(24) == std::make_pair<int>(3, 3));
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * An OpenMP snippet.
 *
 * USAGE (GCC):
 *    g++ -fopenmp -c time.cc
 *    g++ -fopenmp time.o
 *
 * In order to use N cores, the shell environment variable OMP_NUM_THREADS
 * should be set (at execution time not at compilation time):
 *    export OMP_NUM_THREADS=N
 *
 * For instance,
 *    export OMP_NUM_THREADS=2
 *    ./time
 * will use 2 cores.
 */

#include <iostream>
#include <cmath>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <omp.h>

static const int N = 20;              // Number of integrate to compute
static const int CHUNKSIZE = 1;       // Defines the chunk size as 1 contiguous iteration
static const double DELTA = 0.00000001;

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

// Compute exact value of F(x) = x³/3
double F(double x) {
    return pow(x, 3) / 3.0;
}

// Numerical computation of F(x) : integ[a,b] x² dx
double num_F(double b) {
    double a = 0.0;
    double y = 0.0;

    for(double x=a ; x<b ; x+=DELTA) {
        y += x * x * DELTA;
    }

    return y;
}

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

void print_tab(const double (& t)[N])
{
    for(int i=0 ; i<N ; i++) {
        std::cout << std::fixed;
        std::cout << "num_F(" << i << ") = " << t[i] << " - F(" << i << ") = " << F(i) << std::endl;
    }
}

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

int main()
{
    double x[N], y[N];

    // Init x and y
    for(int i=0 ; i<N ; i++) {
        x[i] = i;
        y[i] = 0;
    }

    boost::posix_time::ptime start_time = boost::posix_time::microsec_clock::local_time();
    double omp_start_time = omp_get_wtime();

    // Forks off the threads
    #pragma omp parallel
    {
        // Starts the work sharing construct (static / dynamic)
        #pragma omp for schedule(dynamic, CHUNKSIZE)
        for(int i = 0 ; i < N ; i++)
            y[i] = num_F(x[i]);
    }

    boost::posix_time::ptime end_time = boost::posix_time::microsec_clock::local_time();
    boost::posix_time::time_duration delta_time = end_time - start_time;
    double omp_end_time = omp_get_wtime();
    double omp_delta_time = omp_end_time - omp_start_time;

    // Print y
    print_tab(y);

    // Print delta_time
    std::cerr << "boost : " << delta_time << std::endl;
    std::cerr << "omp   : " << omp_delta_time << std::endl;

    return 0;
}


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/significants.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/simd/sdk/config.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( significants,  BOOST_SIMD_REAL_TYPES)
{

  using nt2::significants;
  using nt2::tag::significants_;
  typedef std::complex<T> cT;

#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(significants(nt2::Inf<cT>(), 1), nt2::Inf<cT>());
  NT2_TEST_EQUAL(significants(nt2::Minf<cT>(), 1), nt2::Minf<cT>());
  NT2_TEST_EQUAL(significants(nt2::Nan<cT>(), 1), nt2::Nan<cT>());
#endif
  NT2_TEST_ULP_EQUAL(significants(cT(0), 1), cT(0), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(25.34), 1), cT(30), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(25.34), 2), cT(25), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(25.34), 3), cT(25.3), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(25.34), 4), cT(25.34), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(-25.34), 1), cT(-30), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(-25.34), 2), cT(-25), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(-25.34), 3), cT(-25.3), 0.5);
  NT2_TEST_ULP_EQUAL(significants(cT(-25.34), 4), cT(-25.34), 0.5);
}


#include "exponential_families.h"

#include <cmath>

#include <boost/math/special_functions/trigamma.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>

// #include "variational_parameters.h"

#include <Eigen/Sparse>
typedef Eigen::Triplet<double> Triplet; // For populating sparse matrices

# if INSTANTIATE_EXPONENTIAL_FAMILIES_H
  # include <stan/math.hpp>
  # include "stan/math/fwd/scal.hpp"
  using var = stan::math::var;
  using fvar = stan::math::fvar<var>;
# endif

using boost::math::lgamma;
using boost::math::digamma;
using boost::math::trigamma;

// The index in a vector of lower diagonal terms of a particular matrix value.
int get_ud_index(int i, int j) {
  // If the column is less than the row it's already an upper diagonal index.
  return j <= i ? (j + i * (i + 1) / 2):
                  (i + j * (j + 1) / 2);
};

////////////////////////////////////////////
// Multivariate log gamma and derivatives

///////////////////////////
// Multivariate normals

MatrixXd GetNormalCovariance(VectorXd const &e_mu, MatrixXd const &e_mu2) {
  return e_mu2 - (e_mu * e_mu.transpose());
}

// Get Cov(mu_i1 mu_i2, mu_c mu_d) from the moment parameters of a multivariate
// normal distribution.
//
// e_mu = E(mu)
// cov_mu = E(mu mu^T) - E(mu) E(mu^T)
double GetNormalFourthOrderCovariance(
    VectorXd const &e_mu, MatrixXd const &cov_mu,
		int i1, int i2, int j1, int j2) {

  return (cov_mu(i1, j1) * cov_mu(i2, j2) +
          cov_mu(i1, j2) * cov_mu(i2, j1) +
	      cov_mu(i1, j1) * e_mu(i2) * e_mu(j2) +
          cov_mu(i1, j2) * e_mu(i2) * e_mu(j1) +
	      cov_mu(i2, j1) * e_mu(i1) * e_mu(j2) +
          cov_mu(i2, j2) * e_mu(i1) * e_mu(j1));
};

// Get Cov(mu_i, mu_j1 mu_j2) from the moment parameters of a multivariate
// normal distribution.
//
// e_mu = E(mu)
// cov_mu = E(mu mu^T) - E(mu) E(mu^T)
double GetNormalThirdOrderCovariance(
    VectorXd const &e_mu, MatrixXd const &cov_mu, int i, int j1, int j2) {

  return e_mu(j1) * cov_mu(i, j2) + e_mu(j2) * cov_mu(i, j1);
};

///////////////////////////////////////////
// Wishart distributions

// Construct the covariance of the elements of a Wishart-distributed
// matrix.
//
// Args:
//   - v_par: The wishart matrix parameter.
//   - n_par: The n parameter of the Wishart distribution.
//
// Returns:
//   - Cov(w_i1_j1, w_i2_j2), where w_i1_j1 and w_i2_j2 are terms of the
//     Wishart matrix parameterized by  and n_par.
double GetWishartLinearCovariance(
    MatrixXd const &v_par, double n_par, int i1, int j1, int i2, int j2) {

  return (n_par * (v_par(i1, j2) * v_par(i2, j1) +
			             v_par(i1, i2) * v_par(j1, j2)));
}

// Construct the covariance between the elements of a Wishart-distributed
// matrix and the log determinant.  A little silly as a function, so
// consider this documentation instead.
//
// Args:
//   - v_par: A linearized representation of the upper triangular portion
//            of the wishart parameter.
//
// Returns:
//   - Cov(w_i1_i2, log(det(w)))
double GetWishartLinearLogDetCovariance(MatrixXd const &v_par, int i1, int i2) {
  return 2.0 * v_par(i1, i2);
}

// As above, but
// Cov(log(det(w), log(det(w))))
// ... where k is the dimension of the matrix.
double GetWishartLogDetVariance(double n_par, int k) {
  return multivariate_trigamma(n_par / 2, k);
}

////////////////////////////////////////
// Gamma distribution

// Return a matrix with Cov((g, log(g))) where
// g ~ Gamma(alpha, beta) (parameterization E[g] = alpha / beta)
MatrixXd get_gamma_covariance(double alpha, double beta) {
    MatrixXd gamma_cov(2, 2);
    gamma_cov(0, 0) = alpha / pow(beta, 2);
    gamma_cov(0, 1) = 1 / beta;
    gamma_cov(1, 0) = gamma_cov(0, 1);
    gamma_cov(1, 1) = boost::math::trigamma(alpha);
    return gamma_cov;
}

////////////////////////////
// Categorical

// Args:
//   p: A size k vector of the z probabilities.
// Returns:
//   The covariance matrix.
MatrixXd GetCategoricalCovariance(VectorXd p) {
  MatrixXd p_outer = (-1) * p * p.transpose();
  MatrixXd p_diagonal = p.asDiagonal();
  p_outer = p_outer + p_diagonal;
  return p_outer;
}

std::vector<Triplet> GetCategoricalCovarianceTerms(VectorXd p, int offset) {
  MatrixXd p_cov = GetCategoricalCovariance(p);
  std::vector<Triplet> terms;
  for (int i=0; i < p_cov.rows(); i++) {
    for (int j=0; j < p_cov.cols(); j++) {
      terms.push_back(Triplet(offset + i, offset + j, p_cov(i, j)));
    }
  }
  return terms;
}

/////////////////////////////////
// Dirichlet

MatrixXd GetLogDirichletCovariance(VectorXd alpha) {
  // Args:
  //  - alpha: A vector of dirichlet parameters.
  //
  // Returns:
  //  - The covariance of the log of a dirichlet distribution
  //    with parameters alpha.

  int k = alpha.size();
  int k_index;
  MatrixXd cov_mat(k, k);

  // Precomute the total.
  double alpha_0 = 0.0;
  for (k_index = 0; k_index < k; k_index++) {
    alpha_0 += alpha(k_index);
  }
  double covariance_term = -1.0 * boost::math::trigamma(alpha_0);
  cov_mat.setConstant(covariance_term);

  // Only the diagonal entries deviate from covariance_term.
  for (k_index = 0; k_index < k; k_index++) {
    cov_mat(k_index, k_index) += boost::math::trigamma(alpha(k_index));
  }
  return cov_mat;
};

///////////////////////////////////
// Coordinates and covariances for sparse matrices

// Assumes that e_mu and e_mu2_offset are stored linearly starting
// at their respective offsets.
// TODO: like everything else, express this in terms of natural parameters.
std::vector<Triplet> get_mvn_covariance_terms(
    VectorXd e_mu, MatrixXd e_mu2, int e_mu_offset, int e_mu2_offset) {

  std::vector<Triplet> terms;
  int k = e_mu.size();
  if (k != e_mu2.rows() || k !=e_mu2.cols()) {
    throw std::runtime_error("e_mu2 is not square");
  }

  MatrixXd cov_mu = GetNormalCovariance(e_mu, e_mu2);

  // Cov(mu, mu^T)
  for (int i = 0; i < k; i++) {
    for (int j = 0; j < k; j++) {
      terms.push_back(Triplet(e_mu_offset + i, e_mu_offset + j, cov_mu(i, j)));
    }
  }

  // Cov(mu, mu mu^T)
  for (int j1 = 0; j1 < k; j1++) {
    for (int j2 = 0; j2 <= j1; j2++) {
      for (int i = 0; i < k; i++) {
        double this_cov = GetNormalThirdOrderCovariance(e_mu, cov_mu, i, j1, j2);
        terms.push_back(
          Triplet(e_mu_offset + i, e_mu2_offset + get_ud_index(j1, j2),
                  this_cov));
        terms.push_back(
          Triplet(e_mu2_offset + get_ud_index(j1, j2), e_mu_offset + i,
                  this_cov));
      }
    }
  }

  // Cov(mu mu^T, mu mu^T)
  for (int i1 = 0; i1 < k; i1++) { for (int i2 = 0; i2 <= i1; i2++) {
    for (int j1 = 0; j1 < k; j1++) { for (int j2 = 0; j2 <= j1; j2++) {
      double this_cov = GetNormalFourthOrderCovariance(e_mu, cov_mu, i1, i2, j1, j2);
      terms.push_back(Triplet(
        e_mu2_offset + get_ud_index(i1, i2),
        e_mu2_offset + get_ud_index(j1, j2),
        this_cov));
      }}
  }}

  return terms;
};

std::vector<Triplet> get_normal_covariance_terms(
    double mean, double info, int e_mu_offset, int e_mu2_offset) {

    MatrixXd cov_mu(1, 1);
    cov_mu << 1 / info;
    VectorXd e_mu(1);
    e_mu << mean;

    std::vector<Triplet> terms;
    terms.push_back(Triplet(e_mu_offset, e_mu_offset, cov_mu(0, 0)));
    double cov_mu_mu2 = GetNormalThirdOrderCovariance(e_mu, cov_mu, 0, 0, 0);
    terms.push_back(Triplet(e_mu_offset, e_mu2_offset, cov_mu_mu2));
    terms.push_back(Triplet(e_mu2_offset, e_mu_offset, cov_mu_mu2));
    double cov_mu2_mu2 = GetNormalFourthOrderCovariance(e_mu, cov_mu, 0, 0, 0, 0);
    terms.push_back(Triplet(e_mu2_offset, e_mu2_offset, cov_mu2_mu2));
    return terms;
};

std::vector<Triplet> get_wishart_covariance_terms(
    MatrixXd v_par, double n_par, int e_lambda_offset, int e_log_det_lambda_offset) {

  std::vector<Triplet> terms;
  int k = v_par.rows();
  if (k != v_par.cols()) {
    throw std::runtime_error("V is not square");
  }

  for (int i1 = 0; i1 < k; i1++) { for (int j1 = 0; j1 <= i1; j1++) {
    int i_ind = e_lambda_offset + get_ud_index(i1, j1);
    double this_cov = GetWishartLinearLogDetCovariance(v_par, i1, j1);
    terms.push_back(Triplet(i_ind, e_log_det_lambda_offset, this_cov));
    terms.push_back(Triplet(e_log_det_lambda_offset, i_ind, this_cov));
	  for (int i2 = 0; i2 < k; i2++) { for (int j2 = 0; j2 <= i2; j2++) {
      int j_ind = e_lambda_offset + get_ud_index(i2, j2);
	    terms.push_back(Triplet(i_ind, j_ind,
        GetWishartLinearCovariance(v_par, n_par, i1, j1, i2, j2)));
	  }}
  }}
  terms.push_back(Triplet(e_log_det_lambda_offset, e_log_det_lambda_offset,
    GetWishartLogDetVariance(n_par, k)));

  return terms;
};

std::vector<Triplet> get_gamma_covariance_terms(
    double alpha, double beta, int e_tau_offset, int e_log_tau_offset) {

  std::vector<Triplet> terms;
  MatrixXd tau_cov = get_gamma_covariance(alpha, beta);
  terms.push_back(Triplet(e_tau_offset, e_tau_offset, tau_cov(0, 0)));
  terms.push_back(Triplet(e_log_tau_offset, e_tau_offset, tau_cov(0, 1)));
  terms.push_back(Triplet(e_tau_offset, e_log_tau_offset, tau_cov(1, 0)));
  terms.push_back(Triplet(e_log_tau_offset, e_log_tau_offset, tau_cov(1, 1)));

  return terms;
};

std::vector<Triplet> get_dirichlet_covariance_terms(VectorXd alpha, int offset) {
  std::vector<Triplet> terms;
  MatrixXd q_cov = GetLogDirichletCovariance(alpha);
  for (int i=0; i < q_cov.rows(); i++) {
    for (int j=0; j < q_cov.cols(); j++) {
      terms.push_back(Triplet(offset + i, offset + j, q_cov(i, j)));
    }
  }

  return terms;
}

# if INSTANTIATE_EXPONENTIAL_FAMILIES_H
  template double multivariate_lgamma(double x, int p);
  template var multivariate_lgamma(var x, int p);
  template fvar multivariate_lgamma(fvar x, int p);

  template double multivariate_digamma(double x, int p);
  template var multivariate_digamma(var x, int p);
  template fvar multivariate_digamma(fvar x, int p);

  template double multivariate_trigamma(double x, int p);
  // Not implemented.
  // template var multivariate_trigamma(var x, int p);
  // template fvar multivariate_trigamma(fvar x, int p);

  template double GetELogDetWishart(MatrixXT<double> v_par, double n_par);
  template var GetELogDetWishart(MatrixXT<var> v_par, var n_par);
  template fvar GetELogDetWishart(MatrixXT<fvar> v_par, fvar n_par);

  template double GetWishartEntropy(MatrixXT<double> const &v_par, double const n_par);
  template var GetWishartEntropy(MatrixXT<var> const &v_par, var const n_par);
  template fvar GetWishartEntropy(MatrixXT<fvar> const &v_par, fvar const n_par);

  template double get_e_log_gamma(double alpha, double beta);
  template var get_e_log_gamma(var alpha, var beta);
  template fvar get_e_log_gamma(fvar alpha, fvar beta);

  template VectorXT<double> GetELogDirichlet(VectorXT<double> alpha);
  template VectorXT<var> GetELogDirichlet(VectorXT<var> alpha);
  template VectorXT<fvar> GetELogDirichlet(VectorXT<fvar> alpha);

  template double GetDirichletEntropy(VectorXT<double> alpha);
  template var GetDirichletEntropy(VectorXT<var> alpha);
  template fvar GetDirichletEntropy(VectorXT<fvar> alpha);

  template double GetMultivariateNormalEntropy(MatrixXT<double>);
  template var GetMultivariateNormalEntropy(MatrixXT<var>);
  template fvar GetMultivariateNormalEntropy(MatrixXT<fvar>);

  template double GetUnivariateNormalEntropy(double);
  template var GetUnivariateNormalEntropy(var);
  template fvar GetUnivariateNormalEntropy(fvar);

  template double GetGammaEntropy(double, double);
  template var GetGammaEntropy(var, var);
  template fvar GetGammaEntropy(fvar, fvar);
# endif


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

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <boost/math/interpolators/cardinal_quintic_b_spline.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif
using boost::math::interpolators::cardinal_quintic_b_spline;

template<class Real>
void test_constant()
{
    Real c = 7.5;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 513;
    std::vector<Real> v(n, c);
    std::pair<Real, Real> left_endpoint_derivatives{0, 0};
    std::pair<Real, Real> right_endpoint_derivatives{0, 0};
    auto qbs = cardinal_quintic_b_spline<Real>(v.data(), v.size(), t0, h, left_endpoint_derivatives, right_endpoint_derivatives);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      CHECK_ULP_CLOSE(c, qbs(t), 3);
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.prime(t), 400*std::numeric_limits<Real>::epsilon());
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.double_prime(t), 60000*std::numeric_limits<Real>::epsilon());
      ++i;
    }

    i = 0;
    while (i < n - 1) {
      Real t = t0 + i*h + h/2;
      CHECK_ULP_CLOSE(c, qbs(t), 5);
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.prime(t), 600*std::numeric_limits<Real>::epsilon());
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.double_prime(t), 30000*std::numeric_limits<Real>::epsilon());
      t = t0 + i*h + h/4;
      CHECK_ULP_CLOSE(c, qbs(t), 4);
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.prime(t), 600*std::numeric_limits<Real>::epsilon());
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.double_prime(t), 10000*std::numeric_limits<Real>::epsilon());
      ++i;
    }
}

template<class Real>
void test_constant_estimate_derivatives()
{
    Real c = 7.5;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 513;
    std::vector<Real> v(n, c);
    auto qbs = cardinal_quintic_b_spline<Real>(v.data(), v.size(), t0, h);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      CHECK_ULP_CLOSE(c, qbs(t), 3);
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.prime(t), 1200*std::numeric_limits<Real>::epsilon());
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.double_prime(t), 200000*std::numeric_limits<Real>::epsilon());
      ++i;
    }

    i = 0;
    while (i < n - 1) {
      Real t = t0 + i*h + h/2;
      CHECK_ULP_CLOSE(c, qbs(t), 8);
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.prime(t), 1200*std::numeric_limits<Real>::epsilon());
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.double_prime(t), 80000*std::numeric_limits<Real>::epsilon());
      t = t0 + i*h + h/4;
      CHECK_ULP_CLOSE(c, qbs(t), 5);
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.prime(t), 1200*std::numeric_limits<Real>::epsilon());
      CHECK_MOLLIFIED_CLOSE(Real(0), qbs.double_prime(t), 38000*std::numeric_limits<Real>::epsilon());
      ++i;
    }
}

template<class Real>
void test_linear()
{
    using std::abs;
    Real m = 8.3;
    Real b = 7.2;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 512;
    std::vector<Real> y(n);
    for (size_t i = 0; i < n; ++i) {
      Real t = i*h;
      y[i] = m*t + b;
    }
    std::pair<Real, Real> left_endpoint_derivatives{m, 0};
    std::pair<Real, Real> right_endpoint_derivatives{m, 0};
    auto qbs = cardinal_quintic_b_spline<Real>(y.data(), y.size(), t0, h, left_endpoint_derivatives, right_endpoint_derivatives);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      if (!CHECK_ULP_CLOSE(m*t+b, qbs(t), 3)) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      if(!CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 100*abs(m*t+b)*std::numeric_limits<Real>::epsilon())) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      if(!CHECK_MOLLIFIED_CLOSE(0, qbs.double_prime(t), 10000*abs(m*t+b)*std::numeric_limits<Real>::epsilon())) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      ++i;
    }

    i = 0;
    while (i < n - 1) {
      Real t = t0 + i*h + h/2;
      if(!CHECK_ULP_CLOSE(m*t+b, qbs(t), 4)) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 1500*std::numeric_limits<Real>::epsilon());
      t = t0 + i*h + h/4;
      if(!CHECK_ULP_CLOSE(m*t+b, qbs(t), 4)) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 3000*std::numeric_limits<Real>::epsilon());
      ++i;
    }
}

template<class Real>
void test_linear_estimate_derivatives()
{
    using std::abs;
    Real m = 8.3;
    Real b = 7.2;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 512;
    std::vector<Real> y(n);
    for (size_t i = 0; i < n; ++i) {
      Real t = i*h;
      y[i] = m*t + b;
    }

    auto qbs = cardinal_quintic_b_spline<Real>(y.data(), y.size(), t0, h);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      if (!CHECK_ULP_CLOSE(m*t+b, qbs(t), 3)) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      if(!CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 100*abs(m*t+b)*std::numeric_limits<Real>::epsilon())) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      if(!CHECK_MOLLIFIED_CLOSE(0, qbs.double_prime(t), 20000*abs(m*t+b)*std::numeric_limits<Real>::epsilon())) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      ++i;
    }

    i = 0;
    while (i < n - 1) {
      Real t = t0 + i*h + h/2;
      if(!CHECK_ULP_CLOSE(m*t+b, qbs(t), 5)) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 1500*std::numeric_limits<Real>::epsilon());
      t = t0 + i*h + h/4;
      if(!CHECK_ULP_CLOSE(m*t+b, qbs(t), 4)) {
          std::cerr << "  Problem at t = " << t << "\n";
      }
      CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 3000*std::numeric_limits<Real>::epsilon());
      ++i;
    }
}

template<class Real>
void test_quadratic()
{
    Real a = Real(1)/Real(16);
    Real b = -3.5;
    Real c = -9;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 513;
    std::vector<Real> y(n);
    for (size_t i = 0; i < n; ++i) {
      Real t = i*h;
      y[i] = a*t*t + b*t + c;
    }
    Real t_max = t0 + (n-1)*h;
    std::pair<Real, Real> left_endpoint_derivatives{b, 2*a};
    std::pair<Real, Real> right_endpoint_derivatives{2*a*t_max + b, 2*a};

    auto qbs = cardinal_quintic_b_spline<Real>(y, t0, h, left_endpoint_derivatives, right_endpoint_derivatives);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 3);
      ++i;
    }

    i = 0;
    while (i < n -1) {
      Real t = t0 + i*h + h/2;
      if(!CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 5)) {
          std::cerr << "  Problem at abscissa t = " << t << "\n";
      }

      t = t0 + i*h + h/4;
      if (!CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 5)) {
          std::cerr << "  Problem abscissa t = " << t << "\n";
      }
      ++i;
    }
}

template<class Real>
void test_quadratic_estimate_derivatives()
{
    Real a = Real(1)/Real(16);
    Real b = -3.5;
    Real c = -9;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 513;
    std::vector<Real> y(n);
    for (size_t i = 0; i < n; ++i) {
      Real t = i*h;
      y[i] = a*t*t + b*t + c;
    }
    auto qbs = cardinal_quintic_b_spline<Real>(y, t0, h);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 3);
      ++i;
    }

    i = 0;
    while (i < n -1) {
      Real t = t0 + i*h + h/2;
      if(!CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 10)) {
          std::cerr << "  Problem at abscissa t = " << t << "\n";
      }

      t = t0 + i*h + h/4;
      if (!CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 6)) {
          std::cerr << "  Problem abscissa t = " << t << "\n";
      }
      ++i;
    }
}

int main()
{
    test_constant<double>();
    test_constant<long double>();

    test_constant_estimate_derivatives<double>();
    test_constant_estimate_derivatives<long double>();

    test_linear<float>();
    test_linear<double>();
    test_linear<long double>();

    test_linear_estimate_derivatives<double>();
    test_linear_estimate_derivatives<long double>();

    test_quadratic<double>();
    test_quadratic<long double>();

    test_quadratic_estimate_derivatives<double>();
    test_quadratic_estimate_derivatives<long double>();

    #ifdef BOOST_HAS_FLOAT128
        test_constant<float128>();
        test_linear<float128>();
        test_linear_estimate_derivatives<float128>();
    #endif

    return boost::math::test::report_errors();
}


#include <armadillo>
#include "armaMex.hpp"
using namespace arma;

void mexFunction(int nlhs,mxArray *plhs[],int nrhs, const mxArray *prhs[]) {

    mat K0 = armaGetPr(prhs[0]);
    mat S = armaGetPr(prhs[1]);
    uvec idr = conv_to<uvec>::from(armaGetPr(prhs[2])) - 1;
    uvec idc = conv_to<uvec>::from(armaGetPr(prhs[3])) - 1;
    uword max_outer_iter, max_inner_iter;
    if (nrhs > 4) max_outer_iter = (uword)armaGetDouble(prhs[4]);
    else max_outer_iter = 100;
    if (nrhs > 5) max_inner_iter = (uword)armaGetDouble(prhs[5]);
    else max_inner_iter = 100;

    uword p = K0.n_cols, iter_outer, iter_inner, p_od = idr.n_elem, i;
    uvec idl = idc * p + idr, idu = idr * p + idc, idd = linspace<uvec>(0, p - 1, p), ida = join_cols(idd * p + idd, idl);
    vec Sida = S(ida), Sidu = S(idu), Did(p + p_od), gradK, Kd = K0.diag();
    mat U(p, p, fill::zeros), W, Kh(p, p, fill::zeros);
    double a, b, mu, diffD = 0;

    double objh, xi0 = 1, xi, sum_grad2;
    while (! K0.is_sympd()) {
        K0 *= 0.9;
        K0.diag() = Kd;
    }
    double obj0 = - 2*accu(log(mat(chol(K0)).diag())) + accu(Sida % K0(ida)) + accu(Sidu % K0(idu));

    for (iter_outer = 0; iter_outer < max_outer_iter; iter_outer ++) {
        W = inv_sympd(K0);
        gradK = Sida - W(ida);
        Did.zeros();
        U.zeros();
        for (iter_inner = 0; iter_inner < max_inner_iter; iter_inner ++) {
            for (i = 0; i < p; i++) {
                a = pow(W(i, i), 2);
                b = gradK(i) + accu(W.col(i) % U.col(i));
                mu = - b / a;
                Did(i) += mu;
                U.row(i) += mu * W.row(i);
                diffD += fabs(mu);
            }

            for (i = 0; i < p_od; i ++) {
                a = pow(W(idl(i)), 2) + W(idr(i), idr(i)) * W(idc(i), idc(i));
                b = gradK(p + i) + accu(W.col(idr(i)) % U.col(idc(i)));
                mu = - b / a;
                Did(p + i) += mu;
                U.row(idr(i)) += mu * W.row(idc(i));
                U.row(idc(i)) += mu * W.row(idr(i));
                diffD += fabs(mu);
            }

            if (diffD < 0.05 * accu(abs(Did))) break;
            else diffD = 0;
        }
        sum_grad2 = accu(Did % gradK);
        xi = xi0;
        while (true) {
            Kh(ida) = K0(ida) + xi * Did;
            Kh(idu) = Kh(idl);
            if (Kh.is_sympd()) {
                objh = - 2*accu(log(mat(chol(Kh)).diag())) + accu(Sida % Kh(ida)) + accu(Sidu % Kh(idu));
                if (objh <= obj0 + 1e-3 * xi * sum_grad2)
                    break;
                else
                    xi /= 2;
            } else
                xi /=2;
        }
        // printf("xi = %e\n", xi);
        if (abs(Kh - K0).max() < 1e-10 && fabs(obj0 - objh ) < 1e-10) {
            break;
        } else {
            obj0 = objh;
            K0 = Kh;
        }
    }

    plhs[0] = armaCreateMxMatrix(p,p,mxDOUBLE_CLASS,mxREAL);
    armaSetPr(plhs[0],Kh);
}


#include <Eigen/SparseCore>
#include <hops/FileReader/CsvReader.hpp>
#include <hops/FileWriter/FileWriterFactory.hpp>
#include <hops/LinearProgram/LinearProgramFactory.hpp>
#include <hops/MarkovChain/MarkovChainFactory.hpp>
#include <hops/Model/MultivariateGaussianModel.hpp>
#include <hops/MarkovChain/Tuning/BinarySearchAcceptanceRateTuner.hpp>
#include <hops/Polytope/NormalizePolytope.hpp>
#include <iostream>
#include <hops/Polytope/MaximumVolumeEllipsoid.hpp>

using RealType = double;

int main(int argc, char **argv) {
    if (argc != 10 && argc != 9) {
        std::cout << "usage: SamplingGaussianTarget A.csv b.csv mean.csv covariance.csv "
                  << "numberOfSamples thinningNumber CHRR|HRR|DikinWalk outputName [startingPoint.csv]"
                  << "\nArgument Description:\n"
                  << "\tA.csv\t\t\t\t nxm dimensional matrix of polytope Ax<b\n"
                  << "\tb.csv\t\t\t\t n dimensional vector of polytope Ax<b\n"
                  << "\tmean.csv\t\t\t m dimensional vector\n"
                  << "\tcovariance.csv\t\t mxm dimensional matrix\n"
                  << "\tnumberOfSamples\t\t number of samples to generate\n"
                  << "\tthinningNumber\t\t number of markov chain iterations per sample\n"
                  << "\talgorithm\t\t\t CHRR or HRR or DikinWalk\n"
                  << "\toutputName\t\t\t name for output\n"
                  << "\t[startingPoint]\t\t optional starting point, useful for resuming sampling" << std::endl;
        exit(0);
    }

    Eigen::SparseMatrix<RealType> A = hops::CsvReader::readMatrix<Eigen::SparseMatrix<double>>(
            argv[1]).cast<RealType>();
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> b = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
            argv[2]).cast<RealType>();
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> mean = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
            argv[3]).cast<RealType>();
    Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> covariance = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
            argv[4]).cast<RealType>();
    long numberOfSamples = std::strtol(argv[5], NULL, 10);
    long thinning = std::strtol(argv[6], NULL, 10);
    std::string chainName = argv[7];

    hops::MultivariateGaussianModel model(mean, covariance);

    std::unique_ptr<hops::MarkovChain> markovChain;
    if (chainName == "DikinWalk") {
        hops::MarkovChainType chainType = hops::MarkovChainType::DikinWalk;
        decltype(b) startingPoint;
        if (argc == 10) {
            startingPoint = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
                    argv[9]).cast<RealType>();
        } else {
            std::unique_ptr<hops::LinearProgram> linearProgram = hops::LinearProgramFactory::createLinearProgram(
                    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>(A.cast<double>()),
                    b.cast<double>();
            startingPoint = linearProgram->computeChebyshevCenter().optimalParameters.cast<RealType>();
        }

        markovChain = hops::MarkovChainFactory::createMarkovChain(chainType,
                                                                  A,
                                                                  b,
                                                                  startingPoint,
                                                                  model);
    } else if (chainName == "CHRR" || chainName == "HRR") {
        hops::MarkovChainType chainType =
                chainName == "CHRR" ? hops::MarkovChainType::CoordinateHitAndRun : hops::MarkovChainType::HitAndRun;
        Eigen::MatrixXd roundingTransformation = hops::MaximumVolumeEllipsoid<double>::construct(
                A,
                b,
                50000, 1e-9).getRoundingTransformation();

        decltype(b) startingPoint;
        if (argc == 10) {
            startingPoint = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
                    argv[9]).cast<RealType>();
        } else {
            std::unique_ptr<hops::LinearProgram> linearProgram = hops::LinearProgramFactory::createLinearProgram(
                    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>((A * roundingTransformation).cast<double>()),
                    b.cast<double>());
            startingPoint = linearProgram->computeChebyshevCenter().optimalParameters.cast<RealType>();
        }
        markovChain = hops::MarkovChainFactory::createMarkovChain<Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>, decltype(b), decltype(model)>(
                chainType,
                Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>(A * roundingTransformation),
                b,
                startingPoint,
                roundingTransformation,
                decltype(startingPoint)::Zero(roundingTransformation.rows()),
                model);
    } else {
        std::cerr << "No chain with chainname " << chainName << std::endl;
        std::exit(1);
    }

    hops::RandomNumberGenerator randomNumberGenerator((std::random_device()()));

    float upperLimitAcceptanceRate = 0.3;
    float lowerLimitAcceptanceRate = 0.20;
    double lowerLimitStepSize = 1e-15;
    double upperLimitStepSize = 1;
    size_t iterationsToTestStepSize = 100 * A.cols();
    size_t maxIterations = 10000 * A.cols();

    bool isTuned = false;
    // Tuning loop
    for (int i = 0; i < 10; ++i) {
        markovChain->draw(randomNumberGenerator, 1, numberOfSamples);
        markovChain->setAttribute(hops::MarkovChainAttribute::STEP_SIZE, 1);

        isTuned = hops::AcceptanceRateTuner::tune(markovChain.get(),
                                                  randomNumberGenerator,
                                                  {lowerLimitAcceptanceRate,
                                                   upperLimitAcceptanceRate,
                                                   lowerLimitStepSize,
                                                   upperLimitStepSize,
                                                   iterationsToTestStepSize,
                                                   maxIterations});
        markovChain->clearHistory();
    }
    std::cout << "Markov chain tuned successfully : " << std::boolalpha << isTuned
              << " (false is not a problem for CHRR|HRR)" << std::endl;
    std::cout << "Current step size: " << markovChain->getAttribute(hops::MarkovChainAttribute::STEP_SIZE) << std::endl;

    auto fileWriter = hops::FileWriterFactory::createFileWriter(std::string(argv[8]) + "_" + markovChain->getName(),
                                                                hops::FileWriterType::CSV);
    markovChain->draw(randomNumberGenerator, numberOfSamples, thinning);
    markovChain->writeHistory(fileWriter.get());
    markovChain->clearHistory();
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "centroid.h"
#include <Eigen/Geometry>

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedc,
  typename Derivedvol>
IGL_INLINE void igl::centroid(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<Derivedc>& cen,
  Derivedvol & vol)
{
  using namespace Eigen;
  assert(F.cols() == 3 && "F should contain triangles.");
  assert(V.cols() == 3 && "V should contain 3d points.");
  const int m = F.rows();
  cen.setZero();
  vol = 0;
  // loop over faces
  for(int f = 0;f<m;f++)
  {
    // "Calculating the volume and centroid of a polyhedron in 3d" [Nuernberg 2013]
    // http://www2.imperial.ac.uk/~rn/centroid.pdf
    // rename corners
    typedef Eigen::Matrix<typename DerivedV::Scalar,1,3> RowVector3S;
    const RowVector3S & a = V.row(F(f,0));
    const RowVector3S & b = V.row(F(f,1));
    const RowVector3S & c = V.row(F(f,2));
    // un-normalized normal
    const RowVector3S & n = (b-a).cross(c-a);
    // total volume via divergence theorem: ∫ 1
    vol += n.dot(a)/6.;
    // centroid via divergence theorem and midpoint quadrature: ∫ x
    cen.array() += (1./24.*n.array()*((a+b).array().square() + (b+c).array().square() +
        (c+a).array().square()).array());
  }
  cen *= 1./(2.*vol);
}

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedc>
IGL_INLINE void igl::centroid(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<Derivedc>& c)
{
  typename Derivedc::Scalar vol;
  return centroid(V,F,c,vol);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::centroid<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&, double&);
// generated by autoexplicit.sh
template void igl::centroid<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1>, float>(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 3, 1, 0, 3, 1> >&, float&);
// generated by autoexplicit.sh
template void igl::centroid<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 3, 1, 0, 3, 1> >&);
// generated by autoexplicit.sh
template void igl::centroid<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> >&);
template void igl::centroid<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
template void igl::centroid<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
#endif


/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#include <Eigen/Dense>

#include "morleyElement.h"

#include "morleyElementBuilder.h"

namespace Vitelotte
{

template < class _Mesh, typename _Scalar >
MorleyElementBuilder<_Mesh, _Scalar>::MorleyElementBuilder(Scalar sigma)
    : m_sigma(sigma)
{
}

template < class _Mesh, typename _Scalar >
unsigned
MorleyElementBuilder<_Mesh, _Scalar>::
    nCoefficients(const Mesh& /*mesh*/, Face /*element*/,
                  SolverError* /*error*/) const
{
    return 36;
}

template < class _Mesh, typename _Scalar >
template < typename Inserter >
void
MorleyElementBuilder<_Mesh, _Scalar>::
    addCoefficients(Inserter& inserter, const Mesh& mesh,
                    Face element, SolverError* error)
{
    if(mesh.valence(element) != 3)
    {
        if(error) error->error("Non-triangular face");
        return;
    }

    // TODO: remove dynamic allocation with dynamic dims.
    Vector p[3];
    Vector v[3];
    bool orient[3];
    int nodes[6];

    typename Mesh::HalfedgeAroundFaceCirculator hit = mesh.halfedges(element);
    --hit;
    for(int i = 0; i < 3; ++i)
    {
        v[i] = (mesh.position(mesh.toVertex(*hit)) -
                mesh.position(mesh.fromVertex(*hit))).template cast<Scalar>();
        orient[i] = mesh.halfedgeOrientation(*hit);
        nodes[i+3] = mesh.edgeGradientNode(*hit).idx();
        ++hit;
        nodes[i] = mesh.toVertexValueNode(*hit).idx();
        p[i] = mesh.position(mesh.toVertex(*hit)).template cast<Scalar>();
    }

    for(int i = 0; i < 6; ++i)
    {
        if(nodes[i] < 0)
        {
            if(error) error->error("Invalid node");
            return;
        }
    }

    typedef MorleyElement<Scalar> Elem;
    Elem elem(p);

    if(elem.doubleArea() <= 0 && error)
    {
        error->warning("Degenerated or reversed triangle");
    }

    typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
    Vector6 dx2;
    Vector6 dxy;
    Vector6 dy2;
    Vector3 bc = Vector3(1, 1, 1) / 3;
    typename Elem::Hessian hessians[6];
    elem.hessian(bc, hessians);

    for(int bi = 0; bi < 6; ++bi)
    {
        dx2(bi) = hessians[bi](0, 0);
        dy2(bi) = hessians[bi](1, 1);
        dxy(bi) = hessians[bi](0, 1);
    }

    for(int i = 0; i < 6; ++i)
    {
        for(int j = i; j < 6; ++j)
        {
            Scalar value =
                    ((dx2(i) + dy2(i)) * (dx2(j) + dy2(j)) +
                    (1-m_sigma) * ( 2*dxy(i)*dxy(j) - dx2(i)*dy2(j) - dy2(i)*dx2(j)));
            value *= elem.doubleArea() / 2;
            if((i < 3 || orient[i%3]) != (j < 3 || orient[j%3]))
            {
                value *= -1;
            }
            inserter.addCoeff(nodes[i], nodes[j], value);
        }
    }
}

}


#ifndef NN_BASIS_FUNCTION_H
#define NN_BASIS_FUNCTION_H

#include <mlpack/core.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <armadillo>
#include <json.hpp>
#include <robotics/basis_functions.hpp>

namespace hsmm {

    typedef mlpack::ann::FFN<mlpack::ann::MeanSquaredError<>,
            mlpack::ann::RandomInitialization> NNmodel;

    class ScalarNNBasis : public robotics::ScalarBasisFun {
        public:
            ScalarNNBasis(arma::ivec hidden_units_per_layer, int njoints);

            ScalarNNBasis(nlohmann::json &stream);

            NNmodel& getNeuralNet() const;

            // Takes into account also the input and output layers.
            int getNumberLayers() const;

            std::pair<arma::mat, arma::vec> getOutputLayerParams() const;

            void setNeuralNet(NNmodel &neural_net);

            arma::vec eval(double t) const;

            // TODO.
            arma::vec deriv(double time, unsigned int order) const;

            unsigned int dim() const;

            nlohmann::json to_stream() const;

            ~ScalarNNBasis() = default;

        private:
            arma::ivec hidden_units_per_layer_;
            mlpack::ann::Linear<>* output_layer_ = nullptr;
            mutable NNmodel neural_net_;
            int neural_net_outputs_;
    };

};

#endif


#include "ValueWithError.hpp"
#include "precompiled.hpp"

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wglobal-constructors"
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#include <boost/test/unit_test.hpp>

#include <iostream>

using namespace error_propagation;

namespace {

  typedef ValueWithError<double> VD;
  using boost::math::pow;

  const double D_EPS             = std::numeric_limits<double>::epsilon();
  const double D_LARGE_EPS       = 1000.0 * std::numeric_limits<double>::epsilon();

  const double half_pi           = boost::math::constants::half_pi<double>();
  const double pi                = boost::math::constants::pi<double>();
  const double e                 = boost::math::constants::e<double>();
  const double ln_two            = boost::math::constants::ln_two<double>();
  const double ln_ten            = boost::math::constants::ln_ten<double>();
  const double one_div_root_two  = boost::math::constants::one_div_root_two<double>();
  const double root_two          = boost::math::constants::root_two<double>();
  const double three_quarters_pi = boost::math::constants::three_quarters_pi<double>();
  const double quarter_pi        = boost::math::constants::pi<double>() / 4.0;
  const double one_div_root_pi   = boost::math::constants::one_div_root_pi<double>();
  const double two_div_root_pi   = boost::math::constants::one_div_root_pi<double>() * 2.0;
  const double euler             = boost::math::constants::euler<double>();

  struct Fixture
  {
    Fixture()
      :
      pvMinus(-2.0,4.0),
      pvPlus(2.0,4.0)
    {}

    const VD pvMinus, pvPlus, pvZero;
  };

} // anonymous namespace

BOOST_FIXTURE_TEST_SUITE(Test_ValueWithError_MathOverloads,Fixture)

BOOST_AUTO_TEST_SUITE(_expm1)

BOOST_AUTO_TEST_CASE(positive)
{
  VD pv(1.0,2.0);
  VD result = expm1(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),e - 1.0,D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0 * e,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD pv(0.0,2.0);
  VD result = expm1(pv);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD pv(-1.0,2.0);
  VD result = expm1(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0 / e - 1.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0 / e,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _expm1

BOOST_AUTO_TEST_SUITE(_exp2)

BOOST_AUTO_TEST_CASE(positive)
{
  VD pv(1.0,2.0);
  VD result = exp2(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),2.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),ln_two * 2.0 * 2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD pv(0.0,2.0);
  VD result = exp2(pv);
  BOOST_CHECK_CLOSE(result.GetValue(), 1.0, D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), ln_two*2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD pv(-1.0,2.0);
  VD result = exp2(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),0.5,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), ln_two * 0.5 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _exp2

BOOST_AUTO_TEST_SUITE(_log2)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(2.0,2.0);
  VD result = log2(val);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / ln_two, D_LARGE_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = log2(val);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-2.0,2.0);
  VD result = log2(val);
  BOOST_CHECK(boost::math::isnan(result.GetValue()));
  BOOST_CHECK_CLOSE(result.GetError(), 0.5 / ln_two * 2.0, D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _log2

BOOST_AUTO_TEST_SUITE(_log1p)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(1.0,2.0);
  VD result = log1p(val);
  BOOST_CHECK_CLOSE(result.GetValue(),ln_two,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0, D_LARGE_EPS);
}

BOOST_AUTO_TEST_CASE(negative_special_case)
{
  // -1.0 is special
  VD val(-1.0,2.0);
  VD result = log1p(val);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-3.0,2.0);
  VD result = log1p(val);
  BOOST_CHECK(boost::math::isnan(result.GetValue()));
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 , D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _log1p

BOOST_AUTO_TEST_SUITE(_asinh)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(1.0,2.0);
  VD result = asinh(val);
  BOOST_CHECK_CLOSE(result.GetValue(), std::log(1.0 + root_two), D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / root_two * 2.0, D_LARGE_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = asinh(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-1.0,2.0);
  VD result = asinh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),std::log(-1.0 + root_two),D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / root_two * 2.0 ,D_LARGE_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _asinh

BOOST_AUTO_TEST_SUITE(_acosh)

BOOST_AUTO_TEST_CASE(positive_special_plus_one)
{
  VD val(1.0,2.0);
  VD result = acosh(val);
  BOOST_CHECK_SMALL(result.GetValue(), D_EPS);
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(2.0,3.0);
  VD result = acosh(val);
  BOOST_CHECK_CLOSE(result.GetValue(), std::log( 2.0 + std::sqrt(3.0) ), D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / std::sqrt(3.0) * 3.0, D_LARGE_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _acosh

BOOST_AUTO_TEST_SUITE(_atanh)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(0.5,2.0);
  VD result = atanh(val);
  BOOST_CHECK_CLOSE(result.GetValue(), 0.5 * std::log(3.0), D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 4.0/3.0 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(positive_one_special)
{
  VD val(1.0,2.0);
  VD result = atanh(val);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = atanh(val);
  BOOST_CHECK_SMALL(result.GetValue(), D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-0.5,2.0);
  VD result = atanh(val);
  BOOST_CHECK_CLOSE(result.GetValue(), -0.5 * std::log(3.0), D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 4.0/3.0 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(negative_one_special)
{
  VD val(-1.0,2.0);
  VD result = atanh(val);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_SUITE_END() // _atanh

BOOST_AUTO_TEST_SUITE(_cbrt)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(1.0,2.0);
  VD result = cbrt(val);
  BOOST_CHECK_CLOSE(result.GetValue(), 1.0, D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / 3.0 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = cbrt(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-1.0,2.0);
  VD result = cbrt(val);
  BOOST_CHECK_CLOSE(result.GetValue(), -1.0, D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / 3.0 * 2.0 , D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _cbrt

BOOST_AUTO_TEST_SUITE(_erf)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(0.2,2.0);
  VD result = erf(val);
  // Taken from Abramowitz and Stegun, 9th Dover printing, Table 7.1 page 310
  BOOST_CHECK_CLOSE(result.GetValue(), 0.2227025892, 1e-8);
  BOOST_CHECK_CLOSE(result.GetError(), two_div_root_pi * std::exp(-0.04) * 2.0 , D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = erf(val);
  BOOST_CHECK_SMALL(result.GetValue(), D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), two_div_root_pi * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-0.2,2.0);
  VD result = erf(val);
  // Taken from Abramowitz and Stegun, 9th Dover printing, Table 7.1 page 310
  // and changed sign
  BOOST_CHECK_CLOSE(result.GetValue(), -0.2227025892, 1e-8);
  BOOST_CHECK_CLOSE(result.GetError(),  two_div_root_pi * std::exp(-0.04) * 2.0 , D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _erf

BOOST_AUTO_TEST_SUITE(_erfc)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(0.2,2.0);
  VD result = erfc(val);
  // Taken from Abramowitz and Stegun, 9th Dover printing, Table 7.1 page 310
  BOOST_CHECK_CLOSE(result.GetValue(), 1.0 - 0.2227025892, 1e-8);
  BOOST_CHECK_CLOSE(result.GetError(), two_div_root_pi * std::exp(-0.04) * 2.0 , D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = erfc(val);
  BOOST_CHECK_CLOSE(result.GetValue(), 1.0 , D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), two_div_root_pi * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-0.2,2.0);
  VD result = erfc(val);
  // Taken from Abramowitz and Stegun, 9th Dover printing, Table 7.1 page 310
  // and changed sign
  BOOST_CHECK_CLOSE(result.GetValue(), 1.0 + 0.2227025892, 1e-8);
  BOOST_CHECK_CLOSE(result.GetError(), two_div_root_pi * std::exp(-0.04) * 2.0 , D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _erfc

BOOST_AUTO_TEST_SUITE(_hypot)

BOOST_AUTO_TEST_SUITE(x_y_pv)

BOOST_AUTO_TEST_CASE(positive)
{
  VD x(3.0,1.0);
  VD y(4.0,2.0);
  VD result = hypot(x,y);
  BOOST_CHECK_CLOSE(result.GetValue(), 5.0, D_LARGE_EPS );
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / 25.0 * std::hypot(3.0 * 1.0, 4.0 * 2.0), D_LARGE_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD x(0.0,1.0);
  VD y(0.0,2.0);
  VD result = hypot(x,y);
  BOOST_CHECK_CLOSE(result.GetValue(), 0.0 , D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_SUITE_END() // x_y_positive

BOOST_AUTO_TEST_SUITE(x_pv_y_double)

BOOST_AUTO_TEST_CASE(positive)
{
  VD x(3.0,1.0);
  double y = 4.0;
  VD result = hypot(x,y);
  BOOST_CHECK_CLOSE(result.GetValue(), 5.0, D_EPS );
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / 25.0 * 3.0 * 1.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD x(0.0,1.0);
  double y = 0.0;
  VD result = hypot(x,y);
  BOOST_CHECK_CLOSE(result.GetValue(), 0.0, D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_SUITE_END() // x_pv_y_double

BOOST_AUTO_TEST_SUITE(x_double_y_pv)

BOOST_AUTO_TEST_CASE(positive)
{
  double x = 4.0;
  VD y(3.0,1.0);
  VD result = hypot(x,y);
  BOOST_CHECK_CLOSE(result.GetValue(), 5.0, D_EPS );
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / 25.0 * 3.0 * 1.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  double x = 0.0;
  VD y(0.0,1.0);
  VD result = hypot(x,y);
  BOOST_CHECK_CLOSE(result.GetValue(), 0.0, D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_SUITE_END() // x_double_y_pv

BOOST_AUTO_TEST_SUITE_END() // _hypot

BOOST_AUTO_TEST_SUITE(_tgamma)

BOOST_AUTO_TEST_CASE(positive_integer)
{
  VD x(4.0,1.0);
  VD result = tgamma(x);
  BOOST_CHECK_CLOSE(result.GetValue(), 6.0, D_EPS ); // gamma(n) = factorial(n-1) = 4*3*2*1: see Abramowitz & Stegun p. 255
  BOOST_CHECK_CLOSE(result.GetError(), (-euler + 1.0 + 0.5 + 1.0/3.0) * 6.0 * x.GetError(), D_LARGE_EPS); // psi(n) * gamma(n), psi(n): see Abramowitz & Stegun p. 258
}

BOOST_AUTO_TEST_CASE(positive_fractional)
{
  VD x(0.5,1.0);
  VD result = tgamma(x);
  BOOST_CHECK_CLOSE(result.GetValue(), 1.772453850905516, D_EPS ); // tabulated gamma(1/2): see Abramowitz & Stegun p. 3
  BOOST_CHECK_CLOSE(result.GetError(), std::abs((-euler - 2.0*ln_two) * 1.772453850905516 * x.GetError()), D_LARGE_EPS); // psi(1/2): see Abramowitz & Stegun p. 258
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD x(0.0,1.0);
  VD result = tgamma(x);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative_integer)
{
  VD x(-1.0,1.0);
  VD result = tgamma(x);
  BOOST_CHECK(boost::math::isnan(result.GetValue()));
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative_fractional)
{
  VD x(-0.5,1.0);
  VD result = tgamma(x);
  // An error threshold of 1e-8 used here as tabulated value for Gamma(3/2) is available from Abramowitz to 10 digits precision only. This test should pass with the given precision on a 32bit system.
  BOOST_CHECK_CLOSE(result.GetValue(), (pi/std::sin(-half_pi)) / 0.8862269254 , 1e-8 ); // tabulated Gamma(3/2), see Abramowitz & Stegun p. 255, reflection formula: Gamma(z)*Gamma(1-z) = pi*csc(pi*z)
  BOOST_CHECK_CLOSE(result.GetError(), std::abs(((-euler - 2.0*ln_two)-1/(-0.5)) * ((pi/std::sin(-half_pi))/0.8862269254) * x.GetError()), 1e-8); // psi(1/2), recurrence formula psi(1+z) = psi(z) + 1/z, see Abramowitz & Stegun p. 258
}

BOOST_AUTO_TEST_SUITE_END() // _tgamma

BOOST_AUTO_TEST_SUITE(_lgamma)

BOOST_AUTO_TEST_CASE(positive_integer)
{
  VD x(4.0,1.0);
  VD result = lgamma(x);
  BOOST_CHECK_CLOSE(result.GetValue(), std::log(6.0), D_EPS ); // gamma(n) = factorial(n-1) = 4*3*2*1, see Abramowitz & Stegun p. 255
  BOOST_CHECK_CLOSE(result.GetError(), (-euler + 1.0 + 0.5 + 1.0/3.0) * x.GetError(), D_LARGE_EPS); // psi(n), see Abramowitz & Stegun p. 258
}

BOOST_AUTO_TEST_CASE(positive_fractional)
{
  VD x(0.5,1.0);
  VD result = lgamma(x);
  BOOST_CHECK_CLOSE(result.GetValue(), std::log(1.772453850905516), D_EPS ); // tabulated gamma(1/2), see Abramowitz & Stegun p. 3
  BOOST_CHECK_CLOSE(result.GetError(), std::abs((-euler - 2.0*ln_two) * x.GetError()), D_EPS); // psi(1/2), see Abramowitz & Stegun p. 258
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD x(0.0,1.0);
  VD result = lgamma(x);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative_integer)
{
  VD x(-1.0,1.0);
  VD result = lgamma(x);
  BOOST_CHECK(boost::math::isinf(result.GetValue())); // TODO: lgamma and tgamma specifications do not match in linux c++ standard library libstdc++.so.6.0.17 from gcc-4.7, check this with 4.8.
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative_fractional)
{
  VD x(-0.5,1.0);
  VD result = lgamma(x);
  // An error threshold of 1e-8 used here as tabulated value for Gamma(3/2) is available from Abramowitz to 10 digits precision only. This test should pass with the given precision on a 32bit system.
  BOOST_CHECK_CLOSE(result.GetValue(), std::log(std::abs((pi/std::sin(-half_pi)) / 0.8862269254)) , 1e-8 ); // tabulated Gamma(3/2), see Abramowitz & Stegun p. 255, reflection formula: Gamma(z)*Gamma(1-z) = pi*csc(pi*z)
  BOOST_CHECK_CLOSE(result.GetError(), std::abs(((-euler - 2.0*ln_two)-1/(-0.5)) * x.GetError()), 1e-8); // psi(1/2), recurrence formula psi(1+z) = psi(z) + 1/z, see Abramowitz & Stegun p. 258
}

BOOST_AUTO_TEST_SUITE_END() // _lgamma

BOOST_AUTO_TEST_SUITE_END() // Test_MathOverloads

#ifdef __clang__
#pragma clang diagnostic pop
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic pop
#endif


//
// Created by Pascal Havé on 2019-08-13.
//

#ifndef LIBKRIGING_LINEARREGRESSION_HPP
#define LIBKRIGING_LINEARREGRESSION_HPP

#include <armadillo>

#include "libKriging/libKriging_exports.h"

/** Basic linear regression
 * @ingroup Regression
 */
class LinearRegression {
 public:
  /** Trivial constructor */
  LIBKRIGING_EXPORT LinearRegression();

  [[nodiscard]] const arma::colvec& coef() const { return m_coef; };
  [[nodiscard]] const double& sig2() const { return m_sig2; };
  [[nodiscard]] const arma::colvec& stderrest() const { return m_stderrest; };

  /** True linear regression computation
   * has to find s such that y ~= X * s
   * The accuracy may be evaluated using the returned standard error
   *
   * @param y : rhs vector of size n
   * @param X : matrix of size n * m
   */
  LIBKRIGING_EXPORT void fit(const arma::vec& y, const arma::mat& X);

  LIBKRIGING_EXPORT std::tuple<arma::colvec, arma::colvec> predict(const arma::mat& X);

 private:
  arma::colvec m_coef;
  double m_sig2{};
  arma::colvec m_stderrest;
};

#endif  // LIBKRIGING_LINEARREGRESSION_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_EVPDF_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_EVPDF_HPP_INCLUDED
#include <nt2/statistics/functions/evpdf.hpp>
#include <boost/assert.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/functions/globalall.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/is_equal.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/uminus.hpp>
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( evpdf_, tag::cpu_
                              , (A0)
                              , (generic_< floating_<A0> >)
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
      {
        result_type tmp = exp(a0);
        tmp *= exp(-tmp);
#ifndef BOOST_SIMD_NO_INFINITIES
        return if_zero_else(eq(a0, Inf<A0>()), tmp);
#else
        return tmp;
#endif
      }
  };

  BOOST_DISPATCH_IMPLEMENT  ( evpdf_, tag::cpu_
                            , (A0)(A1)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
      {
        result_type tmp = exp((a0-a1));
        tmp *= exp(-tmp);
#ifndef BOOST_SIMD_NO_INFINITIES
        return if_zero_else(eq(a0, Inf<A0>()), tmp);
#else
        return tmp;
#endif
      }
  };

  BOOST_DISPATCH_IMPLEMENT  ( evpdf_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                              (generic_< floating_<A2> >)
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(3)
      {
        BOOST_ASSERT_MSG(nt2::globalall(nt2::is_gtz(a2)), "sigma parameter must be positive");
        A0 invsig =  rec(a2);
        result_type tmp = exp((a0-a1)*invsig);
        tmp *= exp(-tmp)*invsig;
#ifndef BOOST_SIMD_NO_INFINITIES
        return if_zero_else(eq(a0, Inf<A0>()), tmp);
#else
        return tmp;
#endif

      }
  };

} }

#endif


/*
*   farthest_sampling_by_sphere
*   by R. Falque
*   27/06/2019
*/

#ifndef FARTHEST_SAMPLING_BY_SPHERE_HPP
#define FARTHEST_SAMPLING_BY_SPHERE_HPP

#include <Eigen/Core>
#include <limits>
#include <iostream>

#include "nanoflannWrapper.hpp"

int argMax(const Eigen::VectorXd & data)
{
    int argmax = 0;
    int max_dim = std::max(data.rows(), data.cols());
    for (int i=0; i<max_dim; i++)
        if (data(argmax) < data(i))
            argmax = i;
    return argmax;
}

inline bool farthest_sampling_by_sphere(const Eigen::MatrixXd & in_cloud, double sample_radius, Eigen::MatrixXd & nodes, Eigen::VectorXi & correspondences)
{

    correspondences = Eigen::VectorXi::Zero(in_cloud.rows());

    nanoflann_wrapper knn_search(in_cloud);
    std::vector<int> node_list;
    Eigen::VectorXd mindst = Eigen::VectorXd::Constant(in_cloud.rows(), -1); // used as NaN

    for (int i=0; i<in_cloud.rows(); i++) {

        if (correspondences(i) == 0) {

            mindst(i) = std::numeric_limits<double>::infinity();

            while ( (correspondences.array()==0).any() ) {

                int maxId = argMax(mindst);

                if ( mindst(maxId)==0 )
                    break;

                std::vector<int> neighbours_id;
                std::vector<double> neighbours_distances;
                knn_search.radius_search(in_cloud.row(maxId), sample_radius, neighbours_id, neighbours_distances);

                bool all_corresp_marked = correspondences(neighbours_id[0])!=0;
                for (int j=0; j<neighbours_id.size(); j++)
                    all_corresp_marked = all_corresp_marked & correspondences(neighbours_id[j])!=0;

                if (all_corresp_marked) {
                    mindst(maxId) = 0;
                    break;
                }

                node_list.push_back(maxId);
                for (int j=0; j<neighbours_id.size(); j++) {
                    if ( mindst( neighbours_id[j] ) > neighbours_distances[j] || mindst(neighbours_id[j])==-1 )  {
                        mindst( neighbours_id[j] ) = neighbours_distances[j];
                        correspondences(neighbours_id[j]) = node_list.size();
                    }
                }
            }
        }
    }

    correspondences = correspondences.array() - 1;
    if ( (correspondences.array() == -1).any() )
    {
        std::cout << "point without correspondences!!!\n";
        std::cin.get();
    }

    nodes.resize(node_list.size(), 3);
    for (int i=0; i<node_list.size(); i++)
        nodes.row(i) << in_cloud.row(node_list[i]);

    return true;
};

// overload the declaration if correspondences are not needed
inline bool farthest_sampling_by_sphere(const Eigen::MatrixXd & in_cloud, double sample_radius, Eigen::MatrixXd & nodes)
{
    Eigen::VectorXi correspondences;
    return farthest_sampling_by_sphere(in_cloud, sample_radius, nodes, correspondences);
};

/*
inline bool fast_poisson_disk_sampling(Eigen::MatrixXd & in_cloud, double minimum_distance, Eigen::MatrixXd & out_cloud)
{
    // Considering implementing the following paper as an alternative:
    // https://www.cs.ubc.ca/~rbridson/docs/bridson-siggraph07-poissondisk.pdf
    int number_of_samples_to_consider = 30;

    // generate the 3D grid

}
*/

#endif


#if !defined(BALSA_EIGEN_STACK_HPP)
#define BALSA_EIGEN_STACK_HPP

#include <Eigen/Dense>
#include <tuple>
#include <vector>
#include <type_traits>
#include <utility>
#include <algorithm>

namespace balsa::eigen {
template<bool Rows, typename... Args, int... N>
auto _stack(std::integer_sequence<int, N...>, const Args &...args) {
    using namespace Eigen;
    using Scalar = typename std::tuple_element<0, std::tuple<Args...>>::type::Scalar;

    constexpr static int minCompileRows = std::min<int>({ Args::RowsAtCompileTime... });
    constexpr static int maxCompileRows = std::max<int>({ Args::RowsAtCompileTime... });
    constexpr static int minCompileCols = std::min<int>({ Args::ColsAtCompileTime... });
    constexpr static int maxCompileCols = std::max<int>({ Args::ColsAtCompileTime... });

    constexpr static int sumCompileRows = (Args::RowsAtCompileTime + ... + 0);
    constexpr static int sumCompileCols = (Args::ColsAtCompileTime + ... + 0);

    //constexpr static int myCompRows = (minCompileRows==Dynamic)?Dynamic:(Rows?S:1)*maxCompileRows;
    //constexpr static int myCompCols = (minCompileCols==Dynamic)?Dynamic:(Rows?1:S)*maxCompileCols;
    constexpr static int myCompRows = (minCompileRows == Dynamic) ? Dynamic : (Rows ? sumCompileRows : maxCompileRows);
    constexpr static int myCompCols = (minCompileCols == Dynamic) ? Dynamic : (Rows ? maxCompileCols : sumCompileCols);
    int rows;
    int cols;
    std::vector<int> offset(1, 0);
    auto push_sum = [&](int size) {
        offset.push_back(offset.back() + size);
    };
    if constexpr (Rows) {
        rows = (args.rows() + ... + 0);
        cols = std::max({ args.cols()... });
        (push_sum(args.rows()), ...);
    } else {
        rows = std::max({ args.rows()... });
        cols = (args.cols() + ... + 0);
        (push_sum(args.cols()), ...);
    }

    using Matf = Matrix<Scalar, myCompRows, myCompCols>;
    Matf A = Matf::Constant(rows, cols, 0);

    if constexpr (Rows) {
        (A.block(offset[N], 0, args.rows(), args.cols()).operator=(args), ...);
    } else {
        (A.block(0, offset[N], args.rows(), args.cols()).operator=(args), ...);
    }

    return A;
}

template<typename... Args>
auto vstack(const Args &...args) {
    return _stack<true>(std::make_integer_sequence<int, sizeof...(Args)>(), std::forward<const Args &>(args)...);
}
template<typename... Args>
auto hstack(const Args &...args) {
    return _stack<false>(std::make_integer_sequence<int, sizeof...(Args)>(), std::forward<const Args &>(args)...);
}

template<typename BeginIt, typename EndIt>
auto hstack_iter(BeginIt beginit, EndIt endit) {
    using CDerived = typename std::decay_t<decltype(*beginit)>;

    constexpr static int CRows = CDerived::RowsAtCompileTime;
    using Index = typename CDerived::Scalar;
    using RetCells = Eigen::Matrix<Index, CRows, Eigen::Dynamic>;
    int ccols = 0;
    int crows = 0;

    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            crows = std::max<int>(crows, c.rows());
            ccols += c.cols();
        }
    }
    if (crows == 0 || ccols == 0) {
        return RetCells{};
    }
    RetCells mC(crows, ccols);
    ccols = 0;
    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            mC.block(0, ccols, c.rows(), c.cols()) = c;
            ccols += c.cols();
        }
    }
    return mC;
}
template<typename BeginIt, typename EndIt>
auto vstack_iter(BeginIt beginit, EndIt endit) {
    using CDerived = typename std::decay_t<decltype(*beginit)>;

    constexpr static int CCols = CDerived::ColsAtCompileTime;
    using Index = typename CDerived::Scalar;
    using RetCells = Eigen::Matrix<Index, Eigen::Dynamic, CCols>;
    int ccols = 0;
    int crows = 0;

    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            ccols = std::max<int>(ccols, c.cols());
            crows += c.rows();
        }
    }
    if (crows == 0 || ccols == 0) {
        return RetCells{};
    }
    RetCells mC(crows, ccols);
    crows = 0;
    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            mC.block(crows, 0, c.rows(), c.cols()) = c;
            crows += c.rows();
        }
    }
    return mC;
}

}// namespace balsa::eigen
#endif


#ifndef STAN_MATH_PRIM_SCAL_PROB_NORMAL_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_NORMAL_RNG_HPP

#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    normal_rng(double mu,
               double sigma,
               RNG& rng) {
      using boost::variate_generator;
      using boost::normal_distribution;

      static const char* function("normal_rng");

      check_finite(function, "Location parameter", mu);
      check_not_nan(function, "Location parameter", mu);
      check_positive(function, "Scale parameter", sigma);
      check_not_nan(function, "Scale parameter", sigma);

      variate_generator<RNG&, normal_distribution<> >
        norm_rng(rng, normal_distribution<>(mu, sigma));
      return norm_rng();
    }

  }
}
#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2015 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "angles.h"
#include <Eigen/Geometry>
#include <cassert>

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedtheta>
void igl::angles(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  Eigen::PlainObjectBase<Derivedtheta>& theta)
{
  theta.resize(F.rows(),F.cols());

  auto corner = [](const Eigen::PlainObjectBase<DerivedV>& x, const Eigen::PlainObjectBase<DerivedV>& y, const Eigen::PlainObjectBase<DerivedV>& z)
  {
    Eigen::RowVector3d v1 = (x-y).normalized();
    Eigen::RowVector3d v2 = (z-y).normalized();

    // http://stackoverflow.com/questions/10133957/signed-angle-between-two-vectors-without-a-reference-plane
    double s = v1.cross(v2).norm();
    double c = v1.dot(v2);

    return atan2(s, c);
  };

  for(unsigned i=0; i<F.rows(); ++i)
  {
    for(unsigned j=0; j<F.cols(); ++j)
    {
      theta(i,j) = corner(
        V.row(F(i,int(j-1+F.cols())%F.cols())),
        V.row(F(i,j)),
        V.row(F(i,(j+1+F.cols())%F.cols()))
        );
    }
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::angles<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif