//  (C) Copyright Sylvain Joyeux 2006
//
// Original code from reverse_graph adaptor
//  (C) Copyright David Abrahams 2000.
//
// 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 UNDIRECTED_GRAPH_HPP
#define UNDIRECTED_GRAPH_HPP

#include <boost/graph/adjacency_iterator.hpp>
#include <boost/graph/properties.hpp>
#include <boost/static_assert.hpp>
#include "iterator_sequence.hh"
#include <boost/iterator/transform_iterator.hpp>

#include <boost/type_traits.hpp>

namespace utilmm {
    struct undirected_graph_tag { };

    /** undirected_graph creates an undirected graph from a bidirectional graph. This can allow
     * to use some undirected-graph-only algorithms on bidirectional graphs
     */
    template <class BidirectionalGraph, class GraphRef = const BidirectionalGraph&>
    class undirected_graph {
	typedef undirected_graph<BidirectionalGraph, GraphRef> Self;
	typedef boost::graph_traits<BidirectionalGraph> Traits;

	BOOST_STATIC_ASSERT(( boost::is_same<boost::bidirectional_tag, typename BidirectionalGraph::directed_category>::value ));

     public:
	typedef BidirectionalGraph base_type;

	// Constructor
	undirected_graph(GraphRef g) : m_g(g) {}

	// Graph requirements
	typedef typename Traits::vertex_descriptor vertex_descriptor;
	typedef boost::undirected_tag directed_category;
	typedef typename Traits::edge_parallel_category edge_parallel_category;
	typedef typename Traits::traversal_category traversal_category;

	// Add a 'reverse' flag to edge descriptors, so that source() and target() know what
	// needs to be done
	typedef std::pair<typename Traits::edge_descriptor, bool> edge_descriptor;
	typedef edge_descriptor (*make_undirected_edge_descriptor)(typename Traits::edge_descriptor);

	// Out edges do not need to be swapped
	static edge_descriptor make_out_edge_descriptor(typename Traits::edge_descriptor e)
	{ return std::make_pair(e, false); }
	class base_out_edge_iterator :
	    public boost::transform_iterator<make_undirected_edge_descriptor, typename Traits::out_edge_iterator>
	{
	    typedef boost::transform_iterator<make_undirected_edge_descriptor, typename Traits::out_edge_iterator>
		base_type;

	public:
	    base_out_edge_iterator() {}
	    base_out_edge_iterator(typename Traits::out_edge_iterator e)
		: base_type(e, make_out_edge_descriptor) {}
	};

	static edge_descriptor make_in_edge_descriptor(typename Traits::edge_descriptor e)
	{ return std::make_pair(e, true); }
	class base_in_edge_iterator :
	    public boost::transform_iterator<make_undirected_edge_descriptor, typename Traits::in_edge_iterator>
	{
	    typedef boost::transform_iterator<make_undirected_edge_descriptor, typename Traits::in_edge_iterator>
		base_type;

	public:
	    base_in_edge_iterator() {}
	    base_in_edge_iterator(typename Traits::in_edge_iterator e)
		: base_type(e, make_in_edge_descriptor) {}
	};

	// Do not swap plain edge iterators
	class edge_iterator :
	    public boost::transform_iterator<make_undirected_edge_descriptor, typename Traits::edge_iterator>
	{
	    typedef boost::transform_iterator<make_undirected_edge_descriptor, typename Traits::edge_iterator>
		base_type;
	public:
	    edge_iterator() {}
	    edge_iterator(typename Traits::edge_iterator e)
		: base_type(e, make_out_edge_descriptor) {}
	};

	// IncidenceGraph requirements
	typedef typename Traits::degree_size_type degree_size_type;
	typedef iterator_sequence
	    < base_in_edge_iterator
	    , base_out_edge_iterator
	    > in_edge_iterator;
	typedef in_edge_iterator out_edge_iterator;

	// AdjacencyGraph requirements
	typedef iterator_sequence
	    < typename Traits::adjacency_iterator
	    , typename BidirectionalGraph::inv_adjacency_iterator
	    > adjacency_iterator;
	typedef adjacency_iterator inv_adjacency_iterator;

	// VertexListGraph requirements
	typedef typename Traits::vertex_iterator vertex_iterator;

	// EdgeListGraph requirements
	typedef typename Traits::edge_iterator   traits_edge_iterator;
	typedef typename Traits::vertices_size_type vertices_size_type;
	typedef typename Traits::edges_size_type edges_size_type;

	// More typedefs used by detail::edge_property_map, vertex_property_map
	typedef typename BidirectionalGraph::edge_property_type
	  edge_property_type;
	typedef typename BidirectionalGraph::vertex_property_type
	  vertex_property_type;
	typedef undirected_graph_tag graph_tag;

	static vertex_descriptor null_vertex()
	{ return Traits::null_vertex(); }

	// Bundled properties support
        typename boost::graph::detail::bundled_result<BidirectionalGraph,
                 edge_descriptor>::type&
        operator[](edge_descriptor x)
        { return m_g[x.first]; }

        typename boost::graph::detail::bundled_result<BidirectionalGraph,
                 edge_descriptor>::type const&
        operator[](edge_descriptor x) const
        { return m_g[x.first]; }

        typename boost::graph::detail::bundled_result<BidirectionalGraph,
                 vertex_descriptor>::type&
        operator[](vertex_descriptor x)
        { return m_g[x]; }

        typename boost::graph::detail::bundled_result<BidirectionalGraph,
                 vertex_descriptor>::type const&
        operator[](vertex_descriptor x) const
        { return m_g[x]; }

	// would be private, but template friends aren't portable enough.
     // private:
	GraphRef m_g;
    };

    template <class BidirectionalGraph>
    inline undirected_graph<BidirectionalGraph>
    make_undirected_graph(const BidirectionalGraph& g)
    {
	return undirected_graph<BidirectionalGraph>(g);
    }

    template <class BidirectionalGraph>
    inline undirected_graph<BidirectionalGraph, BidirectionalGraph&>
    make_undirected_graph(BidirectionalGraph& g)
    {
	return undirected_graph<BidirectionalGraph, BidirectionalGraph&>(g);
    }

    template <class BidirectionalGraph, class GRef>
    std::pair<typename undirected_graph<BidirectionalGraph>::vertex_iterator,
	      typename undirected_graph<BidirectionalGraph>::vertex_iterator>
    vertices(const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	return vertices(g.m_g);
    }

    template <class BidirectionalGraph, class GRef>
    std::pair<typename undirected_graph<BidirectionalGraph>::edge_iterator,
	      typename undirected_graph<BidirectionalGraph>::edge_iterator>
    edges(const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	return edges(g.m_g);
    }

    template <class BidirectionalGraph, class GRef>
    inline std::pair<typename undirected_graph<BidirectionalGraph,GRef>::out_edge_iterator,
		     typename undirected_graph<BidirectionalGraph,GRef>::out_edge_iterator>
    out_edges(const typename BidirectionalGraph::vertex_descriptor u,
	      const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	std::pair<typename BidirectionalGraph::out_edge_iterator,
		  typename BidirectionalGraph::out_edge_iterator>
		      out_edges = boost::out_edges(u, g.m_g);
	std::pair<typename BidirectionalGraph::in_edge_iterator,
		  typename BidirectionalGraph::in_edge_iterator>
		      in_edges = boost::in_edges(u, g.m_g);

	typedef typename undirected_graph<BidirectionalGraph,GRef>::out_edge_iterator edge_iterator;
	return std::make_pair(
		edge_iterator(in_edges.first, in_edges.second, out_edges.first, out_edges.first),
		edge_iterator(in_edges.second, in_edges.second, out_edges.second, out_edges.second));
    }

    template <class BidirectionalGraph, class GRef>
    inline typename BidirectionalGraph::vertices_size_type
    num_vertices(const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	return num_vertices(g.m_g);
    }

    template <class BidirectionalGraph, class GRef>
    inline typename undirected_graph<BidirectionalGraph>::edges_size_type
    num_edges(const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	return num_edges(g.m_g);
    }

    template <class BidirectionalGraph, class GRef>
    inline typename BidirectionalGraph::degree_size_type
    out_degree(const typename BidirectionalGraph::vertex_descriptor u,
	       const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	return in_degree(u, g.m_g) + out_degree(u, g.m_g);
    }

    template <class BidirectionalGraph, class GRef>
    inline std::pair<typename BidirectionalGraph::edge_descriptor, bool>
    edge(const typename BidirectionalGraph::vertex_descriptor u,
	 const typename BidirectionalGraph::vertex_descriptor v,
	 const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	return edge(v, u, g.m_g);
    }

    template <class BidirectionalGraph, class GRef>
    inline std::pair<typename BidirectionalGraph::out_edge_iterator,
	typename BidirectionalGraph::out_edge_iterator>
    in_edges(const typename BidirectionalGraph::vertex_descriptor u,
	     const undirected_graph<BidirectionalGraph,GRef>& g)
    { return out_edges(u, g); }

    template <class BidirectionalGraph, class GRef>
    inline std::pair<typename undirected_graph<BidirectionalGraph,GRef>::adjacency_iterator,
		     typename undirected_graph<BidirectionalGraph,GRef>::adjacency_iterator>
    adjacent_vertices(const typename BidirectionalGraph::vertex_descriptor u,
		      const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	std::pair<typename BidirectionalGraph::adjacency_iterator,
		  typename BidirectionalGraph::adjacency_iterator>
		      adjacency = boost::adjacent_vertices(u, g.m_g);
	std::pair<typename BidirectionalGraph::inv_adjacency_iterator,
		  typename BidirectionalGraph::inv_adjacency_iterator>
		      inv_adjacency = boost::inv_adjacent_vertices(u, g.m_g);

	typedef typename undirected_graph<BidirectionalGraph,GRef>::adjacency_iterator adjacency_iterator;
	return std::make_pair(
		adjacency_iterator(adjacency.first, adjacency.second, inv_adjacency.first, inv_adjacency.first),
		adjacency_iterator(adjacency.second, adjacency.second, inv_adjacency.second, inv_adjacency.second));
    }

    template <class BidirectionalGraph, class GRef>
    inline typename BidirectionalGraph::degree_size_type
    in_degree(const typename BidirectionalGraph::vertex_descriptor u,
	      const undirected_graph<BidirectionalGraph,GRef>& g)
    { return out_degree(u, g); }

    template <class Edge, class BidirectionalGraph, class GRef>
    inline typename boost::graph_traits<BidirectionalGraph>::vertex_descriptor
    source(const Edge& e, const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	if (e.second)
	    return target(e.first, g.m_g);
	else
	    return source(e.first, g.m_g);
    }

    template <class Edge, class BidirectionalGraph, class GRef>
    inline typename boost::graph_traits<BidirectionalGraph>::vertex_descriptor
    target(const Edge& e, const undirected_graph<BidirectionalGraph,GRef>& g)
    {
	if (e.second)
	    return source(e.first, g.m_g);
	else
	    return target(e.first, g.m_g);
    }

    namespace detail {

      struct undirected_graph_vertex_property_selector {
	template <class UndirectedGraph, class Property, class Tag>
	struct bind_ {
	  typedef typename UndirectedGraph::base_type Graph;
	  typedef boost::property_map<Graph, Tag> PMap;
	  typedef typename PMap::type type;
	  typedef typename PMap::const_type const_type;
	};
      };

      struct undirected_graph_edge_property_selector {
	template <class UndirectedGraph, class Property, class Tag>
	struct bind_ {
	  typedef typename UndirectedGraph::base_type Graph;
	  typedef boost::property_map<Graph, Tag> PMap;
	  typedef typename PMap::type type;
	  typedef typename PMap::const_type const_type;
	};
      };

    } // namespace detail

    /** This class adapts a property map on the edges of a graph to a property map
     * on the edges of the corresponding undirected graph */
    template<typename PropertyMap>
    class undirected_property_map
    {
    public:
	undirected_property_map(PropertyMap pmap)
	    : m_map(pmap) {}

	PropertyMap& map() { return m_map; }
	PropertyMap const& map() const { return m_map; }

    private:
	PropertyMap m_map;
    };

    template<typename PropertyMap, typename EdgeDescriptor, typename ValueType>
    void put(undirected_property_map<PropertyMap>& map,
	    EdgeDescriptor e, ValueType value)
    { put(map.map(), e.first, value); }
    template<typename PropertyMap, typename EdgeDescriptor>
    typename boost::property_traits<PropertyMap>::value_type
	get(undirected_property_map<PropertyMap> const& map,
	    EdgeDescriptor e)
    { return get(map.map(), e.first); }

    template<typename PropertyMap>
    undirected_property_map<PropertyMap> make_undirected_edge_map(PropertyMap pmap)
    { return undirected_property_map<PropertyMap>(pmap); }

} // namespace utilmm

// Include specialization in the boost namespace
namespace boost {
    template<typename PropertyMap>
    struct property_traits< utilmm::undirected_property_map<PropertyMap> >
	: property_traits<PropertyMap> {};

    template <>
    struct vertex_property_selector<utilmm::undirected_graph_tag> {
	typedef utilmm::detail::undirected_graph_vertex_property_selector type;
    };

    template <>
    struct edge_property_selector<utilmm::undirected_graph_tag> {
	typedef utilmm::detail::undirected_graph_edge_property_selector type;
    };

    namespace detail {
	template<typename BidirGraph, typename GRef, typename Property>
	struct get_property_map_type { };
	template<typename BidirGraph, typename Property>
	struct get_property_map_type<BidirGraph, const BidirGraph&, Property>
	{ typedef typename property_map<BidirGraph, Property>::const_type type; };
	template<typename BidirGraph, typename Property>
	struct get_property_map_type<BidirGraph, BidirGraph&, Property>
	{ typedef typename property_map<BidirGraph, Property>::type type; };
    }

    template <class BidirGraph, class GRef, class Property>
    typename detail::get_property_map_type<BidirGraph, GRef, Property>::type
    get(Property p, utilmm::undirected_graph<BidirGraph,GRef>& g)
    {
      return get(p, g.m_g);
    }

    template <class BidirGraph, class GRef, class Property>
    typename detail::get_property_map_type<BidirGraph, GRef, Property>::type
    get(Property p, const utilmm::undirected_graph<BidirGraph,GRef>& g)
    {
      return get(p, g.m_g);
    }

    template <class BidirectionalGraph, class GRef, class Property, class Key>
    typename property_traits<
      typename property_map<BidirectionalGraph, Property>::const_type
    >::value_type
    get(Property p, const utilmm::undirected_graph<BidirectionalGraph,GRef>& g, const Key& k)
    {
      return get(p, g.m_g, k);
    }

    template <class BidirectionalGraph, class GRef, class Property, class Key, class Value>
    void
    put(Property p, const utilmm::undirected_graph<BidirectionalGraph,GRef>& g, const Key& k,
	const Value& val)
    {
      put(p, g.m_g, k, val);
    }

    template<typename BidirectionalGraph, typename GRef, typename Tag,
	     typename Value>
    inline void
    set_property(const utilmm::undirected_graph<BidirectionalGraph,GRef>& g, Tag tag,
		 const Value& value)
    {
      set_property(g.m_g, tag, value);
    }

    template<typename BidirectionalGraph, typename GRef, typename Tag>
    inline
    typename graph_property<BidirectionalGraph, Tag>::type
    get_property(const utilmm::undirected_graph<BidirectionalGraph,GRef>& g, Tag tag)
    {
      return get_property(g.m_g, tag);
    }

} // namespace boost

#endif


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
* @file vertex_vertex_thorup2kminus1_test.cpp
* @brief
* @author Jakub Oćwieja
* @version 1.0
* @date 2014-05-11
*/
#include "paal/distance_oracle/vertex_vertex/thorup_2kminus1.hpp"
#include "paal/data_structures/metric/graph_metrics.hpp"

#include "test_utils/sample_graph.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/utils/irange.hpp"

#include <boost/graph/copy.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/test/unit_test.hpp>

using namespace paal;
using SGM = sample_graphs_metrics;

template <typename Graph, typename Oracle, typename GraphMetric, typename IndexMap>
void perform_full_check_helper(const Graph& g, const Oracle& oracle, const GraphMetric& gm, double ratio, IndexMap index) {

    for (auto v: boost::make_iterator_range(vertices(g))) {
        for (auto u: boost::make_iterator_range(vertices(g))) {
            check_result(oracle(u, v), gm(index[u], index[v]), ratio);
        }
    }
}

template <typename Graph, typename Oracle>
void perform_full_check(const Graph& g, const Oracle& oracle, double ratio) {
    using DistanceType = typename boost::property_map<Graph, boost::edge_weight_t>::value_type;
    data_structures::graph_metric<Graph, DistanceType> gm(g);

    perform_full_check_helper(g, oracle, gm, ratio, get(boost::vertex_index, g));
}

template <typename Graph, typename Oracle, typename IndexMap>
void perform_full_check(const Graph& g, const Oracle& oracle, double ratio, IndexMap index) {
    using DistanceType = typename boost::property_map<Graph, boost::edge_weight_t>::value_type;

    // TODO Extend graph_metric interface to accept graphs with external parameters (in this case index_map)
    SGM::Graph gg;
    boost::copy_graph(g, gg, vertex_index_map(index));
    data_structures::graph_metric<SGM::Graph, DistanceType> gm(gg);

    perform_full_check_helper(g, oracle, gm, ratio, index);
}

BOOST_AUTO_TEST_CASE( vv_thorup2kminus1_star_test ) {
    auto g = SGM::get_star_medium();

    for (int k: {2,3,4,5}) {
        perform_full_check(g, make_distance_oracle_thorup2kminus1approximation(g, k), k*2-1);
    }
}

BOOST_AUTO_TEST_CASE( vv_thorup2kminus1_random_star_test ) {
    for (int s: irange(10)) {
        auto g = SGM::get_star_random(s, 10, 20, 50);
        for (auto k: {2,3,4,5}) {
            perform_full_check(g, make_distance_oracle_thorup2kminus1approximation(g, k), 2*k-1);
        }
    }
}

BOOST_AUTO_TEST_CASE( vv_thorup2kminus1_general_test ) {
    auto g = SGM::get_graph_medium();

    for (int k: {2,3,4,5}) {
        perform_full_check(g, make_distance_oracle_thorup2kminus1approximation(g, k), k*2-1);
    }
}

BOOST_AUTO_TEST_CASE( vv_thorup2kminus1_listgraph_test ) {
    SGM::ListGraph g = SGM::get_list_graph_medium();

    using VD = boost::graph_traits<SGM::ListGraph>::vertex_descriptor;
    using Map = std::map<VD, int>;
    using IndexMap = boost::associative_property_map<Map>;

    Map inner_map;
    IndexMap index_map(inner_map);

    for (auto v: boost::make_iterator_range(vertices(g)) | boost::adaptors::indexed()) {
        boost::put(index_map, v.value(), v.index());
    }

    for (int k: {2,3,4,5}) {
        perform_full_check(g, make_distance_oracle_thorup2kminus1approximation(g, k, vertex_index_map(index_map)), k*2-1, index_map);
    }
}


#include <demo/color.h>
#include <demo/memcpy.h>

#include <boost/simd.hpp>
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/store.hpp>

#include <intrin.h>
#include <omp.h>

#include <cstdint>
#include <cstring>
#include <memory>
#include <fstream>

bench::time_point bench::start_;

int main(int, char**)
{
    std::ofstream stream;
    omp_set_num_threads(8);

    stream.open("memory.csv");
    run_memory_benchmark(stream);
    stream.close();

    stream.open("color.csv");
    run_color_benchmark(stream);
    stream.close();

    return 0;
}

void* memcpy_parallel(void* destination, const void* source, size_t num)
{
    auto dst = reinterpret_cast<int8_t*>(destination);
    auto src = reinterpret_cast<const int8_t*>(source);

#pragma omp parallel for
    for (int i = 0; i < num; ++i) {
        dst[i] = src[i];
    }

    return destination;
}

void* memcpy_vector(void* destination, const void* source, size_t num)
{
    const char* src = (const char*)source;
    char* dst = (char*)destination;

    if (!((uintptr_t)src & 15) && !((uintptr_t)dst & 15)) {
        __m128 values[4];
        for (size_t i = num / 64; i--;) {
            _mm_prefetch(src, _MM_HINT_NTA);
            values[0] = *(__m128*)(src + 0);
            values[1] = *(__m128*)(src + 16);
            values[2] = *(__m128*)(src + 32);
            values[3] = *(__m128*)(src + 48);
            _mm_stream_ps((float*)(dst + 0), values[0]);
            _mm_stream_ps((float*)(dst + 16), values[1]);
            _mm_stream_ps((float*)(dst + 32), values[2]);
            _mm_stream_ps((float*)(dst + 48), values[3]);
            src += 64;
            dst += 64;
        }
        num &= 63;
    }

    while (num--) {
        *dst++ = *src++;
    }
    return destination;
}

#define MIN(x, y) ((x) < (y) ? (x) : (y))
#define MAX(x, y) ((x) > (y) ? (x) : (y))
#define CLIP(x, min, max) (MIN(MAX((x), (min)), (max)))

float* yuv_to_rgba(const float* yuv, int size)
{
    const int rgba_size = size / 3 * 4;
    auto rgba = new float[rgba_size];

    for (int i = 0, j = 0; i < size && j < rgba_size; i += 3, j += 4) {
        const auto y = yuv[i];
        const auto u = yuv[i + 1];
        const auto v = yuv[i + 2];
        rgba[j] = CLIP(y + 1.140f * v, .0f, 1.f);
        rgba[j + 1] = CLIP(y - 0.395f * u - 0.581f * v, .0f, 1.f);
        rgba[j + 2] = CLIP(y + 2.032f * u, .0f, 1.f);
        rgba[j + 3] = 1.f;
    }

    return rgba;
}

float* yuv_to_rgba_parallel(const float* yuv, int size)
{
    const int pixel_size = size / 3;
    const int rgba_size = size / 3 * 4;
    auto rgba = new float[rgba_size];

#pragma omp parallel for
    for (int p = 0; p < pixel_size; ++p) {
        const auto i = p * 3;
        const auto j = p * 4;
        const auto y = yuv[i];
        const auto u = yuv[i + 1];
        const auto v = yuv[i + 2];
        rgba[j] = CLIP(y + 1.140f * v, .0f, 1.f);
        rgba[j + 1] = CLIP(y - 0.395f * u - 0.581f * v, .0f, 1.f);
        rgba[j + 2] = CLIP(y + 2.032f * u, .0f, 1.f);
        rgba[j + 3] = 1.f;
    }

    return rgba;
}

constexpr void yuv_to_rgba_vector_result(int j, float* rgba, float* r_store, float* g_store, float* b_store)
{
    for (int n = 0; n < 4; ++n) {
        rgba[j + n * 4] = CLIP(r_store[n], .0f, 1.f);
        rgba[j + n * 4 + 1] = CLIP(g_store[n], .0f, 1.f);
        rgba[j + n * 4 + 2] = CLIP(b_store[n], .0f, 1.f);
        rgba[j + n * 4 + 3] = 1.f;
    }
}

float* yuv_to_rgba_vector(const float* yuv, int size)
{
    static_assert(boost::simd::pack<float>::static_size == 4, "boost pack is not of size 4");
    const int pixel_size = size / 3;
    const int rgba_size = size / 3 * 4;
    auto rgba = new float[rgba_size];
    std::array<float, 4> r_store;
    std::array<float, 4> g_store;
    std::array<float, 4> b_store;

    for (int i = 0, j = 0; i < size && j < rgba_size; i += 3 * 4, j += 4 * 4) {
        boost::simd::pack<float, 4> y{yuv[i], yuv[i + 3], yuv[i + 6], yuv[i + 9]};
        boost::simd::pack<float, 4> u{yuv[i + 1], yuv[i + 4], yuv[i + 7], yuv[i + 10]};
        boost::simd::pack<float, 4> v{yuv[i + 2], yuv[i + 5], yuv[i + 8], yuv[i + 11]};
        auto r = y + 1.140f * v;
        auto g = y - 0.395f * u - 0.581f * v;
        auto b = y + 2.032f * u;
        boost::simd::store(r, r_store.data());
        boost::simd::store(g, g_store.data());
        boost::simd::store(b, b_store.data());
        yuv_to_rgba_vector_result(j, rgba, r_store.data(), g_store.data(), b_store.data());
    }

    return rgba;
}


#include <boost/core/enable_if.hpp>
#include <type_traits>
#include <iostream>

template <typename T>
typename boost::disable_if_c<std::is_pointer_v<T>>::type print(T t)
{
	std::cout << t << '\n';
}

template <typename T>
typename boost::enable_if_c<std::is_pointer_v<T>>::type print(T t)
{
	std::cout << *t << '\n';
}

int main()
{
	int i{ 1 };
	print(i);

	int* p{ &i };
	print(p);
}


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <vector>

#include "matrix.pb.h"
#include "src/collectors/file_collector.h"
#include "src/collectors/memory_collector.h"
#include "src/utils/proto_utils.h"

TEST(collectors, memory) {
  MemoryCollector coll;
  coll.start_collecting();

  std::vector<Eigen::VectorXd> chain(5);
  for (int i = 0; i < 5; i++) {
    chain[i] = Eigen::VectorXd::Ones(3) * i;
    bayesmix::Vector curr;
    to_proto(chain[i], &curr);
    coll.collect(curr);
  }
  coll.finish_collecting();

  int iter = 0;
  bool keep = true;
  while (keep) {
    bayesmix::Vector curr;
    keep = coll.get_next_state(&curr);
    if (!keep) {
      iter--;
      break;
    }
    ASSERT_EQ(curr.size(), 3);
    ASSERT_EQ(curr.data(0), iter);
    iter++;
  }

  ASSERT_EQ(chain[iter](0), chain[4][0]);
}

TEST(collectors, file_writing) {
  FileCollector coll("test.recordio");
  coll.start_collecting();
  std::vector<Eigen::VectorXd> chain(5);
  for (int i = 0; i < 5; i++) {
    chain[i] = Eigen::VectorXd::Ones(3) * i;
    bayesmix::Vector curr;
    to_proto(chain[i], &curr);
    coll.collect(curr);
  }
  coll.finish_collecting();
}

TEST(collectors, file_reading) {
  FileCollector coll("test.recordio");
  coll.start_collecting();

  std::vector<Eigen::VectorXd> chain(5);
  for (int i = 0; i < 5; i++) {
    chain[i] = Eigen::VectorXd::Ones(3) * i;
    bayesmix::Vector curr;
    to_proto(chain[i], &curr);
    coll.collect(curr);
  }
  coll.finish_collecting();

  FileCollector coll2("test.recordio");
  int iter = 0;
  bool keep = true;
  while (keep) {
    bayesmix::Vector curr;
    keep = coll2.get_next_state(&curr);
    if (!keep) {
      iter--;
      break;
    }
    ASSERT_EQ(curr.size(), 3);
    ASSERT_EQ(curr.data(0), iter);
    iter++;
  }
  ASSERT_EQ(chain[iter](0), chain[4][0]);
}


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

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/algorithm/reduce.hpp>
#include <boost/histogram/algorithm/sum.hpp>
#include <boost/histogram/axis/category.hpp>
#include <boost/histogram/axis/integer.hpp>
#include <boost/histogram/axis/regular.hpp>
#include <boost/histogram/axis/variable.hpp>
#include <boost/histogram/detail/throw_exception.hpp>
#include <boost/histogram/unsafe_access.hpp>
#include <vector>
#include "utility_histogram.hpp"

using namespace boost::histogram;
using namespace boost::histogram::algorithm;

template <typename Tag>
void run_tests() {
  // reduce:
  // - does not work with arguments not convertible to double
  // - does not work with category axis, which is not ordered

  using regular = axis::regular<double, axis::transform::id, axis::null_type>;
  {
    auto h = make_s(Tag(), std::vector<int>(), regular(4, 1, 5), regular(3, -1, 2));

    // not allowed: invalid axis index
    BOOST_TEST_THROWS((void)reduce(h, slice(10, 2, 3)), std::invalid_argument);
    // not allowed: repeated indices
    BOOST_TEST_THROWS((void)reduce(h, slice(1, 0, 2), slice(1, 1, 3)),
                      std::invalid_argument);
    BOOST_TEST_THROWS((void)reduce(h, rebin(0, 2), rebin(0, 2)), std::invalid_argument);
    BOOST_TEST_THROWS((void)reduce(h, shrink(1, 0, 2), shrink(1, 0, 2)),
                      std::invalid_argument);
    // not allowed: slice with begin >= end
    BOOST_TEST_THROWS((void)reduce(h, slice(0, 1, 1)), std::invalid_argument);
    BOOST_TEST_THROWS((void)reduce(h, slice(0, 2, 1)), std::invalid_argument);
    // not allowed: shrink with lower == upper
    BOOST_TEST_THROWS((void)reduce(h, shrink(0, 0, 0)), std::invalid_argument);
    // not allowed: shrink axis to zero size
    BOOST_TEST_THROWS((void)reduce(h, shrink(0, 10, 11)), std::invalid_argument);
    // not allowed: rebin with zero merge
    BOOST_TEST_THROWS((void)reduce(h, rebin(0, 0)), std::invalid_argument);

    /*
      matrix layout:
      x ->
    y 1 0 1 0
    | 1 1 0 0
    v 0 2 1 3
    */
    h.at(0, 0) = 1;
    h.at(0, 1) = 1;
    h.at(1, 1) = 1;
    h.at(1, 2) = 2;
    h.at(2, 0) = 1;
    h.at(2, 2) = 1;
    h.at(3, 2) = 3;

    // should do nothing, index order does not matter
    auto hr = reduce(h, shrink(1, -1, 2), rebin(0, 1));
    BOOST_TEST_EQ(hr.rank(), 2);
    BOOST_TEST_EQ(sum(hr), 10);
    BOOST_TEST_EQ(hr.axis(0).size(), h.axis(0).size());
    BOOST_TEST_EQ(hr.axis(1).size(), h.axis(1).size());
    BOOST_TEST_EQ(hr.axis(0).bin(0).lower(), 1);
    BOOST_TEST_EQ(hr.axis(0).bin(3).upper(), 5);
    BOOST_TEST_EQ(hr.axis(1).bin(0).lower(), -1);
    BOOST_TEST_EQ(hr.axis(1).bin(2).upper(), 2);
    BOOST_TEST_EQ(hr, h);
    hr = reduce(h, slice(1, 0, 4), slice(0, 0, 4));
    BOOST_TEST_EQ(hr, h);

    hr = reduce(h, shrink(0, 2, 4));
    BOOST_TEST_EQ(hr.rank(), 2);
    BOOST_TEST_EQ(sum(hr), 10);
    BOOST_TEST_EQ(hr.axis(0).size(), 2);
    BOOST_TEST_EQ(hr.axis(1).size(), h.axis(1).size());
    BOOST_TEST_EQ(hr.axis(0).bin(0).lower(), 2);
    BOOST_TEST_EQ(hr.axis(0).bin(1).upper(), 4);
    BOOST_TEST_EQ(hr.axis(1).bin(0).lower(), -1);
    BOOST_TEST_EQ(hr.axis(1).bin(2).upper(), 2);
    BOOST_TEST_EQ(hr.at(-1, 0), 1); // underflow
    BOOST_TEST_EQ(hr.at(0, 0), 0);
    BOOST_TEST_EQ(hr.at(1, 0), 1);
    BOOST_TEST_EQ(hr.at(2, 0), 0); // overflow
    BOOST_TEST_EQ(hr.at(-1, 1), 1);
    BOOST_TEST_EQ(hr.at(0, 1), 1);
    BOOST_TEST_EQ(hr.at(1, 1), 0);
    BOOST_TEST_EQ(hr.at(2, 1), 0);
    BOOST_TEST_EQ(hr.at(-1, 2), 0);
    BOOST_TEST_EQ(hr.at(0, 2), 2);
    BOOST_TEST_EQ(hr.at(1, 2), 1);
    BOOST_TEST_EQ(hr.at(2, 2), 3);

    /*
      matrix layout:
      x ->
    y 1 0 1 0
    | 1 1 0 0
    v 0 2 1 3
    */

    hr = reduce(h, shrink_and_rebin(0, 2, 5, 2), rebin(1, 3));
    BOOST_TEST_EQ(hr.rank(), 2);
    BOOST_TEST_EQ(sum(hr), 10);
    BOOST_TEST_EQ(hr.axis(0).size(), 1);
    BOOST_TEST_EQ(hr.axis(1).size(), 1);
    BOOST_TEST_EQ(hr.axis(0).bin(0).lower(), 2);
    BOOST_TEST_EQ(hr.axis(0).bin(0).upper(), 4);
    BOOST_TEST_EQ(hr.axis(1).bin(0).lower(), -1);
    BOOST_TEST_EQ(hr.axis(1).bin(0).upper(), 2);
    BOOST_TEST_EQ(hr.at(-1, 0), 2); // underflow
    BOOST_TEST_EQ(hr.at(0, 0), 5);
    BOOST_TEST_EQ(hr.at(1, 0), 3); // overflow

    // test overload that accepts iterable and test option fusion
    std::vector<reduce_option> opts{{shrink(0, 2, 5), rebin(0, 2), rebin(1, 3)}};
    auto hr2 = reduce(h, opts);
    BOOST_TEST_EQ(hr2, hr);
    opts = {rebin(0, 2), slice(0, 1, 4), rebin(1, 3)};
    auto hr3 = reduce(h, opts);
    BOOST_TEST_EQ(hr3, hr);
  }

  // mixed axis types
  {
    axis::regular<> r(5, 0.0, 1.0);
    axis::variable<> v{{1., 2., 3.}};
    axis::category<int> c{{1, 2, 3}};
    auto h = make(Tag(), r, v, c);
    auto hr = algorithm::reduce(h, shrink(0, 0.2, 0.7));
    BOOST_TEST_EQ(hr.axis(0).size(), 3);
    BOOST_TEST_EQ(hr.axis(0).bin(0).lower(), 0.2);
    BOOST_TEST_EQ(hr.axis(0).bin(2).upper(), 0.8);
    BOOST_TEST_EQ(hr.axis(1).size(), 2);
    BOOST_TEST_EQ(hr.axis(1).bin(0).lower(), 1);
    BOOST_TEST_EQ(hr.axis(1).bin(1).upper(), 3);
    BOOST_TEST_THROWS((void)algorithm::reduce(h, rebin(2, 2)), std::invalid_argument);
  }

  // reduce on integer axis, rebin must fail
  {
    auto h = make(Tag(), axis::integer<>(1, 4));
    BOOST_TEST_THROWS((void)reduce(h, rebin(2)), std::invalid_argument);
    auto hr = reduce(h, shrink(2, 3));
    BOOST_TEST_EQ(hr.axis().size(), 1);
    BOOST_TEST_EQ(hr.axis().bin(0), 2);
    BOOST_TEST_EQ(hr.axis().bin(1), 3);
  }

  // reduce on circular axis, shrink must fail, also rebin with remainder
  {
    auto h = make(Tag(), axis::circular<>(4, 1, 4));
    BOOST_TEST_THROWS((void)reduce(h, shrink(0, 2)), std::invalid_argument);
    BOOST_TEST_THROWS((void)reduce(h, rebin(3)), std::invalid_argument);
    auto hr = reduce(h, rebin(2));
    BOOST_TEST_EQ(hr.axis().size(), 2);
    BOOST_TEST_EQ(hr.axis().bin(0).lower(), 1);
    BOOST_TEST_EQ(hr.axis().bin(1).upper(), 4);
  }

  // reduce on variable axis
  {
    auto h = make(Tag(), axis::variable<>({0, 1, 2, 3, 4, 5, 6}));
    auto hr = reduce(h, shrink_and_rebin(1, 5, 2));
    BOOST_TEST_EQ(hr.axis().size(), 2);
    BOOST_TEST_EQ(hr.axis().value(0), 1);
    BOOST_TEST_EQ(hr.axis().value(1), 3);
    BOOST_TEST_EQ(hr.axis().value(2), 5);
  }

  // reduce on axis with inverted range
  {
    auto h = make(Tag(), regular(4, 2, -2));
    auto hr = reduce(h, shrink(1, -1));
    BOOST_TEST_EQ(hr.axis().size(), 2);
    BOOST_TEST_EQ(hr.axis().bin(0).lower(), 1);
    BOOST_TEST_EQ(hr.axis().bin(1).upper(), -1);
  }
}

int main() {
  run_tests<static_tag>();
  run_tests<dynamic_tag>();

  return boost::report_errors();
}


#include <boost/simd/memory/aligned_array.hpp>
#include <boost/simd/memory/is_aligned.hpp>
#include <boost/assert.hpp>

int main()
{
  boost::simd::aligned_array<float,16> x;
  BOOST_ASSERT( boost::simd::is_aligned(&x[0],16) );

  for(std::size_t i=0;i<x.size();++i)
    x[i] = 1.f+i;
}


/**
 * @file test_automata.cpp
 * @author Miroslav Cibulka
 * @created 12/5/15
 * @copyright Copyright (c) 2015 XXX
 * @detail
 *
 */

#include <boost/test/unit_test.hpp>

#include "support.hh"

BOOST_AUTO_TEST_SUITE(automata_tests)

      BOOST_AUTO_TEST_CASE(regression)
        {
          VanNeumannCAutomata automata(.7f);

          for (auto &node : automata())
            BOOST_MESSAGE(node->getState().getValue());

          automata.notify();

          BOOST_MESSAGE("\n\n");

          for (auto &node : automata())
            BOOST_MESSAGE(node->getState().getValue());
        }

  BOOST_AUTO_TEST_SUITE_END()


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

  Copyright 2016 NumScale SAS
  Copyright 2016 J.T. Lapreste

  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/simd/function/insert.hpp>
#include <boost/simd/detail/unroll.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <array>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template<typename A, typename P, typename... N>
void f( brigand::list<N...> const&, A& a, P& p)
{
  using T = typename P::value_type;
  BOOST_SIMD_LOCAL_UNROLL( a[N::value] = T(N::value+1) );
  BOOST_SIMD_LOCAL_UNROLL( (bs::insert(p, N::value, T(N::value+1))) );
}

template <typename T, std::size_t N, typename Env>
void test_st(Env& $)
{
  bs::pack<T, N>  p;
  std::array<T,N> a;

  f( brigand::range<std::size_t, 0, N>(),a,p);

  bs::pack<T, N> ref(&a[0], &a[0] + N);
  STF_IEEE_EQUAL(ref, p);
}

STF_CASE_TPL("Check static insert on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = boost::simd::pack<T>::static_size;

  test_st<T, N>($);
  test_st<T, N/2>($);
  test_st<T, N*2>($);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   Domagoj Saric, Little Endian Ltd.
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef INTERLEAVED_DATA_TRANSFORMATION_HPP_INCLUDED
#define INTERLEAVED_DATA_TRANSFORMATION_HPP_INCLUDED
//------------------------------------------------------------------------------
#include <nt2/signal/details/missing_functionality.hpp> //...mrmlj...to be moved elsewhere...

#include <boost/simd/include/functions/simd/deinterleave_first.hpp>
#include <boost/simd/include/functions/simd/deinterleave_second.hpp>
#include <boost/simd/include/functions/simd/interleave_first.hpp>
#include <boost/simd/include/functions/simd/interleave_second.hpp>

#include <boost/dispatch/attributes.hpp>

#include <boost/assert.hpp>

#include <cstddef>
//------------------------------------------------------------------------------
namespace boost
{
//------------------------------------------------------------------------------
namespace simd
{
//------------------------------------------------------------------------------
namespace details
{
//------------------------------------------------------------------------------

template <typename Vector>
static void interleave_two_channels
(
    Vector const * BOOST_DISPATCH_RESTRICT p_channel0,
    Vector const * BOOST_DISPATCH_RESTRICT p_channel1,
    Vector       * BOOST_DISPATCH_RESTRICT p_interleaved,
    std::size_t                            size
)
{
    BOOST_ASSERT_MSG( size % 2 == 0, "Two way interleaved data must have an even number of elements." );
    size /= 2;
    while ( size-- )
    {
        Vector const channel0( *p_channel0++ );
        Vector const channel1( *p_channel1++ );

        Vector * BOOST_DISPATCH_RESTRICT const p_data0( p_interleaved++ );
        Vector * BOOST_DISPATCH_RESTRICT const p_data1( p_interleaved++ );

        typename Vector::native_type const pair0( boost::simd::interleave_first ( channel0, channel1 ) );
        typename Vector::native_type const pair1( boost::simd::interleave_second( channel0, channel1 ) );

        *p_data0 = pair0;
        *p_data1 = pair1;
    }
}

template <typename Vector>
static void deinterleave_two_channels
(
    Vector const * BOOST_DISPATCH_RESTRICT p_interleaved,
    Vector       * BOOST_DISPATCH_RESTRICT p_channel0,
    Vector       * BOOST_DISPATCH_RESTRICT p_channel1,
    std::size_t                            size
)
{
    BOOST_ASSERT_MSG( size % 2 == 0, "Two way interleaved data must have an even number of elements." );
    size /= 2;
    while ( size-- )
    {
        Vector const pair0( *p_interleaved++ );
        Vector const pair1( *p_interleaved++ );

        *p_channel0++ = boost::simd::deinterleave_first ( pair0, pair1 );
        *p_channel1++ = boost::simd::deinterleave_second( pair0, pair1 );
    }
}

//------------------------------------------------------------------------------
} // namespace details
//------------------------------------------------------------------------------
} // namespace simd
//------------------------------------------------------------------------------
} // namespace boost
//------------------------------------------------------------------------------

#endif // interleaved_data_transformation_hpp


/*
 [auto_generated]
 boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp

 [begin_description]
 Base class for all explicit Runge Kutta steppers.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 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_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_STEPPER_BASE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_STEPPER_BASE_HPP_INCLUDED

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>

namespace boost {
namespace numeric {
namespace odeint {

/*
 * base class for explicit steppers
 * models the stepper concept
 *
 * this class provides the following overloads
    * do_step( sys , x , t , dt )
    * do_step( sys , in , t , out , dt )
    * do_step( sys , x , dxdt_in , t , dt )
    * do_step( sys , in , dxdt_in , t , out , dt )
 */

template<
class Stepper ,
unsigned short Order ,
class State ,
class Value ,
class Deriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
class explicit_stepper_base : public algebra_stepper_base< Algebra , Operations >
{
public:

    #ifndef DOXYGEN_SKIP
    typedef explicit_stepper_base< Stepper , Order , State , Value , Deriv , Time , Algebra , Operations , Resizer > internal_stepper_base_type;
    #endif // DOXYGEN_SKIP

    typedef State state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef Time time_type;
    typedef Resizer resizer_type;
    typedef Stepper stepper_type;
    typedef stepper_tag stepper_category;
    typedef algebra_stepper_base< Algebra , Operations > algebra_stepper_base_type;
    typedef typename algebra_stepper_base_type::algebra_type algebra_type;
    typedef typename algebra_stepper_base_type::operations_type operations_type;
    typedef unsigned short order_type;

    #ifndef DOXYGEN_SKIP
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    #endif // DOXYGEN_SKIP

    static const order_type order_value = Order;

    explicit_stepper_base( const algebra_type &algebra = algebra_type() )
    : algebra_stepper_base_type( algebra )
    { }

    /**
     * \return Returns the order of the stepper.
     */
    order_type order( void ) const
    {
        return order_value;
    }

    /*
     * Version 1 : do_step( sys , x , t , dt )
     *
     * the two overloads are needed in order to solve the forwarding problem
     */
    template< class System , class StateInOut >
    void do_step( System system , StateInOut &x , time_type t , time_type dt )
    {
        do_step_v1( system , x , t , dt );
    }

    /**
     * \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
     */
    template< class System , class StateInOut >
    void do_step( System system , const StateInOut &x , time_type t , time_type dt )
    {
        do_step_v1( system , x , t , dt );
    }

    /*
     * Version 2 : do_step( sys , x , dxdt , t , dt )
     *
      * this version does not solve the forwarding problem, boost.range can not be used
     *
     * the disable is needed to avoid ambiguous overloads if state_type = time_type
     */
    template< class System , class StateInOut , class DerivIn >
    typename boost::disable_if< boost::is_same< DerivIn , time_type > , void >::type
    do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt )
    {
        this->stepper().do_step_impl( system , x , dxdt , t , x , dt );
    }

    /*
     * Version 3 : do_step( sys , in , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    template< class System , class StateIn , class StateOut >
    void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_resizer.adjust_size( in , detail::bind( &internal_stepper_base_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
        sys( in , m_dxdt.m_v ,t );
        this->stepper().do_step_impl( system , in , m_dxdt.m_v , t , out , dt );
    }

    /*
     * Version 4 : do_step( sys , in , dxdt , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
    {
        this->stepper().do_step_impl( system , in , dxdt , t , out , dt );
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_impl( x );
    }

private:

    stepper_type& stepper( void )
    {
        return *static_cast< stepper_type* >( this );
    }

    const stepper_type& stepper( void ) const
    {
        return *static_cast< const stepper_type* >( this );
    }

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class System , class StateInOut >
    void do_step_v1( System system , StateInOut &x , time_type t , time_type dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_resizer.adjust_size( x , detail::bind( &internal_stepper_base_type::template resize_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        sys( x , m_dxdt.m_v ,t );
        this->stepper().do_step_impl( system , x , m_dxdt.m_v , t , x , dt );
    }

    resizer_type m_resizer;

protected:

    wrapped_deriv_type m_dxdt;
};

/******* DOXYGEN *********/

/**
 * \class explicit_stepper_base
 * \brief Base class for explicit steppers without step size control and without dense output.
 *
 * This class serves as the base class for all explicit steppers with algebra and operations.
 * Step size control and error estimation as well as dense output are not provided. explicit_stepper_base
 * is used as the interface in a CRTP (currently recurring template pattern). In order to work
 * correctly the parent class needs to have a method `do_step_impl( system , in , dxdt_in , t , out , dt )`.
 * This is method is used by explicit_stepper_base. explicit_stepper_base derives from
 * algebra_stepper_base. An example how this class can be used is
 *
 * \code
 * template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resizer >
 * class custom_euler : public explicit_stepper_base< 1 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
 * {
 *  public:
 *
 *     typedef explicit_stepper_base< 1 , State , Value , Deriv , Time , Algebra , Operations , Resizer > base_type;
 *
 *     custom_euler( const Algebra &algebra = Algebra() ) { }
 *
 *     template< class Sys , class StateIn , class DerivIn , class StateOut >
 *     void do_step_impl( Sys sys , const StateIn &in , const DerivIn &dxdt , Time t , StateOut &out , Time dt )
 *     {
 *         m_algebra.for_each3( out , in , dxdt , Operations::scale_sum2< Value , Time >( 1.0 , dt );
 *     }
 *
 *     template< class State >
 *     void adjust_size( const State &x )
 *     {
 *         base_type::adjust_size( x );
 *     }
 * };
 * \endcode
 *
 * For the Stepper concept only the `do_step( sys , x , t , dt )` needs to be implemented. But this class
 * provides additional `do_step` variants since the stepper is explicit. These methods can be used to increase
 * the performance in some situation, for example if one needs to analyze `dxdt` during each step. In this case
 * one can use
 *
 * \code
 * sys( x , dxdt , t );
 * stepper.do_step( sys , x , dxdt , t , dt );  // the value of dxdt is used here
 * t += dt;
 * \endcode
 *
 * In detail explicit_stepper_base provides the following `do_step` variants
 *   - `do_step( sys , x , t , dt )` - The classical `do_step` method needed to fullfil the Stepper concept. The state is updated in-place.
 *      A type modelling a Boost.Range can be used for x.
 *   - `do_step( sys , in , t , out , dt )` - This method updates the state out-of-place, hence the result of the step is stored in `out`.
 *   - `do_step( sys , x , dxdt , t , dt )` - This method updates the state in-place, but the derivative at the point `t` must be
 *      explicitely passed in `dxdt`. For an example see the code snippet above.
 *   - `do_step( sys , in , dxdt , t , out , dt )` - This method update the state out-of-place and expects that the derivative at the point
 *     `t` is explicitely passed in `dxdt`. It is a combination of the two `do_step` methods above.
 *
 * \note The system is always passed as value, which might result in poor performance if it contains data. In this case it can be used with `boost::ref`
 * or `std::ref`, for example `stepper.do_step( boost::ref( sys ) , x , t , dt );`
 *
 * \note The time `t` is not advanced by the stepper. This has to done manually, or by the appropriate `integrate` routines or `iterator`s.
 *
 * \tparam Stepper The stepper on which this class should work. It is used via CRTP, hence explicit_stepper_base
 * provides the interface for the Stepper.
 * \tparam Order The order of the stepper.
 * \tparam State The state type for the stepper.
 * \tparam Value The value type for the stepper. This should be a floating point type, like float,
 * double, or a multiprecision type. It must not neccessary be the value_type of the State. For example
 * the State can be a `vector< complex< double > >` in this case the Value must be double.
 * The default value is double.
 * \tparam Deriv The type representing time derivatives of the state type. It is usually the same type as the
 * state type, only if used with Boost.Units both types differ.
 * \tparam Time The type representing the time. Usually the same type as the value type. When Boost.Units is
 * used, this type has usually a unit.
 * \tparam Algebra The algebra type which must fullfil the Algebra Concept.
 * \tparam Operations The type for the operations wich must fullfil the Operations Concept.
 * \tparam Resizer The resizer policy class.
 */

    /**
     * \fn explicit_stepper_base::explicit_stepper_base( const algebra_type &algebra )
     * \brief Constructs a explicit_stepper_base class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn explicit_stepper_base::order_type order( void ) const
     * \return Returns the order of the stepper.
     */

    /**
     * \fn explicit_stepper_base::do_step( System system , StateInOut &x , time_type t , time_type dt )
     * \brief This method performs one step. It transforms the result in-place.
     *
     * \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fullfil the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
     * \param t The value of the time, at which the step should be performed.
     * \param dt The step size.
     */

    /**
     * \fn explicit_stepper_base::do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt )

     * \brief The method performs one step. Additionally to the other method
     * the derivative of x is also passed to this method. It is supposed to be used in the following way:
     *
     * \code
     * sys( x , dxdt , t );
     * stepper.do_step( sys , x , dxdt , t , dt );
     * \endcode
     *
     * The result is updated in place in x. This method is disabled if Time and Deriv are of the same type. In this
     * case the method could not be distinguished from other `do_step` versions.
     *
     * \note This method does not solve the forwarding problem.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fullfil the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param dt The step size.
     */

    /**
     * \fn void explicit_stepper_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
     * \brief The method performs one step. The state of the ODE is updated out-of-place.
     * \note This method does not solve the forwarding problem.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fullfil the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

    /**
     * \fn void explicit_stepper_base::do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
     * \brief The method performs one step. The state of the ODE is updated out-of-place.
     * Furthermore, the derivative of x at t is passed to the stepper.
     * It is supposed to be used in the following way:
     *
     * \code
     * sys( in , dxdt , t );
     * stepper.do_step( sys , in , dxdt , t , out , dt );
     * \endcode
     *
     * \note This method does not solve the forwarding problem.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fullfil the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

    /**
     * \fn void explicit_stepper_base::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_STEPPER_BASE_HPP_INCLUDED


/*
 * Preconditioner.cpp
 *
 *  Created on: Jun 2, 2014
 *      Author: ydjian
 */

#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/NoiseModel.h>
#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <vector>

using namespace std;

namespace gtsam {

/*****************************************************************************/
void PreconditionerParameters::print() const {
  print(cout);
}

/***************************************************************************************/
void PreconditionerParameters::print(ostream &os) const {
  os << "PreconditionerParameters" << endl
     << "kernel:        " << kernelTranslator(kernel_) << endl
     << "verbosity:     " << verbosityTranslator(verbosity_) << endl;
}

/*****************************************************************************/
 ostream& operator<<(ostream &os, const PreconditionerParameters &p) {
  p.print(os);
  return os;
}

/***************************************************************************************/
PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) {
  std::string s = src;  boost::algorithm::to_upper(s);
  if (s == "GTSAM") return PreconditionerParameters::GTSAM;
  else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD;
  /* default is cholmod */
  else return PreconditionerParameters::CHOLMOD;
}

/***************************************************************************************/
PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src)  {
  std::string s = src;  boost::algorithm::to_upper(s);
  if (s == "SILENT") return PreconditionerParameters::SILENT;
  else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY;
  else if (s == "ERROR") return PreconditionerParameters::ERROR;
  /* default is default */
  else return PreconditionerParameters::SILENT;
}

/***************************************************************************************/
std::string PreconditionerParameters::kernelTranslator(PreconditionerParameters::Kernel k)  {
  if ( k == GTSAM ) return "GTSAM";
  if ( k == CHOLMOD ) return "CHOLMOD";
  else return "UNKNOWN";
}

/***************************************************************************************/
std::string PreconditionerParameters::verbosityTranslator(PreconditionerParameters::Verbosity verbosity)  {
  if (verbosity == SILENT)          return "SILENT";
  else if (verbosity == COMPLEXITY) return "COMPLEXITY";
  else if (verbosity == ERROR)      return "ERROR";
  else return "UNKNOWN";
}

/***************************************************************************************/
BlockJacobiPreconditioner::BlockJacobiPreconditioner()
  : Base(), buffer_(0), bufferSize_(0), nnz_(0) {}

/***************************************************************************************/
BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); }

/***************************************************************************************/
void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {

  const size_t n = dims_.size();
  double *ptr = buffer_, *dst = x.data();

  std::copy(y.data(), y.data() + y.rows(), x.data());

  for ( size_t i = 0 ; i < n ; ++i ) {
    const size_t d = dims_[i];
    const size_t sz = d*d;

    const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
    Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
    R.triangularView<Eigen::Upper>().solveInPlace(b);

    dst += d;
    ptr += sz;
  }
}

/***************************************************************************************/
void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const {
  const size_t n = dims_.size();
  double *ptr = buffer_, *dst = x.data();

  std::copy(y.data(), y.data() + y.rows(), x.data());

  for ( size_t i = 0 ; i < n ; ++i ) {
    const size_t d = dims_[i];
    const size_t sz = d*d;

    const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
    Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
    R.transpose().triangularView<Eigen::Upper>().solveInPlace(b);

    dst += d;
    ptr += sz;
  }
}

/***************************************************************************************/
void BlockJacobiPreconditioner::build(
  const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
  const size_t n = keyInfo.size();
  dims_ = keyInfo.colSpec();

  /* prepare the buffer of block diagonals */
  std::vector<Matrix> blocks; blocks.reserve(n);

  /* allocate memory for the factorization of block diagonals */
  size_t nnz = 0;
  for ( size_t i = 0 ; i < n ; ++i ) {
    const size_t dim = dims_[i];
    blocks.push_back(Matrix::Zero(dim, dim));
    // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2  ;
    nnz += dim*dim;
  }

  /* compute the block diagonal by scanning over the factors */
  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
        const Matrix &Ai = jf->getA(it);
        blocks[entry.index()] += (Ai.transpose() * Ai);
      }
    }
    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
      for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
        const Matrix &Hii = hf->info(it, it).selfadjointView();
        blocks[entry.index()] += Hii;
      }
    }
    else {
      throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
    }
  }

  /* if necessary, allocating the memory for cacheing the factorization results */
  if ( nnz > bufferSize_ ) {
    clean();
    buffer_ = new double [nnz];
    bufferSize_ = nnz;
  }
  nnz_ = nnz;

  /* factorizing the blocks respectively */
  double *ptr = buffer_;
  for ( size_t i = 0 ; i < n ; ++i ) {
    /* use eigen to decompose Di */
    const Matrix R = blocks[i].llt().matrixL().transpose();

    /* store the data in the buffer */
    size_t sz = dims_[i]*dims_[i] ;
    std::copy(R.data(), R.data() + sz, ptr);

    /* advance the pointer */
    ptr += sz;
  }
}

/*****************************************************************************/
void BlockJacobiPreconditioner::clean() {
  if ( buffer_ ) {
    delete [] buffer_;
    buffer_ = 0;
    bufferSize_ = 0;
    nnz_ = 0;
  }
}

/***************************************************************************************/
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {

  if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
    return boost::make_shared<DummyPreconditioner>();
  }
  else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
    return boost::make_shared<BlockJacobiPreconditioner>();
  }
  else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
    return boost::make_shared<SubgraphPreconditioner>(*subgraph);
  }

  throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
}

}


#include "rviz_helper/marker_visualizer.hpp"

#include <Eigen/Geometry>
#include <Eigen/SVD>

namespace kr {
namespace viz {

TrajectoryVisualizer::TrajectoryVisualizer(const ros::NodeHandle &nh,
                                           const std::string &topic)
    : MarkerVisualizer(nh, topic) {
  marker_.pose.orientation.w = 1.0;
  SetColor(colors::RED);
  SetScale(0.05);
  SetLifetime(ros::Duration());
  SetAction(Marker::ADD);
  SetType(Marker::LINE_STRIP);
}

void TrajectoryVisualizer::PublishTrajectory(const geometry_msgs::Point &point,
                                             const std_msgs::Header &header) {
  ++total_points_cnt_;
  if (total_points_cnt_ % num_skip_) return;
  marker_.points.push_back(point);
  marker_.header = header;
  Publish(marker_);
}

void TrajectoryVisualizer::PublishTrajectory(const geometry_msgs::Pose &pose,
                                             const std_msgs::Header &header) {
  PublishTrajectory(pose.position, header);
}

CovarianceVisualizer::CovarianceVisualizer(const ros::NodeHandle &nh,
                                           const std::string &topic,
                                           bool transforms_orientation)
    : MarkerVisualizer(nh, topic) {
  //  default to a beige colour
  SetColor(1, 0.9255, 0.5);
  SetAction(Marker::ADD);
  SetType(Marker::SPHERE);
  SetLifetime(ros::Duration());  // last forever
  transform_orientation_ = transforms_orientation;
}

void CovarianceVisualizer::PublishCovariance(const PoseWithCovariance &pose_cov,
                                             const std_msgs::Header &header) {
  marker_.header = header;
  marker_.pose.position = pose_cov.pose.position;
  if (!transform_orientation_) {
    marker_.pose.orientation.w = 1;

    double scale[3] = {0, 0, 0};
    for (int i = 0; i < 3; i++) {
      if (pose_cov.covariance[(i * 6) + i] >= 0) {
        scale[i] = std::sqrt(pose_cov.covariance[(i * 6) + i]);
      }
    }

    SetScale(scale);
  } else {
    using namespace Eigen;
    //  we will automatically determine scale and orientation of the covariance

    Matrix<double,6,6> cov;
    for (int i=0; i < 6; i++) {
      for (int j=0; j < 6; j++) {
        cov(i,j) = pose_cov.covariance[(i*6) + j];
      }
    }
    //  position components
    Matrix3d cov_p = cov.template topLeftCorner<3,3>();
    //  we assume here that covariance is symmetric positive definite
    JacobiSVD<Matrix3d> SVD = cov_p.jacobiSvd(Eigen::ComputeFullU);

    Matrix3d U = SVD.matrixU();         //  U == V in this case
    Vector3d S = SVD.singularValues();

    //  U is a rotation matrix of the ellipse, convert to quaternion
    Eigen::Quaterniond quat(U*U.determinant());
    quat.normalize();

    marker_.pose.orientation.w = quat.w();
    marker_.pose.orientation.x = quat.x();
    marker_.pose.orientation.y = quat.y();
    marker_.pose.orientation.z = quat.z();

    //  set scale
    for (int i=0; i < 3; i++) {
      S[i] = std::sqrt(S[i]);
    }
    SetScale(S[0], S[1], S[2]);
  }
  Publish(marker_);
}

void CovarianceVisualizer::PublishCovariance(
    const PoseWithCovarianceStamped &pose_cov_stamped) {
  PublishCovariance(pose_cov_stamped.pose, pose_cov_stamped.header);
}

void CovarianceVisualizer::PublishCovariance(
    const nav_msgs::Odometry &odometry) {
  PublishCovariance(odometry.pose, odometry.header);
}

void PoseVisualizer::PublishPose(const geometry_msgs::Pose &pose,
                                 const std_msgs::Header &header) {
  PublishTrajectory(pose.position, header);
  tf_pub_.PublishTransform(pose, header);
}

}  // namespace viz
}  // namespace kr


#pragma once
// Parameters to control the behavior of an SDPSolver. See the manual
// for a detailed description of each.
//

#include <El.hpp>
#include <boost/filesystem.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/program_options.hpp>

#include <iostream>

struct Solver_Parameters
{
  int64_t max_iterations, max_runtime, checkpoint_interval;
  bool find_primal_feasible, find_dual_feasible, detect_primal_feasible_jump,
    detect_dual_feasible_jump;
  size_t precision;

  El::BigFloat duality_gap_threshold, primal_error_threshold,
    dual_error_threshold, initial_matrix_scale_primal,
    initial_matrix_scale_dual, feasible_centering_parameter,
    infeasible_centering_parameter, step_length_reduction, max_complementarity,
    min_primal_step, min_dual_step;

  boost::filesystem::path checkpoint_in, checkpoint_out;
  Solver_Parameters() = default;
  boost::program_options::options_description options();
};

std::ostream &operator<<(std::ostream &os, const Solver_Parameters &p);

boost::property_tree::ptree to_property_tree(const Solver_Parameters &p);


#pragma once

#include <cstdlib>
#include <cstdint>
#include <thread>
#include <boost/lexical_cast.hpp>

namespace quasi_succinct {

    class configuration {
    public:
        static configuration const& get() {
            static configuration instance;
            return instance;
        }

        double eps1;
        double eps2;
        uint64_t fix_cost;

        size_t log_partition_size;
        size_t worker_threads;

    private:
        configuration()
        {
            fillvar("QS_EPS1", eps1, 0.03);
            fillvar("QS_EPS2", eps2, 0.3);
            fillvar("QS_FIXCOST", fix_cost, 64);
            fillvar("QS_LOG_PART", log_partition_size, 7);
            fillvar("QS_THREADS", worker_threads, std::thread::hardware_concurrency());
        }

        template <typename T, typename T2>
        void fillvar(const char* envvar, T& var, T2 def)
        {
            const char* val = std::getenv(envvar);
            if (!val || !strlen(val)) {
                var = def;
            } else {
                var = boost::lexical_cast<T>(val);
            }
        }
    };

}


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

#ifndef BOOST_GRAPH_DEGREE_CENTRALITY_HPP
#define BOOST_GRAPH_DEGREE_CENTRALITY_HPP

#include <boost/graph/graph_concepts.hpp>
#include <boost/concept/assert.hpp>

namespace boost {

template <typename Graph>
struct degree_centrality_measure
{
    typedef typename graph_traits<Graph>::degree_size_type degree_type;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_type;
};

template <typename Graph>
struct influence_measure
    : public degree_centrality_measure<Graph>
{
    typedef degree_centrality_measure<Graph> base_type;
    typedef typename base_type::degree_type degree_type;
    typedef typename base_type::vertex_type vertex_type;

    inline degree_type operator ()(vertex_type v, const Graph& g)
    {
        BOOST_CONCEPT_ASSERT(( IncidenceGraphConcept<Graph> ));
        return out_degree(v, g);
    }
};

template <typename Graph>
inline influence_measure<Graph>
measure_influence(const Graph&)
{ return influence_measure<Graph>(); }

template <typename Graph>
struct prestige_measure
    : public degree_centrality_measure<Graph>
{
    typedef degree_centrality_measure<Graph> base_type;
    typedef typename base_type::degree_type degree_type;
    typedef typename base_type::vertex_type vertex_type;

    inline degree_type operator ()(vertex_type v, const Graph& g)
    {
        BOOST_CONCEPT_ASSERT(( BidirectionalGraphConcept<Graph> ));
        return in_degree(v, g);
    }
};

template <typename Graph>
inline prestige_measure<Graph>
measure_prestige(const Graph&)
{ return prestige_measure<Graph>(); }

template <typename Graph, typename Vertex, typename Measure>
inline typename Measure::degree_type
degree_centrality(const Graph& g, Vertex v, Measure measure)
{
    BOOST_CONCEPT_ASSERT(( DegreeMeasureConcept<Measure, Graph> ));
    return measure(v, g);
}

template <typename Graph, typename Vertex>
inline typename graph_traits<Graph>::degree_size_type
degree_centrality(const Graph& g, Vertex v)
{
    return degree_centrality(g, v, measure_influence(g));
}

// These are alias functions, intended to provide a more expressive interface.

template <typename Graph, typename Vertex>
inline typename graph_traits<Graph>::degree_size_type
influence(const Graph& g, Vertex v)
{ return degree_centrality(g, v, measure_influence(g)); }

template <typename Graph, typename Vertex>
inline typename graph_traits<Graph>::degree_size_type
prestige(const Graph& g, Vertex v)
{ return degree_centrality(g, v, measure_prestige(g)); }

template <typename Graph, typename CentralityMap, typename Measure>
inline void
all_degree_centralities(const Graph& g, CentralityMap cent, Measure measure)
{
    BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<Graph> ));
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
    BOOST_CONCEPT_ASSERT(( WritablePropertyMapConcept<CentralityMap,Vertex> ));
    typedef typename property_traits<CentralityMap>::value_type Centrality;

    VertexIterator i, end;
    for(boost::tie(i, end) = vertices(g); i != end; ++i) {
        Centrality c = degree_centrality(g, *i, measure);
        put(cent, *i, c);
    }
}

template <typename Graph, typename CentralityMap>
inline void all_degree_centralities(const Graph& g, CentralityMap cent)
{ all_degree_centralities(g, cent, measure_influence(g)); }

// More helper functions for computing influence and prestige.
// I hate the names of these functions, but influence and prestige
// don't pluralize too well.

template <typename Graph, typename CentralityMap>
inline void all_influence_values(const Graph& g, CentralityMap cent)
{ all_degree_centralities(g, cent, measure_influence(g)); }

template <typename Graph, typename CentralityMap>
inline void all_prestige_values(const Graph& g, CentralityMap cent)
{ all_degree_centralities(g, cent, measure_prestige(g)); }

} /* namespace boost */

#endif


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_CONCEPTS_AREA_CONCEPT_HPP
#define BOOST_GEOMETRY_STRATEGIES_CONCEPTS_AREA_CONCEPT_HPP

#include <boost/concept_check.hpp>

namespace boost { namespace geometry { namespace concepts
{

/*!
    \brief Checks strategy for area
    \ingroup area
*/
template <typename Strategy>
class AreaStrategy
{
#ifndef DOXYGEN_NO_CONCEPT_MEMBERS

    // 1) must define state_type,
    typedef typename Strategy::state_type state_type;

    // 2) must define return_type,
    typedef typename Strategy::return_type return_type;

    // 3) must define point_type, of polygon (segments)
    typedef typename Strategy::segment_point_type spoint_type;

    struct check_methods
    {
        static void apply()
        {
            Strategy const* str = 0;
            state_type *st = 0;

            // 4) must implement a method apply with the following signature
            spoint_type const* sp = 0;
            str->apply(*sp, *sp, *st);

            // 5) must implement a static method result with the following signature
            return_type r = str->result(*st);

            boost::ignore_unused_variable_warning(r);
            boost::ignore_unused_variable_warning(str);
        }
    };

public :
    BOOST_CONCEPT_USAGE(AreaStrategy)
    {
        check_methods::apply();
    }

#endif
};

}}} // namespace boost::geometry::concepts

#endif // BOOST_GEOMETRY_STRATEGIES_CONCEPTS_AREA_CONCEPT_HPP


//---------------------------------------------------------------------------//
//  MIT License
//
//  Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
//  Copyright (c) 2020-2021 Nikita Kaskov <nemo@nil.foundation>

//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//  SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef FILECOIN_STORAGE_PROOFS_POST_FALLBACK_VANILLA_HPP
#define FILECOIN_STORAGE_PROOFS_POST_FALLBACK_VANILLA_HPP

#include <boost/log/trivial.hpp>

#include <nil/crypto3/hash/sha2.hpp>

#include <nil/filecoin/storage/proofs/core/parameter_cache.hpp>
#include <nil/filecoin/storage/proofs/core/sector.hpp>
#include <nil/filecoin/storage/proofs/core/proof/proof.hpp>

namespace nil {
    namespace filecoin {
        namespace post {
            namespace fallback {

                /*************************  SetupParams  ***********************************/

                struct SetupParams {
                    /// Size of the sector in bytes.
                    std::uint64_t sector_size;
                    /// Number of challenges per sector.
                    std::size_t challenge_count;
                    /// Number of challenged sectors.
                    std::size_t sector_count;
                };

                /*************************  PublicParams  ***********************************/

                struct PublicParams : public parameter_set_metadata {
                    virtual std::string identifier() const override {
                        return std::string("FallbackPoSt::PublicParams{{sector_size: ") +
                               std::to_string(sector_size()) + ", challenge_count: " + std::to_string(challenge_count) +
                               " , sector_count: " + std::to_string(sector_count) + "}}";
                    }

                    /// Size of the sector in bytes.
                    std::uint64_t sector_size;
                    /// Number of challenges per sector.
                    std::size_t challenge_count;
                    /// Number of challenged sectors.
                    std::size_t sector_count;
                };

                /*************************  ChallengeRequirements  ***********************************/

                struct ChallengeRequirements {
                    /// The sum of challenges across all challenged sectors. (even across partitions)
                    std::size_t minimum_challenge_count;
                };

                /*************************  PublicSector  ***********************************/

                template<typename Domain>
                struct PublicSector {
                    sector_id_type id;
                    Domain comm_r;
                };

                /*************************  PublicInputs  ***********************************/

                template<typename Domain>
                struct PublicInputs {
                    Domain randomness;
                    Domain prover_id;
                    std::vector<PublicSector<Domain>> sectors;
                    /// Partition index
                    std::size_t k;
                };

                /*************************  PrivateSector  ***********************************/

                template<typename MerkleTreeType>
                struct PrivateSector {
                    MerkleTreeWrapper<typename MerkleTreeType::hash_type, MerkleTreeType::Store,
                                      MerkleTreeType::base_arity, MerkleTreeType::sub_tree_arity,
                                      MerkleTreeType::top_tree_arity>
                        tree;
                    typename MerkleTreeType::hash_type::digest_type comm_c;
                    typename MerkleTreeType::hash_type::digest_type comm_r_last;
                };

                /*************************  PrivateInputs  ***********************************/

                template<typename MerkleTreeType>
                struct PrivateInputs {
                    std::vector<PrivateSector<MerkleTreeType>> sectors;
                };

                /*************************  SectorProof  ***********************************/

                template<typename MerkleProofType>
                struct SectorProof {
                    std::vector<typename MerkleProofType::hash_type::digest_type> leafs() {
                        return std::accumulate(inclusion_proofs.begin(), inclusion_proofs.end(),
                                               std::vector<typename MerkleProofType::hash_type::digest_type>(),
                                               [&](auto &val, const auto &itr) { val.emplace_back(itr.leaf()); });
                    }

                    typename MerkleProofType::hash_type::digest_type comm_r_last() {
                        return inclusion_proofs[0].root();
                    }

                    std::vector<typename MerkleProofType::hash_type::digest_type> commitments() {
                        return std::accumulate(inclusion_proofs.begin(), inclusion_proofs.end(),
                                               std::vector<typename MerkleProofType::hash_type::digest_type>(),
                                               [&](auto &val, const auto &itr) { val.emplace_back(itr.root()()); });
                    }

                    std::vector<std::vector<
                        std::pair<std::vector<typename MerkleProofType::hash_type::digest_type>, std::size_t>>>
                        paths() {
                        return std::accumulate(inclusion_proofs.begin(), inclusion_proofs.end(),
                                               std::vector<typename MerkleProofType::hash_type::digest_type>(),
                                               [&](auto &val, const auto &itr) { val.emplace_back(itr.path()()); });
                    }

                    std::vector<std::vector<std::pair<std::vector<Fr>, std::size_t>>> as_options() {
                        return inclusion_proofs.iter().map(BasicMerkleProof::as_options).collect();
                    }

                    std::vector<MerkleProof<typename MerkleProofType::hash_type, MerkleProofType::Arity,
                                            MerkleProofType::SubTreeArity, MerkleProofType::TopTreeArity>>
                        inclusion_proofs;
                    typename MerkleProofType::hash_type::digest_type comm_c;
                    typename MerkleProofType::hash_type::digest_type comm_r_last;
                };

                /*************************  Proof  ***********************************/

                template<typename MerkleProofType>
                struct Proof {
                    std::vector<SectorProof<MerkleProofType>> sectors;
                };

                /*************************  FallbackPoSt  ***********************************/

                template<typename MerkleTreeType>
                class FallbackPoSt
                    : public proof_scheme<PublicParams, SetupParams,
                                          PublicInputs<typename MerkleTreeType::hash_type::digest_type>,
                                          PrivateInputs<MerkleTreeType>, Proof<typename MerkleTreeType::proof_type>,
                                          ChallengeRequirements> {
                    typedef proof_scheme<PublicParams, SetupParams,
                                         PublicInputs<typename MerkleTreeType::hash_type::digest_type>,
                                         PrivateInputs<MerkleTreeType>, Proof<typename MerkleTreeType::proof_type>,
                                         ChallengeRequirements>
                        policy_type;

                public:
                    typedef typename policy_type::public_params_type public_params_type;
                    typedef typename policy_type::setup_params setup_params_type;
                    typedef typename policy_type::public_inputs public_inputs_type;
                    typedef typename policy_type::private_inputs private_inputs_type;
                    typedef typename policy_type::proof_type proof_type;
                    typedef typename policy_type::requirements_type requirements_type;

                    virtual public_params_type setup(const setup_params_type &p) override {
                        return {p.sector_size, p.challenge_count, p.sector_count};
                    }

                    std::vector<proof_type> prove_all_partitions(const public_params_type &pub_params,
                                                                 const public_inputs_type &pub_inputs,
                                                                 const private_inputs_type &priv_inputs,
                                                                 std::size_t partition_count) {

                        BOOST_ASSERT_MSG(priv_inputs.sectors.size() == pub_inputs.sectors.size(),
                                         "inconsistent number of private and public sectors");

                        std::size_t num_sectors_per_chunk = pub_params.sector_count;
                        std::size_t num_sectors = pub_inputs.sectors.size();

                        BOOST_ASSERT_MSG(num_sectors <= partition_count * num_sectors_per_chunk,
                                         "cannot prove the provided number of sectors:");

                        std::vector<proof_type> partition_proofs;

                        for ((j, (pub_sectors_chunk, priv_sectors_chunk)) :
                             pub_inputs.sectors.chunks(num_sectors_per_chunk)
                                 .zip(priv_inputs.sectors.chunks(num_sectors_per_chunk))
                                 .enumerate()) {
                            BOOST_LOG_TRIVIAL(trace) << std::format("proving partition {}", j);

                            std::vector<proof_type> proofs(num_sectors_per_chunk);

                            for ((i, (pub_sector, priv_sector)) :
                                 pub_sectors_chunk.iter().zip(priv_sectors_chunk.iter()).enumerate()) {
                                const auto tree = priv_sector.tree;
                                const auto sector_id = pub_sector.id;
                                const auto tree_leafs = tree.leafs();

                                BOOST_LOG_TRIVIAL(trace)
                                    << std::format("Generating proof for tree leafs {} and arity {}", tree_leafs,
                                                   MerkleTreeType::base_arity);

                                const auto inclusion_proofs =
                                    (0..pub_params.challenge_count)
                                        .into_par_iter()
                                        .map(| n |
                                             {
                                                 const auto challenge_index =
                                                     ((j * num_sectors_per_chunk + i) * pub_params.challenge_count + n)
                                                         as u64;
                                                 const auto challenged_leaf_start =
                                                     generate_leaf_challenge(pub_params, pub_inputs.randomness,
                                                                             sector_id.into(), challenge_index);

                                                 tree.gen_cached_proof(challenged_leaf_start as usize, None)
                                             })
                                        .collect::<Result<Vec<_>>>();

                                proofs.push_back({inclusion_proofs, priv_sector.comm_c, priv_sector.comm_r_last});
                            }

                            // If there were less than the required number of sectors provided, we duplicate the
                            // last one to pad the proof out, such that it works in the circuit part.
                            while (proofs.size() < num_sectors_per_chunk) {
                                proofs.push_back(proofs[proofs.size() - 1]);
                            }

                            partition_proofs.push_back({proofs});
                        }

                        return partition_proofs;
                    }

                    bool verify_all_partitions(const public_params_type &pub_params,
                                               const public_inputs_type &pub_inputs,
                                               const std::vector<proof_type> &partition_proofs) {
                        std::size_t challenge_count = pub_params.challenge_count;
                        std::size_t num_sectors_per_chunk = pub_params.sector_count;
                        std::size_t num_sectors = pub_inputs.sectors.size();

                        BOOST_ASSERT_MSG(num_sectors <= num_sectors_per_chunk * partition_proofs.size(),
                                         "inconsistent number of sectors");

                        for ((j, (proof, pub_sectors_chunk)) :
                             partition_proofs.iter()
                                 .zip(pub_inputs.sectors.chunks(num_sectors_per_chunk))
                                 .enumerate()) {
                            BOOST_ASSERT_MSG(pub_sectors_chunk.size() <= num_sectors_per_chunk,
                                             "inconsistent number of public sectors");
                            BOOST_ASSERT_MSG(proof.sectors.size() == num_sectors_per_chunk,
                                             "invalid number of sectors in the partition proof");
                            for ((i, (pub_sector, sector_proof)) :
                                 pub_sectors_chunk.iter().zip(proof.sectors.iter()).enumerate()) {
                                const auto sector_id = pub_sector.id;
                                const auto comm_r = &pub_sector.comm_r;
                                const auto comm_c = sector_proof.comm_c;
                                const auto inclusion_proofs = &sector_proof.inclusion_proofs;

                                // Verify that H(Comm_c || Comm_r_last) == Comm_R

                                // comm_r_last is the root of the proof
                                const auto comm_r_last = inclusion_proofs[0].root();

                                if (AsRef ::<[u8]>::as_ref(&<typename MerkleTreeType::hash_type>::Function::hash2(
                                        &comm_c, &comm_r_last, )) != AsRef::<[u8]>::as_ref(comm_r)) {
                                    return false;
                                }

                                BOOST_ASSERT_MSG(challenge_count == inclusion_proofs.len(),
                                                 std::format("unexpected umber of inclusion proofs: {} != {}",
                                                             challenge_count,
                                                             inclusion_proofs.size()));

                                for (std::size_t n = 0,
                                                 inclusion_proofs::iterator inclusion_proof = inclusion_proofs.begin();
                                     inclusion_proof != inclusion_proofs.end(); ++n, ++inclusion_proof) {

                                    const auto challenge_index =
                                        ((j * num_sectors_per_chunk + i) * pub_params.challenge_count + n) as u64;
                                    const auto challenged_leaf_start = generate_leaf_challenge(
                                        pub_params, pub_inputs.randomness, sector_id.into(), challenge_index);

                                    // validate all comm_r_lasts match
                                    if ((*inclusion_proof).root() != comm_r_last) {
                                        return false;
                                    }

                                    // validate the path length
                                    const auto expected_path_length =
                                        (*inclusion_proof).expected_len(pub_params.sector_size as usize / NODE_SIZE);

                                    if (expected_path_length != (*inclusion_proof).path().size()) {
                                        return false;
                                    }

                                    if (!(*inclusion_proof).validate(challenged_leaf_start)) {
                                        return false;
                                    }
                                }
                            }
                        }

                        return true;
                    }

                    virtual proof_type prove(const public_params_type &params,
                                             const public_inputs_type &inputs,
                                             const private_inputs_type &pinputs) override {
                        std::vector<proof_type> proofs = prove_all_partitions(params, inputs, pinputs, 1);
                        auto k;
                        switch (pub_inputs.k) {
                            case None:
                                k = 0;
                                break;
                            case Some(k):
                                k = k;
                                break;
                        };
                        // Because partition proofs require a common setup, the general ProofScheme implementation,
                        // which makes use of `ProofScheme::prove` cannot be used here. Instead, we need to prove all
                        // partitions in one pass, as implemented by `prove_all_partitions` below.
                        BOOST_ASSERT_MSG(
                            k < 1, "It is a programmer error to call StackedDrg::prove with more than one partition.");

                        return proofs[k].to_owned();
                    }

                    bool satisfies_requirements(const public_params_type &public_params,
                                                const requirements_type &requirements, std::size_t partitions) {
                        std::size_t checked = partitions * public_params.sector_count;

                        assert(partitions.checked_mul(public_params.sector_count) == checked);
                        assert(checked.checked_mul(public_params.challenge_count) ==
                               checked * public_params.challenge_count);

                        return checked * public_params.challenge_count >= requirements.minimum_challenge_count;
                    }

                    virtual bool verify(const public_params_type &pub_params,
                                        const public_inputs_type &pub_inputs,
                                        const proof_type &pr) override {
                        return false;
                    }
                };

                /*************************  ???  ***********************************/

                template<typename Domain>
                std::vector<std::uint64_t> generate_sector_challenges(Domain randomness, std::size_t challenge_count,
                                                                      std::uint64_t sector_set_len, Domain prover_id) {

                    std::vector<std::uint64_t> result;
                    result.reserve(challenge_count);

                    for (std::size_t n = 0; n < challenge_count; ++n) {
                        result.push(generate_sector_challenge(randomness, n, sector_set_len, prover_id));
                    }
                }

                /// Generate a single sector challenge.
                template<typename Domain, typename ChallengeHash = crypto3::hashes::sha2<256>>
                std::uint64_t generate_sector_challenge(Domain randomness, std::size_t n, std::uint64_t sector_set_len,
                                                        Domain prover_id) {
                    auto hasher = Sha256();
                    hasher.input(AsRef::<[u8]>::as_ref(&prover_id));
                    hasher.input(AsRef::<[u8]>::as_ref(&randomness));
                    hasher.input(&n.to_le_bytes()[..]);

                    const auto hash = hasher.result();

                    const auto sector_challenge = LittleEndian::read_u64(&hash[..8]);
                    std::uint64_t sector_index = sector_challenge % sector_set_len;

                    return sector_index;
                }

                /// Generates challenge, such that the range fits into the sector.
                template<typename Domain, typename ChallengeHash = crypto3::hashes::sha2<256>>
                std::uint64_t generate_leaf_challenge(const PublicParams &pub_params, const Domain &randomness,
                                                      std::uint64_t sector_id, std::uint64_t leaf_challenge_index) {
                    auto hasher = Sha256();
                    hasher.input(AsRef::<[u8]>::as_ref(&randomness));
                    hasher.input(&sector_id.to_le_bytes()[..]);
                    hasher.input(&leaf_challenge_index.to_le_bytes()[..]);
                    const auto hash = hasher.result();

                    const auto leaf_challenge = LittleEndian::read_u64(&hash[..8]);

                    std::uint64_t challenged_range_index = leaf_challenge % (pub_params.sector_size / NODE_SIZE);

                    return challenged_range_index;
                }

                /// Generate all challenged leaf ranges for a single sector, such that the range fits into the sector.
                template<typename Domain>
                std::vector<std::uint64_t> generate_leaf_challenges(const PublicParams &pub_params,
                                                                    const Domain &randomness, std::uint64_t sector_id,
                                                                    std::size_t challenge_count) {
                    std::vector<std::uint64_t> challenges(challenge_count);

                    for (int leaf_challenge_index = 0; leaf_challenge_index < challenge_count; leaf_challenge_index++) {
                        std::uint64_t challenge =
                            generate_leaf_challenge(pub_params, randomness, sector_id, leaf_challenge_index as u64);
                        challenges.push_back(challenge);
                    }

                    return challenges;
                }
            }    // namespace fallback
        }        // namespace post
    }            // namespace filecoin
}    // namespace nil

#endif


#ifndef MOCHIMOCHI_MNHERD_HPP_
#define MOCHIMOCHI_MNHERD_HPP_

#include <algorithm>
#include <unordered_map>
#include <boost/range/irange.hpp>
#include "../binary/nherd.hpp"

class MNHERD {
private:
  const std::size_t kClass;

private:
  std::unordered_map<std::size_t, NHERD> _nherds;

public:
  MNHERD(const std::size_t dim, const std::size_t n_class, const double C, const int diagonal = 0)
    : kClass(n_class) {
    static_assert(std::numeric_limits<decltype(n_class)>::max() > 2, "Class range Error. (n_class > 2)");

    for (const auto i : boost::irange<std::size_t>(1, kClass + 1)) {
      _nherds.insert(std::pair<std::size_t, NHERD>(i, NHERD(dim, C, diagonal)) );
    }
  }

  virtual ~MNHERD() { }

public:
  void update(const Eigen::VectorXd& feature, const std::size_t label) {
    for(auto& nherd : _nherds) {
      const auto t = (nherd.first == label) ? 1 : -1;
      nherd.second.update(feature, t);
    }
  }

  std::size_t predict(const Eigen::VectorXd& feature) const {
    return std::max_element(_nherds.begin(), _nherds.end(),
                            [&](const auto& p1, const auto& p2) {
                              return p1.second.get_means().dot(feature) < p2.second.get_means().dot(feature);
                            })->first;
  }

};

#endif //MOCHIMOCHI_NHERD_HPP_