//  (C) Copyright John Maddock 2005-2006.
//  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)

#ifndef BOOST_MATH_TOOLS_STATS_INCLUDED
#define BOOST_MATH_TOOLS_STATS_INCLUDED

#ifdef _MSC_VER
#pragma once
#endif

#include <cmath>
#include <boost/cstdint.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{ namespace math{ namespace tools{

template <class T>
class stats
{
public:
   stats()
      : m_min(tools::max_value<T>()),
        m_max(-tools::max_value<T>()),
        m_total(0),
        m_squared_total(0),
        m_count(0)
   {}
   void add(const T& val)
   {
      if(val < m_min)
         m_min = val;
      if(val > m_max)
         m_max = val;
      m_total += val;
      ++m_count;
      m_squared_total += val*val;
   }
   T min BOOST_PREVENT_MACRO_SUBSTITUTION()const{ return m_min; }
   T max BOOST_PREVENT_MACRO_SUBSTITUTION()const{ return m_max; }
   T total()const{ return m_total; }
   T mean()const{ return m_total / static_cast<T>(m_count); }
   boost::uintmax_t count()const{ return m_count; }
   T variance()const
   {
      BOOST_MATH_STD_USING

      T t = m_squared_total - m_total * m_total / m_count;
      t /= m_count;
      return t;
   }
   T variance1()const
   {
      BOOST_MATH_STD_USING

      T t = m_squared_total - m_total * m_total / m_count;
      t /= (m_count-1);
      return t;
   }
   T rms()const
   {
      BOOST_MATH_STD_USING

      return sqrt(m_squared_total / static_cast<T>(m_count));
   }
   stats& operator+=(const stats& s)
   {
      if(s.m_min < m_min)
         m_min = s.m_min;
      if(s.m_max > m_max)
         m_max = s.m_max;
      m_total += s.m_total;
      m_squared_total += s.m_squared_total;
      m_count += s.m_count;
      return *this;
   }
private:
   T m_min, m_max, m_total, m_squared_total;
   boost::uintmax_t m_count;
};

} // namespace tools
} // namespace math
} // namespace boost

#endif


// Copyright (c) 2013, Manuel Blum
// All rights reserved.

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "nn.h"

TEST(nn, sigmoid1)
{
    matrix_t X(3,3);
    matrix_t s(3,3);
    X << -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2;
    s <<  0.1192, 0.1824, 0.2689, 0.3775, 0.5, 0.6225, 0.7311, 0.8176, 0.8808;
    matrix_t Y = NeuralNet::sigmoid(X);
    ASSERT_NEAR((Y.array()-s.array()).maxCoeff(), 0.0, 1e-4);
}

TEST(nn, sigmoid2)
{
    matrix_t X = matrix_t::Random(100,200) * 2;
    matrix_t Y = NeuralNet::sigmoid(X);
    ASSERT_EQ(X.rows(), Y.rows());
    ASSERT_EQ(X.cols(), Y.cols());

    for (int i=0; i<X.rows(); ++i)
    {
        for (int j=0; j<X.cols(); ++j)
        {
            ASSERT_NEAR(Y(i,j), 1/(1+exp(-X(i,j))), 1e-7);
        }
    }
}

TEST(nn, sigmoid_gradient1)
{
    matrix_t X(3,3);
    matrix_t s(3,3);
    X << -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2;
    s <<  0.105, 0.1491, 0.1966, 0.2350, 0.25, 0.2350, 0.1966, 0.1491, 0.1050;
    matrix_t sig = NeuralNet::sigmoid(X);
    matrix_t Y = NeuralNet::sigmoid_gradient(sig);
    ASSERT_NEAR((Y.array()-s.array()).maxCoeff(), 0.0, 1e-4);
}

TEST(nn, sigmoid_gradient2)
{
    matrix_t X = matrix_t::Random(100,200) * 2;
    matrix_t sig = NeuralNet::sigmoid(X);
    matrix_t Y = NeuralNet::sigmoid_gradient(sig);
    ASSERT_EQ(X.rows(), Y.rows());
    ASSERT_EQ(X.cols(), Y.cols());

    for (int i=0; i<X.rows(); ++i)
    {
        for (int j=0; j<X.cols(); ++j)
        {
            double sigmoid = 1/(1+exp(-X(i,j)));
            ASSERT_NEAR(Y(i,j), sigmoid*(1-sigmoid), 1e-9);
        }
    }
}

// compare analytical with numerical gradients
TEST(nn, gradient)
{
    Eigen::VectorXi topo(4);
    topo << 3, 10, 10, 2;
    NeuralNet nn(topo);
    nn.init_weights(0.5);
    int m = 100;
    matrix_t X = matrix_t::Random(m,3);
    matrix_t Y = matrix_t::Random(m,2);
    Y = Y.array() * 0.4 + 0.5;
    ASSERT_GE(Y.minCoeff(), 0.1);
    ASSERT_LE(Y.maxCoeff(), 0.9);
    double lambda = 0.01;
    double e = 1e-4;
    ASSERT_EQ(nn.layer[0].size, topo(0));

    for (int i=1; i<nn.layer.size(); ++i)
    {
        ASSERT_EQ(nn.layer[i].W.rows(), nn.layer[i].size);
        ASSERT_EQ(nn.layer[i].W.cols(), nn.layer[i-1].size);
        ASSERT_EQ(nn.layer[i].b.rows(), nn.layer[i].size);
        ASSERT_EQ(nn.layer[i].size, topo(i));

        for (int j=0; j<nn.layer[i].W.rows(); ++j)
        {
            for (int k=0; k<nn.layer[i].W.cols(); ++k)
            {
                double w = nn.layer[i].W(j,k);
                nn.layer[i].W(j,k) = w - e;
                double j1 = nn.loss(X, Y, lambda);
                nn.layer[i].W(j,k) = w + e;
                double j2 = nn.loss(X, Y, lambda);
                nn.layer[i].W(j,k) = w;
                nn.loss(X, Y, lambda);
                ASSERT_NEAR((j2-j1)/(2*e), nn.layer[i].dEdW(j,k), 1e-9);
            }
        }

        for (int j=0; j<nn.layer[i].b.rows(); ++j)
        {
            double b = nn.layer[i].b(j);
            nn.layer[i].b(j) = b - e;
            double j1 = nn.loss(X, Y, lambda);
            nn.layer[i].b(j) = b + e;
            double j2 = nn.loss(X, Y, lambda);
            nn.layer[i].b(j) = b;
            nn.loss(X, Y, lambda);
            ASSERT_NEAR((j2-j1)/(2*e), nn.layer[i].dEdb(j), 1e-9);
        }
    }
}

TEST(nn, readwrite)
{
    Eigen::VectorXi topo(4);
    topo << 3, 10, 10, 2;
    NeuralNet nn(topo);
    nn.init_weights(0.5);
    nn.write("testnet.nn");
    NeuralNet nnclone("testnet.nn");
    ASSERT_EQ(topo.size(), nnclone.layer.size());

    for (int i=0; i<topo.size(); ++i)
    {
        ASSERT_EQ(topo(i), nnclone.layer[i].size);
    }

    for (int i=1; i<nn.layer.size(); ++i)
    {
        for (int j=0; j<nn.layer[i].W.rows(); ++j)
        {
            for (int k=0; k<nn.layer[i].W.cols(); ++k)
            {
                ASSERT_NEAR(nn.layer[i].W(j,k), nnclone.layer[i].W(j,k), 1e-12);
            }
        }

        for (int j=0; j<nn.layer[i].b.rows(); ++j)
        {
            ASSERT_NEAR(nn.layer[i].b(j), nnclone.layer[i].b(j), 1e-12);
        }
    }
}

TEST(nn, testfunction)
{
    srand((unsigned int) time(NULL));
    Eigen::VectorXi topo(4);
    topo << 2, 10, 10, 1;
    NeuralNet nn(topo);
    matrix_t X = matrix_t::Random(1000,2);
    matrix_t Y = matrix_t::Random(1000,1);

    for (int i=0; i<X.rows(); ++i)
    {
        Y(i,0) = (X(i,0) * X(i,1) + 1.2)*0.4;
    }

    double lambda = 0.01, err;

    for (int i=0; i<300; ++i)
    {
        err = nn.loss(X, Y, lambda);
        nn.rprop();
    }

    ASSERT_LE(err, 0.01);
    nn.write("testnet.nn");
    NeuralNet nnclone("testnet.nn");
    ASSERT_NEAR(nn.loss(X, Y, lambda), nnclone.loss(X, Y, lambda), 1e-12);
}


#include <boost/test/unit_test.hpp>
#include "test-util.h"

#include <faiss/IndexFlat.h>
#include <faiss/utils.h>
BOOST_AUTO_TEST_SUITE(FlatIndex)
/**
test the correctness of flat index using faiss flat index
*/
BOOST_AUTO_TEST_CASE(FP32FlatIndexCPU) {
	int64_t nb = 1024;
	int d = 64;
	std::vector <float> database(nb * d);
	generate_float_vector(d, database.data(), nb);
	decorate_float_vector(d, database.data(), nb);
	L2norm(d, database.data(), nb);

	std::unique_ptr<faiss::Index> index;
	index.reset(new faiss::IndexFlatL2(d));
	index->add(nb, database.data());

	int64_t nq = 128;
	int k = 4;
	std::vector<float> query(nq*d);
	generate_float_vector(d, query.data(), nq);
	decorate_float_vector(d, query.data(), nq);
	L2norm(d, query.data(), nq);

	std::vector<int64_t> I(k * nq);
	std::vector<float> D(k * nq);
	index->search(nq, query.data(), k, D.data(), I.data());

	std::vector<int64_t> I_GT(k * nq);
	std::vector<float> D_GT(k * nq);
	for (int n1 = 0; n1 < nq; ++n1) {
		std::vector<std::pair<float,int64_t>> pResult;
		for (int64_t n2 = 0; n2 < nb; ++n2) {
			float* qv = query.data() + n1*d;
			float* rv = database.data() + n2*d;
			double sum=0;
			for (int d1 = 0; d1 < d; ++d1,++qv,++rv) {
				float r=(*qv-*rv);
				sum += r*r;
			}
			pResult.push_back(std::make_pair((float)sum,n2));
		}
		std::partial_sort(pResult.begin(),pResult.begin()+k,pResult.end(),[](const std::pair<float,long>& p1,
			const std::pair<float,long>& p2)->bool{return p1.first<p2.first;});
		for (int k1 = 0; k1 < k; ++k1) {
			I_GT[n1*k+k1] = pResult[k1].second;
			D_GT[n1*k+k1] = pResult[k1].first;
		}
	}
	int nNumOfInterSec=0;
	for (int64_t n = 0; n < nq; ++n) {
		for (int k1 = 0; k1 < k; k1++) {
			BOOST_CHECK(I_GT[n*k + k1] == I[n*k + k1]);
		}
	}
}

BOOST_AUTO_TEST_CASE(FP32FlatIndexGPU) {
	int64_t nb = 1024;
	int d = 64;
	std::vector <float> database(nb * d);
	generate_float_vector(d, database.data(), nb);
	decorate_float_vector(d, database.data(), nb);
	L2norm(d, database.data(), nb);

	std::unique_ptr<faiss::Index> index;
	index.reset(new faiss::GPU_IndexFlatL2(d));
	index->add(nb, database.data());

	int64_t nq = 128;
	int k = 4;
	std::vector<float> query(nq*d);
	generate_float_vector(d, query.data(), nq);
	decorate_float_vector(d, query.data(), nq);
	L2norm(d, query.data(), nq);

	std::vector<int64_t> I(k * nq);
	std::vector<float> D(k * nq);
	index->search(nq, query.data(), k, D.data(), I.data());

	std::vector<int64_t> I_GT(k * nq);
	std::vector<float> D_GT(k * nq);
	for (int n1 = 0; n1 < nq; ++n1) {
		std::vector<std::pair<float, int64_t>> pResult;
		for (int64_t n2 = 0; n2 < nb; ++n2) {
			float* qv = query.data() + n1 * d;
			float* rv = database.data() + n2 * d;
			double sum = 0;
			for (int d1 = 0; d1 < d; ++d1, ++qv, ++rv) {
				float r = (*qv - *rv);
				sum += r * r;
			}
			pResult.push_back(std::make_pair((float)sum, n2));
		}
		std::partial_sort(pResult.begin(), pResult.begin() + k, pResult.end(), [](const std::pair<float, long>& p1,
			const std::pair<float, long>& p2)->bool {return p1.first<p2.first; });
		for (int k1 = 0; k1 < k; ++k1) {
			I_GT[n1*k + k1] = pResult[k1].second;
			D_GT[n1*k + k1] = pResult[k1].first;
		}
	}
	int nNumOfInterSec = 0;
	for (int64_t n = 0; n < nq; ++n) {
		for (int k1 = 0; k1 < k; k1++) {
			BOOST_CHECK(I_GT[n*k + k1] == I[n*k + k1]);
		}
	}
}

BOOST_AUTO_TEST_CASE(FP16FlatIndexGPU) {
	int64_t nb = 1024;
	int d = 64;
	std::vector <float> database(nb * d);
	generate_float_vector(d, database.data(), nb);
	decorate_float_vector(d, database.data(), nb);
	L2norm(d, database.data(), nb);

	std::unique_ptr<faiss::Index> index;
	index.reset(new faiss::GPU_IndexFlatFP16L2(d));
	index->add(nb, database.data());

	int64_t nq = 128;
	int k = 4;
	std::vector<float> query(nq*d);
	generate_float_vector(d, query.data(), nq);
	decorate_float_vector(d, query.data(), nq);
	L2norm(d, query.data(), nq);

	std::vector<int64_t> I(k * nq);
	std::vector<float> D(k * nq);
	index->search(nq, query.data(), k, D.data(), I.data());

	std::vector<int64_t> I_GT(k * nq);
	std::vector<float> D_GT(k * nq);
	for (int n1 = 0; n1 < nq; ++n1) {
		std::vector<std::pair<float, int64_t>> pResult;
		for (int64_t n2 = 0; n2 < nb; ++n2) {
			float* qv = query.data() + n1 * d;
			float* rv = database.data() + n2 * d;
			double sum = 0;
			for (int d1 = 0; d1 < d; ++d1, ++qv, ++rv) {
				float r = (*qv - *rv);
				sum += r * r;
			}
			pResult.push_back(std::make_pair((float)sum, n2));
		}
		std::partial_sort(pResult.begin(), pResult.begin() + k, pResult.end(), [](const std::pair<float, long>& p1,
			const std::pair<float, long>& p2)->bool {return p1.first<p2.first; });
		for (int k1 = 0; k1 < k; ++k1) {
			I_GT[n1*k + k1] = pResult[k1].second;
			D_GT[n1*k + k1] = pResult[k1].first;
		}
	}
	int nNumOfInterSec=0;
	for (int n = 0; n < nq; ++n) {
		//check intersection
		std::vector<int64_t> GT_Labels;
		std::vector<int64_t> Labels;
		for (int m = 0; m < k; ++m) {
			GT_Labels.push_back(I_GT[n * 4 + m]);
			Labels.push_back(I[n * 4 + m]);
		}
		std::sort(GT_Labels.begin(), GT_Labels.end());
		std::sort(Labels.begin(), Labels.end());
		std::vector<int> v(2 * k);
		auto itr = std::set_intersection(GT_Labels.begin(), GT_Labels.end(), Labels.begin(), Labels.end(), v.begin());
		v.resize(itr - v.begin());
		nNumOfInterSec += v.size();
	}
	BOOST_TEST_MESSAGE("GPU fp16 flat index search results is " << nNumOfInterSec*100.0f / (nq * k) << "% close to that of CPU fp32 flat index ");
}
BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright Nick Thompson, 2020
 * 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 <random>
#include <array>
#include <vector>
#include <boost/math/interpolators/cubic_hermite.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/circular_buffer.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::interpolators::cubic_hermite;
using boost::math::interpolators::cardinal_cubic_hermite;
using boost::math::interpolators::cardinal_cubic_hermite_aos;

template<typename Real>
void test_constant()
{
    Real x0 = 0;
    std::vector<Real> x{x0,1,2,3, 9, 22, 81};
    std::vector<Real> y(x.size());
    for (auto & t : y)
    {
        t = 7;
    }

    std::vector<Real> dydx(x.size(), Real(0));
    auto x_copy = x;
    auto y_copy = y;
    auto dydx_copy = dydx;
    auto hermite_spline = cubic_hermite(std::move(x_copy), std::move(y_copy), std::move(dydx_copy));

    // Now check the boundaries:
    Real tlo = x.front();
    Real thi = x.back();
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(7), hermite_spline(tlo), 2);
        CHECK_ULP_CLOSE(Real(7), hermite_spline(thi), 2);
        CHECK_ULP_CLOSE(Real(0), hermite_spline.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), hermite_spline.prime(thi), 2);
        tlo = boost::math::nextafter(tlo, (std::numeric_limits<Real>::max)());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }

    boost::circular_buffer<Real> x_buf(x.size());
    for (auto & t : x) {
        x_buf.push_back(t);
    }

    boost::circular_buffer<Real> y_buf(x.size());
    for (auto & t : y) {
        y_buf.push_back(t);
    }

    boost::circular_buffer<Real> dydx_buf(x.size());
    for (auto & t : dydx) {
        dydx_buf.push_back(t);
    }

    auto circular_hermite_spline = cubic_hermite(std::move(x_buf), std::move(y_buf), std::move(dydx_buf));

    for (Real t = x[0]; t <= x.back(); t += 0.25) {
        CHECK_ULP_CLOSE(Real(7), circular_hermite_spline(t), 2);
        CHECK_ULP_CLOSE(Real(0), circular_hermite_spline.prime(t), 2);
    }

    circular_hermite_spline.push_back(x.back() + 1, 7, 0);
    CHECK_ULP_CLOSE(Real(0), circular_hermite_spline.prime(x.back()+1), 2);

}

template<typename Real>
void test_linear()
{
    std::vector<Real> x{0,1,2,3};
    std::vector<Real> y{0,1,2,3};
    std::vector<Real> dydx{1,1,1,1};

    auto x_copy = x;
    auto y_copy = y;
    auto dydx_copy = dydx;
    auto hermite_spline = cubic_hermite(std::move(x_copy), std::move(y_copy), std::move(dydx_copy));

    CHECK_ULP_CLOSE(y[0], hermite_spline(x[0]), 0);
    CHECK_ULP_CLOSE(Real(1)/Real(2), hermite_spline(Real(1)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[1], hermite_spline(x[1]), 0);
    CHECK_ULP_CLOSE(Real(3)/Real(2), hermite_spline(Real(3)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[2], hermite_spline(x[2]), 0);
    CHECK_ULP_CLOSE(Real(5)/Real(2), hermite_spline(Real(5)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[3], hermite_spline(x[3]), 0);

    x.resize(45);
    y.resize(45);
    dydx.resize(45);
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
        y[i] = i;
        dydx[i] = 1;
    }

    x_copy = x;
    y_copy = y;
    dydx_copy = dydx;
    hermite_spline = cubic_hermite(std::move(x_copy), std::move(y_copy), std::move(dydx_copy));
    for (Real t = 0; t < x.back(); t += 0.5) {
        CHECK_ULP_CLOSE(t, hermite_spline(t), 0);
        CHECK_ULP_CLOSE(Real(1), hermite_spline.prime(t), 0);
    }

    boost::circular_buffer<Real> x_buf(x.size());
    for (auto & t : x) {
        x_buf.push_back(t);
    }

    boost::circular_buffer<Real> y_buf(x.size());
    for (auto & t : y) {
        y_buf.push_back(t);
    }

    boost::circular_buffer<Real> dydx_buf(x.size());
    for (auto & t : dydx) {
        dydx_buf.push_back(t);
    }

    auto circular_hermite_spline = cubic_hermite(std::move(x_buf), std::move(y_buf), std::move(dydx_buf));

    for (Real t = x[0]; t <= x.back(); t += 0.25) {
        CHECK_ULP_CLOSE(t, circular_hermite_spline(t), 2);
        CHECK_ULP_CLOSE(Real(1), circular_hermite_spline.prime(t), 2);
    }

    circular_hermite_spline.push_back(x.back() + 1, y.back()+1, 1);

    CHECK_ULP_CLOSE(Real(y.back() + 1), circular_hermite_spline(Real(x.back()+1)), 2);
    CHECK_ULP_CLOSE(Real(1), circular_hermite_spline.prime(Real(x.back()+1)), 2);

}

template<typename Real>
void test_quadratic()
{
    std::vector<Real> x(50);
    std::default_random_engine rd;
    std::uniform_real_distribution<Real> dis(0.1,1);
    Real x0 = dis(rd);
    x[0] = x0;
    for (size_t i = 1; i < x.size(); ++i) {
        x[i] = x[i-1] + dis(rd);
    }
    Real xmax = x.back();

    std::vector<Real> y(x.size());
    std::vector<Real> dydx(x.size());
    for (size_t i = 0; i < x.size(); ++i) {
        y[i] = x[i]*x[i]/2;
        dydx[i] = x[i];
    }

    auto s = cubic_hermite(std::move(x), std::move(y), std::move(dydx));
    for (Real t = x0; t <= xmax; t+= 0.0125)
    {
        CHECK_ULP_CLOSE(t*t/2, s(t), 5);
        CHECK_ULP_CLOSE(t, s.prime(t), 138);
    }
}

template<typename Real>
void test_interpolation_condition()
{
    for (size_t n = 4; n < 50; ++n) {
        std::vector<Real> x(n);
        std::vector<Real> y(n);
        std::vector<Real> dydx(n);
        std::default_random_engine rd;
        std::uniform_real_distribution<Real> dis(0,1);
        Real x0 = dis(rd);
        x[0] = x0;
        y[0] = dis(rd);
        for (size_t i = 1; i < n; ++i) {
            x[i] = x[i-1] + dis(rd);
            y[i] = dis(rd);
            dydx[i] = dis(rd);
        }

        auto x_copy = x;
        auto y_copy = y;
        auto dydx_copy = dydx;
        auto s = cubic_hermite(std::move(x_copy), std::move(y_copy), std::move(dydx_copy));
        //std::cout << "s = " << s << "\n";
        for (size_t i = 0; i < x.size(); ++i) {
            CHECK_ULP_CLOSE(y[i], s(x[i]), 2);
            CHECK_ULP_CLOSE(dydx[i], s.prime(x[i]), 2);
        }
    }
}

template<typename Real>
void test_cardinal_constant()
{
    Real x0 = 0;
    Real dx = 2;
    std::vector<Real> y(25);
    for (auto & t : y) {
        t = 7;
    }

    std::vector<Real> dydx(y.size(), Real(0));

    auto hermite_spline = cardinal_cubic_hermite(std::move(y), std::move(dydx), x0, dx);

    for (Real t = x0; t <= x0 + 24*dx; t += 0.25) {
        CHECK_ULP_CLOSE(Real(7), hermite_spline(t), 2);
        CHECK_ULP_CLOSE(Real(0), hermite_spline.prime(t), 2);
    }

    // Array of structs:

    std::vector<std::array<Real, 2>> data(25);
    for (auto & t : data) {
        t[0] = 7;
        t[1] = 0;
    }
    auto hermite_spline_aos = cardinal_cubic_hermite_aos(std::move(data), x0, dx);

    for (Real t = x0; t <= x0 + 24*dx; t += 0.25) {
        if (!CHECK_ULP_CLOSE(Real(7), hermite_spline_aos(t), 2)) {
            std::cerr << "  Wrong evaluation at t = " << t << "\n";
        }
        if (!CHECK_ULP_CLOSE(Real(0), hermite_spline_aos.prime(t), 2)) {
            std::cerr << "  Wrong evaluation at t = " << t << "\n";
        }
    }

    // Now check the boundaries:
    Real tlo = x0;
    Real thi = x0 + (25-1)*dx;
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(7), hermite_spline(tlo), 2);
        CHECK_ULP_CLOSE(Real(7), hermite_spline(thi), 2);
        CHECK_ULP_CLOSE(Real(7), hermite_spline_aos(tlo), 2);
        CHECK_ULP_CLOSE(Real(7), hermite_spline_aos(thi), 2);
        CHECK_ULP_CLOSE(Real(0), hermite_spline.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), hermite_spline.prime(thi), 2);
        CHECK_ULP_CLOSE(Real(0), hermite_spline_aos.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), hermite_spline_aos.prime(thi), 2);

        tlo = boost::math::nextafter(tlo, (std::numeric_limits<Real>::max)());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }

}

template<typename Real>
void test_cardinal_linear()
{
    Real x0 = 0;
    Real dx = 1;
    std::vector<Real> y{0,1,2,3};
    std::vector<Real> dydx{1,1,1,1};
    auto y_copy = y;
    auto dydx_copy = dydx;
    auto hermite_spline = cardinal_cubic_hermite(std::move(y_copy), std::move(dydx_copy), x0, dx);

    CHECK_ULP_CLOSE(y[0], hermite_spline(0), 0);
    CHECK_ULP_CLOSE(Real(1)/Real(2), hermite_spline(Real(1)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[1], hermite_spline(1), 0);
    CHECK_ULP_CLOSE(Real(3)/Real(2), hermite_spline(Real(3)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[2], hermite_spline(2), 0);
    CHECK_ULP_CLOSE(Real(5)/Real(2), hermite_spline(Real(5)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[3], hermite_spline(3), 0);

    y.resize(45);
    dydx.resize(45);
    for (size_t i = 0; i < y.size(); ++i) {
        y[i] = i;
        dydx[i] = 1;
    }

    hermite_spline = cardinal_cubic_hermite(std::move(y), std::move(dydx), x0, dx);
    for (Real t = 0; t < 44; t += 0.5) {
        CHECK_ULP_CLOSE(t, hermite_spline(t), 0);
        CHECK_ULP_CLOSE(Real(1), hermite_spline.prime(t), 0);
    }

    std::vector<std::array<Real, 2>> data(45);
    for (size_t i = 0; i < data.size(); ++i) {
        data[i][0] = i;
        data[i][1] = 1;
    }

    auto hermite_spline_aos = cardinal_cubic_hermite_aos(std::move(data), x0, dx);
    for (Real t = 0; t < 44; t += 0.5) {
        CHECK_ULP_CLOSE(t, hermite_spline_aos(t), 0);
        CHECK_ULP_CLOSE(Real(1), hermite_spline_aos.prime(t), 0);
    }

    Real tlo = x0;
    Real thi = x0 + (45-1)*dx;
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(tlo), hermite_spline(tlo), 2);
        CHECK_ULP_CLOSE(Real(thi), hermite_spline(thi), 2);
        CHECK_ULP_CLOSE(Real(1), hermite_spline.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(1), hermite_spline.prime(thi), 2);
        CHECK_ULP_CLOSE(Real(tlo), hermite_spline_aos(tlo), 2);
        CHECK_ULP_CLOSE(Real(thi), hermite_spline_aos(thi), 2);
        CHECK_ULP_CLOSE(Real(1), hermite_spline_aos.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(1), hermite_spline_aos.prime(thi), 2);

        tlo = boost::math::nextafter(tlo, (std::numeric_limits<Real>::max)());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }

}

template<typename Real>
void test_cardinal_quadratic()
{
    Real x0 = -1;
    Real dx = Real(1)/Real(256);

    std::vector<Real> y(50);
    std::vector<Real> dydx(y.size());
    for (size_t i = 0; i < y.size(); ++i) {
        Real x = x0 + i*dx;
        y[i] = x*x/2;
        dydx[i] = x;
    }

    auto s = cardinal_cubic_hermite(std::move(y), std::move(dydx), x0, dx);
    for (Real t = x0; t <= x0 + 49*dx; t+= 0.0125)
    {
        CHECK_ULP_CLOSE(t*t/2, s(t), 12);
        CHECK_ULP_CLOSE(t, s.prime(t), 70);
    }

    std::vector<std::array<Real, 2>> data(50);
    for (size_t i = 0; i < data.size(); ++i) {
        Real x = x0 + i*dx;
        data[i][0] = x*x/2;
        data[i][1] = x;
    }

    auto saos = cardinal_cubic_hermite_aos(std::move(data), x0, dx);
    for (Real t = x0; t <= x0 + 49*dx; t+= 0.0125)
    {
        CHECK_ULP_CLOSE(t*t/2, saos(t), 12);
        CHECK_ULP_CLOSE(t, saos.prime(t), 70);
    }

    auto [tlo, thi] = s.domain();
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(tlo*tlo/2), s(tlo), 3);
        CHECK_ULP_CLOSE(Real(thi*thi/2), s(thi), 3);
        CHECK_ULP_CLOSE(Real(tlo), s.prime(tlo), 3);
        CHECK_ULP_CLOSE(Real(thi), s.prime(thi), 3);
        CHECK_ULP_CLOSE(Real(tlo*tlo/2), saos(tlo), 3);
        CHECK_ULP_CLOSE(Real(thi*thi/2), saos(thi), 3);
        CHECK_ULP_CLOSE(Real(tlo), saos.prime(tlo), 3);
        CHECK_ULP_CLOSE(Real(thi), saos.prime(thi), 3);

        tlo = boost::math::nextafter(tlo, (std::numeric_limits<Real>::max)());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }
}

template<typename Real>
void test_cardinal_interpolation_condition()
{
    for (size_t n = 4; n < 50; ++n) {
        std::vector<Real> y(n);
        std::vector<Real> dydx(n);
        std::default_random_engine rd;
        std::uniform_real_distribution<Real> dis(0.1,1);
        Real x0 = Real(2);
        Real dx = Real(1)/Real(128);
        for (size_t i = 0; i < n; ++i) {
            y[i] = dis(rd);
            dydx[i] = dis(rd);
        }

        auto y_copy = y;
        auto dydx_copy = dydx;
        auto s = cardinal_cubic_hermite(std::move(y_copy), std::move(dydx_copy), x0, dx);
        for (size_t i = 0; i < y.size(); ++i) {
            CHECK_ULP_CLOSE(y[i], s(x0 + i*dx), 2);
            CHECK_ULP_CLOSE(dydx[i], s.prime(x0 + i*dx), 2);
        }
    }
}

int main()
{
    test_constant<float>();
    test_linear<float>();
    test_quadratic<float>();
    test_interpolation_condition<float>();
    test_cardinal_constant<float>();
    test_cardinal_linear<float>();
    test_cardinal_quadratic<float>();
    test_cardinal_interpolation_condition<float>();

    test_constant<double>();
    test_linear<double>();
    test_quadratic<double>();
    test_interpolation_condition<double>();
    test_cardinal_constant<double>();
    test_cardinal_linear<double>();
    test_cardinal_quadratic<double>();
    test_cardinal_interpolation_condition<double>();

    test_constant<long double>();
    test_linear<long double>();
    test_quadratic<long double>();
    test_interpolation_condition<long double>();
    test_cardinal_constant<long double>();
    test_cardinal_linear<long double>();
    test_cardinal_quadratic<long double>();
    test_cardinal_interpolation_condition<long double>();

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

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


/*
 * math.hpp
 *
 *  Created on: Apr 26, 2021
 *      Author: jelavice
 */
#pragma once
#include <cmath>
#include <vector>

#include <Eigen/Core>

namespace icp_loco{

// Clamps 'value' to be in the range ['min', 'max'].
template <typename T>
T Clamp(const T value, const T min, const T max) {
  if (value > max) {
    return max;
  }
  if (value < min) {
    return min;
  }
  return value;
}

// Calculates 'base'^'exponent'.
template <typename T>
constexpr T Power(T base, int exponent) {
  return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
}

// Calculates a^2.
template <typename T>
constexpr T Pow2(T a) {
  return Power(a, 2);
}

// Converts from degrees to radians.
constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }

// Converts form radians to degrees.
constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }

// Bring the 'difference' between two angles into [-pi; pi].
template <typename T>
T NormalizeAngleDifference(T difference) {
  const T kPi = T(M_PI);
  while (difference > kPi) difference -= 2. * kPi;
  while (difference < -kPi) difference += 2. * kPi;
  return difference;
}

template <typename T>
T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
  return std::atan2(vector.y(), vector.x());
}

template <typename T>
inline void QuaternionProduct(const double* const z, const T* const w,
                              T* const zw) {
  zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
  zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
  zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
  zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
}

} //icp_loco


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Adaptors/Zip.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Random.h"
#include "Molassembler/Temple/Stringify.h"
#include "Molassembler/Temple/constexpr/Array.h"
#include "Molassembler/Temple/constexpr/BTree.h"
#include "Molassembler/Temple/constexpr/Jsf.h"

#include <set>
#include <iostream>

using namespace Scine::Molassembler;

extern Temple::Generator<> generator;

namespace BTreeStaticTests {

constexpr Temple::BTree<unsigned, 3, 20> generateTree() {
  Temple::BTree<unsigned, 3, 20> tree;

  tree.insert(9);
  tree.insert(3);
  tree.insert(5);
  tree.insert(20);

  return tree;
}

constexpr auto testTree = generateTree();
static_assert(testTree.size() == 4, "Size of generated tree is wrong!");

static_assert(
  /* BTree of minimum order 3 has max 5 keys per node and max 6 children per node
   *
   * height  nodes       keys
   * 0       1           5
   * 1       1 + 6       5 + 6*5
   * 2       1 + 6 + 36  5 + 6*5 + 36*5
   *
   * #nodes(h) = sum_{i = 0}^{h} (2t)^i
   *
   *     (2t)^{h + 1} - 1
   *  N = ----------------
   *         2t - 1
   *
   * -> N * (2t - 1) + 1 = (2t)^{h + 1}
   *
   * -> log_2t [N * (2t - 1) + 1] = h + 1
   *
   * -> h = log_2t [N * (2t - 1) + 1] - 1
   *
   */
  Temple::BTreeProperties::minHeight(5, 3) == 0
  && Temple::BTreeProperties::minHeight(35, 3) == 1
  && Temple::BTreeProperties::minHeight(215, 3) == 2,
  "minHeight function is wrong"
);

static_assert(
  Temple::BTreeProperties::maxNodesInTree(0, 3) == 1
  && Temple::BTreeProperties::maxNodesInTree(1, 3) == 7
  && Temple::BTreeProperties::maxNodesInTree(2, 3) == 43
  && Temple::BTreeProperties::maxNodesInTree(3, 3) == 259,
  "maxNodesInTree is wrong"
);

} // namespace BTreeStaticTests

inline unsigned popRandom(std::set<unsigned>& values) {
  auto it = values.begin();

  std::advance(
    it,
    Temple::Random::getSingle<unsigned>(0, values.size() - 1, generator.engine)
  );

  auto value = *it;

  values.erase(it);

  return value;
}

BOOST_AUTO_TEST_CASE(ConstexprBTree, *boost::unit_test::label("Temple")) {
  constexpr unsigned nKeys = 100;

  using namespace std::string_literals;

  std::vector<unsigned> values (nKeys);

  std::iota(
    values.begin(),
    values.end(),
    0
  );

  std::set<unsigned> notInTree {values.begin(), values.end()};
  std::set<unsigned> inTree;

  Temple::BTree<unsigned, 3, nKeys> tree;

  std::string lastTreeGraph;

  std::vector<std::string> decisions;

  auto addElement = [&](const std::string& treeGraph) {
    // Add an element
    auto toAdd = popRandom(notInTree);
    decisions.emplace_back("i"s + std::to_string(toAdd));

    BOOST_TEST_CONTEXT(
      "Element insertion threw. Operation sequence: "
      << Temple::condense(decisions)
      << ". Prior to last operation: \n"
      << treeGraph << "\n\n After last operation: \n"
      << tree.dumpGraphviz()
    ) {
      BOOST_REQUIRE_NO_THROW(tree.insert(toAdd));
    }

    inTree.insert(toAdd);
  };

  auto removeElement = [&](const std::string& treeGraph) {
    // Remove an element
    auto toRemove = popRandom(inTree);
    decisions.emplace_back("r"s + std::to_string(toRemove));

    BOOST_TEST_CONTEXT(
      "Tree element removal failed. Operation sequence: "
        << Temple::condense(decisions)
        << ". Prior to last operation: \n"
        << treeGraph << "\n\n After last operation: \n"
        << tree.dumpGraphviz()
    ) {
      BOOST_REQUIRE_NO_THROW(tree.remove(toRemove));
    }

    notInTree.insert(toRemove);
  };

  auto fullValidation = [&](const std::string& treeGraph) {
    // Validate the tree
    BOOST_TEST_CONTEXT(
      "Tree validation failed. Operation sequence: "
        << Temple::condense(decisions)
        << ". Prior to last operation: \n"
        << treeGraph << "\n\n After last operation: \n"
        << tree.dumpGraphviz()
    ) {
      BOOST_REQUIRE_NO_THROW(tree.validate());
    }

    // Check that elements that weren't inserted aren't falsely contained
    auto notInsertedButContained = Temple::copy_if(
      notInTree,
      [&](const auto& notInTreeValue) -> bool {
        return tree.contains(notInTreeValue);
      }
    );

    // Check that all elements are truly contained or not
    BOOST_REQUIRE_MESSAGE(
      notInsertedButContained.empty(),
      "Not all elements recorded as not in the tree are recognized as such!\n"
        << "Found in the tree, but should not be present: "
        << Temple::condense(notInsertedButContained)
        << "\nSequence of operations: "
        << Temple::condense(decisions)
        << ". Prior to last operation: \n"
        << treeGraph << "\n\n After last operation: \n"
        << tree.dumpGraphviz()
    );

    auto insertedNotContained = Temple::copy_if(
      inTree,
      [&](const auto& inTreeValue) -> bool {
        return !tree.contains(inTreeValue);
      }
    );

    BOOST_REQUIRE_MESSAGE(
      insertedNotContained.empty(),
      "Not all elements recorded as contained in the tree are recognized as such!\n"
        << "Not found in the tree: "
        << Temple::condense(insertedNotContained)
        << "\nSequence of operations: "
        << Temple::condense(decisions)
        << ". Prior to last operation: \n"
        << treeGraph << "\n\n After last operation: \n"
        << tree.dumpGraphviz()
    );
  };

  for(unsigned i = 0; i < 10; ++i) {
    decisions.clear();

    // Heavy insert-delete workload
    for(unsigned nSteps = 0; nSteps < 1000; ++nSteps) {
      lastTreeGraph = tree.dumpGraphviz();

      // Decide whether to insert or remove a random item
      auto decisionFloat = Temple::Random::getSingle<double>(0.0, 1.0, generator.engine);
      if(decisionFloat >= static_cast<double>(inTree.size()) / nKeys) {
        addElement(lastTreeGraph);
      } else {
        removeElement(lastTreeGraph);
      }

      fullValidation(lastTreeGraph);
    }

    BOOST_REQUIRE_MESSAGE(
      Temple::all_of(
        Temple::Adaptors::zip(tree, inTree),
        [&](const unsigned treeValue, const unsigned testValue) -> bool {
          if(treeValue != testValue) {
            std::cout << "Expected " << testValue << ", got " << treeValue << std::endl;
            return false;
          }

          return true;
        }
      ),
      "BTree through-iteration does not yield the same elements as expected!\n"
        << tree.dumpGraphviz()
    );

    // Fill'er up all the way
    while(inTree.size() != nKeys) {
      lastTreeGraph = tree.dumpGraphviz();

      addElement(lastTreeGraph);
      fullValidation(lastTreeGraph);
    }

    // Empty the tree
    while(!inTree.empty()) {
      lastTreeGraph = tree.dumpGraphviz();

      removeElement(lastTreeGraph);
      fullValidation(lastTreeGraph);
    }
  }
}

namespace {

/* Test that if a BTree is instantiated with a specific size, that size
 * definitely fits in the tree
 */

template<size_t minOrder, size_t nElements>
constexpr bool BTreeAllocatedSizeSufficient() {
  Temple::BTree<unsigned, minOrder, nElements> tree;

  for(unsigned i = 0; i < nElements; ++i) {
    tree.insert(i);
  }

  return true;
}

template<size_t minOrder, size_t ... nElements>
constexpr bool testAllBTrees(std::index_sequence<nElements...> /* elements */) {
  Temple::Array<bool, sizeof...(nElements)> results {{
    BTreeAllocatedSizeSufficient<minOrder, 5 + nElements>()...
  }};

  for(unsigned i = 0; i < sizeof...(nElements); ++i) {
    if(!results.at(i)) {
      return false;
    }
  }

  return true;
}

template<size_t ... minOrders>
constexpr bool testAllBTrees(std::index_sequence<minOrders...> /* elements */) {
  Temple::Array<bool, sizeof...(minOrders)> results {{
    testAllBTrees<2 + minOrders>(std::make_index_sequence<45>{})... // Test sizes 5->50
  }};

  for(unsigned i = 0; i < sizeof...(minOrders); ++i) {
    if(!results.at(i)) {
      return false;
    }
  }

  return true;
}

constexpr bool testAllBTrees() {
  return testAllBTrees(std::make_index_sequence<3>{}); // Test min orders 2->5
}

static_assert(
  testAllBTrees(),
  "For some B-Trees, you cannot fit as many elements in as requested at instantiation"
);

} // namespace


#include "eigen-la.h"
#include <Eigen/Core>
#include <Eigen/LU>
//#include <Eigen/LeastSquares>
#include <Eigen/QR>
// #include <Eigen/QR>
#include <Eigen/SVD> // src/SVD/JacobiSVD.h>

using namespace Eigen;

template <class T>
RET rank(Decomposition d, int* v, const void* p, int r, int c) {
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix A((T*)p,r,c);
    switch (d) {
        case ::FullPivLU:
            *v = A.fullPivLu().rank();
            break;
        case ::ColPivHouseholderQR:
            *v = A.colPivHouseholderQr().rank();
            break;
        case ::FullPivHouseholderQR:
            *v = A.fullPivHouseholderQr().rank();
            break;
        case ::JacobiSVD:
            *v = A.jacobiSvd(ComputeThinU | ComputeThinV).rank();
            break;
        default:
            return strdup("Selected decomposition doesn't support rank revealing.");
    }
    return 0;
}
API(rank, (int code, Decomposition d, int* v, const void* p, int r, int c), (d,v,p,r,c));

template <class T>
RET kernel(Decomposition d, void** p0, int* r0, int* c0, const void* p1, int r1, int c1) {
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    if (d != ::FullPivLU)
        return strdup("Selected decomposition doesn't support kernel revealing.");
    MapMatrix A((T*)p1,r1,c1);
    Matrix<T,Dynamic,Dynamic> B = A.fullPivLu().kernel();
    *r0 = B.rows();
    *c0 = B.cols();
    *p0 = malloc(*r0 * *c0 * sizeof(T));
    MapMatrix((T*)*p0, *r0, *c0) = B;
    return 0;
}
API(kernel, (int code, Decomposition d, void** p0, int* r0, int* c0, const void* p1, int r1, int c1), (d,p0,r0,c0,p1,r1,c1));

template <class T>
RET image(Decomposition d, void** p0, int* r0, int* c0, const void* p1, int r1, int c1) {
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    if (d != ::FullPivLU)
        return strdup("Selected decomposition doesn't support image revealing.");
    MapMatrix A((T*)p1,r1,c1);
    Matrix<T,Dynamic,Dynamic> B = A.fullPivLu().image(A);
    *r0 = B.rows();
    *c0 = B.cols();
    *p0 = malloc(*r0 * *c0 * sizeof(T));
    MapMatrix((T*)*p0, *r0, *c0) = B;
    return 0;
}
API(image, (int code, Decomposition d, void** p0, int* r0, int* c0, const void* p1, int r1, int c1), (d,p0,r0,c0,p1,r1,c1));

template <class T>
RET solve(Decomposition d,
    void* px, int rx, int cx,
    const void* pa, int ra, int ca,
    const void* pb, int rb, int cb)
{
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix x((T*)px, rx, cx);
    MapMatrix A((T*)pa, ra, ca);
    MapMatrix b((T*)pb, rb, cb);
    switch (d) {
        case ::PartialPivLU:
            x = A.partialPivLu().solve(b);
            break;
        case ::FullPivLU:
            x = A.fullPivLu().solve(b);
            break;
        case ::HouseholderQR:
            x = A.householderQr().solve(b);
            break;
        case ::ColPivHouseholderQR:
            x = A.colPivHouseholderQr().solve(b);
            break;
        case ::FullPivHouseholderQR:
            x = A.fullPivHouseholderQr().solve(b);
            break;
        case ::LLT:
            x = A.llt().solve(b);
            break;
        case ::LDLT:
            x = A.ldlt().solve(b);
            break;
        case ::JacobiSVD:
            x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
            break;
    }
    return 0;
}
API(solve, (int code, Decomposition d,
    void* px, int rx, int cx,
    const void* pa, int ra, int ca,
    const void* pb, int rb, int cb), (d,px,rx,cx,pa,ra,ca,pb,rb,cb));

template <class T>
RET relativeError(void* e,
    const void* px, int rx, int cx,
    const void* pa, int ra, int ca,
    const void* pb, int rb, int cb)
{
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix x((T*)px, rx, cx);
    MapMatrix A((T*)pa, ra, ca);
    MapMatrix b((T*)pb, rb, cb);
    *(T*)e = (A*x - b).norm() / b.norm();
    return 0;
}
API(relativeError, (int code, void* e,
    const void* px, int rx, int cx,
    const void* pa, int ra, int ca,
    const void* pb, int rb, int cb), (e,px,rx,cx,pa,ra,ca,pb,rb,cb));


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Copyright (c) 2009-2013 Illumina, Inc.
//
// This software is provided under the terms and conditions of the
// Illumina Open Source Software License 1.
//
// You should have received a copy of the Illumina Open Source
// Software License 1 along with this program. If not, see
// <https://github.com/sequencing/licenses/>
//

/// \file

/// \author Chris Saunders
///
#include "blt_util/binomial_test.hh"
#include "blt_util/stat_util.hh"

#include <boost/math/distributions/binomial.hpp>

using boost::math::binomial;
using boost::math::cdf;

#include <algorithm>

bool
is_reject_binomial_p_exact(const double alpha,
                           const double p,
                           const unsigned n_success,
                           const unsigned n_failure) {

    const unsigned n_trial(n_success+n_failure);
    const double obs_p((double)n_success/(double)n_trial);

    double exact_prob;
    if (obs_p <= p) {
        exact_prob=cdf(binomial(n_trial,p),n_success);
    } else {
        exact_prob=cdf(binomial(n_trial,1.-p),n_failure);
    }

    return ((2.*exact_prob)<alpha);
}

bool
is_reject_binomial_p_chi_sqr(const double alpha,
                             const double p,
                             const unsigned n_success,
                             const unsigned n_failure) {

    assert((p>0.) && (p<1.));

    const unsigned n_trial(n_success+n_failure);
    const double e_success(p*n_trial);
    const double e_failure(((double)n_trial)-e_success);

    const double d_success(n_success-e_success);
    const double d_failure(n_failure-e_failure);

    const double xsq((d_success*d_success)/e_success+(d_failure*d_failure)/e_failure);

    return is_chi_sqr_reject(xsq,1,alpha);
}

bool
is_reject_binomial_p(const double alpha,
                     const double p,
                     const unsigned n_success,
                     const unsigned n_failure) {

    static const unsigned exact_test_threshold(250);

    const unsigned n_trial(n_success+n_failure);

    if (n_trial > exact_test_threshold) {
        return is_reject_binomial_p_chi_sqr(alpha,p,n_success,n_failure);
    } else {
        return is_reject_binomial_p_exact(alpha,p,n_success,n_failure);
    }
}


#include <permutation.h>
#include "problemes.h"

#include <boost/rational.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

typedef boost::rational<nombre> fraction;

ENREGISTRER_PROBLEME(121, "Disc game prize fund") {
    // A bag contains one red disc and one blue disc. In a game of chance a player takes a disc at
    // random and its colour is noted. After each turn the disc is returned to the bag, an extra red
    // disc is added, and another disc is taken at random.
    //
    // The player pays £1 to play and wins if they have taken more blue discs than red discs at the
    // end of the game.
    //
    // If the game is played for four turns, the probability of a player winning is exactly 11/120,
    // and so the maximum prize fund the banker should allocate for winning in this game would be £10
    // before they would expect to incur a loss. Note that any payout will be a whole number of pounds
    // and also includes the original £1 paid to play the game, so in the example given the player
    // actually wins £9.
    //
    // Find the maximum prize fund that should be allocated to a single game in which fifteen turns are
    // played.
    nombre limite = 15;
    std::vector<fraction> probabilites;
    for (nombre n = 1; n < limite + 1; ++n) {
        probabilites.emplace_back(1, n + 1);
    }

    fraction probabilite;

    for (nombre bleu = limite / 2 + 1; bleu < limite + 1; ++bleu) {
        std::vector<bool> possibilite(limite - bleu, false);
        possibilite.insert(possibilite.end(), bleu, true);
        for (auto &permutation: permutation::Permutation<std::vector<bool>>(possibilite)) {
            fraction p(1);
            for (size_t n = 0; n < limite; ++n) {
                if (permutation.at(n))
                    p *= probabilites.at(n);
                else
                    p *= fraction(1) - probabilites.at(n);
            }

            probabilite += p;
        }
    }

    nombre resultat = probabilite.denominator() / probabilite.numerator();
    return std::to_string(resultat);
}


#include "curves.h"

#include <algorithm>
#include <array>
#include <cassert>
#include <cstddef>
#include <cstdint>
#include <memory>

#include <boost/optional.hpp>

#include "../shader_core/lerp.h"
#include "../shader_core/rect.h"

static float eval_hermite_interpolation(
	const csc::Float2 p0,
	const csc::Float2 p1,
	const csc::Float2 p2,
	const csc::Float2 p3,
	const float input_x )
{
	std::array<csc::Float2, 32> hermite_curve;

	const auto do_hermite = [](const float p0, const float p1, const float p2, const float p3, const float t) -> float
	{
		const float a{ -p0 / 2.0f + (3.0f * p1) / 2.0f - (3.0f * p2) / 2.0f + p3 / 2.0f };
		const float b{ p0 - (5.0f * p1) / 2.0f + 2.0f * p2 - p3 / 2.0f };
		const float c{ -p0 / 2.0f + p2 / 2.0f };
		const float d{ p1 };

		return a * t*t*t + b * t*t + c * t + d;
	};

	// Assert that x values are in strictly ascending order
	assert(p1.x >= p0.x);
	assert(p2.x >= p1.x);
	assert(p3.x >= p2.x);

	// Populate our whole curve with (x, y) pairs
	for (size_t i = 0; i < hermite_curve.size(); i++) {
		const float t{ static_cast<float>(i) / (hermite_curve.size() - 1) };
		const float x{ do_hermite(p0.x, p1.x, p2.x, p3.x, t) };
		const float y{ do_hermite(p0.y, p1.y, p2.y, p3.y, t) };
		hermite_curve[i] = csc::Float2{ x, y };
	}

	// Look for the two (x, y) pairs that surround our x value
	// The curve is already known to be sorted on x
	size_t before_index{ SIZE_MAX };
	for (size_t i = 0; i < hermite_curve.size(); i++) {
		if (hermite_curve[i].x < input_x) {
			before_index = i;
		}
	}

	// In the case where our x input is before the first point, just return the value of the first point
	// This may happen due to floating point oddities
	if (before_index == SIZE_MAX) {
		return hermite_curve[0].y;
	}

	// In the case where our x input is beyond the last point, just return the value of the last point
	// This may happen due to floating point oddities
	if (before_index == hermite_curve.size() - 1) {
		return hermite_curve[hermite_curve.size() - 1].y;
	}

	const float lerp_t{ (input_x - hermite_curve[before_index].x) / (hermite_curve[before_index + 1].x - hermite_curve[before_index].x) };
	return csc::lerp(hermite_curve[before_index].y, hermite_curve[before_index + 1].y, lerp_t);
}

bool csg::CurvePoint::operator==(const CurvePoint& other) const
{
	return pos == other.pos && interp == other.interp;
}

csg::Curve::Curve(const csc::Float2 min, const csc::Float2 max, const boost::optional<std::vector<CurvePoint>> points) : _min{ min }, _max{ max }
{
	if (points) {
		// Validate that all points are inside the bounds
		const csc::FloatRect bounds{ min, max };
		for (const CurvePoint& this_point : *points) {
			assert(bounds.contains(this_point.pos));
		}
		this->m_points = *points;
	}
	else {
		// Default value is always a straight line from min to max
		this->m_points.push_back(csg::CurvePoint{ min, CurveInterp::CUBIC_HERMITE });
		this->m_points.push_back(csg::CurvePoint{ max, CurveInterp::CUBIC_HERMITE });
	}
	sort_points();
}

float csg::Curve::eval_point(const float input) const
{
	// For speed, this function assumes the control point vector is sorted already
	assert(m_points.size() >= 1);

	// Variable naming convention for points:
	// p1 is the point immediately before input
	// p2 is the point immediately after input
	// In the case of spline interpolation, we need 2 more
	// p0 is the point before p1
	// p3 is the point after p2
	// Any one of the points might not be real, but at least one of p1 or p2 must be real

	boost::optional<size_t> p1_index;
	boost::optional<size_t> p2_index;
	for (size_t i = 0; i < m_points.size(); i++) {
		// Find the point before input
		if (m_points[i].pos.x <= input) {
			p1_index = i;
		}
	}
	for (size_t i = m_points.size(); i > 0; i--) {
		// Find the point after input
		if (m_points[i-1].pos.x > input) {
			p2_index = i-1;
		}
	}

	// Clamp if input is not between two points
	if (p1_index.has_value() == false) {
		return m_points[0].pos.y;
	}
	if (p2_index.has_value() == false) {
		return m_points[m_points.size() - 1].pos.y;
	}

	const csc::Float2 p1{ m_points[*p1_index].pos };
	const csc::Float2 p2{ m_points[*p2_index].pos };
	const csg::CurveInterp p1_interp{ m_points[*p1_index].interp };
	const csg::CurveInterp p2_interp{ m_points[*p2_index].interp };

	const float delta_x{ p2.x - p1.x };
	const float fade{ (input - p1.x) / delta_x };

	const float linear_result{ csc::lerp(p1.y, p2.y, fade) };
	if (p1_interp == csg::CurveInterp::LINEAR && p2_interp == csg::CurveInterp::LINEAR) {
		// End early if possible to avoid doing unnecessary spline math
		return linear_result;
	}

	// Lambdas to generate p0 and p3 for our interp
	const auto gen_p0_pos = [](const std::vector<CurvePoint>& points, const size_t p1_index, const size_t p2_index) -> csc::Float2
	{
		if (p1_index == 0) {
			const csc::Float2 p1{ points[p1_index].pos };
			const csc::Float2 p2{ points[p2_index].pos };
			return p1 - (p2 - p1);
		}
		else {
			return points[p1_index - 1].pos;
		}
	};
	const auto gen_p3_pos = [](const std::vector<CurvePoint>& points, const size_t p1_index, const size_t p2_index) -> csc::Float2
	{
		if (p2_index >= points.size() - 1) {
			const csc::Float2 p1{ points[p1_index].pos };
			const csc::Float2 p2{ points[p2_index].pos };
			return p2 + (p2 - p1);
		}
		else {
			return points[p2_index + 1].pos;
		}
	};
	const csc::Float2 p0{ gen_p0_pos(m_points, *p1_index, *p2_index) };
	const csc::Float2 p3{ gen_p3_pos(m_points, *p1_index, *p2_index) };

	// Hermite spline interpolation

	const float hermite_result{ eval_hermite_interpolation(p0, p1, p2, p3, input) };

	// 0 means to use fully linear interp, 1 means fully hermite
	const auto calc_interp_lerp = [](const csg::CurveInterp p1_interp, const csg::CurveInterp p2_interp, const float t) -> float
	{
		if (p1_interp == csg::CurveInterp::LINEAR && p2_interp == csg::CurveInterp::LINEAR) {
			return 0.0f;
		}
		else if (p1_interp == csg::CurveInterp::CUBIC_HERMITE && p2_interp == csg::CurveInterp::CUBIC_HERMITE) {
			return 1.0f;
		}
		else if (p1_interp == csg::CurveInterp::CUBIC_HERMITE && p2_interp == csg::CurveInterp::LINEAR) {
			return 1.0f - t;
		}
		else if (p1_interp == csg::CurveInterp::LINEAR && p2_interp == csg::CurveInterp::CUBIC_HERMITE) {
			return t;
		}
		else {
			assert(false);
			// To suppress a compiler warning
			return 0.0f;
		}
	};

	const float interp_t{ (input - p1.x) / (p2.x - p1.x) };
	return csc::lerp(linear_result, hermite_result, calc_interp_lerp(p1_interp, p2_interp, interp_t));
}

void csg::Curve::delete_point(const size_t index)
{
	if (m_points.size() < 2) {
		// Curve is in a meaningless state
		assert(false);
	}
	else if (m_points.size() == 2) {
		return;
	}

	m_points.erase(m_points.begin() + index);
	sort_points();
}

size_t csg::Curve::create_point(const float x)
{
	if (x < _min.x) {
		return 0;
	}
	if (x > _max.x) {
		return m_points.size() - 1;
	}
	const float y{ eval_point(x) };
	const CurvePoint new_point{ csc::Float2{ x, y }, csg::CurveInterp::LINEAR };
	m_points.push_back(new_point);
	sort_points();
	// Use linear interp by default
	// If either surrounding point is hermite, then use hermite
	for (size_t i = 0; i < m_points.size(); i++) {
		if (m_points[i] == new_point) {
			// i is the index of the new point
			csg::CurveInterp interp{ csg::CurveInterp::LINEAR };
			if (i > 0 && m_points[i - 1].interp == csg::CurveInterp::CUBIC_HERMITE) {
				interp = csg::CurveInterp::CUBIC_HERMITE;
			}
			if (i + 1 < m_points.size() && m_points[i + 1].interp == csg::CurveInterp::CUBIC_HERMITE) {
				interp = csg::CurveInterp::CUBIC_HERMITE;
			}
			m_points[i].interp = interp;
			return i;
		}
	}
	assert(false);
	return 0;
}

size_t csg::Curve::move_point(const size_t index, const csc::Float2 new_pos)
{
	if (index >= m_points.size()) {
		return index;
	}

	const csc::FloatRect valid_pos{ _min, _max };
	m_points[index].pos = valid_pos.clamp(new_pos);
	const CurvePoint changed_point{ m_points[index] };
	sort_points();

	// Find and return the point's new index
	for (size_t i = 0; i < m_points.size(); i++) {
		if (changed_point == m_points[i]) {
			return i;
		}
	}
	assert(false);
	return index;
}

void csg::Curve::set_interp(const size_t index, const CurveInterp new_interp)
{
	if (index >= m_points.size()) {
		return;
	}
	m_points[index].interp = new_interp;
}

void csg::Curve::set_bounds(const csc::FloatRect bounds_rect)
{
	if (bounds_valid(bounds_rect) == false) {
		return;
	}
	_min = bounds_rect.begin();
	_max = bounds_rect.end();
}

bool csg::Curve::bounds_valid(const csc::FloatRect bounds_rect)
{
	for (const CurvePoint& this_point : m_points) {
		if (bounds_rect.contains(this_point.pos) == false) {
			return false;
		}
	}
	return true;
}

bool csg::Curve::similar(const Curve& other, const float margin) const
{
	if (_min.similar(other._min, margin) == false) {
		return false;
	}
	if (_max.similar(other._max, margin) == false) {
		return false;
	}
	if (m_points.size() != other.m_points.size()) {
		return false;
	}
	for (size_t i = 0; i < m_points.size(); i++) {
		if (m_points[i].interp != other.m_points[i].interp) {
			return false;
		}
		if (m_points[i].pos.similar(other.m_points[i].pos, margin) == false) {
			return false;
		}
	}

	return true;
}

bool csg::Curve::operator==(const Curve& other) const
{
	if (_min != other._min) {
		return false;
	}
	if (_max != other._max) {
		return false;
	}
	if (m_points.size() != other.m_points.size()) {
		return false;
	}
	for (size_t i = 0; i < m_points.size(); i++) {
		if (m_points[i] != other.m_points[i]) {
			return false;
		}
	}

	return true;
}

void csg::Curve::sort_points()
{
	const auto lt_x = [](const CurvePoint a, const CurvePoint b) -> bool {
		return a.pos.x < b.pos.x;
	};
	std::sort(m_points.begin(), m_points.end(), lt_x);
}


#include <Eigen/Core>
#include <iostream>
#include <mathtoolbox/acquisition-functions.hpp>
#include <mathtoolbox/gaussian-process-regression.hpp>
#include <mathtoolbox/probability-distributions.hpp>
#include <timer.hpp>

using Eigen::Matrix2d;
using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::VectorXd;

double CalcFunc(const Vector2d& x)
{
    return mathtoolbox::GetNormalDist(3.0 * x, Vector2d::Zero(), Matrix2d::Identity(), 1.0);
}

int main(int argc, char** argv)
{
    // Define the scene setting
    constexpr int    num_samples     = 10;
    constexpr double noise_intensity = 1e-04;

    // Generate scattered data
    MatrixXd X(2, num_samples);
    VectorXd y(num_samples);
    for (int i = 0; i < num_samples; ++i)
    {
        X.col(i) = Vector2d::Random();
        y(i)     = CalcFunc(X.col(i)) + noise_intensity * (VectorXd::Random(1))(0);
    }

    // Define the kernel type
    const auto kernel_type = mathtoolbox::GaussianProcessRegressor::KernelType::ArdMatern52;

    // Instantiate the interpolation object
    mathtoolbox::GaussianProcessRegressor regressor(X, y, kernel_type);

    // Perform hyperparameter estimation
    const Eigen::Vector3d default_kernel_hyperparams(0.50, 0.50, 0.50);
    regressor.PerformMaximumLikelihood(default_kernel_hyperparams, 1e-04);

    // Calculate EI values and their derivatives
    for (int i = 0; i < 100; ++i)
    {
        constexpr int    num_dims = 2;
        constexpr double epsilon  = 1e-06;

        const VectorXd x_plus = [&]() {
            int index;
            y.maxCoeff(&index);
            return X.col(index);
        }();

        const VectorXd x = Vector2d::Random();

        const VectorXd acquisition_deriv = mathtoolbox::GetExpectedImprovementDerivative(
            x,
            [&](const VectorXd& x) { return regressor.PredictMean(x); },
            [&](const VectorXd& x) { return regressor.PredictStdev(x); },
            x_plus,
            [&](const VectorXd& x) { return regressor.PredictMeanDeriv(x); },
            [&](const VectorXd& x) { return regressor.PredictStdevDeriv(x); });

        VectorXd acquisition_numerical_deriv(num_dims);
        for (int d = 0; d < num_dims; ++d)
        {
            VectorXd delta = VectorXd::Zero(num_dims);
            delta(d)       = epsilon;

            const double value_plus = mathtoolbox::GetExpectedImprovement(
                x + delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                x_plus);

            const double value_minus = mathtoolbox::GetExpectedImprovement(
                x - delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                x_plus);

            acquisition_numerical_deriv(d) = value_plus - value_minus;
        }
        acquisition_numerical_deriv /= 2.0 * epsilon;

        const auto scale     = acquisition_deriv.norm();
        const auto abs_error = (acquisition_deriv - acquisition_numerical_deriv).norm();
        const auto rel_error = abs_error / scale;

        if (scale > 1e-06 && rel_error > 1e-02)
        {
            std::cout << "point location: " << x.transpose() << std::endl;
            std::cout << "analytic      : " << acquisition_deriv.transpose() << std::endl;
            std::cout << "numerical     : " << acquisition_numerical_deriv.transpose() << std::endl;
            std::cout << "error         : " << abs_error << std::endl;

            exit(1);
        }
    }

    // Calculate GP-UCB values and their derivatives
    for (int i = 0; i < 100; ++i)
    {
        constexpr int    num_dims = 2;
        constexpr double epsilon  = 1e-06;

        const double hyperparam = 1.0 + (Eigen::VectorXd::Random(1))(0);

        const VectorXd x = Vector2d::Random();

        const VectorXd acquisition_deriv = mathtoolbox::GetGaussianProcessUpperConfidenceBoundDerivative(
            x,
            [&](const VectorXd& x) { return regressor.PredictMean(x); },
            [&](const VectorXd& x) { return regressor.PredictStdev(x); },
            hyperparam,
            [&](const VectorXd& x) { return regressor.PredictMeanDeriv(x); },
            [&](const VectorXd& x) { return regressor.PredictStdevDeriv(x); });

        VectorXd acquisition_numerical_deriv(num_dims);
        for (int d = 0; d < num_dims; ++d)
        {
            VectorXd delta = VectorXd::Zero(num_dims);
            delta(d)       = epsilon;

            const double value_plus = mathtoolbox::GetGaussianProcessUpperConfidenceBound(
                x + delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                hyperparam);

            const double value_minus = mathtoolbox::GetGaussianProcessUpperConfidenceBound(
                x - delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                hyperparam);

            acquisition_numerical_deriv(d) = value_plus - value_minus;
        }
        acquisition_numerical_deriv /= 2.0 * epsilon;

        const auto scale     = acquisition_deriv.norm();
        const auto abs_error = (acquisition_deriv - acquisition_numerical_deriv).norm();
        const auto rel_error = abs_error / scale;

        if (scale > 1e-06 && rel_error > 1e-02)
        {
            std::cout << "point location: " << x.transpose() << std::endl;
            std::cout << "analytic      : " << acquisition_deriv.transpose() << std::endl;
            std::cout << "numerical     : " << acquisition_numerical_deriv.transpose() << std::endl;
            std::cout << "error         : " << abs_error << std::endl;

            exit(1);
        }
    }

    return 0;
}


#include <iostream>
#include <string>
#include <algorithm>
#include <utility> // std::pair, std::make_pair
#include <cmath> // float comparison
#include <limits>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "persistent_cohomology_multi_field"
#include <boost/test/unit_test.hpp>

#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/reader_utils.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>
#include <gudhi/Persistent_cohomology/Multi_field.h>

using namespace Gudhi;
using namespace Gudhi::persistent_cohomology;
using namespace boost::unit_test;

typedef Simplex_tree<> typeST;

std::string test_rips_persistence(int min_coefficient, int max_coefficient, double min_persistence) {
  // file is copied in CMakeLists.txt
  std::ifstream simplex_tree_stream;
  simplex_tree_stream.open("simplex_tree_file_for_multi_field_unit_test.txt");
  typeST st;
  simplex_tree_stream >> st;
  simplex_tree_stream.close();

  // Display the Simplex_tree
  std::clog << "The complex contains " << st.num_simplices() << " simplices" << " - dimension= " << st.dimension()
      << std::endl;

  // Check
  BOOST_CHECK(st.num_simplices() == 58);
  BOOST_CHECK(st.dimension() == 3);

  // Sort the simplices in the order of the filtration
  st.initialize_filtration();

  // Compute the persistence diagram of the complex
  Persistent_cohomology<Simplex_tree<>, Multi_field> pcoh(st);

  pcoh.init_coefficients(min_coefficient, max_coefficient); // initializes the coefficient field for homology
  // Check infinite rips
  pcoh.compute_persistent_cohomology(min_persistence); // Minimal lifetime of homology feature to be recorded.

  std::ostringstream ossRips;
  pcoh.output_diagram(ossRips);

  std::string strRips = ossRips.str();
  return strRips;
}

void test_rips_persistence_in_dimension(int min_dimension, int max_dimension) {
  // there are 2 discontinued ensembles
  std::string value0("  0 0.25 inf");
  std::string value1("  1 0.4 inf");
  // And a big hole - cut in 2 pieces after 0.3
  std::string value2("  0 0.2 0.3");

  // For dim <= 1 =>
  std::string value3("  1 0.25 inf");
  std::string value4("  2 0.25 inf");
  std::string value5("  1 0.3 inf");
  std::string value6("  2 0.3 inf");
  std::string value7("  2 0.4 inf");

  std::clog << "********************************************************************" << std::endl;
  std::clog << "TEST OF RIPS_PERSISTENT_COHOMOLOGY_MULTI_FIELD MIN_DIM=" << min_dimension << " MAX_DIM=" << max_dimension << " MIN_PERS=0" << std::endl;

  std::string str_rips_persistence = test_rips_persistence(min_dimension, max_dimension, 0.0);
  std::clog << "str_rips_persistence=" << str_rips_persistence << std::endl;

  BOOST_CHECK(str_rips_persistence.find(value0) != std::string::npos); // Check found
  BOOST_CHECK(str_rips_persistence.find(value1) != std::string::npos); // Check found
  BOOST_CHECK(str_rips_persistence.find(value2) != std::string::npos); // Check found

  if ((min_dimension < 2) && (max_dimension < 2)) {
    BOOST_CHECK(str_rips_persistence.find(value3) != std::string::npos); // Check found
    BOOST_CHECK(str_rips_persistence.find(value4) != std::string::npos); // Check found
    BOOST_CHECK(str_rips_persistence.find(value5) != std::string::npos); // Check found
    BOOST_CHECK(str_rips_persistence.find(value6) != std::string::npos); // Check found
    BOOST_CHECK(str_rips_persistence.find(value7) != std::string::npos); // Check found
  } else {
    BOOST_CHECK(str_rips_persistence.find(value3) == std::string::npos); // Check not found
    BOOST_CHECK(str_rips_persistence.find(value4) == std::string::npos); // Check not found
    BOOST_CHECK(str_rips_persistence.find(value5) == std::string::npos); // Check not found
    BOOST_CHECK(str_rips_persistence.find(value6) == std::string::npos); // Check not found
    BOOST_CHECK(str_rips_persistence.find(value7) == std::string::npos); // Check not found
  }

}

BOOST_AUTO_TEST_CASE(rips_persistent_cohomology_multi_field_dim_1_2) {
  test_rips_persistence_in_dimension(0, 1);
}

BOOST_AUTO_TEST_CASE(rips_persistent_cohomology_multi_field_dim_2_3) {
  test_rips_persistence_in_dimension(1, 3);
}

BOOST_AUTO_TEST_CASE(rips_persistent_cohomology_multi_field_dim_1_5) {
  test_rips_persistence_in_dimension(1, 5);
}

// TODO(VR): not working from 6
// std::string str_rips_persistence = test_rips_persistence(6, 0);
// TODO(VR): division by zero
// std::string str_rips_persistence = test_rips_persistence(0, 0);
// TODO(VR): is result OK of :
// test_rips_persistence_in_dimension(3, 4);


#include <iostream>
#include <Eigen/Dense>
#include <math.h>

//Quaternion Exponential
//implements simple 0th-order integration
//ref: quaternion kinematics, section 4.6.1
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qExponential(float dt, Vector3f w)
{
  float wn = w.norm();
  Vector3f wN = w.normalized();
  Quaternionf qReturn;
  qReturn.w() = cos(wn * dt / 2);
  qReturn.vec() = wN * sin(wn * dt / 2);
  return qReturn;
}

int main()
{
  using Eigen::Quaternionf;
  using Eigen::Vector3f;
  float dt = 0.01;
  Vector3f w(1.0, 2.0, -3.0);
  Quaternionf qExp = qExponential(dt, w);
  std::cout << qExp.w() << std::endl << qExp.vec() << std::endl;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_gauss"

#include <boost/test/unit_test.hpp>
#include "nanocv/math/gauss.hpp"
#include <iostream>

BOOST_AUTO_TEST_CASE(test_gauss)
{
        using namespace ncv;

        using std::size_t;

        const std::vector<double> sigmas = { 0.2, 0.5, 0.7, 1.0, 1.5, 2.0, 2.5 };
        const std::vector<double> cutoffs = { 0.001, 0.01, 0.1 };

        const gauss::kernel_normalization normalize = gauss::kernel_normalization::on;

        // test various variances
        for (double sigma : sigmas)
        {
                // test various cutoffs (skip low values in the kernel)
                for (double cutoff : cutoffs)
                {
                        const auto kernel = gauss_kernel_t<double>(sigma, cutoff, normalize);

                        std::cout << "sigma = " << sigma << ", cutoff = " << cutoff << std::endl;
                        std::cout << "kernel = {";
                        for (size_t k = 0; k < kernel.size(); k ++)
                        {
                                std::cout << kernel[k] << (k + 1 == kernel.size() ? "" : ", ");
                        }
                        std::cout << "}" << std::endl << std::endl;

                        /// \todo more tests!

                        // check kernel sum
                        const double sum = kernel.sum();
                        BOOST_CHECK_LE(sum, 1.0 + 1e-8);
                        BOOST_CHECK_GE(sum, 1.0 - 1e-8);
                }
        }
}


#include "../DynAutoDiff/DynAutoDiff.hpp"
#include <algorithm>
#include <boost/test/tools/old/interface.hpp>
#include <eigen3/Eigen/Core>
#include <iostream>
#include <ostream>
#include <vector>

#define BOOST_TEST_MODULE Normal_Test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#define TL 1e-10

using namespace Eigen;
using namespace std;
using namespace DynAutoDiff;

BOOST_AUTO_TEST_SUITE(Losses_Test)

BOOST_AUTO_TEST_CASE(bce_test) {
    vector<TMat<>> v(2);
    v[0] = TMat<>(2, 2);
    // cout << v[0] << endl;

    auto p = vec<double>({0.2, 0.3, 0.4, 0.5, 0.8}, true), y = vec<double>({0, 0, 1, 1, 1});

    auto y1 = binary_cross_entropy(p, y);
    GraphManager gm1(y1);
    gm1.run();

    BOOST_CHECK_CLOSE(y1->v(), 2.4123999590012524, 1e-5);
    BOOST_CHECK_CLOSE(p->g(), 5.0 / 4, 1e-5);
    BOOST_CHECK_CLOSE(p->g(1), 1.4285714382902825, 1e-3);
    BOOST_CHECK_CLOSE(p->g(3), -2, 1e-10);
}
BOOST_AUTO_TEST_CASE(ivecl_test) {}
BOOST_AUTO_TEST_CASE(times_test) {}

BOOST_AUTO_TEST_CASE(division_test) {}
BOOST_AUTO_TEST_SUITE_END()


#include <blitzml/base/math_util.h>

#include <utility>

using std::pair;

namespace BlitzML {

pair<value_t, value_t> compute_quadratic_roots(value_t a, value_t b,
                                                    value_t c) {
  value_t discriminant = b*b - 4*a*c;
  bool result_exists = (discriminant >= 0);
  if (a == 0. && b == 0. && c != 0.) {
    result_exists = false;
  }
  if (!result_exists) {
    value_t nan = std::numeric_limits<value_t>::quiet_NaN();
    return pair<value_t, value_t>(nan, nan);
  }
  if (a == 0.) {
    value_t result = -c/b;
    return pair<value_t, value_t>(result, result);
  }

  value_t sqrt_discriminant = sqrt(discriminant);
  value_t root1 = (-b - sqrt_discriminant) / (2 * a);
  value_t root2 = (-b + sqrt_discriminant) / (2 * a);
  if (a > 0) {
    return pair<value_t, value_t>(root1, root2);
  } else {
    return pair<value_t, value_t>(root2, root1);
  }
}

} // namespace BlitzML


#include <cmath>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include "kernel.h"

const std::map<std::string, double> calculator::WExpression::global_constants
{
    {"pi", boost::math::constants::pi<double>()},
    {"e", boost::math::constants::e<double>()}
};

double calculator::WExpression::expression()
{
    Token t {ts.pop()};
    bool isNeg {false};
    if (t.kind == TokenKind::minus)
        isNeg = true;
    else if (t.kind == TokenKind::plus)
        isNeg = false;
    else
        ts.push(t);

    double left {term()};
    if (isNeg) left = -left;

    while (true)
    {
        Token t {ts.pop()};
        switch(t.kind)
        {
            case TokenKind::plus:
                left += term();
                break;
            case TokenKind::minus:
                left -= term();
                break;
            default:
                ts.push(t);
                return left;
        }
    }
}

double calculator::WExpression::term()
{
    using boost::numeric_cast;
    double left {primary()};

    while (true)
    {
        Token t {ts.pop()};
        switch (t.kind) {
            case TokenKind::multiply:
                left *= primary();
                break;
            case TokenKind::divide:
            {
                double d {primary()};
                if (d == 0.0) throw std::logic_error("division by 0");
                left /= d;
                break;
            }
            case TokenKind::mod:
            {
                double d{primary()};
                int i1 = numeric_cast<int>(left);
                double di1 = numeric_cast<double>(i1);
                if (left - di1 > std::numeric_limits<double>::min())
                    throw std::logic_error("left operand of %(mod) is not integer");
                int i2 = numeric_cast<int>(d);
                double di2 = numeric_cast<double>(i2);
                if (d - di2 > std::numeric_limits<double>::min())
                    throw std::logic_error("right operand of %(mod) is not integer");
                left = i1 % i2;
                break;
            }
            default:
                ts.push(t);
                return left;
        }
    }
}

double calculator::WExpression::primary()
{
    using boost::numeric_cast;
    double result {0.0};
    Token t {ts.pop()};
    auto endBracket = [](TokenKind tk)->TokenKind{
        switch (tk)
        {
            case TokenKind::bracket_left0:
                return TokenKind::bracket_right0;
            case TokenKind::bracket_left1:
                return TokenKind::bracket_right1;
            default:
                return tk;
        }
    };
    switch (t.kind) {
        case TokenKind::number:
        {
            result = t.value;
            break;
        }
        case TokenKind::variable:
        {
            // first find in constants
            auto gcit {global_constants.find(t.name)};
            if (gcit == cend(global_constants))
            {
                auto vit = variables.find(t.name);
                if (vit == cend(variables))
                {
                    throw std::logic_error("undefined symbolic literal " + t.name);
                }
                result = vit->second;
                break;
            }
            result = gcit->second;
            break;
//            auto it {variables.find(t.name)};
//            if (it == end(variables))
//                throw std::logic_error("undefined variable "+t.name);
//            result = it->second;
//            break;
        }
        case TokenKind::function:
        {
            result = function(t.name);
            break;
        }
        case TokenKind::bracket_left0:
        case TokenKind::bracket_left1:
        {
            double d {expression()};
            Token t2 {ts.pop()};
            if (t2.kind != endBracket(t.kind)/*TokenKind::bracket_right0*/)
                throw std::logic_error("bracket_right expected");
            result = d;
            break;
        }
//        case TokenKind::bracket_left1:
//        {
//            double d {expression()};
//            t = ts.pop();
//            if (t.kind != TokenKind::bracket_right1)
//                throw std::logic_error("bracket_right expected");
//            result = d;
//            break;
//        }
        default:
            throw std::logic_error("primary is expected");
    }
    while (true)
    {
        Token tp {ts.pop()};
        switch (tp.kind)
        {
            case TokenKind::factorial:
            {
                unsigned int f = numeric_cast<unsigned int>(result);
                double df = numeric_cast<double>(f);
                if (result-df > std::numeric_limits<double>::min())
                    throw std::logic_error("factorial applied to not integer");
                result = boost::math::factorial<double>(f);
                break;
            }
            default:
            {
                ts.push(tp);
                return result;
            }
        }
    }
//    return result;
//    switch (tf.kind)
//    {
//        case TokenKind::factorial:
//        {
//            unsigned int f = numeric_cast<unsigned int>(result);
//            double df = numeric_cast<double>(f);
//            if (result-df > std::numeric_limits<double>::min())
//                throw std::logic_error("factorial applied to not integer");
//            return boost::math::factorial<double>(f);
//        }
//        default:
//            ts.push(tf);
//            return result;
    //    }
}

double calculator::WExpression::function(std::string function_name)
{
//    double result {0.0};
    Token t {ts.pop()};
    std::vector<double> args;
    auto endBracket = [](TokenKind tk)->TokenKind{
        switch (tk)
        {
            case TokenKind::bracket_left0:
                return TokenKind::bracket_right0;
            case TokenKind::bracket_left1:
                return TokenKind::bracket_right1;
            default:
                return tk;
        }
    };
    switch (t.kind)
    {
        case TokenKind::bracket_left0:
        case TokenKind::bracket_left1:
        {
            while (ist)
            {
                double d {expression()};
                args.push_back(d);
                Token t2 {ts.pop()};
                if (t2.kind != TokenKind::comma)
                {
                    ts.push(t2);
                    break;
                }
            } // args is done
            Token t3 {ts.pop()};
            if (t3.kind != endBracket(t.kind))
                throw std::logic_error("correct bracket_right is expected");
            break;
        }
        default:
            throw std::logic_error("bracket_left is expected");
    }
    if (function_name == "pow")
    {
        if (args.size() != 2)
            throw std::logic_error("2 args for pow function");
        return std::pow(args[0], args[1]);
    }
    else if (function_name == "cos")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for cos function");
        return std::cos(args[0]);
    }
    else if (function_name == "sin")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for sin function");
        return std::sin(args[0]);
    }
    else if (function_name == "tan")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for tan function");
        return std::tan(args[0]);
    }
    else if (function_name == "acos")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for acos function");
        return std::acos(args[0]);
    }
    else if (function_name == "asin")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for asin function");
        return std::asin(args[0]);
    }
    else if (function_name == "atan")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for atan function");
        return std::atan(args[0]);
    }
    else if (function_name == "atan2")
    {
        if (args.size() != 2)
            throw std::logic_error("2 args for atan2 function");
        return std::atan2(args[0], args[1]);
    }
    else if (function_name == "exp")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for exp function");
        return std::exp(args[0]);
    }
    else if (function_name == "log")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for log function");
        return std::log(args[0]);
    }
    else if (function_name == "log10")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for log10 function");
        return std::log10(args[0]);
    }
    else if (function_name == "sqrt")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for sqrt function");
        return std::sqrt(args[0]);
    }
    else if (function_name == "abs")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for abs function");
        return std::fabs(args[0]);
    }
    else
    {
        throw std::logic_error("undefined function"+function_name);
    }

}

calculator::Token calculator::Token_stream::pop()
{
    if (!(data.empty()))
    {
        auto res = data.top();
        data.pop();
        return res;
    }
    Token result;
    if (!ist) return result;
    char sym {0};
    ist >> sym;
    if (ist.eof()) return result;
    switch (sym)
    {
        case '(':
            result.kind = TokenKind::bracket_left0;
            break;
        case ')':
            result.kind = TokenKind::bracket_right0;
            break;
        case '{':
            result.kind = TokenKind::bracket_left1;
            break;
        case '}':
            result.kind = TokenKind::bracket_right1;
            break;
        case '*':
            result.kind = TokenKind::multiply;
            break;
        case '/':
            result.kind = TokenKind::divide;
            break;
        case '%':
            result.kind = TokenKind::mod;
            break;
        case '+':
            result.kind = TokenKind::plus;
            break;
        case '-':
            result.kind = TokenKind::minus;
            break;
        case '!':
            result.kind = TokenKind::factorial;
            break;
        case ',':
            result.kind = TokenKind::comma;
            break;
        case '.':
        case '0':
        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6':
        case '7':
        case '8':
        case '9':
        {
            ist.unget();
            double val {0.0};
            ist >> val;
            result.kind = TokenKind::number;
            result.value = val;
            break;
        }
        default:
        {
            if (isalpha(sym)) // function or variable name: func ( | lsldkm239283
            {
                std::string s;
                s += sym;
                char ch {0};
                while (ist.get(ch) && (isalpha(ch) || isdigit(ch))) s += ch;
                ist.putback(ch);
                ist >> ch;
                if (ch == '(' || ch == '{')
                {
                    ist.putback(ch);
                    result.kind = TokenKind::function;
                    result.name = s;
                    break;
                }
                else
                {
                    ist.putback(ch);
                    result.kind = TokenKind::variable;
                    result.name = s;
                    break;
                }
            }
            else
            {
                throw std::logic_error("bad Token");
            }
        }
    }
    return result;
}


#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <vector>
#include <map>
#include <png.h>
#include <TaskGraph>
#include <TaskUserFunctions.h>
#include <boost/ptr_container/ptr_vector.hpp>
#include "mandelbrot.hpp"

using namespace tg;

const float gamma_exponent = 3.5;
const int max_iterations = 1000;

const int max_alloc = 1024*176; // Allocate up to 176K
const int width = 12800;
const int height = 9600;
const char* output_filename = "mandelbrot_set_tu.png";

struct TgInfo
{
  int startRow;
  int rowCount;
  char* data;
};

int main(int argc, char* argv[])
{
  int threads;
  const Options options = getOptions("mandelbrot_tu", argc, argv);

  if (options.threads < 1 || options.threads > NUM_SPES)
  {
    std::cerr << "Please select a number of threads between 1 and " << NUM_SPES << " (inclusive)." << std::endl;
    exit(EXIT_FAILURE);
  }
  else
  {
    threads = options.threads;
  }

  const int rowSize = width * sizeof(char);
  const int rowMultiple = max_alloc/rowSize;
  unsigned int arraySize[2] = {rowMultiple, width};

  std::cout << "Calculating mandelbrot of size " << width << " x " << height << "." << std::endl;
  std::cout << "Will transfer data in blocks of " << rowSize*rowMultiple << " bytes." << std::endl;

  tuTaskGraph mandel;
  tu_taskgraph(mandel)
  {
    tParameter(tVarNamed(int, startRow, "startRow"));
    tParameter(tVarNamed(int, rowCount, "rowCount"));
    tParameter(tArrayFromListNamed(char, imageData, 2, arraySize, "imageData"));

    tVar(float, x);
    tVar(float, y);
    tVar(float, x0);
    tVar(float, y0);
    tVar(float, xtemp);
    tVar(int, x_pos);
    tVar(int, y_pos);
    tVar(int, iteration);
    tVar(float, pixel);

    tFor(y_pos, startRow, startRow+rowCount-1)
    {
      y0 = (y_pos * (3.0f / height)) - 1.5f;

      tFor(x_pos, 0, width-1)
      {
        // Scale the pixel location to be +- 2 from the origin of the complex plane
        x0 = (x_pos * (3.5f / width)) - 2.5f;
        x = x0;
        y = y0;
        iteration = 0;

        // Test if this location is in the set
        tWhile ((x*x + y*y) < 4.0f && iteration  < max_iterations) {
          xtemp = (x*x) - (y*y) + x0;
          y = 2*x*y + y0;
          x = xtemp;
          iteration+=1;
        }

        // Calculate and set the value of the pixel
        tIf(iteration == max_iterations)
          pixel = 0.0f;
        tElse
          pixel = 8.0f * (iteration + 1.0f - tLogf(tLogf(tSqrtf(x*x + y*y))) / std::log(2.0f)) / max_iterations;

        imageData[y_pos-startRow][x_pos] = tPowf(pixel, (1.0f / gamma_exponent)) * 255.0f;
      }
    }
  }

  mandel.compile(tg::SPU_GCC, false);

  boost::ptr_vector<tuTaskGraph> taskGraphs;
  std::map<tuTaskGraph*, TgInfo> tgInfo;
  TaskFarm farm;

  for(int thread=0; thread<threads; ++thread)
  {
    taskGraphs.push_back(new tuTaskGraph(mandel));
    farm.add(&taskGraphs.back());
    tgInfo[&taskGraphs.back()].data = static_cast<char*>(spu_malloc(rowSize*rowMultiple));
  }

  FILE *output_file;
  png_structp png_ptr;
  png_infop info_ptr;

  if (!(output_file = fopen(output_filename, "wb"))) {
    std::cerr << "Unable to open " << output_filename << " for writing. Aborting now." << std::endl;
    exit(1);
  }

  if (!(png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL))) {
    std::cerr << "Failed to create png write struct. Aborting now." << std::endl;
    exit(1);
  }

  if (!(info_ptr = png_create_info_struct(png_ptr))) {
    std::cerr << "Failed to create png info struct. Aborting now." << std::endl;
    // Clean up
    png_destroy_write_struct(&png_ptr,static_cast<png_infopp>(NULL));
    exit(1);
  }

  png_init_io(png_ptr, output_file);
  png_set_IHDR(png_ptr, info_ptr, width, height, 8,
               PNG_COLOR_TYPE_PALETTE,
               PNG_INTERLACE_NONE,
               PNG_COMPRESSION_TYPE_DEFAULT,
               PNG_FILTER_TYPE_DEFAULT);
  png_set_gamma(png_ptr, 0.5, 0.45455);

  std::vector<png_color> palette(generateRedYellowColourMap());
  png_set_PLTE(png_ptr, info_ptr, &palette[0], palette.size());
  png_write_info(png_ptr, info_ptr);

  int currentRow = 0;

  while(currentRow < height)
  {
    for(int thread=0; thread<threads; ++thread)
    {
      tgInfo[&taskGraphs[thread]].startRow = currentRow;
      tgInfo[&taskGraphs[thread]].rowCount = std::min(rowMultiple, height-currentRow);
      currentRow+=tgInfo[&taskGraphs[thread]].rowCount;

      taskGraphs[thread].setParameter("startRow", &tgInfo[&taskGraphs[thread]].startRow);
      taskGraphs[thread].setParameter("rowCount", &tgInfo[&taskGraphs[thread]].rowCount);
      taskGraphs[thread].setParameter("imageData", tgInfo[&taskGraphs[thread]].data);
    }

    farm.execute();

    for(int thread=0; thread<threads; ++thread)
    {
      for (int line = 0; line<tgInfo[&taskGraphs[thread]].rowCount; ++line)
      {
        png_bytep png_row_ptr = reinterpret_cast<png_bytep>(tgInfo[&taskGraphs[thread]].data + rowSize*line);
        png_write_row(png_ptr, png_row_ptr);
      }
    }
  }

  std::cout << "Done." << std::endl;

  png_write_end(png_ptr, info_ptr);
  png_destroy_write_struct(&png_ptr, &info_ptr);

  for(int thread=0; thread<threads; ++thread)
    spu_free(tgInfo[&taskGraphs[thread]].data);

  exit(EXIT_SUCCESS);
}


#ifndef YANNQ_GEOMETRY_FISHERMATIRX_HPP
#define YANNQ_GEOMETRY_FISHERMATIRX_HPP
#include <Eigen/Core>
#include <Eigen/Dense>

#include "Utilities/Utility.hpp"
#include "Observables/Observable.hpp"

#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>

namespace yannq
{
template<typename Machine>
class FisherMatrix;
} //namespace yannq

namespace Eigen {
namespace internal {
	template<typename Machine>
	struct traits<yannq::FisherMatrix<Machine> > :  public Eigen::internal::traits<Eigen::SparseMatrix<typename Machine::Scalar> > {};
}
} //namespace Eigen

namespace yannq
{
/**
 * Construct the fisher information metric for quantum states
 * */
template<class Machine>
class FisherMatrix
	: public Observable<FisherMatrix<Machine> >,
	public Eigen::EigenBase<FisherMatrix<Machine> >
{
public:
	using Scalar = typename Machine::Scalar;
	using RealScalar = typename remove_complex<Scalar>::type;

	using Matrix = typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
	using Vector = typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
	using RealMatrix = typename Eigen::Matrix<RealScalar, Eigen::Dynamic, Eigen::Dynamic>;
	using RealVector = typename Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;

private:
	uint32_t n_;

	const Machine& qs_;
	RealScalar shift_;

	Matrix deltas_;
	Vector deltaMean_;

public:
	// Required typedefs, constants, and method:
	typedef int StorageIndex;

	enum {
		ColsAtCompileTime = Eigen::Dynamic,
		MaxColsAtCompileTime = Eigen::Dynamic,
		IsRowMajor = false
	};

	Eigen::Index rows() const { return qs_.getDim(); }
	Eigen::Index cols() const { return qs_.getDim(); }

	template<typename Rhs>
	Eigen::Product<FisherMatrix<Machine>, Rhs, Eigen::AliasFreeProduct>
			operator*(const Eigen::MatrixBase<Rhs>& x) const {
	  return Eigen::Product<FisherMatrix<Machine>, Rhs, Eigen::AliasFreeProduct>(*this, x.derived());
	}

	void setShift(RealScalar shift)
	{
		shift_ = shift;
	}

	RealScalar getShift() const
	{
		return shift_;
	}

	void initIter(int nsmp)
	{
		deltas_.setZero(nsmp, qs_.getDim());
	}

	template<class Elt, class State>
	inline void eachSample(int n, Elt&& elt, State&& state)
	{
		(void)state;
		deltas_.row(n) = qs_.logDeriv(elt);
	}

	void finIter()
	{
		deltaMean_ = deltas_.colwise().mean();
		deltas_ = deltas_.rowwise() - deltaMean_.transpose();
	}

	/**
	 * \param weights normalized weights for each sigma
	 * */
	void finIter(const Eigen::Ref<const RealVector>& weights)
	{
		weights_ = weights;
		deltaMean_ = weights.transpose()*deltas_;
		deltas_ = deltas_.rowwise() - deltaMean_.transpose();
	}

	inline
	const Matrix& logDervs() const&
	{
		return deltas_;
	}

	inline
	Matrix logDervs() &&
	{
		return std::move(deltas_);
	}

	inline
	const Vector& oloc() const&
	{
		return deltaMean_;
	}
	inline
	Vector oloc() &&
	{
		return std::move(deltaMean_);
	}

	Matrix corrMat() const
	{
		int nsmp = deltas_.rows();
		if(weights_.size() == 0)
			return (deltas_.adjoint() * deltas_)/nsmp;
		else
			return (deltas_.adjoint() * weights_.asDiagonal() * deltas_);
	}

	RealVector diagCorrMat() const
	{
		RealMatrix sqrDeltas = deltas_.cwiseAbs2();
		return sqrDeltas.colwise().mean();
	}

	FisherMatrix(const Machine& qs)
	  : n_{qs.getN()}, qs_(qs), shift_{1e-3}
	{
	}

	template<class Rhs>
	typename Machine::Vector apply(const Rhs& rhs) const
	{
		assert(rhs.size() == qs_.getDim());
		Vector r = deltas_*rhs;

		Vector res;

		if(weights_.size() == 0)
			res = deltas_.adjoint()*r/r.rows();
		else
			res = deltas_.adjoint() * weights_.asDiagonal() * r;

		return res + Scalar(shift_)*rhs;
	}
};
}//namespace yannq

// Implementation of yannq::SRMat * Eigen::DenseVector though a specialization of internal::generic_product_impl:
namespace Eigen {
namespace internal {
	template<typename Rhs, typename Machine>
	struct generic_product_impl<yannq::FisherMatrix<Machine>, Rhs, SparseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
	: generic_product_impl_base<yannq::FisherMatrix<Machine>, Rhs, generic_product_impl<yannq::FisherMatrix<Machine>, Rhs> >
	{
		typedef typename Product<yannq::FisherMatrix<Machine>, Rhs>::Scalar Scalar;
		template<typename Dest>
		static void scaleAndAddTo(Dest& dst, const yannq::FisherMatrix<Machine>& lhs, const Rhs& rhs, const Scalar& alpha)
		{
			// This method should implement "dst += alpha * lhs * rhs" inplace,
			// however, for iterative solvers, alpha is always equal to 1, so let's not bother about it.
			assert(alpha==Scalar(1) && "scaling is not implemented");
			EIGEN_ONLY_USED_FOR_DEBUG(alpha);

			dst += lhs.apply(rhs);
		}
	};
} //namespace internal
} //namespace Eigen

#endif//YANNQ_GEOMETRY_FISHERMATIRX_HPP


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file bit_ceil.hpp
 *
 * This header defines \c bit_ceil algorithm, which produces a nearest power of 2 integer
 * that is greater or equal to the input integer.
 */

#ifndef BOOST_BIT_OPS_POW2_BIT_CEIL_HPP_INCLUDED_
#define BOOST_BIT_OPS_POW2_BIT_CEIL_HPP_INCLUDED_

#include <limits>
#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>
#include <boost/bit_ops/count/countl_zero.hpp>

namespace boost {
namespace bit_ops {

/*!
 * \brief Returns the nearest power of 2 integer that is greater or equal to \a value
 *
 * \pre \a value must not be zero
 */
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    T
>::type bit_ceil_nz(T value) BOOST_NOEXCEPT
{
    return static_cast< T >(1u) << ((std::numeric_limits< T >::digits - 1u) - bit_ops::countl_zero_nz(value) + ((value & (value - 1u)) != 0u));
}

//! Returns the nearest power of 2 integer that is greater or equal to \a value
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    T
>::type bit_ceil(T value) BOOST_NOEXCEPT
{
    // bit_ceil(0) == bit_ceil(1) == 1
    return value == 0u ? static_cast< T >(1u) : bit_ops::bit_ceil_nz(value);
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_POW2_BIT_CEIL_HPP_INCLUDED_


#include <string>
#include <memory>
#include <complex>
#include <vector>
#include <cmath>
#include <sndfile.h>
#include <fftw3.h>
// #include <boost/format.hpp>

using std::complex;
using std::vector;
using std::pair;

// LPC 係数の次数
const int LPC_ORDER = 64;

// 解析に用いる微小区間 (sec)
const double WINDOW_DURATION = 0.04;

// FFT する
vector<double> fft(const vector<double>& input);

// LPC スペクトル包絡を得る
vector<double> lpc(const vector<double>& input, int order, double df);

// [1 : -1] に正規化
vector<double> normalize(const vector<double>& input);

// ハミング窓をかける
vector<double> hamming(const vector<double>& input);

// デジタルフィルタ
vector<double> freqz(const vector<double>& b, const vector<double>& a, double df, int N);

// フォルマント f1 / f2 を返す
pair<double, double> formant(const vector<double>& input, double df);

// フォルマントから母音を推定する
std::string vowel(double f1, double f2);

// 与えられた範囲の音の平均
double volume(const vector<double>& input);

int main()
{
	// 音声ファイル読み込み
	SF_INFO sinfo;
	std::shared_ptr<SNDFILE> sf(
		sf_open("aiueo.wav", SFM_READ, &sinfo), [](SNDFILE* ptr) { sf_close(ptr); });
	const int frame = sinfo.frames;
	std::unique_ptr<short[]> input(new short[frame]);
	const double sample_rate = sinfo.samplerate;
	const double dt = 1.0 / sample_rate;
	sf_read_short(sf.get(), input.get(), frame);

	// 微小音声区間取り出し
	const int window_size = static_cast<int>(WINDOW_DURATION / dt);
	const double df  = 1.0 / (window_size / sample_rate);
	for (int n = 0; n < frame / window_size; ++n) {
		vector<double> data;
		for (int i = window_size * n; i < window_size * (n + 1) && i < frame; ++i) {
			data.push_back(input[i]);
		}
		auto hamming_result = normalize( hamming(data) );

		auto lpc_result = normalize( lpc(hamming_result, LPC_ORDER, df) );
		auto fft_result = normalize( fft(hamming_result) );

		// 得られた LPC スペクトル包絡線からフォルマントを抽出
		auto formant_result = formant(lpc_result, df);
		double f1 = formant_result.first;
		double f2 = formant_result.second;

		if (volume(data) < 1e4) {
			std::cout << "-";
		} else {
			std::cout << boost::format("%1%  (f1:%2%, f2:%3%)")
				% vowel(f1, f2) % f1 % f2;
		}
		std::cout << std::endl;
	}

	// Gnuplot で結果を確認
	std::shared_ptr<FILE> freq_graph(
		popen("gnuplot -persist", "w"), [](FILE* ptr) { pclose(ptr); });
	fprintf(freq_graph.get(), "load 'freq.plt'\n");

	// std::shared_ptr<FILE> wav_graph(
	// 	popen("gnuplot -persist", "w"), [](FILE* ptr) { pclose(ptr); });
	// fprintf(wav_graph.get(), "load 'wav.plt'\n");

	return 0;
}

// FFT
vector<double> fft(const vector<double>& input)
{
	const int frame = input.size();
	vector<complex<double>> in(frame), out(frame);
	auto plan = fftw_plan_dft_1d(
			frame,
			reinterpret_cast<fftw_complex*>(&in[0]),
			reinterpret_cast<fftw_complex*>(&out[0]),
			FFTW_FORWARD, FFTW_ESTIMATE
	);
	for (int i = 0; i < frame; ++i) {
		in[i] = complex<double>(input[i], 0.0);
	}
	fftw_execute(plan);

	vector<double> fft_result(frame);
	for (int i = 0; i < frame; ++i) {
		fft_result[i] = abs(out[i]);
	}

	return fft_result;
}

// LPC
vector<double> lpc(const vector<double>& input, int order, double df)
{
	const int N = input.size();

	// 自己相関関数
	vector<double> r(N);
	const int lags_num = order + 1;
	for (int l = 0; l < lags_num; ++l) {
		r[l] = 0.0;
		for (int n = 0; n < N - l; ++n) {
			r[l] += input[n] * input[n + l];
		}
	}

	// Levinson-Durbin のアルゴリズムで LPC 係数を計算
	vector<double> a(order + 1, 0.0), e(order + 1, 0.0);
	a[0] = e[0] = 1.0;
	a[1] = - r[1] / r[0];
	e[1] = r[0] + r[1] * a[1];
	for (int k = 1; k < order; ++k) {
		double lambda = 0.0;
		for (int j = 0; j < k + 1; ++j) {
			lambda -= a[j] * r[k + 1 - j];
		}
		lambda /= e[k];

		vector<double> U(k + 2), V(k + 2);
		U[0] = 1.0; V[0] = 0.0;
		for (int i = 1; i < k + 1; ++i) {
			U[i] = a[i];
			V[k + 1 - i] = a[i];
		}
		U[k + 1] = 0.0; V[k + 1] = 1.0;

		for (int i = 0; i < k + 2; ++i) {
			a[i] = U[i] + lambda * V[i];
		}

		e[k + 1] = e[k] * (1.0 - lambda * lambda);
	}

	// LPC 係数から音声信号再現
	// vector<double> lpc_result(N, 0.0);
	// for (int i = 0; i < N; ++i) {
	// 	if (i < order) {
	// 		lpc_result[i] = input[i];
	// 	} else {
	// 		for (int j = 1; j < order; ++j) {
	// 			lpc_result[i] -= a[j] * input[i + 1 - j];
	// 		}
	// 	}
	// }
	// return lpc_result;

	return freqz(e, a, df, N);
}

// 正規化
vector<double> normalize(const vector<double>& input)
{
	// 最大 / 最小値
	auto max = abs( *std::max_element(input.begin(), input.end()) );
	auto min = abs( *std::min_element(input.begin(), input.end()) );
	double factor = std::max(max, min);
	vector<double> result( input.size() );
	std::transform(input.begin(), input.end(), result.begin(), [factor](double x) {
		return x / factor;
	});
	return result;
}

// ハミング窓をかける
vector<double> hamming(const vector<double>& input)
{
	const double N = input.size();
	vector<double> result(N);
	for (int i = 1; i < N - 1; ++i) {
		const double h = 0.54 - 0.46 * cos(2 * M_PI * i / (N - 1));
		result[i] = input[i] * h;
	}
	result[0] = result[N - 1] = 0;
	return result;
}

// デジタルフィルタ
vector<double> freqz(const vector<double>& b, const vector<double>& a, double df, int N)
{
	vector<double> H(N);
	for (int n = 0; n < N + 1; ++n) {
		auto z = std::exp(complex<double>(0.0, -2.0 * M_PI * n / N));
		complex<double> numerator(0.0, 0.0), denominator(0.0, 0.0);
		for (int i = 0; i < b.size(); ++i) {
			numerator += b[b.size() - 1 - i] * pow(z, i);
		}
		for (int i = 0; i < a.size(); ++i) {
			denominator += a[a.size() - 1 - i] * pow(z, i);
		}
		H[n] = abs(numerator / denominator);
	}

	return H;
}

// フォルマント f1 / f2 を返す
pair<double, double> formant(const vector<double>& input, double df)
{
	pair<double, double> result(0.0, 0.0);
	bool is_find_first = false;
	for (int i = 1; i < input.size() - 1; ++i) {
		if (input[i] > input[i-1] && input[i] > input[i+1]) {
			if (!is_find_first) {
				result.first = df * i;
				is_find_first = true;
			} else {
				result.second = df * i;
				break;
			}
		}
	}
	return result;
}

// 母音を推定
// NOTE: 機械学習にする予定
std::string vowel(double f1, double f2)
{
	if (f1 > 600 && f1 < 1400 && f2 > 900  && f2 < 2000) return "a";
	if (f1 > 100 && f1 < 410  && f2 > 1900 && f2 < 3500) return "i";
	if (f1 > 100 && f1 < 700  && f2 > 1100 && f2 < 2000) return "u";
	if (f1 > 400 && f1 < 800  && f2 > 1700 && f2 < 3000) return "e";
	if (f1 > 300 && f1 < 900  && f2 > 500  && f2 < 1300) return "o";
	return "-";
}

// ボリューム
double volume(const vector<double>& input)
{
	double v = 0.0;
	std::for_each(input.begin(), input.end(), [&v](double x) { v += x*x; });
	v /= input.size();
	return v;
}