///////////////////////////////////////////////////////////////////////////////
// detail::fusion::example::joint_view_bind_range.cpp                  		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/range.hpp>
#include <boost/array.hpp>
#include <boost/multi_array.hpp>
#include <boost/fusion/include/pair.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/make_map.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assign/list_inserter.hpp>

#include <boost/iterator/flatten_iterator.hpp>

#include <boost/statistics/detail/fusion/joint_view/flatten_bind_range.hpp>
#include <libs/statistics/detail/fusion/example/joint_view_flatten_bind_range.h>

void example_joint_view_flatten_bind_range(std::ostream&){

	namespace jv = boost::statistics::detail::fusion::joint_view;

	typedef int val_;
    typedef boost::mpl::int_<0> 					k0_;
    typedef boost::mpl::int_<1> 					k1_;
	const int n = 2;

	typedef boost::fusion::pair<k0_,val_> 			p0_;
    typedef boost::fusion::map<p0_>					m0_;
	typedef boost::fusion::pair<k1_,val_> 			p1_;
    typedef boost::fusion::map<p1_>					m1_;

    typedef boost::array<m0_,n>						ar0_;
    typedef boost::range_iterator<ar0_>::type 		it0_;
	typedef boost::multi_array<m1_, 2> 				ma1_;
    typedef boost::range_iterator<ma1_>::type 		it1_;
	typedef boost::multi_array_types::index_range 	idx_range_;
	typedef boost::multi_array_types::index 		idx_;

	typedef boost::array_view_gen<ma1_,2>::type 	view2_ma1_;

    typedef jv::result_of::flatten_bind_range<it0_,it1_> meta_;
    typedef meta_::type 							flat_r_;
	typedef boost::range_iterator<flat_r_>::type	flat_it_;

	ar0_ ar0 = boost::assign::list_of(
    	boost::fusion::make_map<k0_>(1)
    )(
      	boost::fusion::make_map<k0_>(2)
    );

	ma1_ ma1(boost::extents[n][2]);
    {
        std::vector<m1_> vals1;
    	using namespace boost::assign;
        vals1 = list_of(
        	boost::fusion::make_map<k1_>(1)
        )(
        	boost::fusion::make_map<k1_>(2)
        )(
        	boost::fusion::make_map<k1_>(3)
        )(
        	boost::fusion::make_map<k1_>(4)
        );
        ma1.assign(boost::begin(vals1),boost::end(vals1));
	}

	flat_r_ flat_r = jv::flatten_bind_range(
    	boost::begin(ar0),
        boost::end(ar0),
    	boost::begin(ma1),
        boost::end(ma1)
    );

	flat_it_ flat_b = boost::begin(flat_r);
	flat_it_ flat_e = boost::end(flat_r);

	// BUG : post increment causes runtime error.
	while(flat_b!=flat_e){
    	std::cout
        	<< '('
            << boost::fusion::at_key<k0_>(*flat_b)
        	<< ','
            << boost::fusion::at_key<k1_>(*flat_b)
        	<< ')'
            << std::endl;
        ++flat_b;
    }

}


#ifndef PYBNESIAN_UTIL_COMBINATIONS_HPP
#define PYBNESIAN_UTIL_COMBINATIONS_HPP

#include <vector>
#include <unordered_set>
#include <boost/math/special_functions/binomial.hpp>
#include <util/parameter_traits.hpp>

namespace util {

template <typename T>
class Combinations {
public:
    Combinations() = default;

    // TODO: Check m_k > fixed.size().
    template <typename Iter, util::enable_if_iterator_t<Iter, int> = 0>
    Combinations(Iter begin, Iter end, int k)
        : m_elements(begin, end),
          m_fixed(),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {}

    template <typename Iter,
              typename IterFixed,
              util::enable_if_iterator_t<Iter, int> = 0,
              util::enable_if_iterator_t<IterFixed, int> = 0>
    Combinations(Iter begin, Iter end, IterFixed begin_fixed, IterFixed end_fixed, int k)
        : m_elements(begin, end),
          m_fixed(begin_fixed, end_fixed),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {
        static_assert(std::is_same_v<typename std::iterator_traits<Iter>::value_type,
                                     typename std::iterator_traits<IterFixed>::value_type>,
                      "The type of fixed and movable elements should be the same.");
    }

    template <typename V, util::enable_if_vector_of_type_t<V, T, int> = 0>
    Combinations(V elements, int k)
        : m_elements(elements),
          m_fixed(),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {}

    template <typename V,
              typename V2,
              util::enable_if_vector_of_type_t<V, T, int> = 0,
              util::enable_if_vector_of_type_t<V2, T, int> = 0>
    Combinations(V elements, V2 fixed, int k)
        : m_elements(elements),
          m_fixed(fixed),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {}

    class combination_iterator {
    public:
        using iterator_category = std::input_iterator_tag;
        using value_type = std::vector<T>;
        using difference_type = int;
        using pointer = std::vector<T>*;
        using reference = std::vector<T>&;

        combination_iterator() = default;

        combination_iterator(const Combinations<T>* self, int idx) : m_self(self), m_subset(), m_indices(), m_idx(idx) {
            m_subset.reserve(m_self->m_k);

            for (size_t i = 0; i < m_self->m_fixed.size(); ++i) {
                m_subset.push_back(m_self->m_fixed[i]);
            }

            auto p = m_self->m_k - m_self->m_fixed.size();
            m_indices.reserve(p);

            for (size_t i = 0; i < p; ++i) {
                m_subset.push_back(m_self->m_elements[i]);
                m_indices.push_back(i);
            }
        }

        combination_iterator(const combination_iterator& other)
            : m_self(other.m_self), m_subset(other.m_subset), m_indices(other.m_indices), m_idx(other.m_idx) {}

        combination_iterator& operator=(const combination_iterator& other) {
            m_self = other.m_self;
            m_subset = other.m_subset;
            m_indices = other.m_indices;
            m_idx = other.m_idx;
            return *this;
        }

        void next_subset() {
            int offset = m_self->m_fixed.size();
            int p = m_self->m_k - offset;

            for (int i = p - 1; i >= 0; --i) {
                auto k = i + offset;
                auto max_index = m_self->m_elements.size() - p + i;

                if (m_indices[i] < max_index) {
                    ++m_indices[i];
                    m_subset[k] = m_self->m_elements[m_indices[i]];

                    for (int j = i + 1; j < p; ++j) {
                        m_indices[j] = m_indices[j - 1] + 1;
                        m_subset[j + offset] = m_self->m_elements[m_indices[j]];
                    }

                    break;
                }
            }
        }

        combination_iterator& operator++() {
            ++m_idx;
            next_subset();
            return *this;
        }

        combination_iterator operator++(int) {
            combination_iterator return_it(*this);
            ++m_idx;
            next_subset();
            return return_it;
        }

        reference operator*() { return m_subset; }

        pointer operator->() { return &m_subset; }

        bool operator==(const combination_iterator& rhs) { return (m_idx == rhs.m_idx) && (m_self == rhs.m_self); }

        bool operator!=(const combination_iterator& rhs) { return !(*this == rhs); }

    private:
        const Combinations<T>* m_self;
        std::vector<T> m_subset;
        std::vector<size_t> m_indices;
        int m_idx;
    };

    combination_iterator begin() const { return combination_iterator(this, 0); }
    combination_iterator end() const { return combination_iterator(this, num_combinations()); }

    int num_combinations() const { return m_num_combinations; }

private:
    std::vector<T> m_elements;
    std::vector<T> m_fixed;
    int m_k;
    int m_num_combinations;
};

template <typename Iter>
Combinations(Iter, Iter, int) -> Combinations<typename std::iterator_traits<Iter>::value_type>;
template <typename Iter, typename IterFixed>
Combinations(Iter, Iter, IterFixed, IterFixed, int) -> Combinations<typename std::iterator_traits<Iter>::value_type>;
template <typename V>
Combinations(V, int) -> Combinations<typename V::value_type>;
template <typename V, typename V2>
Combinations(V, V2, int) -> Combinations<typename V::value_type>;

template <typename T>
class Combinations2Sets {
public:
    template <typename Iter,
              typename Iter2,
              util::enable_if_iterator_t<Iter, int> = 0,
              util::enable_if_iterator_t<Iter2, int> = 0>
    Combinations2Sets(Iter begin_set1, Iter end_set1, Iter2 begin_set2, Iter2 end_set2, int k)
        : Combinations2Sets(std::vector(begin_set1, end_set1), std::vector(begin_set2, end_set2), k) {
        static_assert(std::is_same_v<typename std::iterator_traits<Iter>::value_type,
                                     typename std::iterator_traits<Iter2>::value_type>,
                      "The elements of both sets should be of the same type");
    }

    template <typename V,
              typename V2,
              util::enable_if_vector_of_type_t<V, T, int> = 0,
              util::enable_if_vector_of_type_t<V2, T, int> = 0>
    Combinations2Sets(V v1, V2 v2, int k)
        : m_comb1(), m_comb2(), m_comb2_valid_combinations(), m_num_combinations(-1), m_k(k) {
        std::sort(v1.begin(), v1.end());
        std::sort(v2.begin(), v2.end());

        std::unordered_set<T> common_elements;
        std::set_intersection(
            v1.begin(), v1.end(), v2.begin(), v2.end(), std::inserter(common_elements, common_elements.end()));

        m_comb1 = Combinations<T>(std::move(v1), m_k);
        if (static_cast<int>(common_elements.size()) < k) {
            m_comb2 = Combinations<T>(std::move(v2), m_k);
            m_comb2_valid_combinations = m_comb2.num_combinations();
        } else {
            for (size_t i = 0, common_start = v2.size() - common_elements.size(); i < common_start; ++i) {
                if (common_elements.count(v2[i]) > 0) {
                    for (size_t j = v2.size() - 1; j >= common_start; --j) {
                        if (common_elements.count(v2[j]) == 0) {
                            std::swap(v2[i], v2[j]);
                        }
                    }
                }
            }

            m_comb2 = Combinations<T>(std::move(v2), m_k);
            m_comb2_valid_combinations =
                m_comb2.num_combinations() -
                std::round(boost::math::binomial_coefficient<double>(common_elements.size(), m_k));
        }

        m_num_combinations = m_comb1.num_combinations() + m_comb2_valid_combinations;
    }

    class combination2set_iterator {
    public:
        using iterator_category = std::input_iterator_tag;
        using value_type = std::vector<T>;
        using difference_type = int;
        using pointer = std::vector<T>*;
        using reference = std::vector<T>&;

        combination2set_iterator(const Combinations2Sets<T>& self, int idx) : m_self(self), it() {
            if (idx < m_self.m_comb1.num_combinations() || m_self.m_comb2_valid_combinations == 0) {
                it = typename Combinations<T>::combination_iterator(&m_self.m_comb1, idx);
            } else {
                it = typename Combinations<T>::combination_iterator(&m_self.m_comb2,
                                                                    idx - m_self.m_comb1.num_combinations());
            }
        }

        combination2set_iterator& operator++() {
            ++it;
            if (it == m_self.m_comb1.end() && m_self.m_comb2_valid_combinations > 0) {
                it = m_self.m_comb2.begin();
            }
            return *this;
        }

        combination2set_iterator operator++(int) {
            combination2set_iterator return_it(*this);
            ++it;
            if (it == m_self.m_comb1.end() && m_self.m_comb2_valid_combinations > 0) {
                it = m_self.m_comb2.begin();
            }
            return return_it;
        }

        reference operator*() { return *it; }

        pointer operator->() { return &it.m_subset; }

        bool operator==(const combination2set_iterator& rhs) { return it == rhs.it; }

        bool operator!=(const combination2set_iterator& rhs) { return !(*this == rhs); }

    private:
        const Combinations2Sets<T>& m_self;
        typename Combinations<T>::combination_iterator it;
    };

    combination2set_iterator begin() { return combination2set_iterator(*this, 0); }
    combination2set_iterator end() { return combination2set_iterator(*this, num_combinations()); }

    int num_combinations() const { return m_num_combinations; }

private:
    Combinations<T> m_comb1;
    Combinations<T> m_comb2;
    int m_comb2_valid_combinations;
    int m_num_combinations;
    int m_k;
};

template <typename Iter, typename Iter2>
Combinations2Sets(Iter, Iter, Iter2, Iter2, int) -> Combinations2Sets<typename std::iterator_traits<Iter>::value_type>;
template <typename V, typename V2>
Combinations2Sets(V, V2, int) -> Combinations2Sets<typename V::value_type>;

template <typename T>
class AllSubsets {
public:
    AllSubsets() = default;

    template <typename Iter, util::enable_if_iterator_t<Iter, int> = 0>
    AllSubsets(Iter begin, Iter end, int min_k, int max_k)
        : m_elements(begin, end), m_fixed(), m_min_k(min_k), m_max_k(max_k), m_num_combinations(0) {
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations += std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i));
        }
    }

    template <typename Iter,
              typename IterFixed,
              util::enable_if_iterator_t<Iter, int> = 0,
              util::enable_if_iterator_t<IterFixed, int> = 0>
    AllSubsets(Iter begin, Iter end, IterFixed begin_fixed, IterFixed end_fixed, int min_k, int max_k)
        : m_elements(begin, end),
          m_fixed(begin_fixed, end_fixed),
          m_min_k(min_k),
          m_max_k(max_k),
          m_num_combinations(0) {
        static_assert(std::is_same_v<typename std::iterator_traits<Iter>::value_type,
                                     typename std::iterator_traits<IterFixed>::value_type>,
                      "The type of fixed and movable elements should be the same.");
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations +=
                std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i - m_fixed.size()));
        }
    }

    template <typename V, util::enable_if_vector_of_type_t<V, T, int> = 0>
    AllSubsets(V elements, int min_k, int max_k)
        : m_elements(elements), m_min_k(min_k), m_max_k(max_k), m_num_combinations(0) {
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations += std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i));
        }
    }

    template <typename V,
              typename V2,
              util::enable_if_vector_of_type_t<V, T, int> = 0,
              util::enable_if_vector_of_type_t<V2, T, int> = 0>
    AllSubsets(V elements, V2 fixed, int min_k, int max_k)
        : m_elements(elements), m_fixed(fixed), m_min_k(min_k), m_max_k(max_k), m_num_combinations(0) {
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations +=
                std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i - m_fixed.size()));
        }
    }

    class allsubsets_iterator {
    public:
        using iterator_category = std::input_iterator_tag;
        using value_type = std::vector<T>;
        using difference_type = int;
        using pointer = std::vector<T>*;
        using reference = std::vector<T>&;

        allsubsets_iterator() = default;

        allsubsets_iterator(const AllSubsets<T>* self, int idx)
            : m_self(self), m_idx(idx), m_current_comb(), m_current_iter(), m_current_k(m_self->m_min_k) {
            if (idx == 0) {
                m_current_comb = Combinations(m_self->m_elements.begin(),
                                              m_self->m_elements.end(),
                                              m_self->m_fixed.begin(),
                                              m_self->m_fixed.end(),
                                              m_current_k);
                m_current_iter = m_current_comb.begin();
            }
        }

        allsubsets_iterator& operator=(const allsubsets_iterator& other) {
            m_self = other.m_self;
            m_idx = other.m_idx;
            m_current_comb = other.m_current_comb;
            m_current_iter = other.m_current_iter;
            m_current_k = other.m_current_k;
            return *this;
        }

        allsubsets_iterator& operator++() {
            ++m_current_iter;
            ++m_idx;

            if (m_current_iter == m_current_comb.end() && m_current_k < m_self->m_max_k) {
                ++m_current_k;
                m_current_comb = Combinations(m_self->m_elements.begin(),
                                              m_self->m_elements.end(),
                                              m_self->m_fixed.begin(),
                                              m_self->m_fixed.end(),
                                              m_current_k);
                m_current_iter = m_current_comb.begin();
            }

            return *this;
        }

        allsubsets_iterator operator++(int) {
            allsubsets_iterator return_it(*this);

            ++m_current_iter;
            ++m_idx;
            if (m_current_iter == m_current_comb.end() && m_current_k < m_self->m_max_k) {
                ++m_current_k;
                m_current_comb = Combinations(m_self->m_elements.begin(),
                                              m_self->m_elements.end(),
                                              m_self->m_fixed.begin(),
                                              m_self->m_fixed.end(),
                                              m_current_k);
                m_current_iter = m_current_comb.begin();
            }

            return return_it;
        }

        reference operator*() { return *m_current_iter; }

        pointer operator->() { return &m_current_iter.m_subset; }

        bool operator==(const allsubsets_iterator& rhs) { return (m_idx == rhs.m_idx) && (m_self == rhs.m_self); }

        bool operator!=(const allsubsets_iterator& rhs) { return !(*this == rhs); }

    private:
        const AllSubsets<T>* m_self;
        int m_idx;
        Combinations<T> m_current_comb;
        typename Combinations<T>::combination_iterator m_current_iter;
        int m_current_k;
    };

    allsubsets_iterator begin() const { return allsubsets_iterator(this, 0); }
    allsubsets_iterator end() const { return allsubsets_iterator(this, num_combinations()); }

    int num_combinations() const { return m_num_combinations; }

private:
    std::vector<T> m_elements;
    std::vector<T> m_fixed;
    int m_min_k;
    int m_max_k;
    int m_num_combinations;
};

template <typename Iter>
AllSubsets(Iter, Iter, int, int) -> AllSubsets<typename std::iterator_traits<Iter>::value_type>;
template <typename Iter, typename IterFixed>
AllSubsets(Iter, Iter, IterFixed, IterFixed, int, int) -> AllSubsets<typename std::iterator_traits<Iter>::value_type>;
template <typename V>
AllSubsets(V, int, int) -> AllSubsets<typename V::value_type>;
template <typename V, typename V2>
AllSubsets(V, V2, int, int) -> AllSubsets<typename V::value_type>;

}  // namespace util

#endif  // PYBNESIAN_UTIL_COMBINATIONS_HPP


#include "inverter.hpp"
#include <Eigen/LU>

Eigen::MatrixXd Inverter::getInverse(const Eigen::MatrixXd &M) {
  return M.inverse();
}

Eigen::MatrixXd Inverter::getInverse(const Eigen::MatrixXd &M, double offset) {
  return M.inverse().array() + offset;
}

std::vector<Eigen::MatrixXd> Inverter::getInverseList(std::vector<Eigen::MatrixXd> matrices) {
  std::vector<Eigen::MatrixXd> result;
  result.reserve(matrices.size());
  for (std::vector<Eigen::MatrixXd>::iterator mat = matrices.begin(); mat != matrices.end(); ++mat) {
    result.push_back(mat->inverse());
  }
  return result;
}

Eigen::MatrixXd Inverter::getInverseRef(const Eigen::Ref<const Eigen::MatrixXd> & M) {
  return M.inverse();
}


//
// Copyright (c) 2018 CNRS
// Authors: Pierre Fernbach
//
// This file is part of bezier_COM_traj
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#define BOOST_TEST_MODULE transition
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include "test_helper.hh"
#include <ndcurves/bezier_curve.h>

using namespace bezier_com_traj;
const double T = 1.5;

ProblemData buildPData(
    const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
  ProblemData pData;
  pData.c0_ = Vector3(0, 0.5, 5.);
  pData.c1_ = Vector3(2, -0.5, 5.);
  pData.dc0_ = Vector3::Zero();
  pData.dc1_ = Vector3::Zero();
  pData.ddc0_ = Vector3::Zero();
  pData.ddc1_ = Vector3::Zero();
  pData.constraints_.flag_ = INIT_POS | INIT_VEL | END_VEL | END_POS;

  MatrixX3 normals(2, 3), positions(2, 3);
  normals.block<1, 3>(0, 0) = Vector3(0, 0, 1);
  positions.block<1, 3>(0, 0) = Vector3(0, 0.1, 0);
  normals.block<1, 3>(1, 0) = Vector3(0, 0, 1);
  positions.block<1, 3>(1, 0) = Vector3(0, -0.1, 0);
  std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
  pData.contacts_.push_back(
      new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first, contacts.second, algo)));

  return pData;
}

std::vector<point_t> generate_wps() { return computeConstantWaypoints(buildPData(), T); }

bezier_wp_t::t_point_t generate_wps_symbolic() { return computeConstantWaypointsSymbolic(buildPData(), T); }

VectorX eval(const waypoint_t& w, const point_t& x) { return w.first * x + w.second; }

void vectorEqual(const VectorX& a, const VectorX& b, const double EPS = 1e-14) {
  BOOST_CHECK_EQUAL(a.size(), b.size());
  BOOST_CHECK((a - b).norm() < EPS);
}

BOOST_AUTO_TEST_SUITE(symbolic)

BOOST_AUTO_TEST_CASE(symbolic_eval_c) {
  std::vector<point_t> pts = generate_wps();
  bezier_wp_t::t_point_t wps = generate_wps_symbolic();
  point_t y(1, 0.2, 4.5);
  pts[2] = y;

  bezier_t c(pts.begin(), pts.end(), 0., T);
  bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T);

  double t = 0.;
  while (t < T) {
    vectorEqual(c(t), eval(c_sym(t), y));
    t += 0.01;
  }
}

BOOST_AUTO_TEST_CASE(symbolic_eval_dc) {
  std::vector<point_t> pts = generate_wps();
  bezier_wp_t::t_point_t wps = generate_wps_symbolic();
  point_t y(1, 0.2, 4.5);
  pts[2] = y;

  bezier_t c(pts.begin(), pts.end(), 0., T);
  bezier_t dc = c.compute_derivate(1);
  bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T);
  bezier_wp_t dc_sym = c_sym.compute_derivate(1);

  double t = 0.;
  while (t < T) {
    vectorEqual(dc(t), eval(dc_sym(t), y));
    t += 0.01;
  }
}

BOOST_AUTO_TEST_CASE(symbolic_eval_ddc) {
  std::vector<point_t> pts = generate_wps();
  bezier_wp_t::t_point_t wps = generate_wps_symbolic();
  point_t y(1, 0.2, 4.5);
  pts[2] = y;

  bezier_t c(pts.begin(), pts.end(), 0., T);
  bezier_t ddc = c.compute_derivate(2);
  bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T);
  bezier_wp_t ddc_sym = c_sym.compute_derivate(2);

  double t = 0.;
  while (t < T) {
    vectorEqual(ddc(t), eval(ddc_sym(t), y), 1e-10);
    t += 0.01;
  }
}

BOOST_AUTO_TEST_CASE(symbolic_eval_jc) {
  std::vector<point_t> pts = generate_wps();
  bezier_wp_t::t_point_t wps = generate_wps_symbolic();
  point_t y(1, 0.2, 4.5);
  pts[2] = y;

  bezier_t c(pts.begin(), pts.end(), 0., T);
  bezier_t jc = c.compute_derivate(3);
  bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T);
  bezier_wp_t jc_sym = c_sym.compute_derivate(3);

  double t = 0.;
  while (t < T) {
    vectorEqual(jc(t), eval(jc_sym(t), y), 1e-10);
    t += 0.01;
  }
}

BOOST_AUTO_TEST_CASE(symbolic_split_c) {
  std::vector<point_t> pts = generate_wps();
  bezier_wp_t::t_point_t wps = generate_wps_symbolic();
  point_t y(1, 0.2, 4.5);
  pts[2] = y;

  bezier_t c(pts.begin(), pts.end(), 0., T);
  bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T);

  double a, b, t, t1, t2;
  for (size_t i = 0; i < 100; ++i) {
    a = (rand() / (double)RAND_MAX) * T;
    b = (rand() / (double)RAND_MAX) * T;
    t1 = std::min(a, b);
    t2 = std::max(a, b);
    // std::cout<<"try extract between : ["<<t1<<";"<<t2<<"] "<<std::endl;
    bezier_t c_e = c.extract(t1, t2);
    bezier_wp_t c_sym_e = c_sym.extract(t1, t2);
    t = t1;
    while (t < t2) {
      vectorEqual(c_e(t), eval(c_sym_e(t), y));
      vectorEqual(c(t), eval(c_sym_e(t), y));
      t += 0.01;
    }
  }
}

BOOST_AUTO_TEST_CASE(symbolic_split_c_bench) {
  using namespace std;

  std::vector<point_t> pts = generate_wps();
  bezier_wp_t::t_point_t wps = generate_wps_symbolic();
  point_t y(1, 0.2, 4.5);
  pts[2] = y;

  bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T);

  std::vector<double> values;
  for (int i = 0; i < 100000; ++i) values.push_back((double)rand() / RAND_MAX);

  clock_t s0, e0;
  std::pair<bezier_wp_t, bezier_wp_t> splitted = c_sym.split(0.5);
  s0 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    splitted = c_sym.split(*cit);
  }
  e0 = clock();

  std::cout << "Time required to split a c curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
            << std::endl;
}

BOOST_AUTO_TEST_CASE(symbolic_split_w) {
  bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(), T);
  point_t y(1, 0.2, 4.5);

  bezier_wp_t w(wps.begin(), wps.end(), 0., T);

  double a, b, t, t1, t2;
  for (size_t i = 0; i < 100; ++i) {
    a = (rand() / (double)RAND_MAX) * T;
    b = (rand() / (double)RAND_MAX) * T;
    t1 = std::min(a, b);
    t2 = std::max(a, b);
    // std::cout<<"try extract between : ["<<t1<<";"<<t2<<"] "<<std::endl;
    bezier_wp_t w_e = w.extract(t1, t2);
    t = t1;
    while (t < t2) {
      vectorEqual(eval(w(t), y), eval(w_e(t), y), 1e-12);
      t += 0.01;
    }
  }
}

BOOST_AUTO_TEST_CASE(symbolic_split_w_bench) {
  bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(), T);
  point_t y(1, 0.2, 4.5);

  bezier_wp_t w(wps.begin(), wps.end(), 0., T);

  std::vector<double> values;
  for (int i = 0; i < 100000; ++i) values.push_back((double)rand() / RAND_MAX);

  clock_t s0, e0;
  std::pair<bezier_wp_t, bezier_wp_t> splitted = w.split(0.5);
  s0 = clock();
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
    splitted = w.split(*cit);
  }
  e0 = clock();

  std::cout << "Time required to split a w curve : " << ((double)(e0 - s0) / CLOCKS_PER_SEC) / 100. << " ms "
            << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


/* vim: set ts=2 sts=2 et sw=2 tw=80: */
/**
 * matrix_matrix.cc
 *
 * Author: Jeff Hajewski
 * Created: 7/27/20127
 *
 * Matrix-matrix multiplication benchmarks
 */
#include <cmath>
#include <iostream>
#include <vector>

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

#include <Eigen/Dense>

#include "matrix_matrix.hpp"
#include "results.hpp"
#include "timer.hpp"
#include "utility.hpp"

#define MAX_ROW_SIZE 10000
#define MAX_COL_SIZE 10000
#define N_MEAN 100
#define N_STD 23.5
#define TOL 0.00001

namespace ublas = boost::numeric::ublas;

ublas::matrix<double> create_ublas_matrix(const size_t rows, const size_t cols) {
  ublas::matrix<double> m(rows, cols);
  for (size_t i = 0; i < rows; ++i) {
    for (size_t j = 0; j < cols; ++j) {
      m(i, j) = random_double(N_MEAN, N_STD);
    }
  }
  return m;
}

Eigen::MatrixXd create_eigen_matrix(const size_t rows, const size_t cols) {
  Eigen::MatrixXd m(rows, cols);
  for (size_t i = 0; i < rows; ++i) {
    for (size_t j = 0; j < cols; ++j) {
      m(i, j) = random_double(N_MEAN, N_STD);
    }
  }
  return m;
}

BenchmarkResults run_matrix_matrix_benchmark(size_t N) {
  BenchmarkResults results = BenchmarkResults(N);
  auto timer = Timer();

  const size_t rows = 10;
  const size_t cols = 10;
  for (size_t i = 0; i < N; ++i) {

    // Create matrices
    auto A_eigen = create_eigen_matrix(rows, cols);
    auto B_eigen = create_eigen_matrix(rows, cols);
    auto A_ublas = create_ublas_matrix(rows, cols);
    auto B_ublas = create_ublas_matrix(rows, cols);

    // Benchmark
    double unused;
    timer.start();
    auto C_eigen = A_eigen * B_eigen;
    unused = C_eigen(0, 0);
    timer.stop();
    auto elapsed = timer.elapsed_time();
    std::cout <<"elapsed: " << elapsed << '\n';
    results.push_back(BenchmarkType::Eigen, timer.elapsed_time());

    timer.start();
    auto C_ublas = ublas::prod(A_ublas, B_ublas);
    unused = C_ublas(0, 0);
    timer.stop();
    results.push_back(BenchmarkType::uBLAS, timer.elapsed_time());
  }
  return results;
}


#include <cstdlib>
#include <dlib/optimization/max_cost_assignment.h>
#include "pairing.h"

using namespace std;

double get_rate(const Route &passenger, const Route &driver) {

  double rate = passenger.r /
                (passenger.r +
                 sqrt((driver.start_long - passenger.start_long) * (driver.start_long - passenger.start_long) +
                      (driver.start_lat - passenger.start_lat) * (driver.start_lat - passenger.start_lat)) +
                 sqrt((driver.end_long - passenger.end_long) * (driver.end_long - passenger.end_long) +
                      (driver.end_lat - passenger.end_lat) * (driver.end_lat - passenger.end_lat))
                );
  return rate;
}


#include "dense-reconstruction/disparity-conversion-utils.h"

#include <limits>
#include <memory>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

namespace dense_reconstruction {
namespace stereo {

void convertDisparityMapToPointCloud(
    const cv::Mat& input_disparity, const cv::Mat& left_image,
    const double baseline, const double focal_length, const double cx,
    const double cy, const int sad_window_size, const int min_disparity,
    const int num_disparities, resources::PointCloud* pointcloud) {
  CHECK_NOTNULL(pointcloud);
  CHECK(pointcloud->empty());
  CHECK_GT(focal_length, 0.0);

  if (left_image.type() != CV_8U) {
    LOG(ERROR)
        << "Pointcloud generation is currently only supported on 8 bit images";
    return;
  }

  const int side_bound = sad_window_size / 2;

  const int max_size = input_disparity.rows * input_disparity.cols;

  pointcloud->xyz.reserve(3 * max_size);
  pointcloud->colors.reserve(3 * max_size);

  cv::Mat input_valid =
      cv::Mat(input_disparity.rows, input_disparity.cols, CV_8U);

  const int lower_x_border = side_bound + min_disparity + num_disparities;
  const int lower_y_border = side_bound;
  const int upper_x_border = input_disparity.cols - side_bound;
  const int upper_y_border = input_disparity.rows - side_bound;

  const int disparity_threshold = (min_disparity + num_disparities - 1) * 16;

  for (int y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) {
    for (int x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) {
      // The last check is because the sky has a bad habit of having a disparity
      // at just less than the max disparity.
      const int16_t disparity_value =
          input_disparity.at<int16_t>(y_pixels, x_pixels);
      if ((x_pixels < lower_x_border) || (y_pixels < lower_y_border) ||
          (x_pixels > upper_x_border) || (y_pixels > upper_y_border) ||
          (disparity_value < 0) || (disparity_value >= disparity_threshold)) {
        input_valid.at<uint8_t>(y_pixels, x_pixels) = 0;
      } else {
        input_valid.at<uint8_t>(y_pixels, x_pixels) = 1;
      }
    }
  }

  // Build pointcloud.
  for (int y_pixels = lower_y_border; y_pixels < upper_y_border; ++y_pixels) {
    for (int x_pixels = lower_x_border; x_pixels < upper_x_border; ++x_pixels) {
      const bool is_valid = input_valid.at<uint8_t>(y_pixels, x_pixels) > 0u;
      const int16_t input_value =
          input_disparity.at<int16_t>(y_pixels, x_pixels);

      double disparity_value;

      // If the filled disparity is valid it must be a freespace ray.
      if (is_valid) {
        disparity_value = static_cast<double>(input_value);
      } else {
        continue;
      }

      if (disparity_value < 1e-6) {
        continue;
      }

      // The 16x is needed as opencv stores disparity maps as 16 * the true
      // values.

      // NOTE(mfehr): This is the reprojection formula based on:
      // Source: http://answers.opencv.org/upfiles/13535653931527438.jpg
      // We undistort such that cx_left and cx_right are identical, therefore
      // the formula can be simplified to:
      // X = -b(x-Cx)/d
      // Y = -b(y - Cy) / d
      // Z = -(f*b)/d
      // with t = -b/d we get:
      // X = t * (x-Cx)
      // Y = t * (y - Cy)
      // Z = t * f

      const double t = -16.0 * baseline / disparity_value;
      const double z = t * focal_length;
      const double x = t * (x_pixels - cx);
      const double y = t * (y_pixels - cy);

      LOG_IF(WARNING, z < 0.0) << "Z is negative, which is weird!";

      uint8_t r, g, b;
      if (left_image.channels() == 3) {
        const cv::Vec3b& color = left_image.at<cv::Vec3b>(y_pixels, x_pixels);
        b = color[0];
        g = color[1];
        r = color[2];
      } else if (left_image.channels() == 4) {
        const cv::Vec4b& color = left_image.at<cv::Vec4b>(y_pixels, x_pixels);
        b = color[0];
        g = color[1];
        r = color[2];
      } else {
        b = left_image.at<uint8_t>(y_pixels, x_pixels);
        g = b;
        r = b;
      }

      pointcloud->xyz.push_back(x);
      pointcloud->xyz.push_back(y);
      pointcloud->xyz.push_back(z);

      pointcloud->colors.push_back(r);
      pointcloud->colors.push_back(g);
      pointcloud->colors.push_back(b);
    }
  }
  CHECK_LE(pointcloud->size(), 3 * max_size);
}

// Convert disparity map to a depth map in the target camera frame.
// NOTE(mfehr): This is definitely not the most efficient way to do this, but it
// uses the available infrastructure. If speed is an issue, this needs to
// be improved.
void convertDisparityMapToDepthMap(
    const cv::Mat& disparity_map, const cv::Mat& first_image_undistorted,
    const double baseline, const double focal_length, const double cx,
    const double cy, const int sad_window_size, const int min_disparity,
    const int num_disparities, const aslam::Camera& target_camera,
    cv::Mat* depth_map) {
  CHECK_NOTNULL(depth_map);
  CHECK(!disparity_map.empty());
  CHECK(!first_image_undistorted.empty());

  // Convert disparity to point cloud.
  resources::PointCloud point_cloud;
  convertDisparityMapToPointCloud(
      disparity_map, first_image_undistorted, baseline, focal_length, cx, cy,
      sad_window_size, min_disparity, num_disparities, &point_cloud);

  // Convert point cloud format to Eigen matrix.
  const size_t point_cloud_size = point_cloud.size();
  const size_t max_index = point_cloud_size * 3u;
  Eigen::Matrix3Xd p_C1_mat(3, point_cloud_size);
  size_t point_idx = 0u;
  for (size_t idx = 0u; idx < max_index; idx += 3u, ++point_idx) {
    CHECK_LT(point_idx, point_cloud_size);
    p_C1_mat.col(point_idx) = Eigen::Vector3d(
        point_cloud.xyz[idx], point_cloud.xyz[idx + 1u],
        point_cloud.xyz[idx + 2u]);
  }

  // Init depth map.
  const size_t height = target_camera.imageHeight();
  const size_t width = target_camera.imageWidth();
  *depth_map = cv::Mat(height, width, CV_16UC1, cv::Scalar(0u));

  // Project all 3D points into the image.
  Eigen::Matrix2Xd img_points_px;
  std::vector<aslam::ProjectionResult> projection_results;
  target_camera.project3Vectorized(
      p_C1_mat, &img_points_px, &projection_results);

  // Convert Z coordinate of points to millimeters.
  constexpr double kMetersToMillimeters = 1e3;

  // Statistics.
  for (point_idx = 0u; point_idx < point_cloud_size; ++point_idx) {
    if (!projection_results[point_idx].isKeypointVisible()) {
      continue;
    }

    const Eigen::Ref<const Eigen::Vector2d>& img_point =
        img_points_px.col(point_idx);

    const int depth_in_mm = std::min(
        static_cast<int>(std::numeric_limits<uint16_t>::max()),
        static_cast<int>(p_C1_mat(2, point_idx) * kMetersToMillimeters));
    CHECK_GE(depth_in_mm, 0);

    const int img_u = static_cast<int>(std::round(img_point[0]));
    const int img_v = static_cast<int>(std::round(img_point[1]));
    depth_map->at<uint16_t>(img_v, img_u) = static_cast<uint16_t>(depth_in_mm);
  }
}

}  // namespace stereo
}  // namespace dense_reconstruction


#include "ALConstraint.h"
#include "BrownianBody.h"
#include "LBFGSBWrapper.h"
#include "Model.h"
#include "OPSMesh.h"
#include "ViscosityBody.h"
#include <Eigen/Eigenvalues>
#include <stdio.h>
#include <string>
#include <vector>
#include <vtkPolyDataReader.h>

using namespace OPS;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Vector3d Vector3d;

int main(int argc, char *argv[]) {
  clock_t t1, t2, t3;
  t1 = clock();

  if (argc != 2) {
    cout << "usage: " << argv[0] << " <filename>\n";
    return -1;
  }

  //******************** Optional parameters ********************//
  bool loggingOn = false;

  // ***************** Read Input VTK File *****************//
  std::string inputFileName = argv[1];

  auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
  vtkSmartPointer<vtkPolyData> mesh;

  reader->SetFileName(inputFileName.c_str());
  reader->ReadAllVectorsOn();
  reader->Update();
  mesh = reader->GetOutput();
  // ********************************************************//

  // ******************* Read Simulation Parameters *********//

  // This flag determines if its a new simulation or a continuation

  double_t re = 1.0, s = 7.0;
  double_t alpha = 1.0, beta = 1.0, gamma = 1.0;
  double_t percentStrain = 15;

  enum Constraint { AvgArea, AvgVol, ExactArea, ExactVol, ExactAreaAndVolume };
  std::string constraintType("NULL"), baseFileName;
  size_t viterMax = 1000;
  size_t nameSuffix = 0;
  size_t step = 0;

  InputParameters miscInp = OPS::readKeyValueInput("miscInp.dat");
  re = std::stod(miscInp["re"]);
  constraintType = miscInp["constraintType"];
  baseFileName = miscInp["baseFileName"];

  // Validate constraint type
  Constraint type;
  if (constraintType.compare("AverageArea") == 0) {
    type = AvgArea;
  } else if (constraintType.compare("AverageVolume") == 0) {
    type = AvgArea;
  } else if (constraintType.compare("ExactArea") == 0) {
    type = ExactArea;
  } else if (constraintType.compare("ExactVolume") == 0) {
    type = ExactVol;
  } else if (constraintType.compare("ExactAreaAndVolume") == 0) {
    type = ExactAreaAndVolume;
  } else {
    std::cout << "Invalid constraint type specified." << std::endl;
    exit(EXIT_FAILURE);
  }

  // Input file should contain the following columns
  // Alpha Beta Gamma PercentStrain AreaConstraint NumIterations PrintStep
  std::ifstream coolFile("schedule.dat");
  assert(coolFile);
  std::vector<std::vector<double_t>> coolVec;
  double_t currAlpha, currBeta, currGamma, currPercentStrain, currViterMax,
      currPrintStep, currArea;

  std::string headerline;
  std::getline(coolFile, headerline);

  while (coolFile >> currAlpha >> currBeta >> currGamma >> currPercentStrain >>
         currArea >> currViterMax >> currPrintStep) {
    std::vector<double> currLine;
    currLine.push_back(currAlpha);
    currLine.push_back(currBeta);
    currLine.push_back(currGamma);
    currLine.push_back(currPercentStrain);
    currLine.push_back(currArea);
    currLine.push_back(currViterMax);
    currLine.push_back(currPrintStep);
    coolVec.push_back(currLine);
  }
  coolFile.close();

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

  // ***************** Create Bodies and Model ****************//
  // Set number of OPS particles
  size_t N = mesh->GetNumberOfPoints();

  // Read point coordinates from input mesh
  Eigen::Matrix3Xd coords(3, N);
  for (auto i = 0; i < N; ++i) {
    Eigen::Vector3d cp = Eigen::Vector3d::Zero();
    mesh->GetPoint(i, &(cp(0)));
    coords.col(i) = cp;
  }

  // Prepare memory for energy, force
  double_t f;
  Eigen::VectorXd x(6 * N), g(6 * N), prevX(3 * N);
  g.setZero(g.size());
  x.setZero(x.size());

  // Fill x with coords and rotVecs
  Eigen::Map<Eigen::Matrix3Xd> xpos(x.data(), 3, N), xrot(&(x(3 * N)), 3, N),
      prevPos(prevX.data(), 3, N);
  xpos = coords;

  // Renormalize the position vectors by average edge length
  x /= getPointCloudAvgEdgeLen(inputFileName);
  prevX = x.head(3 * N);

  // Generate initial rotation vectors either from starting point coordinates
  OPSBody::initialRotationVector(xpos, xrot);

  // The starting average radius
  double_t R0 = xpos.colwise().norm().sum() / N;

  // Create OPSBody
  Eigen::Map<Eigen::Matrix3Xd> posGrad(g.data(), 3, N),
      rotGrad(&g(3 * N), 3, N);
  OPSMesh ops(N, f, R0, xpos, xrot, posGrad, rotGrad, prevPos);
  ops.setMorseDistance(re);
  s = 100 * log(2.0) / (re * percentStrain);
  ops.setMorseWellWidth(s);

  // Create Brownian and Viscosity bodies
  Eigen::Map<Eigen::VectorXd> thermalX(x.data(), 3 * N, 1);
  Eigen::Map<Eigen::VectorXd> thermalG(g.data(), 3 * N, 1);
  double_t brownCoeff = 1.0, viscosity = 1.0;
  BrownianBody brown(3 * N, brownCoeff, f, thermalX, thermalG, prevX);
  ViscosityBody visco(3 * N, viscosity, f, thermalX, thermalG, prevX);

  // Create the Augmented Lagrangian volume constraint body
  ALConstraint *constraint;
  if (type == AvgArea) {
    constraint = new AvgAreaConstraint(N, f, xpos, posGrad);
  } else if (type == AvgVol) {
    constraint = new AvgVolConstraint(N, f, xpos, posGrad);
  } else if (type == ExactArea) {
    vtkSmartPointer<vtkPolyData> poly = ops.getPolyData();
    constraint = new ExactAreaConstraint(N, f, xpos, posGrad, poly);
  } else if (type == ExactVol) {
    vtkSmartPointer<vtkPolyData> poly = ops.getPolyData();
    constraint = new ExactVolConstraint(N, f, xpos, posGrad, poly);
  } else if (type == ExactAreaAndVolume) {
    vtkSmartPointer<vtkPolyData> poly = ops.getPolyData();
    constraint = new ExactAreaVolConstraint(N, f, xpos, posGrad, poly);
    constraint->setTolerance(1e-8);
  }

  // Create Model
  Model model(6 * N, f, g);
  model.addBody(&ops);
  model.addBody(&brown);
  model.addBody(&visco);
  model.addBody(constraint);
  // ****************************************************************//

  // ***************** Prepare Output Data files *********************//
  // Identify the Input structure name
  std::string fname = baseFileName;
  std::stringstream sstm;
  std::string dataOutputFile;

  // Detailed output data file
std::ofstream detailedOP;
  sstm << fname << "-DetailedOutput.dat";
  dataOutputFile = sstm.str();
  sstm.str("");
  sstm.clear();
  detailedOP.open(dataOutputFile.c_str(), std::ofstream::out);
  detailedOP << "Gamma"
             << "\t"
             << "Beta"
             << "\t"
             << "Asphericity"
             << "\t"
             << "MorseEn"
             << "\t"
             << "NormEn"
             << "\t"
             << "CircEn"
             << "\t"
             << "BrownEn"
             << "\t"
             << "ViscoEn"
             << "\t"
             << "MSD"
             << "\t"
             << "RMSAngleDeficit"
             << "\t"
             << "I1"
             << "\t"
             << "I2"
             << "\t"
             << "I3" << std::endl;

  // ************************* Create Solver ************************  //
  size_t m = 5, iprint = 1000, maxIter = 1e5;
  double_t factr = 10.0, pgtol = 1e-8;
  LBFGSBParams solverParams(m, iprint, maxIter, factr, pgtol);
  LBFGSBWrapper solver(solverParams, model, f, x, g);
  solver.turnOffLogging();
  // *****************************************************************//

  // ********************* Prepare data for simulation ****************//
  // Calculate Average Edge Length
  double_t avgEdgeLen = ops.getAverageEdgeLength();
  if (loggingOn) {
    std::cout << "Initial Avg Edge Length = " << avgEdgeLen << std::endl;
  }
  // Renormalize positions such that avgEdgeLen = 1.0
  for (auto i = 0; i < N; ++i) {
    xpos.col(i) = xpos.col(i) / avgEdgeLen;
  }

  // Update the OPSBody member variables as per new positions
  ops.updatePolyData();
  ops.updateNeighbors();
  ops.saveInitialPosition(); /*!< For Mean Squared Displacement */
  avgEdgeLen = ops.getAverageEdgeLength();
  if (loggingOn)
    std::cout << "After renormalizing, Avg Edge Length = " << avgEdgeLen
              << std::endl;
  // ******************************************************************//

  // Create an eigenvalue solver for inertia tensor
  Eigen::SelfAdjointEigenSolver<Matrix3d> saes;

  // ************************ OUTER SOLUTION LOOP **********************//
  size_t printStep;
  for (int z = 0; z < coolVec.size(); z++) {
    alpha = coolVec[z][0];
    beta = coolVec[z][1];
    gamma = coolVec[z][2];
    percentStrain = coolVec[z][3];
    double_t constrainedVal = coolVec[z][4];
    viterMax = coolVec[z][5];
    printStep = (int)coolVec[z][6];

    // Update OPS params
    s = (100 / percentStrain) * log(2.0);
    ops.setFVK(gamma);
    ops.setMorseWellWidth(s);

    // Set up the constraint value as the zero temperature value
    constraint->setConstraint(constrainedVal);

    // For the very first iteration solve at zero temperature first
    if (z == 0) {
      brown.setCoefficient(0.0);
      visco.setViscosity(0.0);
      solver.solve();
    }

    // Update prevX
    prevX = x.head(3 * N);

    // Set the viscosity and Brownian coefficient
    viscosity = alpha;
    brownCoeff = std::sqrt(2 * alpha / beta);
    if (loggingOn) {
      std::cout << "Viscosity = " << viscosity << std::endl;
      std::cout << "Brownian Coefficient = " << brownCoeff << std::endl;
    }
    brown.setCoefficient(brownCoeff);
    visco.setViscosity(viscosity);

    //**************  INNER SOLUTION LOOP ******************//
    // Average energy across time steps
    double_t avgTotalEnergy = 0.0;

    for (int viter = 0; viter < viterMax; viter++) {
      if (loggingOn)
        std::cout << std::endl
                  << "VISCOUS ITERATION: " << step << std::endl
                  << std::endl;

      // Generate Brownian Kicks
      brown.generateParallelKicks();

      // Set the starting guess for Lambda and K for
      // Augmented Lagrangian
      constraint->setLagrangeCoeff(10.0);
      constraint->setPenaltyCoeff(1000.0);

      // *************** Augmented Lagrangian Loop ************** //
      bool constraintMet = false;
      size_t alIter = 0, alMaxIter = 10;

      while (!constraintMet && (alIter < alMaxIter)) {
        if (loggingOn)
          std::cout << "Augmented Lagrangian iteration: " << alIter
                    << std::endl;

        // Solve the unconstrained minimization
        solver.solve();

        // Uzawa update
        constraint->uzawaUpdate();

        // Update termination check quantities
        alIter++;
        constraintMet = constraint->constraintSatisfied();
      }
      if (loggingOn) {
        constraint->printCompletion();
        std::cout << "Constraint satisfied in " << alIter << " iterations."
                  << std::endl
                  << std::endl;
      }
      // *********************************************************//

      // Apply Kabsch Algorithm
      ops.applyKabschAlgorithm();

      // Update kdTree, polyData and neighbors
      ops.updatePolyData();
      ops.updateNeighbors();

      //********** Print relaxed configuration ************//
      // We will print only after every currPrintStep iterations
      if (viter % printStep == 0 && printStep <= viterMax) {
        sstm << fname << "-relaxed-" << nameSuffix++ << ".vtk";
        std::string rName = sstm.str();
        ops.printVTKFile(rName);
        sstm.str("");
        sstm.clear();
      }

      // Calculate the inertia tensor and its eigenvalues
      Matrix3d M = Matrix3d::Zero();
      for (auto ptId = 0; ptId < N; ++ptId) {
        Vector3d xk = xpos.col(ptId);
        M += xk.dot(xk) * Matrix3d::Identity() - xk * xk.transpose();
      }
      // Now calculate the eigen values
      saes.compute(M, Eigen::EigenvaluesOnly);
      Vector3d eigV = saes.eigenvalues();

      double_t I1 = eigV[0];
      double_t I2 = eigV[1];
      double_t I3 = eigV[2];

      std::vector<double_t> msds(2, 0);
      msds = ops.getMSD();

      // Write output to data file
      detailedOP << gamma << "\t" << beta << "\t" << ops.getAsphericity()
                 << "\t" << ops.getMorseEnergy() << "\t"
                 << ops.getNormalityEnergy() << "\t"
                 << ops.getCircularityEnergy() << "\t"
                 << brown.getBrownianEnergy() << "\t"
                 << visco.getViscosityEnergy() << "\t" << msds[0] << "\t"
                 << ops.getRMSAngleDeficit() << "\t" << I1 << "\t" << I2 << "\t"
                 << I3 << std::endl;

      // Update prevX
      prevX = x.head(3 * N);

      if (loggingOn) {
        avgTotalEnergy = (avgTotalEnergy * step + f) / (step + 1);
        std::cout << " Average Total Energy = " << avgTotalEnergy << std::endl;
      }
      step++;
    }
    //************************************************//
  }
  // **********************************************************************//
  detailedOP.close();
  t2 = clock();
  float diff((float)t2 - (float)t1);
  std::cout << "Solution loop execution time: " << diff / CLOCKS_PER_SEC
            << " seconds" << std::endl;
  delete constraint;
  return 1;
}


#include <iostream>
#include <Eigen/Core>
#include <bench/BenchTimer.h>

using namespace Eigen;
using namespace std;

#define END 9

template<int S> struct map_size { enum { ret = S }; };
template<>  struct map_size<10> { enum { ret = 20 }; };
template<>  struct map_size<11> { enum { ret = 50 }; };
template<>  struct map_size<12> { enum { ret = 100 }; };
template<>  struct map_size<13> { enum { ret = 300 }; };

template<int M, int N,int K> struct alt_prod
{
  enum {
    ret = M==1 && N==1 ? InnerProduct
        : K==1 ? OuterProduct
        : M==1 ? GemvProduct
        : N==1 ? GemvProduct
        : GemmProduct
  };
};

void print_mode(int mode)
{
  if(mode==InnerProduct) std::cout << "i";
  if(mode==OuterProduct) std::cout << "o";
  if(mode==CoeffBasedProductMode) std::cout << "c";
  if(mode==LazyCoeffBasedProductMode) std::cout << "l";
  if(mode==GemvProduct) std::cout << "v";
  if(mode==GemmProduct) std::cout << "m";
}

template<int Mode, typename Lhs, typename Rhs, typename Res>
EIGEN_DONT_INLINE void prod(const Lhs& a, const Rhs& b, Res& c)
{
  c.noalias() += typename ProductReturnType<Lhs,Rhs,Mode>::Type(a,b);
}

template<int M, int N, int K, typename Scalar, int Mode>
EIGEN_DONT_INLINE void bench_prod()
{
  typedef Matrix<Scalar,M,K> Lhs; Lhs a; a.setRandom();
  typedef Matrix<Scalar,K,N> Rhs; Rhs b; b.setRandom();
  typedef Matrix<Scalar,M,N> Res; Res c; c.setRandom();

  BenchTimer t;
  double n = 2.*double(M)*double(N)*double(K);
  int rep = 100000./n;
  rep /= 2;
  if(rep<1) rep = 1;
  do {
    rep *= 2;
    t.reset();
    BENCH(t,1,rep,prod<CoeffBasedProductMode>(a,b,c));
  } while(t.best()<0.1);

  t.reset();
  BENCH(t,5,rep,prod<Mode>(a,b,c));

  print_mode(Mode);
  std::cout << int(1e-6*n*rep/t.best()) << "\t";
}

template<int N> struct print_n;
template<int M, int N, int K> struct loop_on_m;
template<int M, int N, int K, typename Scalar, int Mode> struct loop_on_n;

template<int M, int N, int K>
struct loop_on_k
{
  static void run()
  {
    std::cout << "K=" << K << "\t";
    print_n<N>::run();
    std::cout << "\n";

    loop_on_m<M,N,K>::run();
    std::cout << "\n\n";

    loop_on_k<M,N,K+1>::run();
  }
};

template<int M, int N>
struct loop_on_k<M,N,END> { static void run(){} };

template<int M, int N, int K>
struct loop_on_m
{
  static void run()
  {
    std::cout << M << "f\t";
    loop_on_n<M,N,K,float,CoeffBasedProductMode>::run();
    std::cout << "\n";

    std::cout << M << "f\t";
    loop_on_n<M,N,K,float,-1>::run();
    std::cout << "\n";

    loop_on_m<M+1,N,K>::run();
  }
};

template<int N, int K>
struct loop_on_m<END,N,K> { static void run(){} };

template<int M, int N, int K, typename Scalar, int Mode>
struct loop_on_n
{
  static void run()
  {
    bench_prod<M,N,K,Scalar,Mode==-1? alt_prod<M,N,K>::ret : Mode>();

    loop_on_n<M,N+1,K,Scalar,Mode>::run();
  }
};

template<int M, int K, typename Scalar, int Mode>
struct loop_on_n<M,END,K,Scalar,Mode> { static void run(){} };

template<int N> struct print_n
{
  static void run()
  {
    std::cout << map_size<N>::ret << "\t";
    print_n<N+1>::run();
  }
};

template<> struct print_n<END> { static void run(){} };

int main()
{
  loop_on_k<1,1,1>::run();

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// random::categorical_distribution.hpp                                      //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 BOOST_RANDOM_CATEGORICAL_DISTRIBUTION_HPP_ER_2009
#define BOOST_RANDOM_CATEGORICAL_DISTRIBUTION_HPP_ER_2009
#include <algorithm>
#include <numeric>
#include <cmath>
#include <stdexcept>
#include <boost/range.hpp>
#include <boost/format.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/utility/result_of.hpp>

namespace boost{
namespace random{

// Deprecated : see discrete_distribution
//
// Usage:
// typedef categorical_distribution<> rmult_;
// typedef rmult_::value_type         value_type;
// typedef rmult_::result_type        idx_;
//
// Let urng model UniformRandomNumberGenerator and weights a sequence whose
// elements type is convertible to value_type:
//
// rmult_ rmult(weights);
// idx_ idx = rmult(urng);
template<typename Ur = uniform_real<double> >
class categorical_distribution{
    typedef Ur                                          unif_t;
    public:
    typedef typename unif_t::input_type                 input_type;
    typedef typename unif_t::result_type                value_type;

    private:
    typedef std::vector<value_type>                     cont_t;

    public:
    typedef typename range_difference<cont_t>::type     result_type;

    // Construction
    categorical_distribution();

    // Passing sorted from large to small weights will speed up execution.
    template<typename R>
    categorical_distribution( const R& unnormalized_weights );
    categorical_distribution( const categorical_distribution& that );
    categorical_distribution&
    operator=(const categorical_distribution& that);

    // Draw
    template<typename U> result_type operator()(U& urng)const;

    // Access
    const cont_t& cumulative_weights()const;
    value_type normalizing_constant()const;

    // TODO os/is

    private:
    cont_t cum_sums_;
    template<typename R>void set(const R& unnormalized_weights);
};

// Implementation //

    // Construction
    template<typename Ur>
    categorical_distribution<Ur>::categorical_distribution() : cum_sums_(){}

    template<typename Ur>
    template<typename R>
    categorical_distribution<Ur>::categorical_distribution(
        const R& unnormalized_weights
    )
    {
        set(unnormalized_weights);
    }

    template<typename Ur>
    categorical_distribution<Ur>::categorical_distribution(
        const categorical_distribution& that
    )
    :cum_sums_(that.cum_sums_){}

    template<typename Ur>
    categorical_distribution<Ur>&
    categorical_distribution<Ur>::operator=(
        const categorical_distribution& that
    ){
        if(&that!=this){
            this->cum_sums_ = that.cum_sums_;
        }
        return *this;
    }

    template<typename Ur>
    template<typename U>
    typename categorical_distribution<Ur>::result_type
    categorical_distribution<Ur>::operator()(U& urng)const{
        unif_t unif(static_cast<value_type>(0),normalizing_constant());
        typedef typename range_iterator<const cont_t>::type iter_;
        value_type u = unif(urng);
        iter_ i = std::lower_bound(
            boost::begin(this->cum_sums_),
            boost::end(this->cum_sums_),
            u
        );
        BOOST_ASSERT(i!=end(cum_sums_));
        return std::distance(boost::begin(this->cum_sums_),i);
    }

    // Access
    template<typename Ur>
    const typename categorical_distribution<Ur>::cont_t&
    categorical_distribution<Ur>::cumulative_weights()const{
        return (this->cum_sums_);
    }

    template<typename Ur>
    typename categorical_distribution<Ur>::value_type
    categorical_distribution<Ur>::normalizing_constant()const{
        return (this->cum_sums_).back();
    }

    template<typename Ur>
    template<typename R>
    void categorical_distribution<Ur>::set(const R& unnormalized_weights){
        const char* method = "multinormal_distribution::set, error : ";
        static value_type eps = math::tools::epsilon<value_type>();

        cum_sums_.resize(size(unnormalized_weights));

        std::partial_sum(
            boost::begin(unnormalized_weights),
            boost::end(unnormalized_weights),
            boost::begin(this->cum_sums_)
        );

        if(math::isinf(this->normalizing_constant())){
            std::string str = method;
            str += "isinf(nc)";
            throw std::runtime_error(str);
        }
        if(this->normalizing_constant()<eps){
            std::string str = method;
            str += "nc = %1% < eps = %2%";
            format f(str); f%(this->normalizing_constant())%eps;
            throw std::runtime_error(f.str());
        }

    }

}// random
}// boost

#endif


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_one_else_zero.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/touint.hpp>
#include <boost/simd/constant/four.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/assert_utils.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
       return _mm_sqrt_pd(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      using uint_type = bd::as_integer_t<A0,unsigned>;
      return simd::bitwise_cast<A0>(sqrt( simd::bitwise_cast<uint_type>(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return bs::toint(bs::sqrt(bs::tofloat(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      A0 n   = plus(shift_right(a0, 4), Four<A0>());
      A0 n1  = shift_right(n+a0/n, 1);

      auto ok = is_less(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok = is_less(n1, n);
      n  = if_else(ok, n1, n);
      n  = if_plus( is_greater(n*n,a0), n, Mone<A0>());
      return n;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na = is_nez(a0);
      A0 const  z1 = plus(shift_right(a0, 6), Ratio<A0, 16>());
      A0 const  z2 = plus(shift_right(a0,10), Ratio<A0, 256>());
      A0 const  C1 = Ratio<A0, 31679>();
      // choose a proper starting point for approximation
      A0 n  = if_else(is_less(a0, C1), z1, z2);
      auto ok =  is_gtz(n);
      n  = if_else(ok, n, One<A0>());

      A0 n1 = if_else(ok, shift_right(n+a0/n, 1), One<A0>());

      ok = is_less(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok = is_less(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  is_less(n1, n);
      n  = if_else(ok, n1, n);
      n  = if_plus( is_greater(n*n,a0), n, Mone<A0>());

     return if_plus(na, Zero<A0>(), n);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na = is_nez(a0);
      A0 const z1 = plus(shift_right(a0, 6),    Ratio<A0,16>());
      A0 const z2 = plus(shift_right(a0,10),   Ratio<A0,256>());
      A0 const z3 = plus(shift_right(a0,13),  Ratio<A0,2048>());
      A0 const z4 = plus(shift_right(a0,16), Ratio<A0,16384>());
      A0 n  = if_else( is_greater(a0, Ratio<A0,177155824>())
                  , z4
                  , if_else( is_greater(a0, Ratio<A0,4084387>())
                        , z3
                        , if_else( is_greater(a0, Ratio<A0,31679>())
                                , z2
                                , z1
                                )
                        )
                  );
      auto ok =  is_gtz(n);
      n = if_else(ok, n, One<A0>());
      A0 n1 = if_else(ok, shift_right(n+a0/n, 1), One<A0>());

      ok = is_less(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  is_less(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  is_less(n1, n);
      n  = if_else(ok, n1, n);

      A0 tmp = minus(n*minus(n, One<A0>()), One<A0>());
      n  = if_plus( is_greater_equal(tmp+n,a0), n, Mone<A0>());
      n =  if_plus(na, Zero<A0>(), n);

      return n;
     }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return bs::touint(bs::sqrt(bs::tofloat(a0)));
    }
  };

} } }

#endif


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2012-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <boost/multiprecision/cpp_int.hpp>

#include "sdd/dd/definition.hh"

namespace sdd { namespace dd {

/*------------------------------------------------------------------------------------------------*/

template <typename C>
boost::multiprecision::cpp_int
count_combinations(const SDD<C>& x);

/*------------------------------------------------------------------------------------------------*/

}} // namespace sdd::dd


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/ap.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/if.hpp>
#include <boost/hana/lift.hpp>
#include <boost/hana/optional.hpp>
namespace hana = boost::hana;

template <char op>
constexpr auto function = hana::nothing;

template <>
BOOST_HANA_CONSTEXPR_LAMBDA auto function<'+'> = hana::just([](auto x, auto y) {
    return x + y;
});

template <>
BOOST_HANA_CONSTEXPR_LAMBDA auto function<'-'> = hana::just([](auto x, auto y) {
    return x - y;
});

// and so on...

template <char n>
constexpr auto digit = hana::if_(hana::bool_c<(n >= '0' && n <= '9')>,
    hana::just(static_cast<int>(n - 48)),
    hana::nothing
);

template <char x, char op, char y>
BOOST_HANA_CONSTEXPR_LAMBDA auto evaluate = hana::ap(function<op>, digit<x>, digit<y>);

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(evaluate<'1', '+', '2'> == hana::just(1 + 2));

    BOOST_HANA_CONSTANT_CHECK(evaluate<'?', '+', '2'> == hana::nothing);
    BOOST_HANA_CONSTANT_CHECK(evaluate<'1', '?', '2'> == hana::nothing);
    BOOST_HANA_CONSTANT_CHECK(evaluate<'1', '+', '?'> == hana::nothing);
    BOOST_HANA_CONSTANT_CHECK(evaluate<'?', '?', '?'> == hana::nothing);

    static_assert(hana::lift<hana::optional_tag>(123) == hana::just(123), "");
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#include <iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE dirMM test
#include <boost/test/unit_test.hpp>

#include <dpMM/dirNaiveBayes.hpp>
#include <dpMM/niwBaseMeasure.hpp>
#include <dpMM/niwSphere.hpp>
#include <dpMM/dirMMcld.hpp>
#include <dpMM/clTGMMDataGpu.hpp>
#include <dpMM/distribution.hpp>
#include <dpMM/typedef.h>

//BOOST_AUTO_TEST_CASE(niwBaseMeasure_test)
//{
  //MatrixXd Delta(3,3);
  //Delta << 1.0,0.0,0.0,
        //0.0,1.0,0.0,
        //0.0,0.0,1.0;
  //VectorXd theta(3);
  //theta << 1.0,1.0,1.0;
  //double nu = 100.0;
  //double kappa = 100.0;

  //boost::mt19937 rndGen(1);
  //NIW<double> niw(Delta,theta,nu,kappa,&rndGen);

  //NiwMarginalized<double> niwMargBase(niw);

  //VectorXd x(3);
  //x << 1.0,1.0,1.0;
  //cout<< niwMargBase.logLikelihood(x)<< endl;

  //NiwSampled<double> niwSampledBase(niw);

  //x << 1.0,1.0,1.0;
  //cout<< niwSampledBase.logLikelihood(x)<< endl;
//};

BOOST_AUTO_TEST_CASE(dirNaiveBayes_test)
{

	double nu = 4.0;
	double kappa = 4.0;
	MatrixXd Delta(3,3);
	Delta << .1,0.0,0.0,
		0.0,.1,0.0,
		0.0,0.0,.1;
	VectorXd theta(3);
	theta << 0.0,0.0,0.0;

	boost::mt19937 rndGen(9191);
	NIW<double> niw(Delta,theta,nu,kappa,&rndGen);

	boost::shared_ptr<NiwMarginalized<double> > niwMargBase(
		new NiwMarginalized<double>(niw));
	VectorXd alpha(2);
	alpha << 10.,10.;
	Dir<Catd,double> dir(alpha,&rndGen);

	uint Ndoc=5;
	uint Nword=10;
	vector< Matrix<double, Dynamic, Dynamic> > x;
	for(uint i=0; i<Ndoc; ++i) {
		MatrixXd  xdoc(3,Nword);
		for(uint w=0; w<Nword; ++w) {
			if(w<Nword/2)
				xdoc.col(i) << 0.0,0.0,0.0;
			else
				xdoc.col(i) << 10.0,10.0,10.0;
		}

		x.push_back(xdoc);
	}

	//cout<<"------ marginalized ---- NIW "<<endl;
	//DirNaiveBayes<double> naive_marg(dir,niwMargBase);
	//naive_marg.initialize(x);
	//cout<<naive_marg.labels().transpose()<<endl;
	//for(uint32_t t=0; t<30; ++t)
	//{
	//naive_marg.sampleLabels();
	//naive_marg.sampleParameters();
	//cout<<naive_marg.labels().transpose()
		//<<" logJoint="<<naive_marg.logJoint()<<endl;
	//}

	boost::shared_ptr<NiwSampled<double> > niwSampled( new NiwSampled<double>(niw));
	DirNaiveBayes<double> naive_samp(dir,niwSampled);

	naive_samp.initialize( (const vector< Matrix<double, Dynamic, Dynamic> >) x );
	naive_samp.inferAll(30,true);
};

//BOOST_AUTO_TEST_CASE(dirMM_Sphere_test)
//{

  //double nu = 20.0;
  //MatrixXd Delta(2,2);
  //Delta << .01,0.0,
        //0.0,.01;
  //Delta *= nu;

  //boost::mt19937 rndGen(9191);
  //IW<double> iw(Delta,nu,&rndGen);
  //boost::shared_ptr<NiwSphere<double> > niwSp( new NiwSphere<double>(iw,&rndGen));

  //VectorXd alpha(2);
  //alpha << 10.,10.;
  //Dir<Catd,double> dir(alpha,&rndGen);
  //DirMM<double> dirGMM_sp(dir,niwSp);

  //uint32_t N=20;
  //uint32_t K=2;
  //MatrixXd x(3,N);
  //sampleClustersOnSphere<double>(x, K);
  //dirGMM_sp.initialize(x);
  //cout<<"------ sampling -- NIW sphere"<<endl;
  //cout<<dirGMM_sp.labels().transpose()<<endl;
  //for(uint32_t t=0; t<10; ++t)
  //{
    //dirGMM_sp.sampleLabels();
    //dirGMM_sp.sampleParameters();
    //cout<<dirGMM_sp.labels().transpose()
      //<<" logJoint="<<dirGMM_sp.logJoint()<<endl;
  //}
  //MatrixXd logLikes;
  //MatrixXu inds = dirGMM_sp.mostLikelyInds(5,logLikes);
  //cout<<"most likely indices"<<endl;
  //cout<<inds<<endl;
  //cout<<"----------------------------------------"<<endl;
//};

//typedef double myFlt;

//BOOST_AUTO_TEST_CASE(dirMMcld_Sphere_test)
//{

  //uint32_t N=30; //640*480;
  //uint32_t K=6;
  //uint32_t D=3;
  //boost::mt19937 rndGen(9191);
  //// sample datapoints
  //boost::shared_ptr<Matrix<myFlt,Dynamic,Dynamic> > sx(new
      //Matrix<myFlt,Dynamic,Dynamic>(D,N));
  //Matrix<myFlt,Dynamic,Dynamic> mus =  sampleClustersOnSphere(*sx, 3);

  //// alpha
  //Matrix<myFlt,Dynamic,1> alpha(K);
  //alpha << 1.,1.,.1,.1,.1,.1;
  //alpha *= 1;

  //// niw
  //double nu = (1.0)+D+N/100.;
  //Matrix<myFlt,Dynamic,Dynamic> Delta(2,2);
  //Delta << .01,0.0,
        //0.0,.01;
  //Delta *= nu;

  //IW<myFlt> iw(Delta,nu,&rndGen);
  //boost::shared_ptr<NiwSphere<myFlt> > niwSp( new NiwSphere<myFlt>(iw,&rndGen));
////  boost::shared_ptr<NiwSphere<double> > niwSp2( new NiwSphere<double>(iw,&rndGen));

  //Dir<Cat<myFlt>, myFlt> dir(alpha,&rndGen);
  //DirMMcld<NiwSphere<myFlt>,myFlt> dirGMM_sp(dir,niwSp);

////  DirMM<myFlt> dirGMM_cpu(dir,niwSp2);

  //boost::shared_ptr<ClTGMMDataGpu<myFlt> > clsp(
      //new ClTGMMDataGpu<myFlt>(sx, spVectorXu(new VectorXu(N)),&rndGen,K));

  //Matrix<myFlt,Dynamic,1> mu(D);
  //mu<<0.0,0.0,1.0;
  //Matrix<myFlt,Dynamic,Dynamic> Sigma = Matrix<myFlt,Dynamic,Dynamic>::Identity(D,D);

  //dirGMM_sp.initialize(clsp);
////  dirGMM_cpu.initialize(*sx);
  //cout<<"------ sampling -- NIW sphere"<<endl;
  //cout<<counts<myFlt,uint32_t>(dirGMM_sp.labels(),K).transpose()<<endl;
  //Timer t;
  //for(uint32_t i=0; i<5; ++i)
  //{
////    t.tic();
////    dirGMM_cpu.sampleLabels();
////    dirGMM_cpu.sampleParameters();
////    cout<<dirGMM_cpu.labels().transpose()<<endl;
////    t.toctic(" -----------------CPU------------------- fullIteration");
    //t.tic();
    //dirGMM_sp.sampleLabels();
    //dirGMM_sp.sampleParameters();
    //cout<<dirGMM_sp.z().transpose()<<endl;
    //cout<<dirGMM_sp.counts().transpose()<<endl;
    //cout<<dirGMM_sp.means()<<endl;
    //t.toctic(" -----------------GPU------------------- fullIteration");
////        <<" logJoint="<<dirGMM_sp.logJoint()<<endl;
  //}
  //cout<<"true mus: "<<endl;
  //cout <<mus<<endl;
//};


//==============================================================================
//         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::whereij function"

#include <nt2/table.hpp>
#include <nt2/include/functions/size.hpp>
#include <nt2/include/functions/whereij.hpp>
#include <nt2/include/functions/rif.hpp>
#include <nt2/include/functions/is_less_equal.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/logical_and.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/band.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
//#include <boost/lambda/lambda.hpp>
//boost lambda codes hard bool return types preventing the code to work
//we have to use phoenix here when corrected
// NT2_TEST_CASE_TPL( whereij_lambda, NT2_TYPES)
// {
//   namespace bl = boost::lambda;
//   nt2::table<T> a = nt2::rif(nt2::of_size(3, 3), nt2::meta::as_<T>()),
//     b = nt2::zeros(nt2::of_size(3, 3), nt2::meta::as_<T>()),
//     e, f;

//   e = nt2::whereij(        (bl::_1 <= bl::_2),  a, b);
//   NT2_DISPLAY(e);
//   NT2_DISPLAY(nt2::triu(a));
//   NT2_TEST_EQUAL(e, nt2::triu(a));
//   f = nt2::whereij((bl::_1 == bl::_2),  a, b);
//   NT2_TEST_EQUAL(f, nt2::from_diag(nt2::diag_of(a)));
// }

NT2_TEST_CASE_TPL( whereij_1, NT2_TYPES)
{
  nt2::table<T> a = nt2::rif(nt2::of_size(3, 3), nt2::meta::as_<T>()),
                b = nt2::zeros(nt2::of_size(3, 3), nt2::meta::as_<T>()),
                e, f, g, h;

  e = nt2::whereij(nt2::functor<nt2::tag::is_less_equal_>(),  a, b);
  NT2_DISPLAY(e);
  NT2_DISPLAY(nt2::triu(a));
  NT2_TEST_EQUAL(e, nt2::triu(a));
  f = whereij(nt2::functor<nt2::tag::is_equal_>(),  a, b);
  NT2_TEST_EQUAL(f, nt2::from_diag(nt2::diag_of(a)));
  g = nt2::whereij(nt2::functor<nt2::tag::is_equal_>(),  a, nt2::zeros(3, nt2::meta::as_<T>()));
  NT2_TEST_EQUAL(g, nt2::from_diag(nt2::diag_of(a)));
  h = nt2::whereij(nt2::functor<nt2::tag::is_equal_>(),  a, nt2::Zero<T>());
  NT2_TEST_EQUAL(h, nt2::from_diag(nt2::diag_of(a)));
  NT2_DISPLAY(nt2::from_diag(nt2::diag_of(a)));
  NT2_DISPLAY(h);
}

struct fct1
{

  template < class A0, class A1>
  typename nt2::meta::as_logical<A0>::type
  operator ()(const A0& i, const A1& j) const
  {
    return nt2::logical_and(nt2::le(i, nt2::oneplus(j)),
                            nt2::le(j, nt2::oneplus(i))
                           );
  }

};

struct fct2
{

  template < class A0, class A1>
  typename nt2::meta::as_logical<A0>::type
  operator ()(const A0& i, const A1& j) const
  {
    return nt2::eq(i, j);
  }

};

struct fct3
{

  template < class A0, class A1>
  typename nt2::meta::as_logical<A0>::type
  operator ()(const A0& i, const A1& j) const
  {
    return nt2::logical_and(nt2::eq(i, size_t(1) ), nt2::eq(j, size_t(2)));
  }

};

NT2_TEST_CASE_TPL( whereij_func, NT2_TYPES)
{
  nt2::table<T> a = nt2::rif(nt2::of_size(3, 3), nt2::meta::as_<T>()),
    b = nt2::zeros(nt2::of_size(3, 3), nt2::meta::as_<T>()),
    e, f, g;

  e = whereij(fct1(),  a, b);
  NT2_DISPLAY(e);
  NT2_DISPLAY(nt2::band(a, 1, 1));
  NT2_TEST_EQUAL(e, nt2::band(a, 1, 1));
  f = whereij(fct2(),  a, b);
  NT2_TEST_EQUAL(f, nt2::from_diag(nt2::diag_of(a)));
  NT2_TEST_EQUAL(f, nt2::diagonal(a));
  g = whereij(fct3(),  a, b);
  NT2_DISPLAY(g);
}


/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

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

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

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

/**
 * This differs from the API in that the Color3-related functions now return
 * Color3 objects rather than Vec3s. This is so that equality works as expected.
 * Note they will still accept any Vec3.
 */

#ifndef _PIMATH_COLORALGO__H_
#define _PIMATH_COLORALGO__H_

#include <boost/python.hpp>
#include <ImathColorAlgo.h>
#include <ImathColor.h>
#include "util.h"

namespace pimath
{
	namespace bp = boost::python;

	struct ColorAlgoBindUnteplated
	{
		ColorAlgoBindUnteplated()
		{
			Imath::Color4<double> (*hsv2rgb_d4)(
					const Imath::Color4<double> &) = &Imath::hsv2rgb_d;
			Imath::Color4<double> (*rgb2hsv_d4)(
					const Imath::Color4<double> &) = &Imath::rgb2hsv_d;

			bp::def("hsv2rgb_d", hsv2rgb_d3);
			bp::def("hsv2rgb_d", hsv2rgb_d4);
			bp::def("rgb2hsv_d", rgb2hsv_d3);
			bp::def("rgb2hsv_d", rgb2hsv_d4);
		}

		static Imath::Color3<double> hsv2rgb_d3(
				const Imath::Vec3<double> & hsv)
		{
			return Imath::Color3<double>( Imath::hsv2rgb_d( hsv ));
		}

		static Imath::Color3<double> rgb2hsv_d3(
				const Imath::Vec3<double> & rgb)
		{
			return Imath::Color3<double>( Imath::rgb2hsv_d( rgb ));
		}
	};

	template<typename T>
	struct ColorAlgoBind
	{
		typedef Imath::Vec3<T> 		vec3_type;
		typedef Imath::Color3<T>	col3_type;
		typedef Imath::Color4<T>	col4_type;
		ColorAlgoBind()
		{
			col4_type (*hsv2rgb4)(
					const col4_type &) = &Imath::hsv2rgb;
			col4_type (*rgb2hsv4)(
					const col4_type &) = &Imath::rgb2hsv;
			Imath::PackedColor (*rgb2packed3)(
					const vec3_type &) = &Imath::rgb2packed;
			Imath::PackedColor (*rgb2packed4)(
					const col4_type &) = &Imath::rgb2packed;

			bp::def("hsv2rgb", hsv2rgb3);
			bp::def("hsv2rgb", hsv2rgb4);
			bp::def("rgb2hsv", rgb2hsv3);
			bp::def("rgb2hsv", rgb2hsv4);
			bp::def("rgb2packed", rgb2packed3);
			bp::def("rgb2packed", rgb2packed4);
			bp::def("packed2rgb", packed2rgb );
			bp::def("packed2rgba", packed2rgba );
		}

		static col3_type hsv2rgb3( const vec3_type & vec )
		{
			return col3_type( Imath::hsv2rgb( vec ) );
		}

		static col3_type rgb2hsv3( const vec3_type & vec )
		{
			return col3_type( Imath::rgb2hsv( vec ) );
		}

		static bp::object
		packed2rgb( Imath::PackedColor packed )
		{
			col3_type rv;
			Imath::packed2rgb<T>( packed, rv );
			return  bp::object( rv );
		}

		static bp::object
		packed2rgba( Imath::PackedColor packed )
		{
			col4_type rv;
			Imath::packed2rgb<T>( packed, rv);
			return  bp::object( rv );
		}
	};
}

#endif


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc, char const *argv[])
{
    // 证明1：
    cout << "证明1: " << endl;
    // 首先创建创建一个随机的四元数，并将其转变为旋转矩阵
    Vector3d q = Vector3d::Random();
    Isometry3d R = Isometry3d::Identity();                // 矩阵初始化
    R.rotate(q);

    //证明 R*R^T = I
    auto I = R * R.inverse();
    return 0;
}


#include <polyvec/polygon-tracer/error-metrics.hpp>

// polyfit
#include <polyvec/api.hpp>
#include <polyvec/geom.hpp>
#include <polyvec/geometry/path.hpp>
#include <polyvec/core/options.hpp>
#include <polyvec/debug.hpp>
#include <polyvec/geometry/angle.hpp>
#include <polyvec/core/options.hpp>

// c++ stl
#include <algorithm>

// Eigen
#include <Eigen/Geometry>

#define SMOOTHNESS_LIMIT VectorOptions::get()->error_smoothness_limit
#define CONTINUITY_LIMIT VectorOptions::get()->error_continuity_limit
#define INFLECTION_LIMIT VectorOptions::get()->error_inflection_limit
#define INFLECTION_PENALTY VectorOptions::get()->error_inflection_penalty

#define SMOOTHNESS_WEIGHT VectorOptions::get()->error_smoothness_weight
#define ACCURACY_WEIGHT VectorOptions::get()->error_accuracy_weight
#define CONTINUITY_WEIGHT VectorOptions::get()->error_continuity_weight

using namespace polyvec;
using namespace std;

namespace polyfit {
	namespace ErrorMetrics {
		double smoothness(const double rad0, const double rad1, const int flags) {
			const bool is_first = flags & EDGE_FIRST_IN_PATH;
			const bool is_last = flags & EDGE_LAST_IN_PATH;

			double e = 0.;
			e += .5 * (!(is_first) ? smoothness(rad0) : 0.);
			e += .5 * (!(is_last) ?  smoothness(rad1) : 0.);
			return e;
		}

		double smoothness(const double rad0) {
			return SMOOTHNESS_WEIGHT * min((M_PI - rad0) / SMOOTHNESS_LIMIT, 1.0);
		}

		double accuracy(const double d_min, const double d_max, const int flags) {
			const double overflow = max(d_max - 1., 0.) + max(d_min - .5, 0.);
			return ACCURACY_WEIGHT * d_min + max(d_max - .5, 0.) + overflow;
		}

		double accuracy_implicit(const double d_min, const double d_max, const int flags) {
			return ACCURACY_WEIGHT * 2 * max(0., d_max - .5);
		}

		double continuity(const double rad0, const double rad1, const int flags) {
			if ((flags & EDGE_LAST_IN_PATH) || (flags & EDGE_FIRST_IN_PATH)) {
				return 0.;
			}

			if (flags & EDGE_HAS_INFLECTION) {
                return CONTINUITY_WEIGHT * min(1., INFLECTION_PENALTY + (M_PI - min(rad0, rad1)) / INFLECTION_LIMIT  * (1. - INFLECTION_PENALTY));
                //const double a = min(2 * M_PI - rad0 - rad1, CONTINUITY_LIMIT);
                //return CONTINUITY_WEIGHT * a / CONTINUITY_LIMIT;
			}
			else {
				return CONTINUITY_WEIGHT * min(abs(rad0 - rad1) / CONTINUITY_LIMIT, 1.0);
			}
		}

		bool accuracy_within_bounds(const double d_min, const double d_max, const vec2& edge_dir) {
			// This is only checking the distance measures, not the error

			const double phi = atan2(edge_dir.cwiseAbs().minCoeff(), edge_dir.cwiseAbs().maxCoeff());
			const double d_eps = .1;
			// adaptive accuracy
			const double max_threshold = std::min(1.0, 0.5 + (.5 + d_eps) * cos(phi) - .5 * sin(phi));

			return (d_min <= .5 + PF_EPS && d_max <= max_threshold - PF_EPS);

			//return (d_min <= (.5 + PF_EPS) && d_max <= (1. - PF_EPS));
		}

		bool accuracy_within_bounds_relaxed(const double d_min, const double d_max) {
			return d_min < 1. - PF_EPS && d_max < 1. - PF_EPS;
		}

		vec3 calculate_at_edge(const mat2x& points, const mat24& p, const vec4i& v, int flags) {
#if 0
			assert_break(p.cols() == 4);

			const double rad0 = PathUtils::shortest_angle_spanned(p.col(0), p.col(1), p.col(2));
			const double rad1 = PathUtils::shortest_angle_spanned(p.col(1), p.col(2), p.col(3));
			flags |= PathUtils::are_consecutive_angles_opposite(p.col(0), p.col(1), p.col(2), p.col(3)) ? EDGE_HAS_INFLECTION : 0x0;

#if 1
			dbg::info(FMT("error-metric - angles:(%.3f %.3f) inflection: %d", geom::degrees(rad0), geom::degrees(rad1), flags & EDGE_HAS_INFLECTION));
#endif

			const vec2 d_err = PathUtils::distance_bounds_from_points(points, p.block(0, 1, 2, 2), v.segment(1, 2), );

			vec3 error;
			error(0) = smoothness(rad0, rad1, flags);
			error(1) = accuracy(d_err.x(), d_err.y(), flags);
			error(2) = continuity(rad0, rad1, flags);
			return error;
#endif
			return vec3(0., 0., 0.);
		}

		vec3 calculate_inner_sep(double d_min, double d_max, const vec2 p0, const vec2 p1, const vec2 p2, const vec2 p3) {
			const int flags = AngleUtils::have_opposite_convexity(p0, p1, p2, p3) ? EDGE_HAS_INFLECTION : 0x0;
			const double rad0 = AngleUtils::spanned_shortest(p0, p1, p2);
			const double rad1 = AngleUtils::spanned_shortest(p1, p2, p3);
			vec3 e;
			e(SMOOTHNESS) = ErrorMetrics::smoothness(rad0, rad1, flags);
			e(CONTINUITY) = ErrorMetrics::continuity(rad0, rad1, flags);

			if (VectorOptions::get()->accuracy_metric == AccuracyMetric::OneSided) {
				e(ACCURACY) = ErrorMetrics::accuracy(d_min, d_max, flags);
			}
			else if (VectorOptions::get()->accuracy_metric == AccuracyMetric::Implicit) {
				e(ACCURACY) = ErrorMetrics::accuracy_implicit(d_min, d_max, flags);
			}
			else {
				PF_ABORT;
			}
			return e;
		}

		vec3 calculate_first_sep(double d_min, double d_max, const vec2 p1, const vec2 p2, const vec2 p3) {
			const int flags = EDGE_FIRST_IN_PATH;
			const double rad1 = AngleUtils::spanned_shortest(p1, p2, p3);
			vec3 e;
			e(SMOOTHNESS) = ErrorMetrics::smoothness(M_PI, rad1, flags);
			e(CONTINUITY) = ErrorMetrics::continuity(M_PI, rad1, flags);
			if (VectorOptions::get()->accuracy_metric == AccuracyMetric::OneSided) {
				e(ACCURACY) = ErrorMetrics::accuracy(d_min, d_max, flags);
			}
			else if (VectorOptions::get()->accuracy_metric == AccuracyMetric::Implicit) {
				e(ACCURACY) = ErrorMetrics::accuracy_implicit(d_min, d_max, flags);
			}
			else {
				PF_ABORT;
			}
			return e;
		}

		vec3 calculate_last_sep(double d_min, double d_max, const vec2 p1, const vec2 p2, const vec2 p3) {
			const int flags = EDGE_FIRST_IN_PATH;
			const double rad1 = AngleUtils::spanned_shortest(p1, p2, p3);
			vec3 e;
			e(SMOOTHNESS) = ErrorMetrics::smoothness(M_PI, rad1, flags);
			e(CONTINUITY) = ErrorMetrics::continuity(M_PI, rad1, flags);
			if (VectorOptions::get()->accuracy_metric == AccuracyMetric::OneSided) {
				e(ACCURACY) = ErrorMetrics::accuracy(d_min, d_max, flags);
			}
			else if (VectorOptions::get()->accuracy_metric == AccuracyMetric::Implicit) {
				e(ACCURACY) = ErrorMetrics::accuracy_implicit(d_min, d_max, flags);
			}
			else {
				PF_ABORT;
			}
			return e;
		}

		double calculate_inner(double d_min, double d_max, const vec2 p0, const vec2 p1, const vec2 p2, const vec2 p3) {
			const vec3 e = calculate_inner_sep(d_min, d_max, p0, p1, p2, p3);
			return e.sum();
		}

		double calculate_first(double d_min, double d_max, const vec2 p1, const vec2 p2, const vec2 p3) {
			const vec3 e = calculate_first_sep(d_min, d_max, p1, p2, p3);
			return e.sum();
		}

		double calculate_last(double d_min, double d_max, const vec2 p1, const vec2 p2, const vec2 p3) {
			const vec3 e = calculate_last_sep(d_min, d_max, p1, p2, p3);
			return e.sum();
		}
	}
}


// Copyright 2015-2018 Hans Dembinski
//
// 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)

//[ guide_make_dynamic_histogram

#include <boost/histogram.hpp>
#include <cassert>
#include <sstream>
#include <vector>

const char* config = "4 1.0 2.0\n"
                     "5 3.0 4.0\n";

int main() {
  using namespace boost::histogram;

  // read axis config from a config file (mocked here with std::istringstream)
  // and create vector of regular axes, the number of axis is not known at compile-time
  std::istringstream is(config);
  auto v1 = std::vector<axis::regular<>>();
  while (is.good()) {
    unsigned bins;
    double start, stop;
    is >> bins >> start >> stop;
    v1.emplace_back(bins, start, stop);
  }

  // create histogram from iterator range
  // (copying or moving the vector also works, move is shown below)
  auto h1 = make_histogram(v1.begin(), v1.end());
  assert(h1.rank() == v1.size());

  // with a vector of axis::variant (polymorphic axis type that can hold any one of the
  // template arguments at a time) the types and number of axis can vary at run-time
  auto v2 = std::vector<axis::variant<axis::regular<>, axis::integer<>>>();
  v2.emplace_back(axis::regular<>(100, -1.0, 1.0));
  v2.emplace_back(axis::integer<>(1, 7));

  // create dynamic histogram by moving the vector
  auto h2 = make_histogram(std::move(v2));
  assert(h2.rank() == 2);
}

//]


#define _USE_MATH_DEFINES
#include "matplotlibcpp.h"
#include <Eigen/Dense>
#include <cmath>
#include <iostream>

namespace plt = matplotlibcpp;

int main() {
  // Prepare data.
  int n = 5000;
  Eigen::VectorXd x(n), y(n), z(n), w = Eigen::VectorXd::Ones(n);
  for (int i = 0; i < n; ++i) {
    double value = (1.0 + i) / n;
    x(i) = value;
    y(i) = value * value;
    z(i) = value * value * value;
  }

  // Plot line from given x and y data. Color is selected automatically.
  plt::semilogy(x, y);

  // Plot a red dashed line from given x and y data.
  plt::semilogy(x, w, {{"c", "r"}, {"ls", "--"}});

  // Plot a line whose name will show up as "log(x)" in the legend.
  plt::semilogy(x, z, "g:", {{"label", "$x^3$"}});

  // Add graph title
  plt::title("Sample figure");

  // Enable legend.
  plt::legend();

  // show figure
  plt::show();
}


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

#include <vector>

#include "cnn/tests/test_utils.h"
#include "cnn/tensor.h"
#include "cnn/saxe-init.h"

using namespace std;
using namespace cnn;

BOOST_GLOBAL_FIXTURE(TestTensorSetup)

BOOST_AUTO_TEST_CASE(EOrthonormalRandom)
{
  for (int d = 4; d < 128; d += 2) {
    Tensor Q = OrthonormalRandom(d, 1.0);
//    BOOST_REQUIRE_EQUAL(size(Q), Dim({d,d}));

    // check that this is actually returning orthogonal matrices
#if MINERVA_BACKEND
    Tensor I = Q.Trans() * Q;
#endif
#if THPP_BACKEND
    Tensor QT = Q;
    QT.transpose();
    //cerr << str(Q) << endl << str(QT) << endl;
    Tensor I = Zero({d,d});
    I.addmm(0, 1, Q, QT);
    //cerr << str(I) << endl;
#endif
#if EIGEN_BACKEND
    Tensor I = Q.transpose() * Q;
#endif
    double eps = 1e-1;
    for (int i = 0; i < d; ++i)
      for (int j = 0; j < d; ++j)
        BOOST_CHECK_CLOSE(t(I,i,j) + 1., (i == j ? 2. : 1.), eps);
  }
  cerr << "Finished\n";
}

BOOST_AUTO_TEST_CASE(BernoulliInit) {
  Tensor r = RandomBernoulli(Dim({1000,1000}), 0.5f);
  int tot = 0;
  for (int i = 0; i < 1000; ++i)
    for (int j = 0; j < 1000; ++j)
      if (t(r,i,j)) ++tot;
  BOOST_CHECK_GT(tot, 490000);
  BOOST_CHECK_LT(tot, 510000);
}

BOOST_AUTO_TEST_CASE(Rand01) {
  cnn::real tot = 0;
  for (unsigned i = 0; i < 1000000; ++i)
    tot += cnn::rand01();
  BOOST_CHECK_GT(tot, 490000.);
  BOOST_CHECK_LT(tot, 510000.);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

//[make_2d_point
//` Shows the usage of make as a generic constructor for different point types

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/adapted/boost_polygon/point.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

struct mypoint { float _x, _y; };

BOOST_GEOMETRY_REGISTER_POINT_2D(mypoint, float, cs::cartesian, _x, _y)

template <typename Point>
void construct_and_display()
{
    using boost::geometry::make;
    using boost::geometry::get;

    Point p = make<Point>(1, 2);

    std::cout << "x=" << get<0>(p) << " y=" << get<1>(p)
        << " (" << typeid(Point).name() << ")"
        << std::endl;
}

int main()
{
    construct_and_display<boost::geometry::model::d2::point_xy<double> >();
    construct_and_display<boost::geometry::model::d2::point_xy<int> >();
    construct_and_display<boost::tuple<double, double> >();
    construct_and_display<boost::polygon::point_data<int> >();
    construct_and_display<mypoint>();
    return 0;
}

//]

//[make_2d_point_output
/*`
Output (compiled using gcc):
[pre
x=1 y=2 (N5boost8geometry5model2d28point_xyIdNS0_2cs9cartesianEEE)
x=1 y=2 (N5boost8geometry5model2d28point_xyIiNS0_2cs9cartesianEEE)
x=1 y=2 (N5boost6tuples5tupleIddNS0_9null_typeES2_S2_S2_S2_S2_S2_S2_EE)
x=1 y=2 (N5boost7polygon10point_dataIiEE)
x=1 y=2 (7mypoint)
]
*/
//]