#pragma once
#include "IntegerInterval.hpp"
#include "Misc.hpp"
#include "VectorHelpers.hpp"
#include "detail/MultisetsDetail.hpp"
#include <boost/iterator/iterator_facade.hpp>

namespace discreture
{

/**
 *@brief Multisets is a container to iterate over all subsets of a multiset.
 *
 *For example, let's suppose you have the multiset {0,0,0,1,1,2,2,2,2,3,3,5}.
 *First, we encode this as [3,2,4,2,0,1], meaning we take three 0's, two 1's,
 *etc. Then, the set of submultisets of this can be encoded as lists of length 6
 *where the first element is less than 3, the second less than 2, the third less
 *than 4, and so on. So for example, [1,0,0,2,0,1], representing {0,3,3,5}, is
 *in multisets([3,2,4,2,0,1])
 *
 *# Example usage:
 *
 *	multisets X({1,0,3,1});
 *	for (auto&& x : X)
 *		std::cout << x << " ";
 *
 *Prints out:
 *
 *	[ 0 0 0 0 ]
 *	[ 1 0 0 0 ]
 *	[ 0 0 1 0 ]
 *	[ 1 0 1 0 ]
 *	[ 0 0 2 0 ]
 *	[ 1 0 2 0 ]
 *	[ 0 0 3 0 ]
 *	[ 1 0 3 0 ]
 *	[ 0 0 0 1 ]
 *	[ 1 0 0 1 ]
 *	[ 0 0 1 1 ]
 *	[ 1 0 1 1 ]
 *	[ 0 0 2 1 ]
 *	[ 1 0 2 1 ]
 *	[ 0 0 3 1 ]
 *	[ 1 0 3 1 ]
 *
 */

template <class IntType = int, class RAContainerInt = std::vector<IntType>>
class Multisets
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using value_type = RAContainerInt;
    using multiset = value_type;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    class iterator;
    using const_iterator = iterator;
    class reverse_iterator;
    using const_reverse_iterator = reverse_iterator;

public:
    explicit Multisets(const multiset& set) : total_(set), size_(1)
    {
        for (auto x : set)
        {
            size_ *= (x + 1);
        }
    }

    explicit Multisets(IntType size, IntType n = 1)
        : total_(size, n), size_(std::pow(n + 1, size))
    {}

    size_type size() const { return size_; }

    iterator begin() const { return iterator(total_); }

    const iterator end() const
    {
        return iterator::make_invalid_with_id(size());
    }

    reverse_iterator rbegin() const { return reverse_iterator(total_); }

    const reverse_iterator rend() const
    {
        return reverse_iterator::make_invalid_with_id(size());
    }

    //////////////////////////////
    /// @brief Random Access Capabilities for multiset
    /// @param m assumes 0 <= m < size(). Undefined behaviour otherwise
    //////////////////////////////
    multiset operator[](size_type m) const
    {
        assert(m >= 0 && m < size());
        multiset sub(total_.size());
        construct_multiset(sub, total_, m);
        return sub;
    }

    //////////////////////////////
    /// @brief Opposite operator to operator[]
    /// @param sub given a multiset, what would it's index be?
    //////////////////////////////
    size_type get_index(const multiset& sub) const
    {
        assert(sub.size() == total_.size());
        size_type coeff = 1;
        size_type result = 0;
        for (size_t i = 0; i < total_.size(); ++i)
        {
            result += coeff*sub[i];
            coeff *= (total_[i] + 1);
        }
        return result;
    }

    class iterator
        : public boost::iterator_facade<iterator, const multiset&, boost::random_access_traversal_tag>
    {

    public:
        iterator() = default;

        explicit iterator(const multiset& total)
            : ID_(0), n_(total.size()), submulti_(total.size(), 0), total_(&total)
        {}

        size_type ID() const { return ID_; }

        static const iterator make_invalid_with_id(size_type id)
        {
            return iterator(id);
        }

    private:
        explicit iterator(size_type id) : ID_(id) {}

        void increment()
        {
            ++ID_;
            next_multiset(submulti_, *total_, n_);
        }

        void decrement()
        {
            --ID_;
            prev_multiset(submulti_, *total_, n_);
        }

        const multiset& dereference() const { return submulti_; }

        // It only makes sense to compare iterators from the SAME multiset.
        bool equal(const iterator& it) const { return ID_ == it.ID_; }

        void advance(difference_type m)
        {
            ID_ += m;
            construct_multiset(submulti_, *total_, ID_);
        }

        difference_type distance_to(const iterator& it) const
        {
            return static_cast<difference_type>(it.ID()) - ID();
        }

    private:
        size_type ID_{0};
        size_type n_{0};
        multiset submulti_{};
        multiset const* total_{nullptr};

        friend class boost::iterator_core_access;
    };

    class reverse_iterator
        : public boost::iterator_facade<reverse_iterator,
                                        const multiset&,
                                        boost::random_access_traversal_tag>
    {
    public:
        reverse_iterator() = default;

        explicit reverse_iterator(const multiset& total)
            : ID_(0), n_(total.size()), submulti_(total), total_(&total)
        {}

        size_type ID() const { return ID_; }

        static const reverse_iterator make_invalid_with_id(size_type id)
        {
            return reverse_iterator(id);
        }

    private:
        explicit reverse_iterator(size_type id) : ID_(id) {}

        // prefix
        void increment()
        {
            ++ID_;
            prev_multiset(submulti_, *total_, n_);
        }

        void decrement()
        {
            --ID_;
            next_multiset(submulti_, *total_, n_);
        }

        void advance(difference_type m)
        {
            size_type s = 1;
            for (auto x : *total_)
                s *= (x + 1);
            ID_ += m;
            construct_multiset(submulti_, *total_, s - ID_ - 1);
        }

        const multiset& dereference() const { return submulti_; }

        // It only makes sense to compare iterators from the SAME multiset.
        bool equal(const reverse_iterator& it) const { return ID_ == it.ID_; }

        difference_type distance_to(const reverse_iterator& other) const
        {
            return static_cast<difference_type>(other.ID()) - ID();
        }

    private:
        size_type ID_{0};
        size_type n_{0}; // must have n_ = submulti_.size() = total_->size()
        multiset submulti_{};
        multiset const* total_{nullptr};

        friend class boost::iterator_core_access;
    };

    template <class Func>
    void for_each(Func f) const
    {
        switch (total_.size())
        {
            // clang-format off
        case 0: detail::for_each_multiset<multiset,0>::apply(total_,f); break;
        case 1: detail::for_each_multiset<multiset,1>::apply(total_,f); break;
        case 2: detail::for_each_multiset<multiset,2>::apply(total_,f); break;
        case 3: detail::for_each_multiset<multiset,3>::apply(total_,f); break;
        case 4: detail::for_each_multiset<multiset,4>::apply(total_,f); break;
        case 5: detail::for_each_multiset<multiset,5>::apply(total_,f); break;
        case 6: detail::for_each_multiset<multiset,6>::apply(total_,f); break;
        case 7: detail::for_each_multiset<multiset,7>::apply(total_,f); break;
        case 8: detail::for_each_multiset<multiset,8>::apply(total_,f); break;
        case 9: detail::for_each_multiset<multiset,9>::apply(total_,f); break;
        case 10: detail::for_each_multiset<multiset,10>::apply(total_,f); break;
        case 11: detail::for_each_multiset<multiset,11>::apply(total_,f); break;
        case 12: detail::for_each_multiset<multiset,12>::apply(total_,f); break;
        case 13: detail::for_each_multiset<multiset,13>::apply(total_,f); break;
        case 14: detail::for_each_multiset<multiset,14>::apply(total_,f); break;
        case 15: detail::for_each_multiset<multiset,15>::apply(total_,f); break;
        case 16: detail::for_each_multiset<multiset,16>::apply(total_,f); break;
        case 17: detail::for_each_multiset<multiset,17>::apply(total_,f); break;
        case 18: detail::for_each_multiset<multiset,18>::apply(total_,f); break;
        case 19: detail::for_each_multiset<multiset,19>::apply(total_,f); break;
        case 20: detail::for_each_multiset<multiset,20>::apply(total_,f); break;
            // clang-format on

        default:
            for (auto& x : (*this))
            {
                f(x);
            }

            break;
        }
    }

    static void next_multiset(multiset& sub, const multiset& total)
    {
        next_multiset(sub, total, total.size());
    }

    static void next_multiset(multiset& sub, const multiset& total, size_type n)
    {
        assert(n == sub.size());
        assert(n == total.size());
        for (auto i : NN(n))
        {
            if (can_increment(i, sub, total))
            {
                ++sub[i];

                return;
            }
            sub[i] = 0;
        }
    }

    static void prev_multiset(multiset& sub, const multiset& total, size_t n)
    {
        assert(n == sub.size());
        assert(n == total.size());

        for (auto i : NN(n))
        {
            if (sub[i] != 0)
            {
                --sub[i];
                return;
            }
            sub[i] = total[i];
        }
    }

    static void construct_multiset(multiset& sub,
                                   const multiset& total,
                                   size_type m)
    {
        assert(sub.size() == total.size());
        size_type n = total.size();
        if (n == 0)
            return;
        for (auto&& s : sub)
            s = 0;
        std::vector<size_type> coeffs(n);
        coeffs[0] = 1;
        for (auto i : II(1, n))
        {
            coeffs[i] = coeffs[i - 1]*(total[i - 1] + 1);
        }

        for (difference_type i = n - 1; i >= 0; --i)
        {
            size_type w = coeffs[i];
            auto t = big_integer_interval(total[i] + 1)
                       .partition_point(
                         [m, w](size_type a) { return a*w <= m; }) -
              1;
            sub[i] = t;
            m -= w*t;
            if (m <= 0)
                break;
        }
    }

private:
    multiset total_;
    size_type size_;

    static bool can_increment(size_t index,
                              const multiset& sub,
                              const multiset& total)
    {
        return sub[index] < total[index];
    }
};

using boost::container::static_vector;

using multisets = Multisets<int>;
using multisets_stack = Multisets<int, static_vector<int, 48>>;

} // namespace discreture


// (C) Copyright 2007-2009 Andrew Sutton
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/directed_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/clustering_coefficient.hpp>

using namespace std;
using namespace boost;

// number of vertices in the graph
static const unsigned N = 5;

template <typename Graph>
struct vertex_vector
{
    typedef graph_traits<Graph> traits;
    typedef vector<typename traits::vertex_descriptor> type;
};

template <typename Graph>
void build_graph(Graph& g, typename vertex_vector<Graph>::type& v)
{
    // add vertices
    for(size_t i = 0; i < N; ++i) {
        v[i] = add_vertex(g);
    }

    // add edges
    add_edge(v[0], v[1], g);
    add_edge(v[1], v[2], g);
    add_edge(v[2], v[0], g);
    add_edge(v[3], v[4], g);
    add_edge(v[4], v[0], g);
}

template <typename Graph>
void test_undirected()
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    typedef exterior_vertex_property<Graph, double> ClusteringProperty;
    typedef typename ClusteringProperty::container_type ClusteringContainer;
    typedef typename ClusteringProperty::map_type ClusteringMap;

    Graph g;
    vector<Vertex> v(N);
    build_graph(g, v);

    ClusteringContainer cc(num_vertices(g));
    ClusteringMap cm(cc, g);

    BOOST_ASSERT(num_paths_through_vertex(g, v[0]) == 3);
    BOOST_ASSERT(num_paths_through_vertex(g, v[1]) == 1);
    BOOST_ASSERT(num_paths_through_vertex(g, v[2]) == 1);
    BOOST_ASSERT(num_paths_through_vertex(g, v[3]) == 0);
    BOOST_ASSERT(num_paths_through_vertex(g, v[4]) == 1);

    BOOST_ASSERT(num_triangles_on_vertex(g, v[0]) == 1);
    BOOST_ASSERT(num_triangles_on_vertex(g, v[1]) == 1);
    BOOST_ASSERT(num_triangles_on_vertex(g, v[2]) == 1);
    BOOST_ASSERT(num_triangles_on_vertex(g, v[3]) == 0);
    BOOST_ASSERT(num_triangles_on_vertex(g, v[4]) == 0);

    // TODO: Need a FP approximation to assert here.
    // BOOST_ASSERT(clustering_coefficient(g, v[0]) == double(1)/3);
    BOOST_ASSERT(clustering_coefficient(g, v[1]) == 1);
    BOOST_ASSERT(clustering_coefficient(g, v[2]) == 1);
    BOOST_ASSERT(clustering_coefficient(g, v[3]) == 0);
    BOOST_ASSERT(clustering_coefficient(g, v[4]) == 0);

    all_clustering_coefficients(g, cm);

    // TODO: Need a FP approximation to assert here.
    // BOOST_ASSERT(cm[v[0]] == double(1)/3);
    BOOST_ASSERT(cm[v[1]] == 1);
    BOOST_ASSERT(cm[v[2]] == 1);
    BOOST_ASSERT(cm[v[3]] == 0);
    BOOST_ASSERT(cm[v[4]] == 0);

    // I would have used check_close, but apparently, that requires
    // me to link this against a library - which I don't really want
    // to do. Basically, this makes sure that that coefficient is
    // within some tolerance (like 1/10 million).
    double coef = mean_clustering_coefficient(g, cm);
    BOOST_ASSERT((coef - (7.0f / 15.0f)) < 1e-7f);
}

int
main(int, char *[])
{
    typedef undirected_graph<> Graph;
    // typedef directed_graph<> Digraph;

    // TODO: write a test for directed clustering coefficient.

    test_undirected<Graph>();
    // test<Digraph>();
}


#ifndef COORDINATE_CALCULATION
#define COORDINATE_CALCULATION

#include "util/coordinate.hpp"

#include <boost/optional.hpp>

#include <algorithm>
#include <utility>
#include <vector>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

namespace detail
{
const constexpr long double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
const constexpr long double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr long double EARTH_RADIUS = 6372797.560856;
}

//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs);

double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

// get the length of a full coordinate vector, using one of our basic functions to compute distances
template <class BinaryOperation>
double getLength(const std::vector<Coordinate> &coordinates, BinaryOperation op)
{
    if (coordinates.empty())
        return 0.;

    double result = 0;
    const auto functor = [&result, op](const Coordinate lhs, const Coordinate rhs) {
        result += op(lhs, rhs);
        return false;
    };
    // side-effect find adding up distances
    std::adjacent_find(coordinates.begin(), coordinates.end(), functor);

    return result;
}

// Find the closest distance and location between coordinate and the line connecting source and
// target:
//             coordinate
//                 |
//                 |
// source -------- x -------- target.
// returns x as well as the distance between source and x as ratio ([0,1])
inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate)
{
    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
    // dot product of two un-normed vectors
    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
    // squared length of the slope vector
    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
                                static_cast<double>(slope_vector.lat * slope_vector.lat);

    if (squared_length < std::numeric_limits<double>::epsilon())
    {
        return {0, source};
    }

    const double normed_ratio = unnormed_ratio / squared_length;
    double clamped_ratio = normed_ratio;
    if (clamped_ratio > 1.)
    {
        clamped_ratio = 1.;
    }
    else if (clamped_ratio < 0.)
    {
        clamped_ratio = 0.;
    }

    return {clamped_ratio,
            {
                FloatLongitude{1.0 - clamped_ratio} * source.lon +
                    target.lon * FloatLongitude{clamped_ratio},
                FloatLatitude{1.0 - clamped_ratio} * source.lat +
                    target.lat * FloatLatitude{clamped_ratio},
            }};
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio);

Coordinate centroid(const Coordinate lhs, const Coordinate rhs);

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate);

// Get angle of line segment (A,C)->(C,B)
double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third);

// find the center of a circle through three coordinates
boost::optional<Coordinate> circleCenter(const Coordinate first_coordinate,
                                         const Coordinate second_coordinate,
                                         const Coordinate third_coordinate);

// find the radius of a circle through three coordinates
double circleRadius(const Coordinate first_coordinate,
                    const Coordinate second_coordinate,
                    const Coordinate third_coordinate);

// factor in [0,1]. Returns point along the straight line between from and to. 0 returns from, 1
// returns to
Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to);

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate);

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate);

} // ns coordinate_calculation
} // ns util
} // ns osrm

#endif // COORDINATE_CALCULATION


#include <Eigen/Dense>
#include <Eigen/LU>

#include <iostream>
#include <iomanip>

template <class Matrix>
class ImpedanceMap {
public:
    ImpedanceMap(double R_, double W_) : R(R_), W(W_) {
        // TODO: build and factorize A0 into lu
    };

    double operator()(double Rx) {
        // TODO: compute the impedance from voltages and resistance
    };
private:
    Eigen::PartialPivLU< Matrix > lu; //< Store lu decomposition of a for efficiency
    double R, W; //< Resistance R and source voltage W
    Matrix rhs; //< Store rhs vector prescribing sink and source voltages
};

int main(void) {
    ImpedanceMap<Eigen::MatrixXd> IM = ImpedanceMap<Eigen::MatrixXd>(1, 1);

    std::cout << std::setw(30) << "Impedance [Ohm]" << std::setw(30) << "R_x [Ohm]" << std::endl;
    for(auto Rx = 1; Rx <= 1024; Rx *= 2) {
        std::cout << std::setw(30) << IM(Rx)        << std::setw(30) << " " << Rx << std::endl;
    }

}

Nummerical Methods for CSE/solutions/solution_3/impedancemap.cpp


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE linalg_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/eigenio_matrixmarket.h"
#include "votca/tools/linalg.h"

using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(linalg_test)

BOOST_AUTO_TEST_CASE(linalg_constrained_qrsolve_test) {

  Eigen::VectorXd b = Eigen::VectorXd::Zero(3);
  b(0) = 11;
  b(1) = -3;
  b(2) = 8;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, 3);
  A(0, 0) = 1;
  A(0, 1) = 1;
  A(0, 2) = 1;
  A(1, 0) = 1;
  A(1, 1) = -1;
  A(2, 1) = 1;
  A(2, 2) = 1;

  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(1, 3);
  B(0, 1) = -1;
  B(0, 2) = 3;
  Eigen::VectorXd x = linalg_constrained_qrsolve(A, b, B);
  Eigen::VectorXd x_ref = Eigen::VectorXd::Zero(3);
  x_ref(0) = 3;
  x_ref(1) = 6;
  x_ref(2) = 2;

  bool equal = x_ref.isApprox(x, 1e-7);

  if (!equal) {
    std::cout << "result" << std::endl;
    std::cout << x << std::endl;
    std::cout << "ref" << std::endl;
    std::cout << x_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(equal, true);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file fastmks_test.cpp
 * @author Ryan Curtin
 *
 * Ensure that fast max-kernel search is correct.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/fastmks/fastmks.hpp>
#include <mlpack/core/kernels/linear_kernel.hpp>
#include <mlpack/core/kernels/polynomial_kernel.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::tree;
using namespace mlpack::fastmks;
using namespace mlpack::kernel;
using namespace mlpack::metric;

BOOST_AUTO_TEST_SUITE(FastMKSTest);

/**
 * Compare single-tree and naive.
 */
BOOST_AUTO_TEST_CASE(SingleTreeVsNaive)
{
  // First create a random dataset.
  arma::mat data;
  data.randn(5, 1000);
  LinearKernel lk;

  // Now run FastMKS naively.
  FastMKS<LinearKernel> naive(data, lk, false, true);

  arma::Mat<size_t> naiveIndices;
  arma::mat naiveProducts;
  naive.Search(10, naiveIndices, naiveProducts);

  // Now run it in single-tree mode.
  FastMKS<LinearKernel> single(data, lk, true);

  arma::Mat<size_t> singleIndices;
  arma::mat singleProducts;
  single.Search(10, singleIndices, singleProducts);

  // Compare the results.
  for (size_t q = 0; q < singleIndices.n_cols; ++q)
  {
    for (size_t r = 0; r < singleIndices.n_rows; ++r)
    {
      BOOST_REQUIRE_EQUAL(singleIndices(r, q), naiveIndices(r, q));
      BOOST_REQUIRE_CLOSE(singleProducts(r, q), naiveProducts(r, q), 1e-5);
    }
  }
}

/**
 * Compare dual-tree and naive.
 */
BOOST_AUTO_TEST_CASE(DualTreeVsNaive)
{
  // First create a random dataset.
  arma::mat data;
  data.randn(10, 5000);
  LinearKernel lk;

  // Now run FastMKS naively.
  FastMKS<LinearKernel> naive(data, lk, false, true);

  arma::Mat<size_t> naiveIndices;
  arma::mat naiveProducts;
  naive.Search(10, naiveIndices, naiveProducts);

  // Now run it in dual-tree mode.
  FastMKS<LinearKernel> tree(data, lk);

  arma::Mat<size_t> treeIndices;
  arma::mat treeProducts;
  tree.Search(10, treeIndices, treeProducts);

  for (size_t q = 0; q < treeIndices.n_cols; ++q)
  {
    for (size_t r = 0; r < treeIndices.n_rows; ++r)
    {
      BOOST_REQUIRE_EQUAL(treeIndices(r, q), naiveIndices(r, q));
      BOOST_REQUIRE_CLOSE(treeProducts(r, q), naiveProducts(r, q), 1e-5);
    }
  }
}

/**
 * Compare dual-tree and single-tree on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(DualTreeVsSingleTree)
{
  // First create a random dataset.
  arma::mat data;
  data.randu(8, 5000);
  PolynomialKernel pk(5.0, 2.5);

  FastMKS<PolynomialKernel> single(data, pk, true);

  arma::Mat<size_t> singleIndices;
  arma::mat singleProducts;
  single.Search(10, singleIndices, singleProducts);

  // Now run it in dual-tree mode.
  FastMKS<PolynomialKernel> tree(data, pk);

  arma::Mat<size_t> treeIndices;
  arma::mat treeProducts;
  tree.Search(10, treeIndices, treeProducts);

  for (size_t q = 0; q < treeIndices.n_cols; ++q)
  {
    for (size_t r = 0; r < treeIndices.n_rows; ++r)
    {
      BOOST_REQUIRE_EQUAL(treeIndices(r, q), singleIndices(r, q));
      BOOST_REQUIRE_CLOSE(treeProducts(r, q), singleProducts(r, q), 1e-5);
    }
  }
}

/**
 * Test sparse FastMKS (how useful is this, I'm not sure).
 */
BOOST_AUTO_TEST_CASE(SparseFastMKSTest)
{
  // First create a random sparse dataset.
  arma::sp_mat dataset;
  dataset.sprandu(10, 100, 0.3);

  FastMKS<LinearKernel, arma::sp_mat> sparsemks(dataset);

  arma::mat denseset(dataset);
  FastMKS<LinearKernel> densemks(denseset);

  // Store the results in these.
  arma::Mat<size_t> sparseIndices, denseIndices;
  arma::mat sparseKernels, denseKernels;

  // Do the searches.
  sparsemks.Search(3, sparseIndices, sparseKernels);
  densemks.Search(3, denseIndices, denseKernels);

  // Make sure the results are the same.
  for (size_t i = 0; i < sparseIndices.n_cols; ++i)
  {
    for (size_t j = 0; j < sparseIndices.n_rows; ++j)
    {
      if (std::abs(sparseKernels(j, i)) > 1e-15)
        BOOST_REQUIRE_CLOSE(sparseKernels(j, i), denseKernels(j, i), 1e-5);
      else
        BOOST_REQUIRE_SMALL(denseKernels(j, i), 1e-15);
      BOOST_REQUIRE_EQUAL(sparseIndices(j, i), denseIndices(j, i));
    }
  }
}

BOOST_AUTO_TEST_CASE(SparsePolynomialFastMKSTest)
{
  // Do it again with the polynomial kernel, just to be sure.
  arma::sp_mat dataset;
  dataset.sprandu(10, 100, 0.3);
  arma::mat denseset(dataset);

  PolynomialKernel pk(3);

  for (size_t i = 0; i < 100; ++i)
    for (size_t j = 0; j < 100; ++j)
      if (std::abs(pk.Evaluate(dataset.col(i), dataset.col(j))) < 1e-10)
        BOOST_REQUIRE_SMALL(pk.Evaluate(denseset.col(i), denseset.col(j)), 1e-10);
      else
        BOOST_REQUIRE_CLOSE(pk.Evaluate(dataset.col(i), dataset.col(j)),
                            pk.Evaluate(denseset.col(i), denseset.col(j)),
                            1e-5);

  FastMKS<PolynomialKernel, arma::sp_mat> sparsepoly(dataset);
  FastMKS<PolynomialKernel> densepoly(denseset);

  // Store the results in these.
  arma::Mat<size_t> sparseIndices, denseIndices;
  arma::mat sparseKernels, denseKernels;

  // Do the searches.
  sparsepoly.Search(3, sparseIndices, sparseKernels);
  densepoly.Search(3, denseIndices, denseKernels);

  // Make sure the results are the same.
  for (size_t i = 0; i < sparseIndices.n_cols; ++i)
  {
    for (size_t j = 0; j < sparseIndices.n_rows; ++j)
    {
      if (std::abs(sparseKernels(j, i)) > 1e-15)
        BOOST_REQUIRE_CLOSE(sparseKernels(j, i), denseKernels(j, i), 1e-5);
      else
        BOOST_REQUIRE_SMALL(denseKernels(j, i), 1e-15);
      BOOST_REQUIRE_EQUAL(sparseIndices(j, i), denseIndices(j, i));
    }
  }

}

BOOST_AUTO_TEST_SUITE_END();


/*! \file
    \brief Demonstration of some simple 2D plot features.
    \details Uses some simple math functions to generate curves.
    This demonstrates plotting some simple math functions with most of the 2-D defaults,
    just changing a few typical details.
    The detailed output shows the plot settings for each plot.
    See default_2d_plot.cpp for using \b all defaults.
    See also demo_2d_plot.cpp for use of some of the very many options.
*/

// demo_2d_simple.cpp

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2007, 2008, 2012, 2018, 2021

// 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 <boost/svg_plot/svg_2d_plot.hpp> // For plot package.
  using boost::svg::svg_2d_plot;

#include <boost/svg_plot/show_2d_settings.hpp>
// Only needed for showing which settings in use.
// Use this when the plot doesn't look as you want it to be.

#include <boost/quan/unc_init.hpp>  // for setUncDefaults

#include <iostream>
  using std::cout;
  using std::endl;
  using std::boolalpha;
#include <map>
  using std::map;
#include <cmath>
  using std::sqrt;
#include <string>
  using std::string;

// using namespace boost::svg;
// may be *very convenient* if using any SVG named colors,
// to avoid writing
  using boost::svg::red;
  using boost::svg::yellow;
  using boost::svg::orange;
  using boost::svg::blue;
  // for every color used.

  // and other enum options used:
  using boost::svg::square;
  using boost::svg::circlet;

// Some example of functions to plot.
double f(double x)
{
  return sqrt(x);
}

double g(double x)
{
  return -2 + x*x;
}

double h(double x)
{
  return -1 + 2 * x;
}

int main()
{
  std::cout << "Demonstration of a simple 2D plot showing data points with markers and with lines joining points." << std::endl;
  using boost::quan::setUncDefaults;
  try
  {
    // Some containers for (sorted) sample data.
    map<double, double> data1;
    map<double, double> data2;
    map<double, double> data3;

    for(double i = -5; i <= 10.; i += 1.)
    { // Several data points for each function.
      data1[i] = f(i);
      data2[i] = g(i);
      data3[i] = h(i);
      // List if desired:
      // cout << i << ' '<< data1[i] << ' ' << data2[i] << ' '<< data3[i] << endl;
    }

    setUncDefaults(std::cout);  // Set the uncertain class defaults.
    svg_2d_plot my_plot; // Class to hold the plot settings.
    // Uses most defaults, but scale settings are usually sensible.

    // Add the data series to the plot:
    my_plot.title("demo_2d_simple");
    cout << " my_plot.title() " << my_plot.title() << endl;
    my_plot.x_label("X-axis").y_label("Y-axis"); // Note chaining, the easy way to add lots of options.

    std::string s = my_plot.title();

    my_plot.plot(data1, "Sqrt(x)").fill_color(red);
    my_plot.plot(data2, "-2 + x^2").fill_color(orange).size(5);
    my_plot.plot(data3, "-1 + 2x").fill_color(yellow).bezier_on(true).line_color(blue).shape(square);
    cout << " my_plot.title() " << my_plot.title() << endl;

    my_plot.write("./demo_2d_simple.svg");
    cout << " my_plot.title() " << my_plot.title() << endl;

    show_2d_plot_settings(my_plot);
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output:

Demonstration of a simple 2D plot showing data points with markers and with lines joining points.
my_plot.title() demo_2d_simple
my_plot.title() demo_2d_simple
font size 12
x value_font_space 24
x left-right border_margin = 24
plot_left before margin 30
plot_left after margin 54
plot_right before margin 498
plot_right after margin 474

y value_font_space 24
y top-botton border_margin = 24
plot_top before margin 54
plot_top after margin 54
plot_bottom before margin 474
plot_bottom after margin 474
my_plot.title()

SVG 2-D plot settings
(default units pixels)========================
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(255,255,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title ""
image x_size 500
image y_size 400
image_filename
legend_on false
legend_place 2
legend_top_left -1, -1, legend_bottom_right -1, -1
legend_background_color blank
legend_border_color RGB(255,255,0)
legend_color blank
legend_title ""
legend_title_font_size 14
legend_font_weight
legend_width 0
legend_lines true
limit points stroke color RGB(119,136,153)
limit points fill color RGB(250,235,215)
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(119,136,153)
plot_border_width 2
plot_window_on true
plot_window_x 84.2, 474
plot_window_x_left 84.2
plot_window_x_right 474
plot_window_y 71, 323
plot_window_y_top 71
plot_window_y_bottom 323
title_on true
title ""
title_color blank
title_font_alignment 2
title_font_decoration
title_font_family Lucida Sans Unicode
title_font_rotation 0
title_font_size 18
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family Lucida Sans Unicode
x_values_precision 3
x_values_ioflags 200 iosFormatFlags (0x200) dec.
y_values_precision 3
y_values_font_size() 3
y_values_ioflags 200 iosFormatFlags (0x200) dec.
y_values_color blank
y_values_font_family() Lucida Sans Unicode
y_values_font_size() 12
x_max 10
x_min -10
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label "X-axis"
x_label_color blank
x_label_font_family Lucida Sans Unicode
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on false
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 5
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 2
x_minor_tick_width 1
x_minor_grid_on false
x_minor_grid_color RGB(200,220,255)
x_minor_grid_width 0.5
x_range() -10, 10
x_num_minor_ticks 4
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis bottom
y_axis_position y_axis_position intersects X-axis (X range includes zero)
x_axis_position x_axis_position intersects Y axis (Y range includes zero)
x_plusminus_on false
x_plusminus_color blank
x_df_on false
x_df_color RGB(0,0,0)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on "true"
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Lucida Sans Unicode
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on false
y_major_grid_color RGB(200,220,255)
y_major_grid_width 1
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  5
y_major_tick_width  2
y_minor_grid_on false
y_minor_grid_color  RGB(200,220,255)
y_minor_grid_width 0.5
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 2
y_minor_tick_width 1
y_range() -10, 10
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis left
y_max 10
y_min -10
y_values_on false
y_plusminus_on false
y_plusminus_color blank
x_addlimits_on false
x_addlimits_color RGB(0,0,0)
y_df_on false
y_df_color RGB(0,0,0)
y_prefix ""
y_separator ""
y_suffix ""
confidence alpha 0.05
data lines width 2
Press any key to continue . . .

*/


#define BOOST_TEST_MODULE party

#include <chrono>
#include <iostream>
#include <set>

#include <boost/test/unit_test.hpp>

#include "party.hpp"

using clock_type = std::chrono::system_clock;
using namespace party;

BOOST_AUTO_TEST_SUITE(functional_tests)

// functions should work for any movable element type
// including non-comparable and non-copyable types

struct element {
	element(element&& other) = default;
	element& operator = (element&& other) = default;
	element(size_t value = 0) : value(value) {}
	operator int () const {return value;}
	size_t value;
};

BOOST_AUTO_TEST_CASE(seventh_bell_number) {
	std::cout << "Constructing unordered partitions" << std::endl;
	std::vector<size_t> elements {1,2,3,4,5,6,7};

	size_t counter = 0;

	// set up generation for elements on non-copyable type
	auto root = first_set_partition<element>(elements);
	auto start = clock_type::now();
	for(bool is_valid(1); is_valid; is_valid = next_unordered_partition(root)) {
//		std::cout << counter << ":\t" << root << std::endl;
		++counter;
	}

	std::cout << counter << " partitions generated" << std::endl;
	BOOST_CHECK_EQUAL(counter, 877);

	root = last_set_partition<element>(elements);

	for(bool is_valid(1); is_valid; is_valid = prev_unordered_partition(root)) {
//		std::cout << counter << ":\t" << root << std::endl;
		--counter;
	}

	std::cout << counter << " partitions left after reverse" << std::endl;
	BOOST_CHECK_EQUAL(counter, 0);

	auto finish = clock_type::now();
	auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(finish - start).count();
	std::cout << elapsed << " milliseconds elapsed" << std::endl;
}

BOOST_AUTO_TEST_CASE(fifth_ordered_bell_number) {
	std::cout << "Constructing ordered paritions" << std::endl;
	std::vector<size_t> elements {1,2,3,4,5};

	auto start = clock_type::now();
	size_t counter = 0;

	// set up generation for elements on non-copyable type
	auto root = first_set_partition<element>(elements);
	for(bool is_valid(1); is_valid; is_valid = next_ordered_partition(root)) {
//		std::cout << ++counter << ":\t" << root << std::endl;
		++counter;
	}

	std::cout << counter << " partitions constructed" << std::endl;
	BOOST_CHECK_EQUAL(counter, 541);

	root = last_set_partition<element>(elements);
	for(bool is_valid(1); is_valid; is_valid = prev_ordered_partition(root)) {
//		std::cout << --counter << ":\t" << root << std::endl;
		--counter;
	}

	std::cout << counter << " partitions left after reverse" << std::endl;
	BOOST_CHECK_EQUAL(counter, 0);

	auto finish = clock_type::now();
	auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(finish - start).count();
	std::cout << elapsed << " milliseconds elapsed" << std::endl;
}

BOOST_AUTO_TEST_CASE(Ono_and_Nakano_extension) {
	std::vector<size_t> elements {1,2,3,4,5,6};
	std::vector<extension<size_t>> exhaustive, forward, reverse;
	std::set<std::pair<size_t, size_t>> relation {
		{1,2}, {1,4}, {2,4}, {3,5}, {3,6}, {5,6}
	};

	// find extensions with exhaustive search
	auto use_exhaustive_search = [&] () {
		extension<size_t> root;
		root.elements = elements;
		root.index.resize(elements.size());
		std::iota(root.index.begin(), root.index.end(), 0);
		while (true) {

			auto e_begin = root.elements.begin(), e_end = root.elements.end();
			auto r_iter = relation.begin(), r_end = relation.end();
			for(; r_iter != r_end; ++r_iter) {

				// find positions of the elements spesified by the relation
				size_t source = std::distance(e_begin, std::find(e_begin, e_end, r_iter->first));
				size_t target = std::distance(e_begin, std::find(e_begin, e_end, r_iter->second));

				// check relation requirements
				if (source > target) break;
			}

			if (r_iter == r_end) exhaustive.emplace_back(root);
			if (!std::next_permutation(root.index.begin(), root.index.end())) break;
			for(size_t i = 0; i < root.index.size(); ++i) {
				root.elements[i] = elements[root.index[i]];
			}
		}
	};

	// find extensions with forward enumeration algorithm
	auto use_forward_enumeration = [&] () {
		auto ps = make_poset<size_t>(elements, relation);
		auto root = first_poset_extension(ps);

		do forward.push_back(root);
		while (next_poset_extension(root, ps));
	};

	// find extensions with reverse enumeration algorithm
	auto use_reverse_enumeration = [&] () {
		auto ps = make_poset<size_t>(elements, relation);
		auto root = last_poset_extension(ps);

		do reverse.push_back(root);
		while (prev_poset_extension(root, ps));
	};

	use_exhaustive_search();
	use_forward_enumeration();
	use_reverse_enumeration();

	std::cout << "exhaustive search : " << exhaustive.size() << std::endl;
	std::cout << "forward enumeration : " << forward.size() << std::endl;
	std::cout << "reverse enumeration : " << reverse.size() << std::endl;

	BOOST_CHECK(exhaustive.size() == forward.size());
	BOOST_CHECK(exhaustive.size() == reverse.size());

//	std::reverse(forward.begin(), forward.end());
//	BOOST_CHECK(forward == reverse);
}

BOOST_AUTO_TEST_CASE(opartition_example_by_Ono_and_Nakano) {
	std::vector<size_t> elements {1,2,3,4,5,6};
	std::vector<partition<size_t>> exhaustive, forward, reverse;
	std::set<std::pair<size_t, size_t>> relation {
		{1,2}, {1,4}, {2,4}, {3,5}, {3,6}, {5,6}
	};

	// find partitions using exhaustive search
	auto use_exhaustive_search = [&] () {
		auto root = first_set_partition<size_t>(elements);
		while (true) {

			auto e_begin = root.elements.begin(), e_end = root.elements.end();
			auto r_iter = relation.begin(), r_end = relation.end();
			for(; r_iter != r_end; ++r_iter) {

				// find positions of the elements spesified by the relation
				size_t source = std::distance(e_begin, std::find(e_begin, e_end, r_iter->first));
				size_t target = std::distance(e_begin, std::find(e_begin, e_end, r_iter->second));

				// check relation requirements
				if (source > target) break;

				// items should belong to different segments
				auto s_begin = root.segments.begin();
				auto s_min = s_begin + std::min(source, target);
				auto s_max = s_begin + std::max(source, target);
				while (s_max != s_min && !*s_max) --s_max;
				if (s_max == s_min) break;
			}

			if (r_iter == r_end) exhaustive.emplace_back(root);
			if (!next_ordered_partition(root)) break;
		}
	};

	// find partitions with forward enumeration algorithm
	auto use_forward_enumeration = [&] () {
		auto ps = make_poset<size_t>(elements, relation);
		auto root = first_poset_partition(ps);

		do forward.push_back(root);
		while (next_poset_partition(root, ps));
	};

	// find partitions with reverse enumeration algorithm
	auto use_reverse_enumeration = [&] () {
		auto ps = make_poset<size_t>(elements, relation);
		auto root = last_poset_partition(ps);

		do reverse.push_back(root);
		while (prev_poset_partition(root, ps));
	};

	use_exhaustive_search();
	use_forward_enumeration();
	use_reverse_enumeration();

	std::cout << "exhaustive search : " << exhaustive.size() << std::endl;
	std::cout << "forward enumeration : " << forward.size() << std::endl;
	std::cout << "reverse enumeration : " << reverse.size() << std::endl;

	BOOST_CHECK(exhaustive.size() == forward.size());
	BOOST_CHECK(exhaustive.size() == reverse.size());

//	std::reverse(forward.begin(), forward.end());
//	BOOST_CHECK(forward == reverse);
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <igl/read_triangle_mesh.h>
#include <igl/write_triangle_mesh.h>
#include <igl/writeOFF.h>
#include <igl/per_vertex_normals.h>
#include <igl/cotmatrix.h>
#include <igl/cotmatrix_entries.h>
#include <igl/adjacency_list.h>
#include <igl/triangle_triangle_adjacency.h>
#include <igl/barycenter.h>
#include <igl/massmatrix.h>
#include <igl/writeOBJ.h>
#include <fstream>
#include <cmath>
#include <array>

void findRotations(const Eigen::MatrixXd& N0,
                   const Eigen::MatrixXd& N1,
                   std::vector<Eigen::Matrix3d>& rot) {

    const auto n = N0.rows();
    rot.resize(n);

    for(int i = 0; i < n; ++i) {
        Eigen::Vector3d n1 = N0.row(i);
        Eigen::Vector3d n2 = N1.row(i);
        Eigen::Vector3d v = n1.cross(n2);
        const double c = n1.dot(n2);

        if(c > -1 + 1e-8) {
            const double coeff = 1 / (1 + c);
            Eigen::Matrix3d v_x;
            v_x << 0.0, -v(2), v(1), v(2), 0.0, -v(0), -v(1), v(0), 0.0;
            rot[i] = Eigen::Matrix3d::Identity() + v_x + coeff * v_x * v_x;
        } else{
            rot[i] = -Eigen::Matrix3d::Identity();
        }
    }
}

std::vector<std::vector<int>> collectNeighbours(const std::vector<std::vector<int>>& adj,
                                                const Eigen::MatrixXd& V,
                                                const Eigen::MatrixXd& N,
                                                const double r,
                                                const double nr) {

    std::vector<int> stack;
    std::vector<int> flag(V.rows(), -1);
    std::vector<std::vector<int>> result(V.rows());
    const double normalConeThreshold = cos(nr * M_PI / 180.);

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

        stack.push_back(i);
        flag[i] = i;

        while(!stack.empty()) {
            auto id = stack.back();
            stack.pop_back();

            result[i].push_back(id);

            for (int j : adj[id]) {
                if(flag[j] != i && (V.row(i) - V.row(j)).norm() < r && (N.row(i).dot(N.row(j))) > normalConeThreshold) {
                    stack.push_back(j);
                    flag[j] = i;
                }
            }
        }
    }

    return result;
}

void fitNormals(const std::vector<std::vector<int>>& nbh,
                const Eigen::MatrixXd& V,
                const Eigen::MatrixXd& N,
                Eigen::MatrixXd& N2,
                const double cosineThreshold,
                const double sigma = 1.) {

    const auto nv = nbh.size();
    N2.resize(nv, 3);
    double angleThreshold = cosineThreshold * M_PI / 180.;

    for(int i = 0; i < nv; ++i) {

        const auto& nbi = nbh[i];

        Eigen::MatrixXd NN(nbi.size(), 3);

        for (int k = 0; k < nbi.size(); ++k) {
            NN.row(k) = N.row(nbi[k]);
        }

        Eigen::DiagonalMatrix<double, -1> W(nbi.size());

        if(sigma < 10.) {
            for(int i = 0; i < W.diagonal().size(); ++i) {
                double dot = NN.row(0).dot(NN.row(i));
                if (dot >= 1.){
                    W.diagonal()(i) = 1;
                } else if(dot < 0) {
                    W.diagonal()(i) = 0;
                } else {
                    W.diagonal()(i) = std::exp(-std::pow(acos(dot) / angleThreshold / sigma, 2));
                }
            }
        } else {
            W.diagonal().setOnes();
        }

        Eigen::JacobiSVD<Eigen::Matrix3d> svd(NN.transpose() * W * NN, Eigen::ComputeFullV);
        Eigen::Matrix3d frame = svd.matrixV();
        N2.row(i) = (frame.leftCols(2) * frame.leftCols(2).transpose() * N.row(i).transpose()).normalized();
    }
}

void assembleRHS(const Eigen::MatrixXd& C,
                 const Eigen::MatrixXd& V,
                 const Eigen::MatrixXi& F,
                 const std::vector<Eigen::Matrix3d>& R,
                 Eigen::MatrixXd& rhs) {

    const auto nv = V.rows();
    rhs.resize(nv, 3);
    rhs.setZero();

    for(int i = 0; i < F.rows(); ++i)  {
        for(int j = 0; j < 3; ++j)  {
            int v0 = F(i, (j + 1) % 3);
            int v1 = F(i, (j + 2) % 3);

            Eigen::Vector3d b = C(i,j) * R[i] * (V.row(v0) - V.row(v1)).transpose();
            rhs.row(v0) -= b.transpose();
            rhs.row(v1) += b.transpose();
        }
    }
}

std::vector<std::vector<int>> triangleAdjacency(const Eigen::MatrixXi& F, const size_t nv) {

    std::vector<std::vector<int>> vnbhs(nv);
    const auto nf = F.rows();

    for(int i = 0; i < nf; ++i) {
        for(int j = 0; j < 3; ++j) {
            vnbhs[F(i, j)].push_back(i);
        }
    }

    std::vector<int> flags(nf, -1);
    std::vector<std::vector<int>> ret(nf);

    for(int i = 0; i < nf; ++i) {
        for(int j = 0; j < 3; ++j) {
            for(int k : vnbhs[F(i, j)]) {
                if(k != i && flags[k] != i) {
                    ret[i].push_back(k);
                    flags[k] = i;
                }
            }
        }
    }

    return ret;
}

void center(Eigen::MatrixXd& V) {
    V.rowwise() -= V.colwise().mean();;
    V /= 2. * V.rowwise().norm().maxCoeff();
}

void gaussThinning(const std::string &mesh_folder,
                   const Eigen::MatrixXd &V_in,
                   const Eigen::MatrixXi &F,
                   Eigen::MatrixXd &V,
                   const int number_iterations = 100,
                   double minConeAngle = 2.5,
                   double smooth = 1e-5,
                   double start_angle = 25,
                   double radius = 0.1,
                   double sigma = 2.) {

    double coneAngle = start_angle;
    double r = radius;
    double eps = 1e-3;

    V = V_in;
    const auto nv = V.rows();
    center(V);

    igl::writeOFF(mesh_folder + "/normalized.off", V, F);

    Eigen::SparseMatrix<double> I(nv, nv);
    I.setIdentity();

    Eigen::MatrixXi TT;
    Eigen::MatrixXd B, b, C, N, N2;
    std::vector<Eigen::Matrix3d> rot;
    Eigen::SparseMatrix<double> L, M;
    std::vector<std::vector<int>> nbhs;
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> chol;

    auto tt = triangleAdjacency(F, nv);
    igl::triangle_triangle_adjacency(F, TT);
    igl::cotmatrix_entries(V, F, C);
    igl::cotmatrix(V, F, L);
    igl::massmatrix(V, F, igl::MASSMATRIX_TYPE_BARYCENTRIC, M);

    if(smooth) {
        chol.compute(-L + smooth * L.transpose() * L + eps * M);
    } else {
        chol.compute(-L + eps * M);
    }

    for(int k = 0; k < number_iterations; ++k) {

        igl::per_face_normals(V, F, N);
        igl::barycenter(V, F, B);

        nbhs = collectNeighbours(tt, B, N, r, coneAngle);
        if(coneAngle > minConeAngle) coneAngle *= .95;

        fitNormals(nbhs, V, N, N2, coneAngle, sigma);
        findRotations(N, N2, rot);
        assembleRHS(C, V, F, rot, b);

        V = chol.solve(eps * M * V - b);

        if (k % std::max(1, (number_iterations / 10)) == 0) {
            std::cout << "writing " + mesh_folder + ": " << k << "\n";
            igl::writeOFF(mesh_folder + "/out" + std::to_string(k) + ".off", V, F);
        }
    }

    return;
}

void runExperiment(std::string folder, std::string inputFile, std::string outputFile, const int iters, const double minAngle, const double start_angle = 25, const double radius = 0.1, const double smooth = 1e-5) {
    Eigen::MatrixXd V_in, V_out;
    Eigen::MatrixXi F;
    igl::read_triangle_mesh(folder + "/" + inputFile, V_in, F);
    gaussThinning(folder, V_in, F, V_out, iters, minAngle, smooth, start_angle, radius);
    igl::write_triangle_mesh(folder + "/" + outputFile, V_out, F);
}

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

    if(argc < 6) {
        std::cout << "Need input file, output file, output directory, number of iterations and minimum search cone. Running default experiments..." << std::endl;

        /* run default experiments here .... */
        runExperiment("./examples/architecture", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/boat", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/bumpy", "input.off", "out.obj", 150, 7.5);

        runExperiment("./examples/bunny", "input.off", "out.obj", 500, 2.5);

        runExperiment("./examples/bunny_high", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/bunny_small", "input.off", "out.obj", 500, 5.0);

        runExperiment("./examples/coffee", "input.off", "out.obj", 500, 2.5);

        runExperiment("./examples/cone", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/cone_high", "input.off", "out.obj", 100, 2.5, 25, 0.015);

        runExperiment("./examples/curved_fold", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/cylinder", "input.off", "out.obj", 300, 7.5);

        runExperiment("./examples/dog", "input.off", "out.obj", 100, 5.0);

        runExperiment("./examples/dome", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/dress_high", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/drill", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/einstein", "input.off", "out.obj", 300, 7.5, 60, 0.015);

        runExperiment("./examples/face", "input.off", "out.obj", 100, 5.0);

        runExperiment("./examples/fandisk", "input.off", "out.obj", 1000, 5.0);

        runExperiment("./examples/fertility", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/guitar", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/lilium", "input.off", "out.obj", 100, 5.0);

        runExperiment("./examples/mask", "input.off", "out.obj", 500, 2.5);

        runExperiment("./examples/nut", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/swing", "input.off", "out.obj", 500, 5.0);
    } else
    {
        std::string  infile = argv[1];
        std::string  outfile = argv[2];
        std::string  folder = argv[3];

        auto numIters = std::atoi(argv[4]);
        auto minAngle = std::stold(argv[5]);

        std::cout << "Processing " << infile << " with " << numIters << " iterations and mimimum cone angle " << minAngle << ". Output directory is " << folder << std::endl;

        runExperiment(folder, infile, outfile, numIters, minAngle);
    }

    return 0;
}


//          Copyright Carl Philipp Reh 2009 - 2015.
//          Copyright Philipp Middendorf 2009 - 2015.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FCPPT_ALGORITHM_LEVENSHTEIN_HPP_INCLUDED
#define FCPPT_ALGORITHM_LEVENSHTEIN_HPP_INCLUDED

#include <fcppt/literal.hpp>
#include <fcppt/cast/to_signed.hpp>
#include <fcppt/cast/to_unsigned.hpp>
#include <fcppt/container/grid/object.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/range/empty.hpp>
#include <boost/range/size.hpp>
#include <boost/range/size_type.hpp>
#include <boost/range/value_type.hpp>
#include <algorithm>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace algorithm
{

/**
\brief Calculates the Levenshtein distance
\ingroup fcpptalgorithm
\details
See http://en.wikipedia.org/wiki/Levenshtein_distance for an explanation of the algorithm.
\pre

Range::size_type and Range::value_type exist

bool Range::empty() const exists

size_type Range::size() const exists

Range::operator[] exists

Range::value_type has to have an operator==

\note
The code is taken quite literally from:
http://www.merriampark.com/ldcpp.htm
*/
template<typename Range>
typename
boost::range_size<Range>::type
levenshtein(
	Range const &source,
	Range const &target)
{
	typedef typename
	boost::range_size<Range>::type
	size_type;

	typedef typename
	boost::range_difference<Range>::type
	difference_type;

	typedef typename
	boost::range_value<Range>::type
	element_type;

	size_type const
		n =
			boost::size(
				source),
		m =
			boost::size(
				target);

	if(boost::empty(source))
		return m;

	if(boost::empty(target))
		return n;

	typedef
	fcppt::container::grid::object
	<
		size_type,
		2
	>
	grid;

	typedef typename
	grid::dim
	dim;

	typedef typename
	grid::pos
	pos;

	grid matrix(
		dim(
			n + 1,
			m + 1
		),
		fcppt::literal<
			size_type
		>(
			0
		)
	);

	// Step 2

	for (size_type i = 0; i <= n; i++)
		matrix[pos(i,0u)] = i;

	for (size_type j = 0; j <= m; j++)
		matrix[pos(0u,j)] = j;

	for (difference_type i = 1; i <= fcppt::cast::to_signed(n); i++)
	{
		element_type const &s_i =
			*(boost::begin(
				source) +
			i-1);

		for (difference_type j = 1; j <= fcppt::cast::to_signed(m); j++)
		{
			element_type const &t_j =
				*(boost::begin(
					target) +
				j-1);

			size_type const cost(
				s_i == t_j
				?
					0u
				:
					1u
			);

			// Step 6

			size_type const
				above =
					matrix[
						pos(
							fcppt::cast::to_unsigned(
								i-1),
							fcppt::cast::to_unsigned(
								j))],
				left =
					matrix[
						pos(
							fcppt::cast::to_unsigned(
								i),
							fcppt::cast::to_unsigned(
								j-1))],
				diag =
					matrix[
						pos(
							fcppt::cast::to_unsigned(
								i-1),
							fcppt::cast::to_unsigned(
								j-1))];

			size_type cell =
				::std::min(
					above + 1u,
					::std::min(
						left + 1u,
						diag + cost));

			// Step 6A: Cover transposition, in addition to deletion,
			// insertion and substitution. This step is taken from:
			// Berghel, Hal ; Roach, David : "An Extension of Ukkonen's
			// Enhanced Dynamic Programming ASM Algorithm"
			// (http://www.acm.org/~hlb/publications/asm/asm.html)
			if(i>fcppt::literal<difference_type>(2) && j>fcppt::literal<difference_type>(2))
			{
				size_type trans =
					matrix[
						pos(
							fcppt::cast::to_unsigned(
								i-2),
							fcppt::cast::to_unsigned(
								j-2))
					] +
						1u;

				if(*(boost::begin(source) + i-2) != t_j)
					trans++;

				if(s_i != *(boost::begin(target) + j-2))
					trans++;

				if(cell>trans)
					cell = trans;
			}

			matrix[
				pos(
					fcppt::cast::to_unsigned(
						i),
					fcppt::cast::to_unsigned(
						j))] =
				cell;
		}
	}

	return
		matrix[
			pos(
				n,
				m
			)
		];
}

}
}

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 polynom toolbox - polyfit/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of polynom components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
///
#include <nt2/include/functions/polyfit.hpp>
#include <nt2/include/functions/polyval.hpp>
#include <nt2/include/functions/isulpequal.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/table.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/complexify.hpp>

#include <boost/array.hpp>

NT2_TEST_CASE_TPL ( polyfit_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::polyfit;
  using nt2::tag::polyfit_;
  typedef std::complex<T> cT;
  nt2::table<cT> x =  nt2::complexify(nt2::_(T(1), T(3)));
  nt2::table<cT> p =  nt2::complexify(nt2::_(T(1), T(3)));
  nt2::table<cT> y =  nt2::polyval(p, x);
  NT2_DISPLAY(y);
  NT2_DISPLAY(x);
  NT2_DISPLAY(p);
  NT2_DISPLAY(nt2::polyfit(x, y));
  nt2::table<cT> p1 =nt2::polyfit(x, y);
  NT2_DISPLAY(p1);
  NT2_DISPLAY(nt2::polyval(p1, x));
  NT2_DISPLAY(y);
  NT2_TEST(nt2::isulpequal(nt2::polyval(p1, x), y, 0.5));

  NT2_DISPLAY(polyfit(x, y, 2));
  nt2::table<cT> p2 =polyfit(x, y, 2);
  NT2_DISPLAY(p2);
  NT2_DISPLAY(nt2::polyval(p2, x));
  NT2_DISPLAY(y);
  NT2_TEST(nt2::isulpequal(nt2::polyval(p2, x), y, 0.5));

  nt2::table<cT> r;
  T df, normr;
  nt2::table<cT> mu;
  nt2::tie(p, r, df, normr, mu) = nt2::polyfit(x, y);
  NT2_DISPLAY(p);
  NT2_DISPLAY(r);
  NT2_DISPLAY(df);
  NT2_DISPLAY(normr);
  NT2_DISPLAY(mu);
  NT2_DISPLAY(polyval(p, (x-mu(1))/mu(2)));
  NT2_DISPLAY(y);
  NT2_TEST(nt2::isulpequal(nt2::polyval(p, (x-mu(1))/mu(2)), y, T(5.0)));

  //////////////////////////////////////////////////////
  // TODO This does not work s being a structure defined in polyfit.hpp
  //   nt2::polyfit_infos<T> s;
  //   nt2::tie(p, s) = polyfit(x, y);
  //   NT2_DISPLAY(p);
  //   NT2_DISPLAY(s.r);
  //   NT2_DISPLAY(s.df);
  //    NT2_DISPLAY(s.normr);
  //////////////////////////////////////////////////////

} // end of test for floating_

NT2_TEST_CASE_TPL ( polyfit_real__4_0,  NT2_REAL_TYPES)
{

  using nt2::polyfit;
  using nt2::tag::polyfit_;
  typedef std::complex<T> cT;
  cT a [] = {0.0, 1.0, 2.0, 3.0,  4.0,  5.0};
  cT b [] = {0.0, 0.8, 0.9, 0.1, -0.8, -1.0};
  cT c [] = {0.087037037037037245923, -0.81349206349206493183, 1.6931216931216956922, -0.039682539682541172199 };
  nt2::table<cT> x(nt2::of_size(1,6));
  nt2::table<cT> y(nt2::of_size(1,6));
  nt2::table<cT> zz(nt2::of_size(1,4)); //, c+0, c+4); DOESS NOT WORK TODO

 for(int i=0; i < 6; ++i)
   {
     x(i+1) = cT(a[i]);
     y(i+1) = cT(b[i]);
   }
 for(int i=0; i < 4; ++i)
   {
     zz(i+1) = cT(c[i]);
   }

  NT2_DISPLAY(x);
  NT2_DISPLAY(y);
  nt2::table<cT> z = polyfit(x, y, 3);
  NT2_DISPLAY(z);
  NT2_TEST_ULP_EQUAL(z, zz, 500);
} // end of test for floating_


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007.

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

// test_weibull.cpp

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/test/test_exec_monitor.hpp> // Boost.Test
#include <boost/test/floating_point_comparison.hpp>

#include <boost/math/distributions/weibull.hpp>
    using boost::math::weibull_distribution;
#include <boost/math/tools/test.hpp>

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;
#include <limits>
  using std::numeric_limits;

template <class RealType>
void check_weibull(RealType shape, RealType scale, RealType x, RealType p, RealType q, RealType tol)
{
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         weibull_distribution<RealType>(shape, scale),       // distribution.
         x),                                            // random variable.
         p,                                             // probability.
         tol);                                          // %tolerance.
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         complement(
            weibull_distribution<RealType>(shape, scale),    // distribution.
            x)),                                        // random variable.
         q,                                             // probability complement.
         tol);                                          // %tolerance.
   BOOST_CHECK_CLOSE(
      ::boost::math::quantile(
         weibull_distribution<RealType>(shape, scale),       // distribution.
         p),                                            // probability.
         x,                                             // random variable.
         tol);                                          // %tolerance.
   BOOST_CHECK_CLOSE(
      ::boost::math::quantile(
         complement(
            weibull_distribution<RealType>(shape, scale),    // distribution.
            q)),                                        // probability complement.
         x,                                             // random variable.
         tol);                                          // %tolerance.
}

template <class RealType>
void test_spots(RealType)
{
   // Basic sanity checks
   //
   // These test values were generated for the normal distribution
   // using the online calculator at
   // http://espse.ed.psu.edu/edpsych/faculty/rhale/hale/507Mat/statlets/free/pdist.htm
   //
   // Tolerance is just over 5 decimal digits expressed as a persentage:
   // that's the limit of the test data.
   RealType tolerance = 2e-5f * 100;
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;

   using std::exp;

   check_weibull(
      static_cast<RealType>(0.25),     // shape
      static_cast<RealType>(0.5),     // scale
      static_cast<RealType>(0.1),     // x
      static_cast<RealType>(0.487646),   // p
      static_cast<RealType>(1-0.487646),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.25),     // shape
      static_cast<RealType>(0.5),     // scale
      static_cast<RealType>(0.5),     // x
      static_cast<RealType>(1-0.367879),   // p
      static_cast<RealType>(0.367879),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.25),     // shape
      static_cast<RealType>(0.5),     // scale
      static_cast<RealType>(1),     // x
      static_cast<RealType>(1-0.304463),   // p
      static_cast<RealType>(0.304463),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.25),     // shape
      static_cast<RealType>(0.5),     // scale
      static_cast<RealType>(2),     // x
      static_cast<RealType>(1-0.243117),   // p
      static_cast<RealType>(0.243117),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.25),     // shape
      static_cast<RealType>(0.5),     // scale
      static_cast<RealType>(5),     // x
      static_cast<RealType>(1-0.168929),   // p
      static_cast<RealType>(0.168929),   // q
      tolerance);

   check_weibull(
      static_cast<RealType>(0.5),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(0.1),     // x
      static_cast<RealType>(0.200371),   // p
      static_cast<RealType>(1-0.200371),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.5),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(0.5),     // x
      static_cast<RealType>(0.393469),   // p
      static_cast<RealType>(1-0.393469),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.5),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(1),     // x
      static_cast<RealType>(1-0.493069),   // p
      static_cast<RealType>(0.493069),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.5),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(2),     // x
      static_cast<RealType>(1-0.367879),   // p
      static_cast<RealType>(0.367879),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(0.5),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(5),     // x
      static_cast<RealType>(1-0.205741),   // p
      static_cast<RealType>(0.205741),   // q
      tolerance);

   check_weibull(
      static_cast<RealType>(2),     // shape
      static_cast<RealType>(0.25),     // scale
      static_cast<RealType>(0.1),     // x
      static_cast<RealType>(0.147856),   // p
      static_cast<RealType>(1-0.147856),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(2),     // shape
      static_cast<RealType>(0.25),     // scale
      static_cast<RealType>(0.5),     // x
      static_cast<RealType>(1-0.018316),   // p
      static_cast<RealType>(0.018316),   // q
      tolerance);

   /*
   This test value came from
   http://espse.ed.psu.edu/edpsych/faculty/rhale/hale/507Mat/statlets/free/pdist.htm
   but appears to be grossly incorrect: certainly it does not agree with the values
   I get from pushing numbers into a calculator (0.0001249921878255106610615995196123).
   Strangely other test values generated for the same shape and scale parameters do look OK.
   check_weibull(
      static_cast<RealType>(3),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(0.1),     // x
      static_cast<RealType>(1.25E-40),   // p
      static_cast<RealType>(1-1.25E-40),   // q
      tolerance);
      */
   check_weibull(
      static_cast<RealType>(3),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(0.5),     // x
      static_cast<RealType>(0.015504),   // p
      static_cast<RealType>(1-0.015504),   // q
      tolerance * 10); // few digits in test value
   check_weibull(
      static_cast<RealType>(3),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(1),     // x
      static_cast<RealType>(0.117503),   // p
      static_cast<RealType>(1-0.117503),   // q
      tolerance);
   check_weibull(
      static_cast<RealType>(3),     // shape
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(2),     // x
      static_cast<RealType>(1-0.367879),   // p
      static_cast<RealType>(0.367879),   // q
      tolerance);

   //
   // Tests for PDF
   //
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(0.25, 0.5), static_cast<RealType>(0.1)),
      static_cast<RealType>(0.856579),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(0.25, 0.5), static_cast<RealType>(0.5)),
      static_cast<RealType>(0.183940),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(0.25, 0.5), static_cast<RealType>(5)),
      static_cast<RealType>(0.015020),
      tolerance * 10); // fewer digits in test value
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(0.5, 2), static_cast<RealType>(0.1)),
      static_cast<RealType>(0.894013),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(0.5, 2), static_cast<RealType>(0.5)),
      static_cast<RealType>(0.303265),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(0.5, 2), static_cast<RealType>(1)),
      static_cast<RealType>(0.174326),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(2, 0.25), static_cast<RealType>(0.1)),
      static_cast<RealType>(2.726860),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(2, 0.25), static_cast<RealType>(0.5)),
      static_cast<RealType>(0.293050),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(3, 2), static_cast<RealType>(1)),
      static_cast<RealType>(0.330936),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(weibull_distribution<RealType>(3, 2), static_cast<RealType>(2)),
      static_cast<RealType>(0.551819),
      tolerance);

   //
   // These test values were obtained using the formulas at
   // http://en.wikipedia.org/wiki/Weibull_distribution
   // which are subtly different to (though mathematically
   // the same as) the ones on the Mathworld site
   // http://mathworld.wolfram.com/WeibullDistribution.html
   // which are the ones used in the implementation.
   // The assumption is that if both computation methods
   // agree then the implementation is probably correct...
   // What's not clear is which method is more accurate.
   //
   tolerance = (std::max)(
      boost::math::tools::epsilon<RealType>(),
      static_cast<RealType>(boost::math::tools::epsilon<double>())) * 5 * 100; // 5 eps as a percentage
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;
   weibull_distribution<RealType> dist(2, 3);
   RealType x = static_cast<RealType>(0.125);

   BOOST_MATH_STD_USING // ADL of std lib math functions

   // mean:
   BOOST_CHECK_CLOSE(
      mean(dist)
      , dist.scale() * boost::math::tgamma(1 + 1 / dist.shape()), tolerance);
   // variance:
   BOOST_CHECK_CLOSE(
      variance(dist)
      , dist.scale() * dist.scale() * boost::math::tgamma(1 + 2 / dist.shape()) - mean(dist) * mean(dist), tolerance);
   // std deviation:
   BOOST_CHECK_CLOSE(
    standard_deviation(dist)
    , sqrt(variance(dist)), tolerance);
   // hazard:
   BOOST_CHECK_CLOSE(
    hazard(dist, x)
    , pdf(dist, x) / cdf(complement(dist, x)), tolerance);
   // cumulative hazard:
   BOOST_CHECK_CLOSE(
    chf(dist, x)
    , -log(cdf(complement(dist, x))), tolerance);
   // coefficient_of_variation:
   BOOST_CHECK_CLOSE(
    coefficient_of_variation(dist)
    , standard_deviation(dist) / mean(dist), tolerance);
   // mode:
   BOOST_CHECK_CLOSE(
    mode(dist)
    , dist.scale() * pow((dist.shape() - 1) / dist.shape(), 1/dist.shape()), tolerance);
   // median:
   BOOST_CHECK_CLOSE(
    median(dist)
    , dist.scale() * pow(log(static_cast<RealType>(2)), 1 / dist.shape()), tolerance);
   // skewness:
   BOOST_CHECK_CLOSE(
    skewness(dist),
    (boost::math::tgamma(1 + 3/dist.shape()) * pow(dist.scale(), RealType(3)) - 3 * mean(dist) * variance(dist) - pow(mean(dist), RealType(3))) / pow(standard_deviation(dist), RealType(3)),
    tolerance * 100);
   // kertosis:
   BOOST_CHECK_CLOSE(
    kurtosis(dist)
    , kurtosis_excess(dist) + 3, tolerance);
   // kertosis excess:
   BOOST_CHECK_CLOSE(
    kurtosis_excess(dist),
    (pow(dist.scale(), RealType(4)) * boost::math::tgamma(1 + 4/dist.shape())
         - 3 * variance(dist) * variance(dist)
         - 4 * skewness(dist) * variance(dist) * standard_deviation(dist) * mean(dist)
         - 6 * variance(dist) * mean(dist) * mean(dist)
         - pow(mean(dist), RealType(4))) / (variance(dist) * variance(dist)),
    tolerance * 1000);

   //
   // Special cases:
   //
   BOOST_CHECK(pdf(dist, 0) == 0);
   BOOST_CHECK(cdf(dist, 0) == 0);
   BOOST_CHECK(cdf(complement(dist, 0)) == 1);
   BOOST_CHECK(quantile(dist, 0) == 0);
   BOOST_CHECK(quantile(complement(dist, 1)) == 0);

   //
   // Error checks:
   //
   BOOST_CHECK_THROW(weibull_distribution<RealType>(0, -1), std::domain_error);
   BOOST_CHECK_THROW(weibull_distribution<RealType>(-1, 1), std::domain_error);
   BOOST_CHECK_THROW(pdf(dist, -1), std::domain_error);
   BOOST_CHECK_THROW(cdf(dist, -1), std::domain_error);
   BOOST_CHECK_THROW(cdf(complement(dist, -1)), std::domain_error);
   BOOST_CHECK_THROW(quantile(dist, 1), std::overflow_error);
   BOOST_CHECK_THROW(quantile(complement(dist, 0)), std::overflow_error);

} // template <class RealType>void test_spots(RealType)

int test_main(int, char* [])
{

  // Check that can construct weibull distribution using the two convenience methods:
  using namespace boost::math;
  weibull myw1(2); // Using typedef
   weibull_distribution<> myw2(2); // Using default RealType double.

    // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x0582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif

   return 0;
} // int test_main(int, char* [])

/*

Output:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_weibull.exe"
Running 1 test case...
Tolerance for type float is 0.002 %
Tolerance for type float is 5.96046e-005 %
Tolerance for type double is 0.002 %
Tolerance for type double is 1.11022e-013 %
Tolerance for type long double is 0.002 %
Tolerance for type long double is 1.11022e-013 %
Tolerance for type class boost::math::concepts::real_concept is 0.002 %
Tolerance for type class boost::math::concepts::real_concept is 1.11022e-013 %
*** No errors detected

*/


#include "segmatch/descriptors/eigenvalue_based.hpp"

#include <cfenv>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <glog/logging.h>
#include <pcl/common/common.h>

#pragma STDC FENV_ACCESS on

namespace segmatch {

/// \brief Utility function for swapping two values.
template<typename T>
bool swap_if_gt(T& a, T& b) {
  if (a > b) {
    std::swap(a, b);
    return true;
  }
  return false;
}

// EigenvalueBasedDescriptor methods definition
EigenvalueBasedDescriptor::EigenvalueBasedDescriptor(const DescriptorsParameters& parameters) {}

void EigenvalueBasedDescriptor::describe(const Segment& segment, Features* features) {
  CHECK_NOTNULL(features);
  std::feclearexcept(FE_ALL_EXCEPT);

  // Find the variances.
  const size_t kNPoints = segment.point_cloud.points.size();
  PointCloud variances;
  for (size_t i = 0u; i < kNPoints; ++i) {
    variances.push_back(PclPoint());
    variances.points[i].x = segment.point_cloud.points[i].x - segment.centroid.x;
    variances.points[i].y = segment.point_cloud.points[i].y - segment.centroid.y;
    variances.points[i].z = segment.point_cloud.points[i].z - segment.centroid.z;
  }

  // Find the covariance matrix. Since it is symmetric, we only bother with the upper diagonal.
  const std::vector<size_t> row_indices_to_access = {0,0,0,1,1,2};
  const std::vector<size_t> col_indices_to_access = {0,1,2,1,2,2};
  Eigen::Matrix3f covariance_matrix;
  for (size_t i = 0u; i < row_indices_to_access.size(); ++i) {
    const size_t row = row_indices_to_access[i];
    const size_t col = col_indices_to_access[i];
    double covariance = 0;
    for (size_t k = 0u; k < kNPoints; ++k) {
      covariance += variances.points[k].data[row] * variances.points[k].data[col];
    }
    covariance /= kNPoints;
    covariance_matrix(row,col) = covariance;
    covariance_matrix(col,row) = covariance;
  }

  // Compute eigenvalues of covariance matrix.
  constexpr bool compute_eigenvectors = false;
  Eigen::EigenSolver<Eigen::Matrix3f> eigenvalues_solver(covariance_matrix, compute_eigenvectors);
  std::vector<float> eigenvalues(3, 0.0);
  eigenvalues.at(0) = eigenvalues_solver.eigenvalues()[0].real();
  eigenvalues.at(1) = eigenvalues_solver.eigenvalues()[1].real();
  eigenvalues.at(2) = eigenvalues_solver.eigenvalues()[2].real();
  if (eigenvalues_solver.eigenvalues()[0].imag() != 0.0 ||
      eigenvalues_solver.eigenvalues()[1].imag() != 0.0 ||
      eigenvalues_solver.eigenvalues()[2].imag() != 0.0 ) {
    LOG(ERROR) << "Eigenvalues should not have non-zero imaginary component.";
  }

  // Sort eigenvalues from smallest to largest.
  swap_if_gt(eigenvalues.at(0), eigenvalues.at(1));
  swap_if_gt(eigenvalues.at(0), eigenvalues.at(2));
  swap_if_gt(eigenvalues.at(1), eigenvalues.at(2));

  // Normalize eigenvalues.
  double sum_eigenvalues = eigenvalues.at(0) + eigenvalues.at(1) + eigenvalues.at(2);
  double e1 = eigenvalues.at(0) / sum_eigenvalues;
  double e2 = eigenvalues.at(1) / sum_eigenvalues;
  double e3 = eigenvalues.at(2) / sum_eigenvalues;
  LOG_IF(ERROR, e1 == e2 || e2 == e3 || e1 == e3) << "Eigenvalues should not be equal.";

  // Store inside features.
  const double sum_of_eigenvalues = e1 + e2 + e3;
  constexpr double kOneThird = 1.0/3.0;
  CHECK_NE(e1, 0.0);
  CHECK_NE(sum_of_eigenvalues, 0.0);

  const double kNormalizationPercentile = 1.0;

  const double kLinearityMax = 28890.9 * kNormalizationPercentile;
  const double kPlanarityMax = 95919.2 * kNormalizationPercentile;
  const double kScatteringMax = 124811 * kNormalizationPercentile;
  const double kOmnivarianceMax = 0.278636 * kNormalizationPercentile;
  const double kAnisotropyMax = 124810 * kNormalizationPercentile;
  const double kEigenEntropyMax = 0.956129 * kNormalizationPercentile;
  const double kChangeOfCurvatureMax = 0.99702 * kNormalizationPercentile;

  const double kNPointsMax = 13200 * kNormalizationPercentile;

  Feature eigenvalue_feature;
  eigenvalue_feature.push_back(FeatureValue("linearity", (e1 - e2) / e1 / kLinearityMax));
  eigenvalue_feature.push_back(FeatureValue("planarity", (e2 - e3) / e1 / kPlanarityMax));
  eigenvalue_feature.push_back(FeatureValue("scattering", e3 / e1 / kScatteringMax));
  eigenvalue_feature.push_back(FeatureValue("omnivariance", std::pow(e1 * e2 * e3, kOneThird) / kOmnivarianceMax));
  eigenvalue_feature.push_back(FeatureValue("anisotropy", (e1 - e3) / e1 / kAnisotropyMax));
  eigenvalue_feature.push_back(FeatureValue("eigen_entropy",
                                            (e1 * std::log(e1)) + (e2 * std::log(e2)) + (e3 * std::log(e3)) / kEigenEntropyMax));
  eigenvalue_feature.push_back(FeatureValue("change_of_curvature", e3 / sum_of_eigenvalues / kChangeOfCurvatureMax));

  PointI point_min, point_max;

  pcl::getMinMax3D(segment.point_cloud, point_min, point_max);

  double diff_x, diff_y, diff_z;

  diff_x = point_max.x - point_min.x;
  diff_y = point_max.y - point_min.y;
  diff_z = point_max.z - point_min.z;

  if (diff_z < diff_x && diff_z < diff_y) {
    eigenvalue_feature.push_back(FeatureValue("pointing_up", 0.2));
  } else {
    eigenvalue_feature.push_back(FeatureValue("pointing_up", 0.0));
  }

  // eigenvalue_feature.push_back(FeatureValue("n_points", kNPoints / kNPointsMax));

  CHECK_EQ(eigenvalue_feature.size(), kDimension) << "Feature has the wrong dimension";
  features->push_back(eigenvalue_feature);

  // Check that there were no overflows, underflows, or invalid float operations.
  if (std::fetestexcept(FE_OVERFLOW)) {
    LOG(ERROR) << "Overflow error in eigenvalue feature computation.";
  } else if (std::fetestexcept(FE_UNDERFLOW)) {
    LOG(ERROR) << "Underflow error in eigenvalue feature computation.";
  } else if (std::fetestexcept(FE_INVALID)) {
    LOG(ERROR) << "Invalid Flag error in eigenvalue feature computation.";
  } else if (std::fetestexcept(FE_DIVBYZERO)) {
    LOG(ERROR) << "Divide by zero error in eigenvalue feature computation.";
  }
}

} // namespace segmatch


#include <Eigen/Dense>

#include "benchmark.hpp"

int main() {
  // Array - Blas
  for (size_t k = 10; k < 1000; k+=10) {
    size_t m = k;
    double *A = create_random_sq_matrix(m);
    double *B = create_random_sq_matrix(m);
    double *C = (double *) malloc(sizeof(double) * m * m);

    sleep(0.1);
    Eigen::MatrixXd A_;
    Eigen::MatrixXd B_;
    Eigen::MatrixXd C_;
    A_.resize(m, m);
    B_.resize(m, m);
    C_.resize(m, m);

    size_t index = 0;
    for (size_t i = 0; i < m; i++) {
      for (size_t j = 0; j < m; j++) {
        A_(i, j) = A[index];
        B_(i, j) = B[index];
        index++;
      }
    }

    struct timespec t = tic();
    C_ = A_ * B_;
    printf("matrix_size: %ld\teigen matmul: %fs\n", m, toc(&t));

    free(A);
    free(B);
    free(C);
  }

  return 0;
}