#ifndef PARMCB_SPTREES_HPP_
#define PARMCB_SPTREES_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/throw_exception.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/serialization/vector.hpp>

#include <parmcb/detail/lex_dijkstra.hpp>
#include <parmcb/detail/util.hpp>

#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>

#include <memory>
#include <stack>
#include <functional>

namespace parmcb {

    template<class Graph, class WeightMap> class SPNode;
    template<class Graph, class WeightMap> class SPTree;
    template<class Graph, class WeightMap, class T> struct SPSubtree;
    template<class Graph, class WeightMap, bool ParallelUsingTBB> class SPTrees;
    template<class Graph, class WeightMap> class CandidateCycle;
    template<class Graph> struct SerializableCandidateCycle;
    template<class Graph, class WeightMap> struct SerializableMinOddCycle;
    template<class Graph, class WeightMap> struct SerializableMinOddCycleMinOp;

    template<class Graph, class WeightMap>
    class SPNode {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        SPNode() :
                _vertex(), _parity(false), _weight(WeightType()), _pred(), _has_pred(false) {
        }

        SPNode(Vertex vertex, WeightType weight) :
                _vertex(vertex), _parity(false), _weight(WeightType()), _pred(), _has_pred(false) {
        }

        SPNode(Vertex vertex, WeightType weight, const Edge &pred) :
                _vertex(vertex), _parity(false), _weight(weight), _pred(pred), _has_pred(true) {
        }

        void add_child(std::shared_ptr<SPNode<Graph, WeightMap>> c) {
            _children.push_back(c);
        }

        std::vector<std::shared_ptr<SPNode<Graph, WeightMap>>>& children() {
            return _children;
        }

        Vertex& vertex() {
            return _vertex;
        }

        bool& parity() {
            return _parity;
        }

        WeightType& weight() {
            return _weight;
        }

        const Edge& pred() {
            return _pred;
        }

        bool has_pred() {
            return _has_pred;
        }

    private:
        Vertex _vertex;
        bool _parity;
        WeightType _weight;
        Edge _pred;
        bool _has_pred;
        std::vector<std::shared_ptr<SPNode<Graph, WeightMap>>> _children;
    };

    template<class Graph, class WeightMap, class T>
    struct SPSubtree {
        T info;
        std::shared_ptr<SPNode<Graph, WeightMap>> root;

        SPSubtree(T info, std::shared_ptr<SPNode<Graph, WeightMap>> root) :
                info(info), root(root) {
        }
    };

    template<class Graph, class WeightMap>
    class SPTree {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        SPTree(std::size_t id, const Graph &g, const VertexIndexMapType& index_map, const WeightMap &weight_map, const Vertex &source) :
                _id(id), _g(g), _weight_map(weight_map), _index_map(index_map), _source(
                        source), _tree_node_map(boost::num_vertices(g)), _first_in_path(boost::num_vertices(g)) {
            initialize();
        }

        void update_parities(const std::set<Edge> &edges) {
            std::stack<SPSubtree<Graph, WeightMap, bool>> stack;
            stack.emplace(false, _root);

            while (!stack.empty()) {
                SPSubtree<Graph, WeightMap, bool> r = stack.top();
                stack.pop();

                r.root->parity() = r.info;
                for (auto c : r.root->children()) {
                    bool is_signed = edges.find(c->pred()) != edges.end();
                    stack.emplace(SPSubtree<Graph, WeightMap, bool> { static_cast<bool>(r.info ^ is_signed), c });
                }
            }
        }

        std::shared_ptr<SPNode<Graph, WeightMap>> node(const Vertex &v) const {
            return _tree_node_map[_index_map[v]];
        }

        const Vertex& source() const {
            return _source;
        }

        const Graph& graph() const {
            return _g;
        }

        const std::size_t id() const {
            return _id;
        }

        const Vertex first(const Vertex &v) {
            auto vindex = _index_map[v];
            return _first_in_path[vindex];
        }

        template<class EdgeIterator>
        std::vector<CandidateCycle<Graph, WeightMap>> create_candidate_cycles(EdgeIterator begin,
                EdgeIterator end) const {
            // collect tree edges
            std::set<Edge> tree_edges;
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto vindex = _index_map[v];
                std::shared_ptr<SPNode<Graph, WeightMap>> n = _tree_node_map[vindex];
                if (n != nullptr && n->has_pred()) {
                    tree_edges.insert(n->pred());
                }
            }

            // loop over (non-tree) provided edges and create candidate cycles
            std::vector<CandidateCycle<Graph, WeightMap>> cycles;
            for (EdgeIterator it = begin; it != end; it++) {
                Edge e = *it;
                if (tree_edges.find(e) != tree_edges.end()) {
                    continue;
                }

                // non-tree edge
                std::shared_ptr<SPNode<Graph, WeightMap>> v = node(boost::source(e, _g));
                if (v == nullptr) {
                    continue;
                }
                std::shared_ptr<SPNode<Graph, WeightMap>> u = node(boost::target(e, _g));
                if (u == nullptr) {
                    continue;
                }

                if (_first_in_path[_index_map[v->vertex()]] == _first_in_path[_index_map[u->vertex()]]) {
                    // shortest paths start with the same vertex, discard
                    continue;
                }

                WeightType cycle_weight = boost::get(_weight_map, e) + v->weight() + u->weight();
                cycles.emplace_back(_id, e, cycle_weight);
            }
            return cycles;
        }

        std::vector<CandidateCycle<Graph, WeightMap>> create_candidate_cycles() const {
            auto itPair = boost::edges(_g);
            return create_candidate_cycles(itPair.first, itPair.second);
        }

        std::vector<SerializableCandidateCycle<Graph>> create_serializable_candidate_cycles(
                const ForestIndex<Graph> &forest_index) {
            // collect tree edges
            std::set<Edge> tree_edges;
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto vindex = _index_map[v];
                std::shared_ptr<SPNode<Graph, WeightMap>> n = _tree_node_map[vindex];
                if (n != nullptr && n->has_pred()) {
                    tree_edges.insert(n->pred());
                }
            }

            // loop over all non-tree edges and create candidate cycles
            std::vector<SerializableCandidateCycle<Graph>> cycles;
            for (const auto &e : boost::make_iterator_range(boost::edges(_g))) {
                if (tree_edges.find(e) != tree_edges.end()) {
                    continue;
                }

                // non-tree edge
                std::shared_ptr<SPNode<Graph, WeightMap>> v = node(boost::source(e, _g));
                if (v == nullptr) {
                    continue;
                }
                std::shared_ptr<SPNode<Graph, WeightMap>> u = node(boost::target(e, _g));
                if (u == nullptr) {
                    continue;
                }

                if (_first_in_path[_index_map[v->vertex()]] == _first_in_path[_index_map[u->vertex()]]) {
                    // shortest paths start with the same vertex, discard
                    continue;
                }

                cycles.emplace_back(_source, forest_index(e));
            }

            return cycles;
        }

    private:
        const std::size_t _id;
        const Graph &_g;
        const WeightMap &_weight_map;
        const VertexIndexMapType &_index_map;
        const Vertex _source;

        /*
         * Shortest path tree root
         */
        std::shared_ptr<SPNode<Graph, WeightMap>> _root;
        /*
         * Map from vertex to shortest path tree node
         */
        std::vector<std::shared_ptr<SPNode<Graph, WeightMap>>> _tree_node_map;
        /*
         * First vertex in shortest path from root to a vertex.
         */
        std::vector<Vertex> _first_in_path;

        void initialize() {
            // run shortest path
            std::vector<WeightType> dist(boost::num_vertices(_g), (std::numeric_limits<WeightType>::max)());
            boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, WeightType>, Vertex, WeightType&> dist_map(
                    parmcb::detail::VertexIndexFunctor<Graph, WeightType>(dist, _index_map));
            std::vector<std::tuple<bool, Edge>> pred(boost::num_vertices(_g), std::make_tuple(false, Edge()));
            boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, std::tuple<bool, Edge>>, Vertex,
                    std::tuple<bool, Edge>&> pred_map(
                    parmcb::detail::VertexIndexFunctor<Graph, std::tuple<bool, Edge> >(pred, _index_map));
            lex_dijkstra(_g, _weight_map, _source, dist_map, pred_map);

            // create tree nodes and mapping
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto vindex = _index_map[v];
                auto p = boost::get(pred_map, v);
                if (v == _source) {
                    _tree_node_map[vindex] = std::shared_ptr<SPNode<Graph, WeightMap>>(
                            new SPNode<Graph, WeightMap>(v, dist[vindex]));
                    _root = _tree_node_map[vindex];
                } else if (std::get<0>(p)) {
                    Edge e = std::get<1>(p);
                    _tree_node_map[vindex] = std::shared_ptr<SPNode<Graph, WeightMap>>(
                            new SPNode<Graph, WeightMap>(v, dist[vindex], e));
                }
            }

            // link tree nodes
            for (boost::tie(vi, viend) = boost::vertices(_g); vi != viend; ++vi) {
                auto v = *vi;
                auto p = boost::get(pred_map, v);
                if (std::get<0>(p)) {
                    auto e = std::get<1>(p);
                    auto u = boost::opposite(e, v, _g);
                    auto vindex = _index_map[v];
                    auto uindex = _index_map[u];
                    _tree_node_map[uindex]->add_child(_tree_node_map[vindex]);
                }
            }

            // compute first in path
            compute_first_in_path();
        }

        void compute_first_in_path() {
            std::stack<SPSubtree<Graph, WeightMap, Vertex>> stack;
            stack.emplace(_source, _root);

            while (!stack.empty()) {
                SPSubtree<Graph, WeightMap, Vertex> r = stack.top();
                stack.pop();

                if (r.root == _root) {
                    auto v = r.root->vertex();
                    auto vindex = _index_map[v];
                    _first_in_path[vindex] = v;
                    for (auto c : r.root->children()) {
                        stack.emplace(SPSubtree<Graph, WeightMap, Vertex> { static_cast<Vertex>(c->vertex()), c });
                    }
                } else {
                    auto v = r.root->vertex();
                    auto vindex = _index_map[v];
                    _first_in_path[vindex] = r.info;
                    for (auto c : r.root->children()) {
                        stack.emplace(SPSubtree<Graph, WeightMap, Vertex> { static_cast<Vertex>(r.info), c });
                    }
                }
            }
        }

    };

    template<class Graph, class WeightMap>
    class CandidateCycle {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        CandidateCycle(std::size_t tree, const Edge &e, WeightType weight) :
                _tree(tree), _e(e), _weight(weight) {
        }

        CandidateCycle(const CandidateCycle &c) :
                _tree(c._tree), _e(c._e), _weight(c._weight) {
        }

        CandidateCycle& operator=(const CandidateCycle &other) {
            if (this != &other) {
                _tree = other._tree;
                _e = other._e;
                _weight = other._weight;
            }
            return *this;
        }

        std::size_t tree() const {
            return _tree;
        }

        const Edge& edge() const {
            return _e;
        }

        const WeightType& weight() const {
            return _weight;
        }

    private:
        std::size_t _tree;
        Edge _e;
        WeightType _weight;
    };

    template<class Graph>
    struct SerializableCandidateCycle {
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename ForestIndex<Graph>::size_type Edge;

        SerializableCandidateCycle() {
        }

        SerializableCandidateCycle(Vertex v, Edge e) :
                v(v), e(e) {
        }

        template<typename Archive>
        void serialize(Archive &ar, const unsigned) {
            ar & v;
            ar & e;
        }

        Vertex v;
        Edge e;
    };

    template<class Graph, class WeightMap>
    struct SerializableMinOddCycle {
        typedef typename ForestIndex<Graph>::size_type Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        SerializableMinOddCycle() :
                exists(false) {
        }

        SerializableMinOddCycle(std::vector<Edge> edges, WeightType weight, bool exists) :
                edges(edges), weight(weight), exists(exists) {
        }

        SerializableMinOddCycle(const SerializableMinOddCycle<Graph, WeightMap> &c) :
                edges(c.edges), weight(c.weight), exists(c.exists) {
        }

        SerializableMinOddCycle<Graph, WeightMap>& operator=(const SerializableMinOddCycle<Graph, WeightMap> &other) {
            if (this != &other) {
                edges = other.edges;
                weight = other.weight;
                exists = other.exists;
            }
            return *this;
        }

        template<typename Archive>
        void serialize(Archive &ar, const unsigned) {
            ar & edges;
            ar & weight;
            ar & exists;
        }

        std::vector<Edge> edges;
        WeightType weight;
        bool exists;
    };

    template<class Graph, class WeightMap>
    struct SerializableMinOddCycleMinOp {

        const SerializableMinOddCycle<Graph, WeightMap>& operator()(
                const SerializableMinOddCycle<Graph, WeightMap> &lhs,
                const SerializableMinOddCycle<Graph, WeightMap> &rhs) const {
            if (!lhs.exists || !rhs.exists) {
                if (lhs.exists) {
                    return lhs;
                } else {
                    return rhs;
                }
            }
            // both valid, compare
            if (lhs.weight < rhs.weight) {
                return lhs;
            }
            return rhs;
        }

    };

    template<class Graph, class WeightMap>
    class CandidateCycleToSerializableConverter {
    public:
        CandidateCycleToSerializableConverter(const std::vector<parmcb::SPTree<Graph, WeightMap>> &trees,
                const ForestIndex<Graph> &forest_index) :
                trees(trees), forest_index(forest_index) {
        }

        SerializableCandidateCycle<Graph> operator()(const CandidateCycle<Graph, WeightMap> &cycle) const {
            return SerializableCandidateCycle<Graph>(trees.at(cycle.tree()).source(), forest_index(cycle.edge()));
        }

    private:
        const std::vector<parmcb::SPTree<Graph, WeightMap>> &trees;
        const ForestIndex<Graph> &forest_index;
    };

    template<class Graph, class WeightMap>
    class CandidateCycleBuilder {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        CandidateCycleBuilder(const Graph &g, const WeightMap &weight_map) :
                g(g), weight_map(weight_map) {
        }

        std::tuple<std::set<Edge>, WeightType, bool> operator()(const std::vector<parmcb::SPTree<Graph, WeightMap>> &trees,
                const CandidateCycle<Graph, WeightMap> &c, const std::set<Edge> &signed_edges, bool use_weight_limit,
                WeightType weight_limit) const {

            std::shared_ptr<SPNode<Graph, WeightMap>> v = trees[c.tree()].node(boost::source(c.edge(), g));
            std::shared_ptr<SPNode<Graph, WeightMap>> u = trees[c.tree()].node(boost::target(c.edge(), g));

            Edge e = c.edge();
            if (v->parity() ^ u->parity() ^ (signed_edges.find(e) != signed_edges.end())) {
                // odd cycle, validate
                bool valid = true;
                WeightType cycle_weight = boost::get(weight_map, e);
                std::set<Edge> result;
                result.insert(e);

                if (use_weight_limit && cycle_weight > weight_limit) {
                    return std::make_tuple(std::set<Edge> { }, 0.0, false);
                }

                // first part
                Vertex w = boost::source(c.edge(), g);
                std::shared_ptr<SPNode<Graph, WeightMap>> ws = trees[c.tree()].node(w);
                while (ws->has_pred()) {
                    Edge a = ws->pred();
                    if (result.insert(a).second == false) {
                        valid = false;
                        break;
                    }
                    cycle_weight += boost::get(weight_map, a);
                    if (use_weight_limit && cycle_weight > weight_limit) {
                        valid = false;
                        break;
                    }
                    w = boost::opposite(a, w, g);
                    ws = trees[c.tree()].node(w);
                }

                if (!valid) {
                    return std::make_tuple(std::set<Edge> { }, 0.0, false);
                }

                // second part
                w = boost::target(c.edge(), g);
                ws = trees[c.tree()].node(w);
                while (ws->has_pred()) {
                    Edge a = ws->pred();
                    if (result.insert(a).second == false) {
                        valid = false;
                        break;
                    }
                    cycle_weight += boost::get(weight_map, a);
                    if (use_weight_limit && cycle_weight > weight_limit) {
                        valid = false;
                        break;
                    }
                    w = boost::opposite(a, w, g);
                    ws = trees[c.tree()].node(w);
                }

                if (!valid) {
                    return std::make_tuple(std::set<Edge> { }, 0.0, false);
                }

                return std::make_tuple(result, cycle_weight, true);
            }
            return std::make_tuple(std::set<Edge> { }, 0.0, false);
        }

    private:
        const Graph &g;
        const WeightMap &weight_map;
    };

    template<class Graph, class WeightMap, bool ParallelUsingTBB>
    class ShortestOddCycleLookup {
    public:
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        ShortestOddCycleLookup(const Graph &g, const WeightMap &weight_map,
                std::vector<parmcb::SPTree<Graph, WeightMap>> &trees,
                std::vector<parmcb::CandidateCycle<Graph, WeightMap>> &cycles, bool sorted_cycles) :
                g(g), weight_map(weight_map), candidate_cycle_builder(g, weight_map), trees(trees), cycles(cycles), sorted_cycles(
                        sorted_cycles) {
        }

        std::tuple<std::set<Edge>, WeightType, bool> operator()(const std::set<Edge> &edges) {
            return compute_shortest_odd_cycle(edges);
        }

    private:

        template<bool is_tbb_enabled = ParallelUsingTBB>
        std::tuple<std::set<Edge>, WeightType, bool> compute_shortest_odd_cycle(const std::set<Edge> &edges,
                typename std::enable_if<!is_tbb_enabled>::type* = 0) {

            for (std::size_t i = 0; i < trees.size(); i++) {
                trees[i].update_parities(edges);
            }

            std::tuple<std::set<Edge>, WeightType, bool> min;

            for (CandidateCycle<Graph, WeightMap> c : cycles) {
                std::tuple<std::set<Edge>, WeightType, bool> cc = candidate_cycle_builder(trees, c, edges,
                        std::get<2>(min), std::get<1>(min));

                if (std::get<2>(cc)) {
                    if (sorted_cycles) {
                        return cc;
                    }

                    if (!std::get<2>(min)) {
                        min = cc;
                    } else {
                        if (std::get<1>(cc) < std::get<1>(min)) {
                            min = cc;
                        }
                    }
                }
            }
            return min;
        }

        template<bool is_tbb_enabled = ParallelUsingTBB>
        std::tuple<std::set<Edge>, WeightType, bool> compute_shortest_odd_cycle(const std::set<Edge> &edges,
                typename std::enable_if<is_tbb_enabled>::type* = 0) {

            tbb::parallel_for(tbb::blocked_range<std::size_t>(0, trees.size()),
                    [&](const tbb::blocked_range<std::size_t> &r) {
                        for (std::size_t i = r.begin(); i != r.end(); ++i) {
                            trees[i].update_parities(edges);
                        }
                    });

            std::less<WeightType> compare = std::less<WeightType>();
            typedef std::tuple<std::set<Edge>, WeightType, bool> cycle_t;
            auto cycle_min = [compare](const cycle_t &c1, const cycle_t &c2) {
                if (!std::get<2>(c1) || !std::get<2>(c2)) {
                    if (std::get<2>(c1)) {
                        return c1;
                    } else {
                        return c2;
                    }
                }
                // both valid, compare
                if (!compare(std::get<1>(c2), std::get<1>(c1))) {
                    return c1;
                }
                return c2;
            };

            return tbb::parallel_reduce(tbb::blocked_range<std::size_t>(0, cycles.size()),
                    std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                    [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                        for (std::size_t i = r.begin(); i < r.end(); i++) {
                            auto c = cycles[i];
                            auto cc = candidate_cycle_builder(trees, c, edges, std::get<2>(running_min),
                                    std::get<1>(running_min));
                            if (std::get<2>(cc)) {
                                if (!std::get<2>(running_min) || compare(std::get<1>(cc), std::get<1>(running_min))) {
                                    running_min = cc;
                                }
                            }
                        }
                        return running_min;
                    },
                    cycle_min);
        }

        const Graph &g;
        const WeightMap &weight_map;
        const CandidateCycleBuilder<Graph, WeightMap> candidate_cycle_builder;
        std::vector<parmcb::SPTree<Graph, WeightMap>> &trees;
        std::vector<parmcb::CandidateCycle<Graph, WeightMap>> &cycles;
        bool sorted_cycles;
    };

} // parmcb

#endif


//
// Copyright (c) 2015-2016 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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 NUDB_DETAIL_BUCKET_HPP
#define NUDB_DETAIL_BUCKET_HPP

#include <nudb/error.hpp>
#include <nudb/type_traits.hpp>
#include <nudb/detail/bulkio.hpp>
#include <nudb/detail/field.hpp>
#include <nudb/detail/format.hpp>
#include <boost/assert.hpp>
#include <cstddef>
#include <cstdint>
#include <cstring>

namespace nudb {
namespace detail {

// Returns bucket index given hash, buckets, and modulus
//
inline
nbuck_t
bucket_index(nhash_t h, nbuck_t buckets, std::uint64_t modulus)
{
    BOOST_ASSERT(modulus <= 0x100000000ULL);
    auto n = h % modulus;
    if(n >= buckets)
        n -= modulus / 2;
    return static_cast<nbuck_t>(n);
}

//------------------------------------------------------------------------------

// Tag for constructing empty buckets
struct empty_t
{
    constexpr empty_t() = default;
};

static empty_t constexpr empty{};

// Allows inspection and manipulation of bucket blobs in memory
template<class = void>
class bucket_t
{
    nsize_t block_size_;    // Size of a key file block
    nkey_t size_;           // Current key count
    noff_t spill_;          // Offset of next spill record or 0
    std::uint8_t* p_;       // Pointer to the bucket blob

public:
    struct value_type
    {
        noff_t offset;
        nhash_t hash;
        nsize_t size;
    };

    bucket_t() = default;
    bucket_t(bucket_t const&) = default;
    bucket_t& operator=(bucket_t const&) = default;

    bucket_t(nsize_t block_size, void* p);

    bucket_t(nsize_t block_size, void* p, empty_t);

    nsize_t
    block_size() const
    {
        return block_size_;
    }

    // Serialized bucket size.
    // Excludes empty
    nsize_t
    actual_size() const
    {
        return bucket_size(size_);
    }

    bool
    empty() const
    {
        return size_ == 0;
    }

    bool
    full() const
    {
        return size_ >=
            detail::bucket_capacity(block_size_);
    }

    nkey_t
    size() const
    {
        return size_;
    }

    // Returns offset of next spill record or 0
    //
    noff_t
    spill() const
    {
        return spill_;
    }

    // Set offset of next spill record
    //
    void
    spill(noff_t offset);

    // Clear contents of the bucket
    //
    void
    clear();

    // Returns the record for a key
    // entry without bounds checking.
    //
    value_type const
    at(nkey_t i) const;

    value_type const
    operator[](nkey_t i) const
    {
        return at(i);
    }

    // Returns index of entry with prefix
    // equal to or greater than the given prefix.
    //
    nkey_t
    lower_bound(nhash_t h) const;

    void
    insert(noff_t offset, nsize_t size, nhash_t h);

    // Erase an element by index
    //
    void
    erase(nkey_t i);

    // Read a full bucket from the
    // file at the specified offset.
    //
    template<class File>
    void
    read(File& f, noff_t, error_code& ec);

    // Read a compact bucket
    //
    template<class File>
    void
    read(bulk_reader<File>& r, error_code& ec);

    // Write a compact bucket to the stream.
    // This only writes entries that are not empty.
    //
    void
    write(ostream& os) const;

    // Write a bucket to the file at the specified offset.
    // The full block_size() bytes are written.
    //
    template<class File>
    void
    write(File& f,noff_t offset, error_code& ec) const;

private:
    // Update size and spill in the blob
    void
    update();
};

//------------------------------------------------------------------------------

template<class _>
bucket_t<_>::
bucket_t(nsize_t block_size, void* p)
    : block_size_(block_size)
    , p_(reinterpret_cast<std::uint8_t*>(p))
{
    // Bucket Record
    istream is(p_, block_size);
    detail::read<uint16_t>(is, size_);  // Count
    detail::read<uint48_t>(is, spill_); // Spill
}

template<class _>
bucket_t<_>::
bucket_t(nsize_t block_size, void* p, empty_t)
    : block_size_(block_size)
    , size_(0)
    , spill_(0)
    , p_(reinterpret_cast<std::uint8_t*>(p))
{
    clear();
}

template<class _>
void
bucket_t<_>::
spill(noff_t offset)
{
    spill_ = offset;
    update();
}

template<class _>
void
bucket_t<_>::clear()
{
    size_ = 0;
    spill_ = 0;
    std::memset(p_, 0, block_size_);
}

template<class _>
auto
bucket_t<_>::
at(nkey_t i) const ->
    value_type const
{
    value_type result;
    // Bucket Entry
    auto const w =
        field<uint48_t>::size +         // Offset
        field<uint48_t>::size +         // Size
        field<f_hash>::size;            // Prefix
    // Bucket Record
    detail::istream is{p_ +
        field<std::uint16_t>::size +    // Count
        field<uint48_t>::size +         // Spill
        i * w, w};
    // Bucket Entry
    detail::read<uint48_t>(
        is, result.offset);             // Offset
    detail::read_size48(
        is, result.size);               // Size
    detail::read<f_hash>(
        is, result.hash);               // Hash
    return result;
}

template<class _>
nkey_t
bucket_t<_>::
lower_bound(nhash_t h) const
{
    // Bucket Entry
    auto const w =
        field<uint48_t>::size +         // Offset
        field<uint48_t>::size +         // Size
        field<f_hash>::size;            // Hash
    // Bucket Record
    auto const p = p_ +
        field<std::uint16_t>::size +    // Count
        field<uint48_t>::size +         // Spill
        // Bucket Entry
        field<uint48_t>::size +         // Offset
        field<uint48_t>::size;          // Size
    nkey_t step;
    nkey_t first = 0;
    nkey_t count = size_;
    while(count > 0)
    {
        step = count / 2;
        nkey_t i = first + step;
        nhash_t h1;
        readp<f_hash>(p + i * w, h1);
        if(h1 < h)
        {
            first = i + 1;
            count -= step + 1;
        }
        else
        {
            count = step;
        }
    }
    return first;
}

template<class _>
void
bucket_t<_>::
insert(
    noff_t offset, nsize_t size, nhash_t h)
{
    auto const i = lower_bound(h);
    // Bucket Record
    auto const p = p_ +
        field<
            std::uint16_t>::size +  // Count
        field<uint48_t>::size;      // Spill
    // Bucket Entry
    auto const w =
        field<uint48_t>::size +     // Offset
        field<uint48_t>::size +     // Size
        field<f_hash>::size;        // Hash
    std::memmove(
        p +(i + 1)  * w,
        p + i       * w,
        (size_ - i) * w);
    ++size_;
    update();
    // Bucket Entry
    ostream os{p + i * w, w};
    detail::write<uint48_t>(
        os, offset);                // Offset
    detail::write<uint48_t>(
        os, size);                  // Size
    detail::write<f_hash>(
        os, h);                     // Prefix
}

template<class _>
void
bucket_t<_>::
erase(nkey_t i)
{
    // Bucket Record
    auto const p = p_ +
        field<std::uint16_t>::size +    // Count
        field<uint48_t>::size;          // Spill
    auto const w =
        field<uint48_t>::size +         // Offset
        field<uint48_t>::size +         // Size
        field<f_hash>::size;            // Hash
    --size_;
    if(i < size_)
        std::memmove(
            p +  i      * w,
            p +(i + 1) * w,
           (size_ - i) * w);
    std::memset(p + size_ * w, 0, w);
    update();
}

template<class _>
template<class File>
void
bucket_t<_>::
read(File& f, noff_t offset, error_code& ec)
{
    auto const cap = bucket_capacity(block_size_);
    // Excludes padding to block size
    f.read(offset, p_, bucket_size(cap), ec);
    if(ec)
        return;
    istream is{p_, block_size_};
    detail::read<std::uint16_t>(is, size_); // Count
    detail::read<uint48_t>(is, spill_);     // Spill
    if(size_ > cap)
    {
        ec = error::invalid_bucket_size;
        return;
    }
}

template<class _>
template<class File>
void
bucket_t<_>::
read(bulk_reader<File>& r, error_code& ec)
{
    // Bucket Record(compact)
    auto is = r.prepare(
        detail::field<std::uint16_t>::size +
        detail::field<uint48_t>::size, ec);
    if(ec)
        return;
    detail::read<std::uint16_t>(is, size_); // Count
    detail::read<uint48_t>(is, spill_);     // Spill
    update();
    // Excludes empty bucket entries
    auto const w = size_ * (
        field<uint48_t>::size +             // Offset
        field<uint48_t>::size +             // Size
        field<f_hash>::size);               // Hash
    is = r.prepare(w, ec);
    if(ec)
        return;
    std::memcpy(p_ +
        field<std::uint16_t>::size +        // Count
        field<uint48_t>::size,              // Spill
        is.data(w), w);                     // Entries
}

template<class _>
void
bucket_t<_>::
write(ostream& os) const
{
    // Does not pad up to the block size. This
    // is called to write to the data file.
    auto const size = actual_size();
    // Bucket Record
    std::memcpy(os.data(size), p_, size);
}

template<class _>
template<class File>
void
bucket_t<_>::
write(File& f, noff_t offset, error_code& ec) const
{
    // Includes zero pad up to the block
    // size, to make the key file size always
    // a multiple of the block size.
    auto const size = actual_size();
    std::memset(p_ + size, 0, block_size_ - size);
    // Bucket Record
    f.write(offset, p_, block_size_, ec);
    if(ec)
        return;
}

template<class _>
void
bucket_t<_>::
update()
{
    // Bucket Record
    ostream os{p_, block_size_};
    detail::write<std::uint16_t>(os, size_);    // Count
    detail::write<uint48_t>(os, spill_);        // Spill
}

using bucket = bucket_t<>;

//------------------------------------------------------------------------------

//  Spill bucket if full.
//  The bucket is cleared after it spills.
//
template<class File>
void
maybe_spill(
    bucket& b, bulk_writer<File>& w, error_code& ec)
{
    if(b.full())
    {
        // Spill Record
        auto const offset = w.offset();
        auto os = w.prepare(
            field<uint48_t>::size + // Zero
            field<uint16_t>::size + // Size
            b.actual_size(), ec);
        if(ec)
            return;
        write<uint48_t>(os, 0ULL);  // Zero
        write<std::uint16_t>(
            os, b.actual_size());   // Size
        auto const spill =
            offset + os.size();
        b.write(os);                // Bucket
        // Update bucket
        b.clear();
        b.spill(spill);
    }
}

} // detail
} // nudb

#endif


// this must define the same symbol as the main module file (numpy requirement)
#define PY_ARRAY_UNIQUE_SYMBOL andres_graph_PyArray_API
#define NO_IMPORT_ARRAY

#include <Python.h>
#include <boost/python.hpp>
#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>
#include <andres/graph/hdf5/graph.hxx>
#include <andres/graph/hdf5/grid-graph.hxx>
#include <vector>
#include <string>

#include "andres/graph/grid-graph.hxx"
#include "andres/functional.hxx"
#include "andres/graph/graph.hxx"
#include "andres/graph/threadpool.hxx"
#include "andres/graph/neighborhood.hxx"
#include "andres/graph/multicut-lifted/lifted_mc_model.hxx"

#include "image.hxx"

namespace bp = boost::python;
namespace agraph = andres::graph;

template<class MODEL>
void addLongRangeEdges(
    MODEL & model,
    vigra::NumpyArray<2, float> edgePmap, // in 0-1
    const float beta = 0.5f,
    const int minRadius = 2,
    const int maxRadius = 7
){
    GRAPH_CHECK_OP(model.originalGraph().shape(0), == ,edgePmap.shape(0),"");
    GRAPH_CHECK_OP(model.originalGraph().shape(1), == ,edgePmap.shape(1),"");

    typedef vigra::TinyVector<int,   2> Coord;
    const auto shape = edgePmap.shape();

    auto clipToImg = [&](const Coord & coord){
        auto c = coord;
        for(auto i=0; i<2; ++i){
            c[i] = std::max(0, c[i]);
            c[i] = std::min(int(shape[i]),c[i]);
        }
        return c;
    };

    // move a coordinate to local min
    auto moveToMin = [&](const Coord & coord){
        Coord coords[5] = {
                coord,
                clipToImg(coord+Coord(0,1)),
                clipToImg(coord+Coord(1,0)),
                clipToImg(coord+Coord(-1,-1)),
                clipToImg(coord+Coord(-1, 0))
        };
        auto minVal = std::numeric_limits<float>::infinity();
        auto minCoord = Coord();
        for(size_t i=0; i<5; ++i){
            const auto val = edgePmap[coord];
            if(val<minVal){
                minVal = val;
                minCoord = coord;
            }
        }
        return minCoord;
    };

    auto & originalGraph = model.originalGraph();
    auto & liftedGraph = model.liftedGraph();

    int rad = 5;

    auto pOpt = agraph::ParallelOptions();
    const auto nThreads = pOpt.numThreads(-1).getActualNumThreads();

    typedef vigra::TinyVector<float, 2> FCoord;
    auto node = [&](const Coord & coord){
        return size_t(coord[0] + coord[1]*shape[0]);
    };

    std::mutex graphMutex;
    std::mutex mapMutex;

    std::set<size_t > processed;

    struct ToAdd{
        size_t u,v;
        float w;
    };

    size_t bufferSize = 1000000;
    std::vector<std::vector<ToAdd> > buffers(nThreads);
    for(auto & vec : buffers){
        vec.reserve(bufferSize+1);
    }

    auto addToBuffer = [&](std::vector<ToAdd> & buffer,const size_t u_, const size_t v_, const float w_){

        ToAdd ta;
        ta.u=u_;
        ta.v=v_;
        ta.w=w_;
        buffer.push_back(ta);
        //std::cout<<"buffer size"<<buffer.size()<<"\n";
        if(buffer.size()>=bufferSize){

            std::unique_lock<std::mutex> lock(graphMutex);
            //std::cout<<"clear buffer\n";
            for(const auto & ta  : buffer){
                const auto fe = liftedGraph.findEdge(ta.u,ta.v);
                // not yet  in lifted graph
                // therefore cannot be processed
                if(!fe.first){
                    //std::cout<<"a\n";
                    const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                    processed.insert(e);
                }
                // edge is in lifted graph
                else{
                    //std::cout<<"b\n";
                    auto fm = processed.find(fe.second);
                    // not yet processed
                    if(fm == processed.end()){
                        std::cout<<"b1\n";
                        const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                        processed.insert(e);
                    }
                    else{
                        std::cout<<"b2\n";
                    }
                }
            }
            buffer.resize(0);
        }
    };

    std::cout<<"lifted graph edge num "<<liftedGraph.numberOfEdges()<<"\n";
    agraph::parallel_foreach(
        nThreads,
        shape[1],
        [&](const int threadId, const int y){
            auto & buffer = buffers[threadId];
            for(int x=0; x<shape[0]; ++x){
                const auto p = Coord(x,y);
                const auto u = node(p);
                GRAPH_CHECK_OP(u,<,originalGraph.numberOfVertices(),"");
                GRAPH_CHECK_OP(u,<,liftedGraph.numberOfVertices(),"");
                const auto start = clipToImg(p-maxRadius);
                const auto end = clipToImg(p+maxRadius+1);
                auto q = Coord();
                for(q[0]=start[0]; q[0]<end[0]; ++q[0])
                for(q[1]=start[1]; q[1]<end[1]; ++q[1]){

                    GRAPH_CHECK_OP(q[0],>=,0,"");
                    GRAPH_CHECK_OP(q[1],>=,0,"");
                    GRAPH_CHECK_OP(q[0],<,shape[0],"");
                    GRAPH_CHECK_OP(q[1],<,shape[1],"");
                    const auto v = node(q);

                    size_t e;
                    if( norm(p-q) < float(minRadius))
                        continue;
                    if(p==q || v>u){
                        continue;
                    }

                    GRAPH_CHECK_OP(v,<,originalGraph.numberOfVertices(),"");
                    GRAPH_CHECK_OP(v,<,liftedGraph.numberOfVertices(),"");
                    const auto qf = FCoord(q);
                    const auto pq = q-p;
                    const auto dist = vigra::norm(pq);
                    const auto step =  pq*(1.0f / (dist * 1.3f + 0.5f));
                    auto pOnLine = FCoord(p);
                    auto noMax = true;
                    auto maxVal = -1.0f*std::numeric_limits<float>::infinity();
                    while(Coord(pOnLine)!=q){
                        //std::cout<<"pol "<<pOnLine<<"\n";
                        auto iCord = Coord(pOnLine);
                        if(iCord != p){
                            noMax = false;
                            maxVal = std::max(edgePmap[iCord], maxVal);
                        }
                        pOnLine += step;
                    }
                    const double p1 = std::max(std::min(maxVal,0.999f),0.001f);
                    const double p0 = 1.0 - p1;
                    auto w = std::log(p0/p1) + beta;

                    addToBuffer(buffer, u,v,w);
                }
            }
        }
    );

    // clear whats left in buffers
    for(const auto & buffer : buffers){
        for(const auto & ta : buffer){
            const auto fe = liftedGraph.findEdge(ta.u,ta.v);
            // not yet  in lifted graph
            // therefore cannot be processed
            if(!fe.first){
                const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                processed.insert(e);
            }
            // edge is in lifted graph
            else{
                auto fm = processed.find(fe.second);
                // not yet processed
                if(fm == processed.end()){
                    const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                    processed.insert(e);
                }
            }
        }
    }
    std::cout<<"lifted graph edge num "<<liftedGraph.numberOfEdges()<<"\n";
}

template<class MODEL>
void addLongRangeNH(
    MODEL & model,
    const int radius = 7
){
    for(auto u=0; u<model.originalGraph().numberOfVertices(); ++u){
        const auto otherNodes = agraph::verticesInGraphNeigborhood(model.originalGraph(), u, radius);
        for(const auto v : otherNodes){
            if(u!=v){
                model.setCost(u,v,0.0);
            }
        }
    }
}

template<class LiftedMcModel>
double evalCut(
    LiftedMcModel & liftedMcModel,
    vigra::NumpyArray<1, uint8_t> cut
){
    auto s = 0.0;
    for(auto e=0; e<liftedMcModel.liftedGraph().numberOfEdges(); ++e){
        if(cut[e]){
            s += liftedMcModel.edgeCosts()[e];
        }
    }
    return s;
}

template<class LiftedMcModel>
void setCosts(
    LiftedMcModel & liftedMcModel,
    vigra::NumpyArray<1, vigra::TinyVector<uint64_t, 2> > uv,
    vigra::NumpyArray<1, float> costs,
    const bool overwrite
){
    GRAPH_CHECK_OP(uv.size(), == , costs.size(), "shape mismatch: uv and costs have different size");

    const auto u = uv.bindElementChannel(0);
    const auto v = uv.bindElementChannel(1);
    const auto c = vigra::MultiArrayView<1, float>(costs);
    liftedMcModel.setCosts(u.begin(), u.end(), v.begin(), c.begin(), overwrite);
}

template<class LiftedMcModel>
vigra::NumpyAnyArray edgeLabelsToNodeLabels(
    const LiftedMcModel & model,
    vigra::NumpyArray<1, uint8_t> edgeLabels,
    vigra::NumpyArray<1, uint64_t> nodeLabels
){
    vigra::TinyVector<int,1> shape(model.originalGraph().numberOfVertices());
    nodeLabels.reshapeIfEmpty(shape);
    model.getNodeLabels(edgeLabels, nodeLabels);
    return nodeLabels;
}

template<class LiftedMcModel>
vigra::NumpyAnyArray nodeLabelsToEdgeLabels(
    const LiftedMcModel & model,
    vigra::NumpyArray<1, uint64_t> nodeLabels,
    vigra::NumpyArray<1, uint8_t> edgeLabels
){
    vigra::TinyVector<int,1> shape(model.liftedGraph().numberOfEdges());
    edgeLabels.reshapeIfEmpty(shape);
    model.getEdgeLabels(nodeLabels, edgeLabels);
    return edgeLabels;
}

template<class LiftedMcModel>
void fuseGtObjective(
    LiftedMcModel & model,
    vigra::NumpyArray<2, uint64_t> nodeLabels,
    const size_t rr,
    const double beta,
    const bool verbose
){
    if(verbose)
        std::cout<<"nodeLabel.shape "<<nodeLabels.shape()<<"\n";

    const auto & originalGraph = model.originalGraph();
    const auto & liftedGraph = model.liftedGraph();
    auto nV = originalGraph.numberOfVertices();
    auto nGt = nodeLabels.shape(1);

    std::vector<std::set<size_t> > extendedNh(nV);
    std::vector<std::set<size_t> > extendedNh2(nV);

    for(auto n=0; n<originalGraph.numberOfVertices(); ++n){
        for(auto iter = originalGraph.verticesFromVertexBegin(n); iter!=originalGraph.verticesFromVertexEnd(n); ++iter){
            extendedNh[n].insert(*iter);
            extendedNh2[n].insert(*iter);
        }
    }

    for(auto r=0;r<2;++r){

        for(auto n=0; n<originalGraph.numberOfVertices(); ++n){
            auto & thisNodeNhSet = extendedNh2[n];
            for(auto iter = originalGraph.verticesFromVertexBegin(n); iter!=originalGraph.verticesFromVertexEnd(n); ++iter){
                auto otherNode = *iter;
                const auto & otherNodeNhSet = extendedNh[otherNode];
                thisNodeNhSet.insert(otherNodeNhSet.begin(), otherNodeNhSet.end());
                thisNodeNhSet.erase(n);
            }
        }
        extendedNh = extendedNh2;
    }

    for(auto u=0; u<originalGraph.numberOfVertices(); ++u){
        const auto & enh = extendedNh[u];
        //std::cout<<"node "<<u<<" |enh| = "<<enh.size()<<"\n";

        const auto uGt = nodeLabels.bindInner(u);
        for(const auto v : enh){
            const auto vGt = nodeLabels.bindInner(v);
            auto p1 = 0.0;
            for(size_t gtc=0; gtc<nGt; ++gtc){
                p1 += uGt[gtc] != vGt[gtc] ?  1 : 0;
            }
            p1 /= nGt;
            //if (p1>0.1)
            //    std::cout<<"   p1 "<<p1<<"\n";

            p1 = std::max(std::min(0.999, p1),0.001);
            auto p0 = 1.0 - p1;
            auto w = std::log(p0/p1) + std::log((1.0-beta)/beta);
            model.setCost(u,v,w);
        }
    }
}

template<class LiftedMcModel>
void fuseGtObjectiveGrid(
    LiftedMcModel & model,
    vigra::NumpyArray<3, uint64_t> nodeLabels,
    vigra::NumpyArray<1, double>    pExpert,
    vigra::NumpyArray<2, float>     cutRegularizer,
    const size_t rr,
    const double beta,
    //const double cLocal,
    const bool verbose
){
    if(verbose)
        std::cout<<"nodeLabel.shape "<<nodeLabels.shape()<<"\n";

    const auto & originalGraph = model.originalGraph();
    const auto & liftedGraph = model.liftedGraph();
    auto nV = originalGraph.numberOfVertices();
    typedef vigra::TinyVector<int,2> Coord;
    Coord shape(originalGraph.shape(0), originalGraph.shape(1));

    auto nGt = nodeLabels.shape(2);

    std::vector<std::set<size_t> > extendedNh(nV);
    std::vector<std::set<size_t> > extendedNh2(nV);

    auto clipToImg = [&](const Coord & coord){
        auto c = coord;
        for(auto i=0; i<2; ++i){
            c[i] = std::max(0, c[i]);
            c[i] = std::min(int(shape[i]),c[i]);
        }
        return c;
    };
    auto node = [&](const Coord & coord){
        return size_t(coord[0] + coord[1]*shape[0]);
    };

    Coord p;

    for(p[1]=0; p[1]<shape[1]; ++p[1])
    for(p[0]=0; p[0]<shape[0]; ++p[0]){

        const auto u = node(p);
        const auto start = clipToImg(p-int(rr));
        const auto end = clipToImg(p+int(rr)+1);

        auto lp = nodeLabels.bindInner(p[0]).bindInner(p[1]);
        auto q = Coord();

        for(q[1]=start[1]; q[1]<end[1]; ++q[1])
        for(q[0]=start[0]; q[0]<end[0]; ++q[0]){
            const auto v = node(q);
            if(p!=q && u < v){
                auto d = vigra::norm(p-q);
                if(d<=float(rr)){
                    if(d<=1.5)
                        d*=0.5;
                    auto lq = nodeLabels.bindInner(q[0]).bindInner(q[1]);
                    auto p0 = 0.0;
                    auto p1 = 0.0;
                    for(size_t gtc=0; gtc<nGt; ++gtc){
                        p0 += (lp[gtc] == lq[gtc]) ?  pExpert(gtc) : 0.0;
                        p1 += (lp[gtc] != lq[gtc]) ?  pExpert(gtc) : 0.0;
                    }
                    auto Z = p0+p1;
                    p0/=Z;
                    p1/=Z;
                    //if (p1>0.1)
                    //    std::cout<<"   p1 "<<p1<<"\n";

                    p1 = std::max(std::min(0.99999, p1),0.00001);
                    p0 = 1.0 - p1;
                    auto w = std::log(p0/p1) + std::log((1.0-beta)/beta);
                    if(d<=1.01){
                        auto c = (cutRegularizer[p] + cutRegularizer[q])/2.0;
                        w += c;
                    }
                    model.setCost(u,v,w*d);
                }
            }
        }
    }
}

template<class LiftedMcModel>
void thinObjectSeededSeg(
    LiftedMcModel & model,
    vigra::NumpyArray<2, uint64_t>  linkConstraints,
    vigra::NumpyArray<2, float>     cutCosts,
    const double constraintCost,
    const bool verbose
){

    auto shape = linkConstraints.shape();
    auto tshape = cutCosts.shape();
    GRAPH_CHECK_OP(shape[0],==,model.originalGraph().shape(0),"");
    GRAPH_CHECK_OP(shape[1],==,model.originalGraph().shape(1),"");

    GRAPH_CHECK_OP(shape[0]*2-1,==,tshape[0],"");
    GRAPH_CHECK_OP(shape[1]*2-1,==,tshape[1],"");

    typedef vigra::TinyVector<int,2> Coord;
    ///  link constraints
    std::map<uint64_t, std::vector<size_t> >   constraintSet;

    auto node = [&](const Coord & c){
        return c[0] + c[1]*shape[0];
    };
    std::cout<<"local terms\n";
    for(auto y=0; y<shape[1]; ++y)
    for(auto x=0; x<shape[0]; ++x){
        const Coord c(x,y);
        auto u = node(c);
        if(linkConstraints(x,y)!=255){
            constraintSet[linkConstraints(x,y)].push_back(u);
        }
        if(x+1<shape[0]){
            const Coord c2(x+1,y);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
        if(y+1<shape[1]){
            const Coord c2(x,y+1);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
    }
    std::cout<<"to vecvec\n";
    std::vector< std::vector<size_t> >   cVecVec;
    for(auto iter = constraintSet.begin(); iter!=constraintSet.end(); ++iter){
        cVecVec.push_back(iter->second);
    }
    std::cout<<"must link terms\n";
    // must link constraints
    for(auto & inodes : cVecVec){
        GRAPH_CHECK_OP(inodes.size(),>=,1,"");
        std::cout<<"inodes "<<inodes.size()<<"\n";
        if(inodes.size() >1){
            for(auto i=0; i<inodes.size()-1; ++i)
            for(auto j=i+1; j<inodes.size(); ++j){
                model.setCost(inodes[i],inodes[j], constraintCost);
            }
        }
    }
    std::cout<<"cannot link terms\n";
    // cannot link constraints
    for(auto i=0; i<cVecVec.size()-1; ++i){
        const auto & nodesI = cVecVec[i];
        for(auto j=i+1; j<cVecVec.size(); ++j){
            const auto & nodesJ = cVecVec[j];

            for(auto in : nodesI)
            for(auto jn : nodesJ){
                model.setCost(in,jn,-1.0*constraintCost);
            }
        }
    }

}

template<class LiftedMcModel>
void lmSuperpixel(
    LiftedMcModel & model,
    vigra::NumpyArray<2, float>     cutCosts,
    const size_t seedRadius = 10,
    const size_t stepSize = 2,
    const double sigma = 3.0,
    const double seedRepulsion = 1.0
){

    const int shape[2] = {
        int(model.originalGraph().shape(0)),
        int(model.originalGraph().shape(1))
    };
    auto tshape = cutCosts.shape();

    typedef vigra::TinyVector<int,2> Coord;

    auto node = [&](const Coord & c){
        return c[0] + c[1]*shape[0];
    };
    std::cout<<"generate seeds\n";

    std::vector<Coord> seeds;

    for(auto y=seedRadius+1; y<shape[1]-seedRadius-1; y += seedRadius)
    for(auto x=seedRadius+1; x<shape[0]-seedRadius-1; x += seedRadius){
        const Coord c(x,y);
        seeds.push_back(c);
    }

    std::cout<<"add repulsion number of seeds "<<seeds.size()<<"\n";
    GRAPH_CHECK_OP(seeds.size(),>=,2,"seedRadius is to large");
    // seed repulsion
    auto c=0;
    auto r=0;
    for(auto si=0; si<seeds.size()-1; ++si)
    for(auto sj=si+1; sj<seeds.size(); ++sj){
        const auto & ci = seeds[si];
        const auto & cj = seeds[sj];
        if(vigra::norm(ci-cj)<5*seedRadius){
            model.setCost(node(ci),node(cj),-1.0*seedRepulsion*seedRadius*100.0);
            ++c;
        }
        else{
            ++r;
        }
    }
    std::cout<<"repulsive edges "<<c<<" rejected "<<r<<"\n";

    std::cout<<"add seed attractiveness \n";

    auto nd = [&](const double dist){
        return (1.0/(sigma*std::sqrt(2.0*3.1415926))) *std::exp(-0.5*std::pow(dist/sigma,2));
    };
    for(auto & uCoord  : seeds){
        Coord vCoord;
        for(vCoord[0] = uCoord[0]-seedRadius; vCoord[0]<uCoord[0]+seedRadius+1;++vCoord[0])
        for(vCoord[1] = uCoord[1]-seedRadius; vCoord[1]<uCoord[1]+seedRadius+1;++vCoord[1]){
            auto  d = vigra::norm(uCoord-vCoord);
            auto  w = nd(d);
            model.setCost(node(uCoord), node(vCoord), w*20.0);
        }
    }

    std::cout<<"add grid graph cost attractiveness \n";
    std::cout<<"local terms\n";
    for(auto y=0; y<shape[1]; ++y)
    for(auto x=0; x<shape[0]; ++x){
        const Coord c(x,y);
        auto u = node(c);
        if(x+1<shape[0]){
            const Coord c2(x+1,y);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
        if(y+1<shape[1]){
            const Coord c2(x,y+1);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
    }
    std::cout<<"build model done\n";
}

template<class LiftedMcModel>
void andresImageSegModel(
    LiftedMcModel & model,
    vigra::NumpyArray<2, float> pmapImage,
    const float bias = 0.5
){
    const auto & shape = pmapImage.shape();
    // convert to andres image
    Image image(shape.begin(),shape.begin());
    for(auto y=0; y<shape[1]; ++y)
    for(auto x=0; x<shape[0]; ++x){
        image(x,y) = pmapImage(x,y);
    }

    const auto & graph = model.originalGraph();

    // boundary-probability-to-multicut-problem-image.cxx
    std::vector<float> edgeCutProbabilities;
    PixelCutProbabilityFromEdgesWTA<float, size_t> pixelCutProbabilityWTA(image);
    constructGraphAndEdgeProbabilities(pixelCutProbabilityWTA, graph, edgeCutProbabilities, bias);

}

template<class LiftedMcModel>
void loadBsdModel(LiftedMcModel & model, const std::string & filename){
    auto fileHandle = agraph::hdf5::openFile(filename);
    agraph::hdf5::load(fileHandle,"graph",model._originalGraph());
    agraph::hdf5::load(fileHandle,"graph-lifted",model._liftedGraph());
    std::vector<size_t> shape;
    auto & ec = model._edgeCosts();
    //ec.resize(model.liftedGraph().numberOfEdges());
    std::vector<double > ecd;
    agraph::hdf5::load(fileHandle, "edge-cut-probabilities", shape, ecd);

    transform(
       ecd.begin(),
       ecd.end(),
       ecd.begin(),
       andres::NegativeLogProbabilityRatio<double,double>()
    );

    ec.resize(ecd.size());
    std::copy(ecd.begin(), ecd.end(),ec.begin());
    agraph::hdf5::closeFile(fileHandle);

    transform(
       ec.begin(),
       ec.end(),
       ec.begin(),
       andres::NegativeLogProbabilityRatio<double,double>()
    );

}

template<class OG, class F>
void exportLiftedMcModelT(const std::string & clsName, F && f){
    typedef OG originalGraph;
    typedef agraph::multicut_lifted::LiftedMcModel<originalGraph, float> LiftedMcModel;

    auto cls = bp::class_<LiftedMcModel>
    (
        clsName.c_str(),
        bp::init<
            const originalGraph&
        >(
            bp::arg("originalGraph")
        )[bp::with_custodian_and_ward<1 /*custodian == self*/, 2 /*ward == const originalGraph & */>()]
    )
        .def("originalGraph",&LiftedMcModel::originalGraph , bp::return_internal_reference<>())
        .def("liftedGraph",&LiftedMcModel::liftedGraph , bp::return_internal_reference<>())
        .def("_setCosts",vigra::registerConverters(&setCosts<LiftedMcModel>))
        .def("_setCost",&LiftedMcModel::setCost)
        .def("evalCut",vigra::registerConverters(&evalCut<LiftedMcModel>))
        .def("edgeLabelsToNodeLabels",vigra::registerConverters(&edgeLabelsToNodeLabels<LiftedMcModel>),
            (
                bp::arg("edgeLabels"),
                bp::arg("out") = bp::object()
            )
        )
        .def("loadBsdModel", &loadBsdModel<LiftedMcModel>)
        .def("nodeLabelsToEdgeLabels",vigra::registerConverters(&nodeLabelsToEdgeLabels<LiftedMcModel>),
            (
                bp::arg("nodeLabels"),
                bp::arg("out") = bp::object()
            )
        );

    f(cls);

    bp::def("fuseGtObjective",vigra::registerConverters(&fuseGtObjective<LiftedMcModel>),
        (
            bp::arg("model"),
            bp::arg("nodeLabels"),
            bp::arg("rr") = 2,
            bp::arg("beta") = 0.5,
            bp::arg("verbose") = true
        )
    );
}

template<class MODEL>
vigra::NumpyAnyArray flattenLabels(
    const MODEL & model,
    vigra::NumpyArray<2, uint64_t> labels2d,
    vigra::NumpyArray<1, uint64_t> out
){
    vigra::TinyVector<int, 1> shape(labels2d.size());
    out.reshapeIfEmpty(shape);
    auto c=0;
    for(auto y=0; y<labels2d.shape(1); ++y)
    for(auto x=0; x<labels2d.shape(0); ++x){
        out[c] = labels2d(x,y);
        ++c;
    }
    return out;
}

void exportLiftedMcModel(){
    typedef agraph::GridGraph<2> GridGraph2D;
    typedef agraph::GridGraph<3> GridGraph3D;
    typedef agraph::Graph<> Graph;
    typedef agraph::multicut_lifted::LiftedMcModel<GridGraph2D, float> LiftedMcModelGridGraph2D;
    typedef agraph::multicut_lifted::LiftedMcModel<Graph, float> LiftedMcModelGraph;
    {
        typedef agraph::multicut_lifted::LiftedMcModel<GridGraph2D, float> LiftedMcModelGridGraph2D;
        exportLiftedMcModelT<GridGraph2D>("LiftedMcModelGridGraph2D",
            [&](
                bp::class_< LiftedMcModelGridGraph2D > & cls
            ){
                cls
                    .def("flattenLabels",
                        vigra::registerConverters(&flattenLabels< LiftedMcModelGridGraph2D >),
                        (
                            bp::arg("labels"),
                            bp::arg("out") =  bp::object()
                        )
                    )
                ;
            }
        );
    }
    exportLiftedMcModelT<GridGraph3D>("LiftedMcModelGridGraph3D",
        [&](
            bp::class_<agraph::multicut_lifted::LiftedMcModel<GridGraph3D, float> > & cls
        ){

        }
    );
    exportLiftedMcModelT<Graph>("LiftedMcModelGraph",
        [&](
            bp::class_<agraph::multicut_lifted::LiftedMcModel<Graph, float> > & cls
        ){

        }
    );

    bp::def("addLongRangeEdges",vigra::registerConverters(&addLongRangeEdges<LiftedMcModelGridGraph2D>))
    ;

    bp::def("addLongRangeNH",vigra::registerConverters(&addLongRangeNH<LiftedMcModelGridGraph2D>))
    ;
    bp::def("addLongRangeNH",vigra::registerConverters(&addLongRangeNH<LiftedMcModelGraph>))
    ;

    bp::def("fuseGtObjectiveGrid",vigra::registerConverters(&fuseGtObjectiveGrid<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("nodeLabels"),
            bp::arg("pExpert"),
            bp::arg("cutRegularizer"),
            bp::arg("rr") = 2,
            bp::arg("beta") = 0.5,
            //bp::arg("cLocal") = 100,
            bp::arg("verbose") = true
        )
    );

    bp::def("thinObjectSeededSeg",vigra::registerConverters(&thinObjectSeededSeg<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("linkConstraints"),
            bp::arg("cutCosts"),
            bp::arg("c") = 1000.0,
            bp::arg("verbose") = true
        )
    );

    bp::def("lmSuperpixel",vigra::registerConverters(&lmSuperpixel<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("cutCosts"),
            bp::arg("seedRadius") = 10,
            bp::arg("stepSize") = 2,
            bp::arg("sigma") = 3.0,
            bp::arg("seedRepulsion") =1.0
        )
    );

    bp::def("andresImageSegModel",vigra::registerConverters(&andresImageSegModel<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("pmap"),
            bp::arg("bias") = 0.5
        )
    );
}


#include "segmatch/descriptors/cnn.hpp"

#include <algorithm>
#include <math.h>
#include <stdlib.h> /* system, NULL, EXIT_FAILURE */
#include <string>

#include <Eigen/Core>

#include <opencv2/opencv.hpp>

#include <glog/logging.h>

#include <pcl/common/common.h>
#include <pcl/common/transforms.h>

#include <laser_slam/benchmarker.hpp>

#include "segmatch/database.hpp"
#include "segmatch/utilities.hpp"

#include <cstdio>

namespace segmatch {

void CNNDescriptor::describe(const Segment& segment, Features* features) {
  CHECK(false) << "Not implemented";
}

template <typename T>
std::vector<size_t> getIndexesInDecreasingOrdering(const std::vector<T> &v) {

  // initialize original index locations
  std::vector<size_t> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  std::sort(idx.begin(), idx.end(),
            [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});

  return idx;
}

void CNNDescriptor::describe(SegmentedCloud* segmented_cloud_ptr) {
  CHECK_NOTNULL(segmented_cloud_ptr);
  constexpr double kMinChangeBeforeDescription = 0.1; // 0.2

  BENCHMARK_START("SM.Worker.Describe.Preprocess");
  BENCHMARK_RECORD_VALUE("SM.Worker.Describe.NumSegmentsTotal",
                         segmented_cloud_ptr->getNumberOfValidSegments());

  std::vector<tf_graph_executor::Array3D> batch_nn_input;
  std::vector<tf_graph_executor::Array3D> batch_nn_input_vis;
  std::vector<Id> described_segment_ids;
  std::vector<PclPoint> scales;
  std::vector<PclPoint> thresholded_scales;
  std::vector<std::vector<float> > scales_as_vectors;
  std::vector<PclPoint> rescaled_point_cloud_centroids;
  std::vector<PclPoint> point_mins;
  std::vector<double> alignments_rad;
  std::vector<size_t> nums_occupied_voxels;

  for (std::unordered_map<Id, Segment>::iterator it = segmented_cloud_ptr->begin();
      it != segmented_cloud_ptr->end(); ++it) {

    const PointCloud& point_cloud = it->second.getLastView().point_cloud;
    const size_t num_points = point_cloud.size();

    // Skip describing the segment if it did not change enough.
    if (static_cast<double>(num_points) < static_cast<double>(
        it->second.getLastView().n_points_when_last_described) *
        (1.0 + kMinChangeBeforeDescription)) continue;

    if (params_.use_vis_views && it->second.bestViewPts < 50) {
      continue;
    }

    described_segment_ids.push_back(it->second.segment_id);

    // Align with PCA.
    double alignment_rad;
    Eigen::Vector4f pca_centroid;
    pcl::compute3DCentroid(point_cloud, pca_centroid);
    Eigen::Matrix3f covariance_3d;
    computeCovarianceMatrixNormalized(point_cloud, pca_centroid, covariance_3d);
    const Eigen::Matrix2f covariance_2d = covariance_3d.block(0, 0, 2u, 2u);
    Eigen::EigenSolver<Eigen::Matrix2f> eigen_solver(covariance_2d, true);

    alignment_rad = atan2(eigen_solver.eigenvectors()(1,0).real(),
                          eigen_solver.eigenvectors()(0,0).real());

    if (eigen_solver.eigenvalues()(0).real() <
        eigen_solver.eigenvalues()(1).real()) {
      alignment_rad += 0.5*M_PI;
    }

    // Rotate the segment.
    alignment_rad = -alignment_rad;
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.rotate(Eigen::AngleAxisf(alignment_rad, Eigen::Vector3f::UnitZ()));
    PointCloud rotated_point_cloud;
    pcl::transformPointCloud(point_cloud, rotated_point_cloud, transform);

    // Get most points on the lower half of y axis (by rotation).
    PclPoint point_min, point_max;
    pcl::getMinMax3D(rotated_point_cloud, point_min, point_max);
    double centroid_y = point_min.y + (point_max.y - point_min.y) / 2.0;
    unsigned int n_below = 0;
    for (const auto& point : rotated_point_cloud.points) {
      if (point.y < centroid_y) ++n_below;
    }
    if (static_cast<double>(n_below) < static_cast<double>(rotated_point_cloud.size()) / 2.0) {
      alignment_rad += M_PI;
      Eigen::Affine3f adjustment_transform = Eigen::Affine3f::Identity();
      adjustment_transform.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
      pcl::transformPointCloud(rotated_point_cloud, rotated_point_cloud, adjustment_transform);
    }

    alignments_rad.push_back(alignment_rad);

    if (save_debug_data_) {
      Segment aligned_segment = it->second;
      aligned_segment.getLastView().point_cloud = rotated_point_cloud;
      aligned_segments_.addValidSegment(aligned_segment);
    }

    PointCloud rescaled_point_cloud;
    pcl::getMinMax3D(rotated_point_cloud, point_min, point_max);
    point_mins.push_back(point_min);

    // "Fit scaling" using the largest dimension as scale.
    PclPoint scale;
    scale.x = point_max.x - point_min.x;
    scale.y = point_max.y - point_min.y;
    scale.z = point_max.z - point_min.z;
    scales.push_back(scale);

    PclPoint thresholded_scale;
    thresholded_scale.x = std::max(scale.x, min_x_scale_m_);
    thresholded_scale.y = std::max(scale.y, min_y_scale_m_);
    thresholded_scale.z = std::max(scale.z, min_z_scale_m_);
    thresholded_scales.push_back(thresholded_scale);

    std::vector<float> scales_as_vector;
    scales_as_vector.push_back(scale.x);
    scales_as_vector.push_back(scale.y);
    scales_as_vector.push_back(scale.z);
    scales_as_vectors.push_back(scales_as_vector);

    for (const auto& point: rotated_point_cloud.points) {
      PclPoint point_new;

      point_new.x = (point.x - point_min.x) / thresholded_scale.x
          * static_cast<float>(n_voxels_x_dim_ - 1u);
      point_new.y = (point.y - point_min.y) / thresholded_scale.y
          * static_cast<float>(n_voxels_y_dim_ - 1u);
      point_new.z = (point.z - point_min.z) / thresholded_scale.z
          * static_cast<float>(n_voxels_z_dim_ - 1u);

      rescaled_point_cloud.points.push_back(point_new);
    }
    rescaled_point_cloud.width = 1;
    rescaled_point_cloud.height = rescaled_point_cloud.points.size();

    PclPoint centroid = calculateCentroid(rescaled_point_cloud);
    rescaled_point_cloud_centroids.push_back(centroid);

    unsigned int n_occupied_voxels = 0;
    tf_graph_executor::Array3D nn_input(n_voxels_x_dim_, n_voxels_y_dim_, n_voxels_z_dim_);
    for (const auto& point: rescaled_point_cloud.points) {
      const unsigned int ind_x = floor(point.x + static_cast<float>(n_voxels_x_dim_ - 1) / 2.0
                                       - centroid.x);
      const unsigned int ind_y = floor(point.y + static_cast<float>(n_voxels_y_dim_ - 1) / 2.0
                                       - centroid.y);
      const unsigned int ind_z = floor(point.z + static_cast<float>(n_voxels_z_dim_ - 1) / 2.0
                                       - centroid.z);

      if (ind_x >= 0 && ind_x < n_voxels_x_dim_ &&
          ind_y >= 0 && ind_y < n_voxels_y_dim_ &&
          ind_z >= 0 && ind_z < n_voxels_z_dim_){

        if (nn_input.container[ind_x][ind_y][ind_z] == 0.0) {
          ++n_occupied_voxels;
        }
        nn_input.container[ind_x][ind_y][ind_z] = 1.0;
      }
    }
    nums_occupied_voxels.push_back(n_occupied_voxels);

    it->second.getLastView().n_occupied_voxels = n_occupied_voxels;
    it->second.getLastView().n_points_when_last_described = num_points;

    batch_nn_input.push_back(nn_input);

    tf_graph_executor::Array3D nn_input_vis(n_vis_h_dim_, n_vis_w_dim_, n_vis_c_dim_);

    if (params_.use_vis_views) {
      const auto &visViews = segmented_cloud_ptr->getVisViews();

      // LOG(INFO) << "it->second.bestViewTs = " << it->second.bestViewTs;
      int bestV = -1;
      for (int v = 0; v < visViews.size(); ++v) {
        // LOG(INFO) << "visViews[v].getTime() = " << visViews[v].getTime();
        if (it->second.bestViewTs == visViews[v].getTime()) {
          bestV = v;
          break;
        }
      }
      if (bestV == -1) {
        LOG(INFO) << "Found bestV == -1 ";
        LOG(INFO) << "it->second.segment_id = " << it->second.segment_id;
        LOG(INFO) << "it->second.getLastView().timestamp_ns = " << it->second.getLastView().timestamp_ns;
        for (const auto &view : it->second.views) {
          LOG(INFO) << "view.timestamp_ns = " << view.timestamp_ns;
        }
        LOG(INFO) << "it->second.bestViewPts = " << it->second.bestViewPts;
        LOG(INFO) << "it->second.bestViewTs = " << it->second.bestViewTs;
        for (int v = 0; v < visViews.size(); ++v) {
          LOG(INFO) << "visViews[v].getTime() = " << visViews[v].getTime();
        }
      }
      CHECK_GE(bestV, 0);
      // Could be precomputed, but would need extra memory
      // laser_slam_ros::VisualView::MatrixInt mask = visViews[bestV].getMask(it->second.getLastView().point_cloud);
      laser_slam_ros::VisualView::MatrixInt mask = it->second.bestMask;
      const laser_slam_ros::VisualView::Matrix &intensity = visViews[bestV].getIntensity();
      const laser_slam_ros::VisualView::Matrix &range = visViews[bestV].getRange();
      float meanMaskRange = 0.0f;
      int maskRangeCnt = 0;
      for (int r = 0; r < n_vis_h_dim_; ++r) {
        for (int c = 0; c < n_vis_w_dim_; ++c) {
          if (mask(r, c) > 0) {
            meanMaskRange += range(r, c);
            maskRangeCnt += 1;
            mask(r, c) = 1.0;
          }
        }
      }
      if (maskRangeCnt < it->second.bestViewPts - 20) {
        LOG(INFO) << "\n\n\n\n\nmaskRangeCnt = " << maskRangeCnt;
        LOG(INFO) << "it->second.bestViewPts = " << it->second.bestViewPts;
        LOG(INFO) << "it->second.bestViewTs = " << it->second.bestViewTs;
        LOG(INFO) << "it->second.getLastView().point_cloud.size() = " << it->second.getLastView().point_cloud.size() << "\n\n\n\n\n";
      }
      CHECK_GT(maskRangeCnt, 0);
      meanMaskRange /= maskRangeCnt;
      // {
      //   auto intensityScaled = (intensity.array() - 209.30) / 173.09;
      //   auto rangeScaled = (range.array() - meanMaskRange) * 500.0 / (7632.0);
      //   LOG(INFO) << "int mean = " << intensityScaled.mean();
      //   LOG(INFO) << "int stddev = " << std::sqrt((intensityScaled - intensityScaled.mean()).square().mean());
      //   LOG(INFO) << "range mean = " << rangeScaled.mean();
      //   LOG(INFO) << "range stddev = " << std::sqrt((rangeScaled - rangeScaled.mean()).square().mean());
      //   // LOG(INFO) << "mask(r, c) = \n" << mask;
      // }
      for (int r = 0; r < n_vis_h_dim_; ++r) {
        for (int c = 0; c < n_vis_w_dim_; ++c) {
          // MulRan
          nn_input_vis.container[r][c][0] = (intensity(r, c) - 209.30) / 173.09;
          nn_input_vis.container[r][c][1] = mask(r, c);
          nn_input_vis.container[r][c][2] = (range(r, c) - meanMaskRange) * 500.0 / (7632.0);
        }
      }

      // if (true) {
      //   std::string dir("/tmp/online_matcher/debug");
      //   // const laser_slam_ros::VisualView::Matrix &intensity = visViews[bestV].getIntensity();
      //   // const laser_slam_ros::VisualView::Matrix &range = visViews[bestV].getRange();
      //   // const laser_slam_ros::VisualView::MatrixInt &mask = vis_view.getMask(segment_view.point_cloud);
      //
      //   std::string segmentDir = dir;
      //   {
      //     char dirname[100];
      //     sprintf(dirname, "%06ld", it->second.segment_id);
      //     segmentDir = (boost::filesystem::path(dir) / std::string(dirname)).string();
      //     database::ensureDirectoryExists(segmentDir);
      //   }
      //
      //   cv::Mat intensityMat(intensity.rows(), intensity.cols(), CV_16UC1, cv::Scalar(0));
      //   cv::Mat rangeMat(range.rows(), range.cols(), CV_16UC1, cv::Scalar(0));
      //   // cv::Mat intensityMono(intensity.rows(), intensity.cols(), CV_8UC1, cv::Scalar(0));
      //   cv::Mat maskMat(mask.rows(), mask.cols(), CV_8UC1, cv::Scalar(0));
      //   for (int r = 0; r < intensity.rows(); ++r) {
      //     for (int c = 0; c < intensity.cols(); ++c) {
      //       // KITTI
      //       // intensityMat.at<uint16_t>(r, c) = intensity(r, c)*65535.0f;
      //       // MulRan
      //       intensityMat.at<uint16_t>(r, c) = intensity(r, c);
      //       // intensityMono.at<uint8_t>(r, c) = std::min((int)(intensity(r, c)*255.0/1500.0), 255);
      //       // up to 65.535 * 2 m
      //       rangeMat.at<uint16_t>(r, c) = std::min(range(r, c) * 500.0f, 65535.0f);
      //     }
      //   }
      //   for (int r = 0; r < mask.rows(); ++r) {
      //     for (int c = 0; c < mask.cols(); ++c) {
      //       if (mask(r, c) > 0) {
      //         maskMat.at<uint8_t>(r, c) = 255;
      //       } else {
      //         maskMat.at<uint8_t>(r, c) = 0;
      //       }
      //     }
      //   }
      //
      //   char filename[100];
      //   // sprintf(filename, "%06ld_%03d", segment_id, view_idx);
      //   sprintf(filename, "%ld_%ld", it->second.getLastView().timestamp_ns, it->second.bestViewTs);
      //   cv::imwrite((boost::filesystem::path(segmentDir) / (filename + std::string("_int.png"))).string(), intensityMat);
      //   cv::imwrite((boost::filesystem::path(segmentDir) / (filename + std::string("_range.png"))).string(), rangeMat);
      //   cv::imwrite((boost::filesystem::path(segmentDir) / (filename + std::string("_mask.png"))).string(), maskMat);
      // }
    }

    batch_nn_input_vis.push_back(nn_input_vis);
  }
  BENCHMARK_RECORD_VALUE("SM.Worker.Describe.NumSegmentsDescribed",
                         batch_nn_input.size());
  BENCHMARK_STOP("SM.Worker.Describe.Preprocess");

  if (!batch_nn_input.empty()) {
    BENCHMARK_START("SM.Worker.Describe.ForwardPass");
    std::vector<std::vector<float> > cnn_descriptors;
    std::vector<tf_graph_executor::Array3D> reconstructions;
    std::vector<std::vector<float> > semantics;
    if (batch_nn_input.size() < mini_batch_size_) {
      if (!params_.use_vis_views) {
        graph_executor_->batchFullForwardPass(batch_nn_input,
                                              kInputTensorName,
                                              scales_as_vectors,
                                              kScalesTensorName,
                                              kFeaturesTensorName,
                                              kReconstructionTensorName,
                                              cnn_descriptors,
                                              reconstructions);
      }
      else {
        // LOG(INFO) << "Executing with visual views batch_nn_input.size() < mini_batch_size_";
        graph_executor_->batchFullForwardPassVisViews(batch_nn_input,
                                                      kInputTensorName,
                                                      batch_nn_input_vis,
                                                      kInputVisTensorName,
                                                      scales_as_vectors,
                                                      kScalesTensorName,
                                                      kFeaturesTensorName,
                                                      kReconstructionTensorName,
                                                      cnn_descriptors,
                                                      reconstructions);
      }

    } else {
      std::vector<tf_graph_executor::Array3D> mini_batch;
      std::vector<tf_graph_executor::Array3D> mini_batch_vis;
      std::vector<std::vector<float> > mini_batch_scales;
      for (size_t i = 0u; i < batch_nn_input.size(); ++i) {
        mini_batch.push_back(batch_nn_input[i]);
        mini_batch_vis.push_back(batch_nn_input_vis[i]);
        mini_batch_scales.push_back(scales_as_vectors[i]);
        if (mini_batch.size() == mini_batch_size_) {
          std::vector<std::vector<float> > mini_batch_cnn_descriptors;
          std::vector<tf_graph_executor::Array3D> mini_batch_reconstructions;

          if (!params_.use_vis_views) {
            graph_executor_->batchFullForwardPass(mini_batch,
                                                  kInputTensorName,
                                                  mini_batch_scales,
                                                  kScalesTensorName,
                                                  kFeaturesTensorName,
                                                  kReconstructionTensorName,
                                                  mini_batch_cnn_descriptors,
                                                  mini_batch_reconstructions);
          }
          else {
            // LOG(INFO) << "Executing with visual views mini_batch.size() == mini_batch_size_";
            graph_executor_->batchFullForwardPassVisViews(mini_batch,
                                                          kInputTensorName,
                                                          mini_batch_vis,
                                                          kInputVisTensorName,
                                                          mini_batch_scales,
                                                          kScalesTensorName,
                                                          kFeaturesTensorName,
                                                          kReconstructionTensorName,
                                                          mini_batch_cnn_descriptors,
                                                          mini_batch_reconstructions);
          }

          cnn_descriptors.insert(cnn_descriptors.end(),
                                 mini_batch_cnn_descriptors.begin(),
                                 mini_batch_cnn_descriptors.end());
          reconstructions.insert(reconstructions.end(),
                                 mini_batch_reconstructions.begin(),
                                 mini_batch_reconstructions.end());
          mini_batch_scales.clear();
          mini_batch.clear();
          mini_batch_vis.clear();
        }
      }
      if (!mini_batch.empty()) {
        std::vector<std::vector<float> > mini_batch_cnn_descriptors;
        std::vector<tf_graph_executor::Array3D> mini_batch_reconstructions;

        if (!params_.use_vis_views) {
          graph_executor_->batchFullForwardPass(mini_batch,
                                                kInputTensorName,
                                                mini_batch_scales,
                                                kScalesTensorName,
                                                kFeaturesTensorName,
                                                kReconstructionTensorName,
                                                mini_batch_cnn_descriptors,
                                                mini_batch_reconstructions);
        }
        else {
          // LOG(INFO) << "Executing with visual views !mini_batch.empty()";
          graph_executor_->batchFullForwardPassVisViews(mini_batch,
                                                        kInputTensorName,
                                                        mini_batch_vis,
                                                        kInputVisTensorName,
                                                        mini_batch_scales,
                                                        kScalesTensorName,
                                                        kFeaturesTensorName,
                                                        kReconstructionTensorName,
                                                        mini_batch_cnn_descriptors,
                                                        mini_batch_reconstructions);
        }

        cnn_descriptors.insert(cnn_descriptors.end(),
                               mini_batch_cnn_descriptors.begin(),
                               mini_batch_cnn_descriptors.end());
        reconstructions.insert(reconstructions.end(),
                               mini_batch_reconstructions.begin(),
                               mini_batch_reconstructions.end());
      }
    }

    // Execute semantics graph.
    semantics = semantics_graph_executor_->batchExecuteGraph(
        cnn_descriptors, kInputTensorName, kSemanticsOutputName);

    CHECK_EQ(cnn_descriptors.size(), described_segment_ids.size());
    BENCHMARK_STOP("SM.Worker.Describe.ForwardPass");

    BENCHMARK_START("SM.Worker.Describe.SaveFeatures");
    // Write the features.
    for (size_t i = 0u; i < described_segment_ids.size(); ++i) {
      Segment* segment;
      CHECK(segmented_cloud_ptr->findValidSegmentPtrById(described_segment_ids[i], &segment));
      std::vector<float> nn_output = cnn_descriptors[i];

      Feature cnn_feature("cnn");
      for (size_t j = 0u; j < nn_output.size(); ++j) {
        cnn_feature.push_back(FeatureValue("cnn_" + std::to_string(j), nn_output[j]));
      }

      // Push the scales.
      cnn_feature.push_back(FeatureValue("cnn_scale_x", scales[i].x));
      cnn_feature.push_back(FeatureValue("cnn_scale_y", scales[i].y));
      cnn_feature.push_back(FeatureValue("cnn_scale_z", scales[i].z));

      segment->getLastView().features.replaceByName(cnn_feature);

      std::vector<float> semantic_nn_output = semantics[i];
      segment->getLastView().semantic = std::distance(semantic_nn_output.begin(),
                                                      std::max_element(semantic_nn_output.begin(),
                                                                       semantic_nn_output.end()));

      // Generate the reconstructions.
      PointCloud reconstruction;
      const double reconstruction_threshold = 0.75;
      const double ratio_voxels_to_reconstruct = 1.5;
      const bool reconstruct_by_probability = true;

      PclPoint point;
      const PclPoint point_min = point_mins[i];
      const PclPoint scale = thresholded_scales[i];
      const PclPoint centroid = rescaled_point_cloud_centroids[i];

      if (reconstruct_by_probability) {
        for (unsigned int x = 0u; x < n_voxels_x_dim_; ++x) {
          for (unsigned int y = 0u; y < n_voxels_y_dim_; ++y) {
            for (unsigned int z = 0u; z < n_voxels_z_dim_; ++z) {
                if (reconstructions[i].container[x][y][z] >= reconstruction_threshold) {
                  point.x = point_min.x + scale.x *
                    (static_cast<float>(x) - x_dim_min_1_ / 2.0 + centroid.x) / x_dim_min_1_;
                  point.y = point_min.y + scale.y *
                    (static_cast<float>(y) - y_dim_min_1_ / 2.0 + centroid.y) / y_dim_min_1_;
                  point.z = point_min.z + scale.z *
                    (static_cast<float>(z) - z_dim_min_1_ / 2.0 + centroid.z) / z_dim_min_1_;
                  reconstruction.points.push_back(point);
                }
            }
          }
        }
      } else {
                const unsigned int n_voxels_in_original_segment = nums_occupied_voxels[i];
      const unsigned int n_voxels_in_reconstructed_segment =
        floor(double(n_voxels_in_original_segment) * ratio_voxels_to_reconstruct);

      // Order by occupancy probability.
      std::vector<double> probs;
      std::vector<PclPoint> indices;
      for (unsigned int x = 0u; x < n_voxels_x_dim_; ++x) {
        for (unsigned int y = 0u; y < n_voxels_y_dim_; ++y) {
          for (unsigned int z = 0u; z < n_voxels_z_dim_; ++z) {
              probs.push_back(reconstructions[i].container[x][y][z]);
              PclPoint indice;
              indice.x = x;
              indice.y = y;
              indice.z = z;
              indices.push_back(indice);
          }
        }
      }
      std::vector<size_t> indexes_in_decreasing_order = getIndexesInDecreasingOrdering(probs);

      for (unsigned int j = 0u; j < n_voxels_in_reconstructed_segment; ++j){
          point.x = point_min.x + scale.x *
          (static_cast<float>(indices[indexes_in_decreasing_order[j]].x) - x_dim_min_1_ / 2.0 + centroid.x) / x_dim_min_1_;
          point.y = point_min.y + scale.y *
          (static_cast<float>(indices[indexes_in_decreasing_order[j]].y) - y_dim_min_1_ / 2.0 + centroid.y) / y_dim_min_1_;
          point.z = point_min.z + scale.z *
          (static_cast<float>(indices[indexes_in_decreasing_order[j]].z) - z_dim_min_1_ / 2.0 + centroid.z) / z_dim_min_1_;
          reconstruction.points.push_back(point);
      }
      }

      reconstruction.width = 1;
      reconstruction.height = reconstruction.points.size();

      Eigen::Affine3f transform = Eigen::Affine3f::Identity();
      transform.rotate(Eigen::AngleAxisf(-alignments_rad[i], Eigen::Vector3f::UnitZ()));
      pcl::transformPointCloud(reconstruction, reconstruction, transform);
      segment->getLastView().reconstruction = reconstruction;

      // TODO RD remove if compressing reconstruction not needed.
      /*unsigned int publish_every_x_points = 5;
      segment->getLastView().reconstruction_compressed.clear();
      segment->getLastView().reconstruction_compressed.reserve(reconstruction.points.size() / publish_every_x_points);
      unsigned int z = 0;
      for (const auto& point : reconstruction.points) {
        if (z % publish_every_x_points == 0) {
            segment->getLastView().reconstruction_compressed.points.emplace_back(point.x, point.y, point.z);
        }
        ++z;
      }*/
    }
    BENCHMARK_STOP("SM.Worker.Describe.SaveFeatures");
  }
}

void CNNDescriptor::exportData() const {
  if (save_debug_data_) {
    database::exportSegmentsAndFeatures("/tmp/aligned_segments",
                                        aligned_segments_, true);
  }
}

} // namespace segmatch


// Copyright (c) 2014 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "keystore.h"
#include "chainparams.h"

#include <vector>

#include <boost/test/unit_test.hpp>

using namespace std;

BOOST_AUTO_TEST_SUITE(hd_tests)

BOOST_AUTO_TEST_CASE(hd_test1)
{
    CHDChain newHdChain;
    string strMnemonic = "sorry jelly twice human measure impact caution despair inner stock spare gossip connect someone industry provide panel decade dose loud tunnel famous knife belt";
    string strMnemonicPassphrase = "";

    SecureVector vchMnemonic(strMnemonic.begin(), strMnemonic.end());
    SecureVector vchMnemonicPassphrase(strMnemonicPassphrase.begin(), strMnemonicPassphrase.end());

    BOOST_CHECK(newHdChain.SetMnemonic(vchMnemonic, vchMnemonicPassphrase, true));
    BOOST_CHECK(Params().ExtCoinType() == 0x80000166);

    SecureVector vchSeed = newHdChain.GetSeed();

    CExtKey masterKey;              //hd master key
    CExtKey purposeKey;             //key at m/purpose'
    CExtKey cointypeKey;            //key at m/purpose'/coin_type'
    CExtKey accountKey;             //key at m/purpose'/coin_type'/account'
    CExtKey changeKey;              //key at m/purpose'/coin_type'/account'/change
    CExtKey childKey;               //key at m/purpose'/coin_type'/account'/change/address_index
    CKey newKey;
    CPubKey newPubKey;
    int nAccountIndex = 0;
    int nChildIndex = 0;
    bool fInternal = false;

    masterKey.SetMaster(&vchSeed[0], vchSeed.size());

    masterKey.Derive(purposeKey, 44 | 0x80000000);
    purposeKey.Derive(cointypeKey, Params().ExtCoinType());
    cointypeKey.Derive(accountKey, nAccountIndex | 0x80000000);
    accountKey.Derive(changeKey, fInternal ? 1 : 0);
    changeKey.Derive(childKey, nChildIndex);

    newKey = childKey.key;
    newPubKey = newKey.GetPubKey();
    BOOST_CHECK(newPubKey.GetHex() == "5c0d4f0b9383a9d83addc16929cd90f5cfacbc3ab35b1ee930fc215b9dffe8e402");

    nAccountIndex = 1;
    cointypeKey.Derive(accountKey, nAccountIndex | 0x80000000);
    accountKey.Derive(changeKey, fInternal ? 1 : 0);
    changeKey.Derive(childKey, nChildIndex);

    newKey = childKey.key;
    newPubKey = newKey.GetPubKey();
    BOOST_CHECK(newPubKey.GetHex() == "c6a5832438db1047eea30ce51213aa976f96e029f3c626dd36c85d4bc3ea75da03");
}

BOOST_AUTO_TEST_SUITE_END()


//---------------------------------------------------------------------------//
//!
//! \file   Utility_UnivariateDistribution.hpp
//! \author Alex Robinson
//! \brief  Univariate distribution class declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_ONE_D_DISTRIBUTION_HPP
#define UTILITY_ONE_D_DISTRIBUTION_HPP

// Std Lib Includes
#include <stdexcept>

// Boost Includes
#include <boost/serialization/split_member.hpp>

// FRENSIE Includes
#include "Utility_UnivariateDistributionType.hpp"
#include "Utility_OStreamableObject.hpp"
#include "Utility_InterpolationPolicy.hpp"
#include "Utility_CosineInterpolationPolicy.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_DistributionTraits.hpp"
#include "Utility_DistributionSerializationHelpers.hpp"
#include "Utility_ExplicitSerializationTemplateInstantiationMacros.hpp"

/*! \defgroup univariate_distributions Univariate Distributions
 */

namespace Utility{

/*! The unit-aware univariate distribution
 * \details When the IndependentUnit type and/or the DependentUnit type
 * resolves to void, the IndepQuantity and/or the DepQuantity type reduces to
 * double respectively.
 * \ingroup univariate_distributions
 */
template<typename IndependentUnit, typename DependentUnit = void>
class UnitAwareUnivariateDistribution : public OStreamableObject
{
  // Typedef for this type
  typedef UnitAwareUnivariateDistribution<IndependentUnit,DependentUnit> ThisType;

protected:

  //! The independent unit traits typedef
  typedef UnitTraits<IndependentUnit> IndepUnitTraits;

  //! The inverse independent unit traits typedef
  typedef UnitTraits<typename UnitTraits<IndependentUnit>::InverseUnit> InverseIndepUnitTraits;

  //! The dependent unit traits typedef
  typedef UnitTraits<DependentUnit> DepUnitTraits;

  //! The distribution normalization unit traits typedef
  typedef UnitTraits<typename UnitTraits<typename UnitTraits<DependentUnit>::InverseUnit>::template GetMultipliedUnitType<typename UnitTraits<IndependentUnit>::InverseUnit>::type> DistNormUnitTraits;

  //! The distribution normalization quantity type
  typedef typename DistNormUnitTraits::template GetQuantityType<double>::type DistNormQuantity;

public:

  //! The independent unit type
  typedef IndependentUnit IndepUnit;

  //! The dependent unit type
  typedef DependentUnit DepUnit;

  //! The independent quantity type
  typedef typename IndepUnitTraits::template GetQuantityType<double>::type IndepQuantity;

  //! The inverse independent quantity type
  typedef typename InverseIndepUnitTraits::template GetQuantityType<double>::type InverseIndepQuantity;

  //! The dependent quantity type
  typedef typename DepUnitTraits::template GetQuantityType<double>::type DepQuantity;

  //! Constructor
  UnitAwareUnivariateDistribution()
  { /* ... */ }

  //! Destructor
  virtual ~UnitAwareUnivariateDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  virtual DepQuantity evaluate( const IndepQuantity indep_var_value ) const = 0;

  //! Evaluate the PDF
  virtual InverseIndepQuantity evaluatePDF( const IndepQuantity indep_var_value ) const = 0;

  //! Return a random sample from the distribution
  virtual IndepQuantity sample() const = 0;

  //! Return a random sample and record the number of trials
  virtual IndepQuantity sampleAndRecordTrials( DistributionTraits::Counter& trials ) const = 0;

  //! Return the upper bound of the distribution independent variable
  virtual IndepQuantity getUpperBoundOfIndepVar() const = 0;

  //! Return the lower bound of the distribution independent variable
  virtual IndepQuantity getLowerBoundOfIndepVar() const = 0;

  //! Return the distribution type
  virtual UnivariateDistributionType getDistributionType() const = 0;

  //! Test if the distribution is tabular
  virtual bool isTabular() const;

  //! Test if the distribution is continuous
  virtual bool isContinuous() const = 0;

  //! Test if the distribution is compatible with the interpolation type
  template<typename InterpPolicy>
  bool isCompatibleWithInterpType() const;

  //! Test if the distribution has the same bounds
  bool hasSameBounds( const UnitAwareUnivariateDistribution<IndependentUnit,DependentUnit>& distribution ) const;

protected:

  //! Test if the dependent variable can be zero within the indep bounds
  virtual bool canDepVarBeZeroInIndepBounds() const = 0;

  //! Test if interpolation can ever be used
  virtual bool canInterpolationBeUsed() const;

  //! Test if the independent variable is compatible with Lin processing
  virtual bool isIndepVarCompatibleWithProcessingType(
                                        const LinIndepVarProcessingTag ) const;

  //! Test if the independent variable is compatible with Log processing
  virtual bool isIndepVarCompatibleWithProcessingType(
                                        const LogIndepVarProcessingTag ) const;

  //! Test if the independent variable is compatible with LogCos processing
  virtual bool isIndepVarCompatibleWithProcessingType(
                                     const NudgedLogCosIndepVarProcessingTag ) const;

  //! Test if the independent variable is compatible with LogCos processing
  virtual bool isIndepVarCompatibleWithProcessingType(
                                     const LogCosIndepVarProcessingTag ) const;

  //! Test if the dependent variable is compatible with Lin processing
  virtual bool isDepVarCompatibleWithProcessingType(
                                          const LinDepVarProcessingTag ) const;

  //! Test if the dependent variable is compatible with Log processing
  virtual bool isDepVarCompatibleWithProcessingType(
                                          const LogDepVarProcessingTag ) const;

  //! Test if the dependent variable is compatible with LogCos processing
  virtual bool isDepVarCompatibleWithProcessingType(
                                       const NudgedLogCosDepVarProcessingTag ) const;

  //! Test if the dependent variable is compatible with LogCos processing
  virtual bool isDepVarCompatibleWithProcessingType(
                                       const LogCosDepVarProcessingTag ) const;

  //! Add distribution data to the stream
  template<typename... Types>
  void toStreamDistImpl( std::ostream& os, const Types&... data ) const;

  //! Add distribution data (including the dist limits) to the stream
  template<typename... Types>
  void toStreamWithLimitsDistImpl( std::ostream& os, const Types&... data ) const;

private:

  // Save the distribution to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const
  { /* ... */ }

  // Load the distribution from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version )
  { /* ... */ }

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;
};

/*! The univariate distribution (unit-agnostic)
 * \ingroup univariate_distributions
 */
typedef UnitAwareUnivariateDistribution<void,void> UnivariateDistribution;

/*! \brief Exception thrown by UnivariateDistribution objects when an invalid
 * parameter is encountered.
 * \ingroup univariate_distributions
 */
class BadUnivariateDistributionParameter : public std::logic_error
{
public:
  BadUnivariateDistributionParameter( const std::string& msg )
    : std::logic_error( msg )
  { /* ... */ }

  ~BadUnivariateDistributionParameter() throw()
  { /* ... */ }
};

} // end Utility namespace

BOOST_SERIALIZATION_ASSUME_ABSTRACT_DISTRIBUTION2( UnitAwareUnivariateDistribution );
BOOST_SERIALIZATION_DISTRIBUTION2_VERSION( UnitAwareUnivariateDistribution, 0 );

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_UnivariateDistribution_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_ONE_D_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end Utility_UnivariateDistribution.hpp
//---------------------------------------------------------------------------//


#ifndef _all_hpp_
#define _all_hpp_

#include <Eigen/SVD>
#include <Matrix.hpp>
#include <cassert>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <tuple>
#include <vector>

#endif /* _all_h_ */