//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_COLOR_CONVERT_HPP
#define BOOST_GIL_COLOR_CONVERT_HPP

#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/cmyk.hpp>
#include <boost/gil/color_base_algorithm.hpp>
#include <boost/gil/gray.hpp>
#include <boost/gil/metafunctions.hpp>
#include <boost/gil/pixel.hpp>
#include <boost/gil/rgb.hpp>
#include <boost/gil/rgba.hpp>
#include <boost/gil/utilities.hpp>

#include <algorithm>
#include <functional>

namespace boost { namespace gil {

/// Support for fast and simple color conversion.
/// Accurate color conversion using color profiles can be supplied separately in a dedicated module.

// Forward-declare
template <typename P> struct channel_type;

////////////////////////////////////////////////////////////////////////////////////////
///
///                 COLOR SPACE CONVERSION
///
////////////////////////////////////////////////////////////////////////////////////////

/// \ingroup ColorConvert
/// \brief Color Convertion function object. To be specialized for every src/dst color space
template <typename C1, typename C2>
struct default_color_converter_impl {};

/// \ingroup ColorConvert
/// \brief When the color space is the same, color convertion performs channel depth conversion
template <typename C>
struct default_color_converter_impl<C,C> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        static_for_each(src,dst,default_channel_converter());
    }
};

namespace detail {

/// red * .3 + green * .59 + blue * .11 + .5

// The default implementation of to_luminance uses float0..1 as the intermediate channel type
template <typename RedChannel, typename GreenChannel, typename BlueChannel, typename GrayChannelValue>
struct rgb_to_luminance_fn {
    GrayChannelValue operator()(const RedChannel& red, const GreenChannel& green, const BlueChannel& blue) const {
        return channel_convert<GrayChannelValue>(float32_t(
            channel_convert<float32_t>(red  )*0.30f +
            channel_convert<float32_t>(green)*0.59f +
            channel_convert<float32_t>(blue )*0.11f) );
    }
};

// performance specialization for unsigned char
template <typename GrayChannelValue>
struct rgb_to_luminance_fn<uint8_t,uint8_t,uint8_t, GrayChannelValue> {
    GrayChannelValue operator()(uint8_t red, uint8_t green, uint8_t blue) const {
        return channel_convert<GrayChannelValue>(uint8_t(
            ((uint32_t(red  )*4915 + uint32_t(green)*9667 + uint32_t(blue )*1802) + 8192) >> 14));
    }
};

template <typename GrayChannel, typename RedChannel, typename GreenChannel, typename BlueChannel>
typename channel_traits<GrayChannel>::value_type rgb_to_luminance(const RedChannel& red, const GreenChannel& green, const BlueChannel& blue) {
    return rgb_to_luminance_fn<RedChannel,GreenChannel,BlueChannel,
                               typename channel_traits<GrayChannel>::value_type>()(red,green,blue);
}

}   // namespace detail

/// \ingroup ColorConvert
/// \brief Gray to RGB
template <>
struct default_color_converter_impl<gray_t,rgb_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        get_color(dst,red_t())  =
            channel_convert<typename color_element_type<P2, red_t  >::type>(get_color(src,gray_color_t()));
        get_color(dst,green_t())=
            channel_convert<typename color_element_type<P2, green_t>::type>(get_color(src,gray_color_t()));
        get_color(dst,blue_t()) =
            channel_convert<typename color_element_type<P2, blue_t >::type>(get_color(src,gray_color_t()));
    }
};

/// \ingroup ColorConvert
/// \brief Gray to CMYK
template <>
struct default_color_converter_impl<gray_t,cmyk_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        get_color(dst,cyan_t())=
            channel_traits<typename color_element_type<P2, cyan_t   >::type>::min_value();
        get_color(dst,magenta_t())=
            channel_traits<typename color_element_type<P2, magenta_t>::type>::min_value();
        get_color(dst,yellow_t())=
            channel_traits<typename color_element_type<P2, yellow_t >::type>::min_value();
        get_color(dst,black_t())=
            channel_convert<typename color_element_type<P2, black_t >::type>(get_color(src,gray_color_t()));
    }
};

/// \ingroup ColorConvert
/// \brief RGB to Gray
template <>
struct default_color_converter_impl<rgb_t,gray_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        get_color(dst,gray_color_t()) =
            detail::rgb_to_luminance<typename color_element_type<P2,gray_color_t>::type>(
                get_color(src,red_t()), get_color(src,green_t()), get_color(src,blue_t())
            );
    }
};

/// \ingroup ColorConvert
/// \brief RGB to CMYK (not the fastest code in the world)
///
/// k = min(1 - r, 1 - g, 1 - b)
/// c = (1 - r - k) / (1 - k)
/// m = (1 - g - k) / (1 - k)
/// y = (1 - b - k) / (1 - k)
template <>
struct default_color_converter_impl<rgb_t,cmyk_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        using T2 = typename channel_type<P2>::type;
        get_color(dst,cyan_t())    = channel_invert(channel_convert<T2>(get_color(src,red_t())));          // c = 1 - r
        get_color(dst,magenta_t()) = channel_invert(channel_convert<T2>(get_color(src,green_t())));        // m = 1 - g
        get_color(dst,yellow_t())  = channel_invert(channel_convert<T2>(get_color(src,blue_t())));         // y = 1 - b
        get_color(dst,black_t())   = (std::min)(get_color(dst,cyan_t()),
                                                (std::min)(get_color(dst,magenta_t()),
                                                           get_color(dst,yellow_t())));   // k = minimum(c, m, y)
        T2 x = channel_traits<T2>::max_value()-get_color(dst,black_t());                  // x = 1 - k
        if (x>0.0001f) {
            float x1 = channel_traits<T2>::max_value()/float(x);
            get_color(dst,cyan_t())    = (T2)((get_color(dst,cyan_t())    - get_color(dst,black_t()))*x1);                // c = (c - k) / x
            get_color(dst,magenta_t()) = (T2)((get_color(dst,magenta_t()) - get_color(dst,black_t()))*x1);                // m = (m - k) / x
            get_color(dst,yellow_t())  = (T2)((get_color(dst,yellow_t())  - get_color(dst,black_t()))*x1);                // y = (y - k) / x
        } else {
            get_color(dst,cyan_t())=get_color(dst,magenta_t())=get_color(dst,yellow_t())=0;
        }
    }
};

/// \ingroup ColorConvert
/// \brief CMYK to RGB (not the fastest code in the world)
///
/// r = 1 - min(1, c*(1-k)+k)
/// g = 1 - min(1, m*(1-k)+k)
/// b = 1 - min(1, y*(1-k)+k)
template <>
struct default_color_converter_impl<cmyk_t,rgb_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        using T1 = typename channel_type<P1>::type;
        get_color(dst,red_t())  =
            channel_convert<typename color_element_type<P2,red_t>::type>(
                channel_invert<T1>(
                    (std::min)(channel_traits<T1>::max_value(),
                             T1(channel_multiply(get_color(src,cyan_t()),channel_invert(get_color(src,black_t())))+get_color(src,black_t())))));
        get_color(dst,green_t())=
            channel_convert<typename color_element_type<P2,green_t>::type>(
                channel_invert<T1>(
                    (std::min)(channel_traits<T1>::max_value(),
                             T1(channel_multiply(get_color(src,magenta_t()),channel_invert(get_color(src,black_t())))+get_color(src,black_t())))));
        get_color(dst,blue_t()) =
            channel_convert<typename color_element_type<P2,blue_t>::type>(
                channel_invert<T1>(
                    (std::min)(channel_traits<T1>::max_value(),
                             T1(channel_multiply(get_color(src,yellow_t()),channel_invert(get_color(src,black_t())))+get_color(src,black_t())))));
    }
};

/// \ingroup ColorConvert
/// \brief CMYK to Gray
///
/// gray = (1 - 0.212c - 0.715m - 0.0722y) * (1 - k)
template <>
struct default_color_converter_impl<cmyk_t,gray_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const  {
        get_color(dst,gray_color_t())=
            channel_convert<typename color_element_type<P2,gray_color_t>::type>(
                channel_multiply(
                    channel_invert(
                       detail::rgb_to_luminance<typename color_element_type<P1,black_t>::type>(
                            get_color(src,cyan_t()),
                            get_color(src,magenta_t()),
                            get_color(src,yellow_t())
                       )
                    ),
                    channel_invert(get_color(src,black_t()))));
    }
};

namespace detail {
template <typename Pixel>
typename channel_type<Pixel>::type alpha_or_max_impl(const Pixel& p, mpl::true_) {
    return get_color(p,alpha_t());
}
template <typename Pixel>
typename channel_type<Pixel>::type alpha_or_max_impl(const Pixel&  , mpl::false_) {
    return channel_traits<typename channel_type<Pixel>::type>::max_value();
}
} // namespace detail

// Returns max_value if the pixel has no alpha channel. Otherwise returns the alpha.
template <typename Pixel>
typename channel_type<Pixel>::type alpha_or_max(const Pixel& p) {
    return detail::alpha_or_max_impl(p, mpl::contains<typename color_space_type<Pixel>::type,alpha_t>());
}

/// \ingroup ColorConvert
/// \brief Converting any pixel type to RGBA. Note: Supports homogeneous pixels only.
template <typename C1>
struct default_color_converter_impl<C1,rgba_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        using T2 = typename channel_type<P2>::type;
        pixel<T2,rgb_layout_t> tmp;
        default_color_converter_impl<C1,rgb_t>()(src,tmp);
        get_color(dst,red_t())  =get_color(tmp,red_t());
        get_color(dst,green_t())=get_color(tmp,green_t());
        get_color(dst,blue_t()) =get_color(tmp,blue_t());
        get_color(dst,alpha_t())=channel_convert<T2>(alpha_or_max(src));
    }
};

/// \ingroup ColorConvert
///  \brief Converting RGBA to any pixel type. Note: Supports homogeneous pixels only.
///
/// Done by multiplying the alpha to get to RGB, then converting the RGB to the target pixel type
/// Note: This may be slower if the compiler doesn't optimize out constructing/destructing a temporary RGB pixel.
///       Consider rewriting if performance is an issue
template <typename C2>
struct default_color_converter_impl<rgba_t,C2> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        using T1 = typename channel_type<P1>::type;
        default_color_converter_impl<rgb_t,C2>()(
            pixel<T1,rgb_layout_t>(channel_multiply(get_color(src,red_t()),  get_color(src,alpha_t())),
                                   channel_multiply(get_color(src,green_t()),get_color(src,alpha_t())),
                                   channel_multiply(get_color(src,blue_t()), get_color(src,alpha_t())))
            ,dst);
    }
};

/// \ingroup ColorConvert
/// \brief Unfortunately RGBA to RGBA must be explicitly provided - otherwise we get ambiguous specialization error.
template <>
struct default_color_converter_impl<rgba_t,rgba_t> {
    template <typename P1, typename P2>
    void operator()(const P1& src, P2& dst) const {
        static_for_each(src,dst,default_channel_converter());
    }
};

/// @defgroup ColorConvert Color Space Converion
/// \ingroup ColorSpaces
/// \brief Support for conversion between pixels of different color spaces and channel depths

/// \ingroup PixelAlgorithm ColorConvert
/// \brief class for color-converting one pixel to another
struct default_color_converter {
    template <typename SrcP, typename DstP>
    void operator()(const SrcP& src,DstP& dst) const {
        using SrcColorSpace = typename color_space_type<SrcP>::type;
        using DstColorSpace = typename color_space_type<DstP>::type;
        default_color_converter_impl<SrcColorSpace,DstColorSpace>()(src,dst);
    }
};

/// \ingroup PixelAlgorithm
/// \brief helper function for converting one pixel to another using GIL default color-converters
///     where ScrP models HomogeneousPixelConcept
///           DstP models HomogeneousPixelValueConcept
template <typename SrcP, typename DstP>
inline void color_convert(const SrcP& src, DstP& dst) {
    default_color_converter()(src,dst);
}

} }  // namespace boost::gil

#endif


#ifndef OSRM_ENGINE_ROUTING_BASE_MLD_HPP
#define OSRM_ENGINE_ROUTING_BASE_MLD_HPP

#include "engine/algorithm.hpp"
#include "engine/datafacade.hpp"
#include "engine/routing_algorithms/routing_base.hpp"
#include "engine/search_engine_data.hpp"

#include "util/typedefs.hpp"

#include <boost/assert.hpp>

#include <algorithm>
#include <iterator>
#include <limits>
#include <tuple>
#include <vector>

namespace osrm
{
namespace engine
{
namespace routing_algorithms
{
namespace mld
{

namespace
{
// Unrestricted search (Args is const PhantomNodes &):
//   * use partition.GetQueryLevel to find the node query level based on source and target phantoms
//   * allow to traverse all cells
template <typename MultiLevelPartition>
inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
                                 NodeID node,
                                 const PhantomNodes &phantom_nodes)
{
    auto level = [&partition, node](const SegmentID &source, const SegmentID &target) {
        if (source.enabled && target.enabled)
            return partition.GetQueryLevel(source.id, target.id, node);
        return INVALID_LEVEL_ID;
    };
    return std::min(std::min(level(phantom_nodes.source_phantom.forward_segment_id,
                                   phantom_nodes.target_phantom.forward_segment_id),
                             level(phantom_nodes.source_phantom.forward_segment_id,
                                   phantom_nodes.target_phantom.reverse_segment_id)),
                    std::min(level(phantom_nodes.source_phantom.reverse_segment_id,
                                   phantom_nodes.target_phantom.forward_segment_id),
                             level(phantom_nodes.source_phantom.reverse_segment_id,
                                   phantom_nodes.target_phantom.reverse_segment_id)));
}

inline bool checkParentCellRestriction(CellID, const PhantomNodes &) { return true; }

// Restricted search (Args is LevelID, CellID):
//   * use the fixed level for queries
//   * check if the node cell is the same as the specified parent onr
template <typename MultiLevelPartition>
inline LevelID getNodeQueryLevel(const MultiLevelPartition &, NodeID, LevelID level, CellID)
{
    return level;
}

inline bool checkParentCellRestriction(CellID cell, LevelID, CellID parent)
{
    return cell == parent;
}
}

// Heaps only record for each node its predecessor ("parent") on the shortest path.
// For re-constructing the actual path we need to trace back all parent "pointers".
// In contrast to the CH code MLD needs to know the edges (with clique arc property).

using PackedEdge = std::tuple</*from*/ NodeID, /*to*/ NodeID, /*from_clique_arc*/ bool>;
using PackedPath = std::vector<PackedEdge>;

template <bool DIRECTION, typename OutIter>
inline void retrievePackedPathFromSingleHeap(const SearchEngineData<Algorithm>::QueryHeap &heap,
                                             const NodeID middle,
                                             OutIter out)
{
    NodeID current = middle;
    NodeID parent = heap.GetData(current).parent;

    while (current != parent)
    {
        const auto &data = heap.GetData(current);

        if (DIRECTION == FORWARD_DIRECTION)
        {
            *out = std::make_tuple(parent, current, data.from_clique_arc);
            ++out;
        }
        else if (DIRECTION == REVERSE_DIRECTION)
        {
            *out = std::make_tuple(current, parent, data.from_clique_arc);
            ++out;
        }

        current = parent;
        parent = heap.GetData(parent).parent;
    }
}

template <bool DIRECTION>
inline PackedPath
retrievePackedPathFromSingleHeap(const SearchEngineData<Algorithm>::QueryHeap &heap,
                                 const NodeID middle)
{
    PackedPath packed_path;
    retrievePackedPathFromSingleHeap<DIRECTION>(heap, middle, std::back_inserter(packed_path));
    return packed_path;
}

// Trace path from middle to start in the forward search space (in reverse)
// and from middle to end in the reverse search space. Middle connects paths.

inline PackedPath
retrievePackedPathFromHeap(const SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                           const SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                           const NodeID middle)
{
    // Retrieve start -> middle. Is in reverse order since tracing back starts from middle.
    auto packed_path = retrievePackedPathFromSingleHeap<FORWARD_DIRECTION>(forward_heap, middle);
    std::reverse(begin(packed_path), end(packed_path));

    // Retrieve middle -> end. Is already in correct order, tracing starts from middle.
    auto into = std::back_inserter(packed_path);
    retrievePackedPathFromSingleHeap<REVERSE_DIRECTION>(reverse_heap, middle, into);

    return packed_path;
}

template <bool DIRECTION, typename Algorithm, typename... Args>
void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
                        typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                        const NodeID node,
                        const EdgeWeight weight,
                        Args... args)
{
    const auto &partition = facade.GetMultiLevelPartition();
    const auto &cells = facade.GetCellStorage();
    const auto &metric = facade.GetCellMetric();

    const auto level = getNodeQueryLevel(partition, node, args...);

    if (level >= 1 && !forward_heap.GetData(node).from_clique_arc)
    {
        if (DIRECTION == FORWARD_DIRECTION)
        {
            // Shortcuts in forward direction
            const auto &cell = cells.GetCell(metric, level, partition.GetCell(level, node));
            auto destination = cell.GetDestinationNodes().begin();
            for (auto shortcut_weight : cell.GetOutWeight(node))
            {
                BOOST_ASSERT(destination != cell.GetDestinationNodes().end());
                const NodeID to = *destination;

                if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to)
                {
                    const EdgeWeight to_weight = weight + shortcut_weight;
                    BOOST_ASSERT(to_weight >= weight);
                    if (!forward_heap.WasInserted(to))
                    {
                        forward_heap.Insert(to, to_weight, {node, true});
                    }
                    else if (to_weight < forward_heap.GetKey(to))
                    {
                        forward_heap.GetData(to) = {node, true};
                        forward_heap.DecreaseKey(to, to_weight);
                    }
                }
                ++destination;
            }
        }
        else
        {
            // Shortcuts in backward direction
            const auto &cell = cells.GetCell(metric, level, partition.GetCell(level, node));
            auto source = cell.GetSourceNodes().begin();
            for (auto shortcut_weight : cell.GetInWeight(node))
            {
                BOOST_ASSERT(source != cell.GetSourceNodes().end());
                const NodeID to = *source;

                if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to)
                {
                    const EdgeWeight to_weight = weight + shortcut_weight;
                    BOOST_ASSERT(to_weight >= weight);
                    if (!forward_heap.WasInserted(to))
                    {
                        forward_heap.Insert(to, to_weight, {node, true});
                    }
                    else if (to_weight < forward_heap.GetKey(to))
                    {
                        forward_heap.GetData(to) = {node, true};
                        forward_heap.DecreaseKey(to, to_weight);
                    }
                }
                ++source;
            }
        }
    }

    // Boundary edges
    for (const auto edge : facade.GetBorderEdgeRange(level, node))
    {
        const auto &edge_data = facade.GetEdgeData(edge);
        if (DIRECTION == FORWARD_DIRECTION ? edge_data.forward : edge_data.backward)
        {
            const NodeID to = facade.GetTarget(edge);

            if (!facade.ExcludeNode(to) &&
                checkParentCellRestriction(partition.GetCell(level + 1, to), args...))
            {
                BOOST_ASSERT_MSG(edge_data.weight > 0, "edge_weight invalid");
                const EdgeWeight to_weight = weight + edge_data.weight;

                if (!forward_heap.WasInserted(to))
                {
                    forward_heap.Insert(to, to_weight, {node, false});
                }
                else if (to_weight < forward_heap.GetKey(to))
                {
                    forward_heap.GetData(to) = {node, false};
                    forward_heap.DecreaseKey(to, to_weight);
                }
            }
        }
    }
}

template <bool DIRECTION, typename Algorithm, typename... Args>
void routingStep(const DataFacade<Algorithm> &facade,
                 typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                 typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                 NodeID &middle_node,
                 EdgeWeight &path_upper_bound,
                 const bool force_loop_forward,
                 const bool force_loop_reverse,
                 Args... args)
{
    const auto node = forward_heap.DeleteMin();
    const auto weight = forward_heap.GetKey(node);

    BOOST_ASSERT(!facade.ExcludeNode(node));

    // Upper bound for the path source -> target with
    // weight(source -> node) = weight weight(to -> target) ≤ reverse_weight
    // is weight + reverse_weight
    // More tighter upper bound requires additional condition reverse_heap.WasRemoved(to)
    // with weight(to -> target) = reverse_weight and all weights ≥ 0
    if (reverse_heap.WasInserted(node))
    {
        auto reverse_weight = reverse_heap.GetKey(node);
        auto path_weight = weight + reverse_weight;

        // MLD uses loops forcing only to prune single node paths in forward and/or
        // backward direction (there is no need to force loops in MLD but in CH)
        if (!(force_loop_forward && forward_heap.GetData(node).parent == node) &&
            !(force_loop_reverse && reverse_heap.GetData(node).parent == node) &&
            (path_weight >= 0) && (path_weight < path_upper_bound))
        {
            middle_node = node;
            path_upper_bound = path_weight;
        }
    }

    // Relax outgoing edges from node
    relaxOutgoingEdges<DIRECTION>(facade, forward_heap, node, weight, args...);
}

// With (s, middle, t) we trace back the paths middle -> s and middle -> t.
// This gives us a packed path (node ids) from the base graph around s and t,
// and overlay node ids otherwise. We then have to unpack the overlay clique
// edges by recursively descending unpacking the path down to the base graph.

using UnpackedNodes = std::vector<NodeID>;
using UnpackedEdges = std::vector<EdgeID>;
using UnpackedPath = std::tuple<EdgeWeight, UnpackedNodes, UnpackedEdges>;

template <typename Algorithm, typename... Args>
UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
                    const DataFacade<Algorithm> &facade,
                    typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                    typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                    const bool force_loop_forward,
                    const bool force_loop_reverse,
                    EdgeWeight weight_upper_bound,
                    Args... args)
{
    if (forward_heap.Empty() || reverse_heap.Empty())
    {
        return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>());
    }

    const auto &partition = facade.GetMultiLevelPartition();

    BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
    BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);

    // run two-Target Dijkstra routing step.
    NodeID middle = SPECIAL_NODEID;
    EdgeWeight weight = weight_upper_bound;
    EdgeWeight forward_heap_min = forward_heap.MinKey();
    EdgeWeight reverse_heap_min = reverse_heap.MinKey();
    while (forward_heap.Size() + reverse_heap.Size() > 0 &&
           forward_heap_min + reverse_heap_min < weight)
    {
        if (!forward_heap.Empty())
        {
            routingStep<FORWARD_DIRECTION>(facade,
                                           forward_heap,
                                           reverse_heap,
                                           middle,
                                           weight,
                                           force_loop_forward,
                                           force_loop_reverse,
                                           args...);
            if (!forward_heap.Empty())
                forward_heap_min = forward_heap.MinKey();
        }
        if (!reverse_heap.Empty())
        {
            routingStep<REVERSE_DIRECTION>(facade,
                                           reverse_heap,
                                           forward_heap,
                                           middle,
                                           weight,
                                           force_loop_reverse,
                                           force_loop_forward,
                                           args...);
            if (!reverse_heap.Empty())
                reverse_heap_min = reverse_heap.MinKey();
        }
    };

    // No path found for both target nodes?
    if (weight >= weight_upper_bound || SPECIAL_NODEID == middle)
    {
        return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>());
    }

    // Get packed path as edges {from node ID, to node ID, from_clique_arc}
    auto packed_path = retrievePackedPathFromHeap(forward_heap, reverse_heap, middle);

    // Beware the edge case when start, middle, end are all the same.
    // In this case we return a single node, no edges. We also don't unpack.
    const NodeID source_node = !packed_path.empty() ? std::get<0>(packed_path.front()) : middle;

    // Unpack path
    std::vector<NodeID> unpacked_nodes;
    std::vector<EdgeID> unpacked_edges;
    unpacked_nodes.reserve(packed_path.size());
    unpacked_edges.reserve(packed_path.size());

    unpacked_nodes.push_back(source_node);

    for (auto const &packed_edge : packed_path)
    {
        NodeID source, target;
        bool overlay_edge;
        std::tie(source, target, overlay_edge) = packed_edge;
        if (!overlay_edge)
        { // a base graph edge
            unpacked_nodes.push_back(target);
            unpacked_edges.push_back(facade.FindEdge(source, target));
        }
        else
        { // an overlay graph edge
            LevelID level = getNodeQueryLevel(partition, source, args...);
            CellID parent_cell_id = partition.GetCell(level, source);
            BOOST_ASSERT(parent_cell_id == partition.GetCell(level, target));

            LevelID sublevel = level - 1;

            // Here heaps can be reused, let's go deeper!
            forward_heap.Clear();
            reverse_heap.Clear();
            forward_heap.Insert(source, 0, {source});
            reverse_heap.Insert(target, 0, {target});

            // TODO: when structured bindings will be allowed change to
            // auto [subpath_weight, subpath_source, subpath_target, subpath] = ...
            EdgeWeight subpath_weight;
            std::vector<NodeID> subpath_nodes;
            std::vector<EdgeID> subpath_edges;
            std::tie(subpath_weight, subpath_nodes, subpath_edges) = search(engine_working_data,
                                                                            facade,
                                                                            forward_heap,
                                                                            reverse_heap,
                                                                            force_loop_forward,
                                                                            force_loop_reverse,
                                                                            INVALID_EDGE_WEIGHT,
                                                                            sublevel,
                                                                            parent_cell_id);
            BOOST_ASSERT(!subpath_edges.empty());
            BOOST_ASSERT(subpath_nodes.size() > 1);
            BOOST_ASSERT(subpath_nodes.front() == source);
            BOOST_ASSERT(subpath_nodes.back() == target);
            unpacked_nodes.insert(
                unpacked_nodes.end(), std::next(subpath_nodes.begin()), subpath_nodes.end());
            unpacked_edges.insert(unpacked_edges.end(), subpath_edges.begin(), subpath_edges.end());
        }
    }

    return std::make_tuple(weight, std::move(unpacked_nodes), std::move(unpacked_edges));
}

// Alias to be compatible with the CH-based search
template <typename Algorithm>
inline void search(SearchEngineData<Algorithm> &engine_working_data,
                   const DataFacade<Algorithm> &facade,
                   typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                   typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                   EdgeWeight &weight,
                   std::vector<NodeID> &unpacked_nodes,
                   const bool force_loop_forward,
                   const bool force_loop_reverse,
                   const PhantomNodes &phantom_nodes,
                   const EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
{
    // TODO: change search calling interface to use unpacked_edges result
    std::tie(weight, unpacked_nodes, std::ignore) = search(engine_working_data,
                                                           facade,
                                                           forward_heap,
                                                           reverse_heap,
                                                           force_loop_forward,
                                                           force_loop_reverse,
                                                           weight_upper_bound,
                                                           phantom_nodes);
}

// TODO: refactor CH-related stub to use unpacked_edges
template <typename RandomIter, typename FacadeT>
void unpackPath(const FacadeT &facade,
                RandomIter packed_path_begin,
                RandomIter packed_path_end,
                const PhantomNodes &phantom_nodes,
                std::vector<PathData> &unpacked_path)
{
    const auto nodes_number = std::distance(packed_path_begin, packed_path_end);
    BOOST_ASSERT(nodes_number > 0);

    std::vector<NodeID> unpacked_nodes;
    std::vector<EdgeID> unpacked_edges;
    unpacked_nodes.reserve(nodes_number);
    unpacked_edges.reserve(nodes_number);

    unpacked_nodes.push_back(*packed_path_begin);
    if (nodes_number > 1)
    {
        util::for_each_pair(
            packed_path_begin,
            packed_path_end,
            [&facade, &unpacked_nodes, &unpacked_edges](const auto from, const auto to) {
                unpacked_nodes.push_back(to);
                unpacked_edges.push_back(facade.FindEdge(from, to));
            });
    }

    annotatePath(facade, phantom_nodes, unpacked_nodes, unpacked_edges, unpacked_path);
}

template <typename Algorithm>
double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                          const DataFacade<Algorithm> &facade,
                          typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                          typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                          const PhantomNode &source_phantom,
                          const PhantomNode &target_phantom,
                          EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
{
    forward_heap.Clear();
    reverse_heap.Clear();

    const PhantomNodes phantom_nodes{source_phantom, target_phantom};
    insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);

    EdgeWeight weight = INVALID_EDGE_WEIGHT;
    std::vector<NodeID> unpacked_nodes;
    std::vector<EdgeID> unpacked_edges;
    std::tie(weight, unpacked_nodes, unpacked_edges) = search(engine_working_data,
                                                              facade,
                                                              forward_heap,
                                                              reverse_heap,
                                                              DO_NOT_FORCE_LOOPS,
                                                              DO_NOT_FORCE_LOOPS,
                                                              weight_upper_bound,
                                                              phantom_nodes);

    if (weight == INVALID_EDGE_WEIGHT)
    {
        return std::numeric_limits<double>::max();
    }

    std::vector<PathData> unpacked_path;

    annotatePath(facade, phantom_nodes, unpacked_nodes, unpacked_edges, unpacked_path);

    return getPathDistance(facade, unpacked_path, source_phantom, target_phantom);
}

} // namespace mld
} // namespace routing_algorithms
} // namespace engine
} // namespace osrm

#endif // OSRM_ENGINE_ROUTING_BASE_MLD_HPP


#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "unsupported/Eigen/CXX11/src/Tensor/TensorDeviceCuda.h"
//#include "unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include "bboxes.h"
#include "matcher.h"
#include "wtoolkit.h"
#include "wtoolkit_cuda.h"
#include <future>

using namespace tensorflow;
using namespace std;

typedef Eigen::ThreadPoolDevice CPUDevice;
typedef Eigen::GpuDevice GPUDevice;
REGISTER_OP("TopPool")
    .Attr("T: {float,double}")
    .Input("tensor: T")
	.Output("output_tensor:T")
	.SetShapeFn(shape_inference::UnchangedShape);
REGISTER_OP("TopPoolGrad")
    .Attr("T: {float,double}")
    .Input("tensor: T")
    .Input("fw_output: T")
    .Input("backprop: T")
	.Output("output_tensor:T")
	.SetShapeFn(shape_inference::UnchangedShape);

template <typename Device, typename T>
class TopPoolOp: public OpKernel {
};
template <typename T>
class TopPoolOp<CPUDevice,T>: public OpKernel {
	public:
		explicit TopPoolOp(OpKernelConstruction* context) : OpKernel(context) {
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THISV1("TopPool");
			const Tensor &_tensor = context->input(0);

			OP_REQUIRES(context, _tensor.dims() == 4, errors::InvalidArgument("tensor must be 4-dimensional"));

			auto         tensor        = _tensor.template tensor<T,4>();
			TensorShape  outshape      = _tensor.shape();
			Tensor      *output_tensor = nullptr;

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape, &output_tensor));

			auto           output      = output_tensor->template tensor<T,4>();
			constexpr auto kProcessDim = 1;
			const auto     process_nr  = _tensor.dim_size(kProcessDim);

            output.chip(process_nr-1,kProcessDim) = tensor.chip(process_nr-1,kProcessDim);

            for(auto i=process_nr-2; i>=0; --i){
                output.chip(i,kProcessDim) = output.chip(i+1,kProcessDim).cwiseMax(tensor.chip(i,kProcessDim));
            }
        }
};
template <typename Device, typename T>
class TopPoolGradOp: public OpKernel {
};
template <typename T>
class TopPoolGradOp<CPUDevice,T>: public OpKernel {
	public:
		explicit TopPoolGradOp(OpKernelConstruction* context) : OpKernel(context) {
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THISV1("TopPoolGrad");
			const Tensor &_tensor    = context->input(0);
			const Tensor &_fw_output = context->input(1);
			const Tensor &_backprop  = context->input(2);

			OP_REQUIRES(context, _tensor.dims() == 4, errors::InvalidArgument("tensor must be 4-dimensional"));
			OP_REQUIRES(context, _fw_output.dims() == 4, errors::InvalidArgument("forward output must be 4-dimensional"));
			OP_REQUIRES(context, _backprop.dims() == 4, errors::InvalidArgument("backprop must be 4-dimensional"));

			auto         tensor        = _tensor.template tensor<T,4>();
			auto         fw_output     = _fw_output.template tensor<T,4>();
			auto         backprop      = _backprop.template tensor<T,4>();
			TensorShape  outshape      = _tensor.shape();
			Tensor      *output_tensor = nullptr;

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape, &output_tensor));

			auto           output      = output_tensor->template tensor<T,4>();
			constexpr auto kProcessDim = 1;
			const auto     process_nr  = _tensor.dim_size(kProcessDim);
            Eigen::Tensor<int,3> index(_tensor.dim_size(0),_tensor.dim_size(2),_tensor.dim_size(3));

            output.setZero();
            output.chip(process_nr-1,kProcessDim) = backprop.chip(process_nr-1,kProcessDim);
            index.setConstant(process_nr-1);

            for(auto i=process_nr-2; i>=0; --i){
                Eigen::Tensor<bool,3,Eigen::RowMajor> t = (fw_output.chip(i+1,kProcessDim) < tensor.chip(i,kProcessDim));
                assign_grad(t,i,index,backprop,output);
            }
        }

        template<typename IT,typename BPT,typename OT,typename ST>
        void assign_grad(const ST& select,int cur_index,IT& index, const BPT& backprop,OT& output) {
            auto idx = 0;
            for(auto i=0; i<index.dimension(0); ++i) {
                for(auto j=0; j<index.dimension(1); ++j) {
                    for(auto k=0; k<index.dimension(2); ++k) {
                        if(select(i,j,k)){
                                idx = cur_index;
                                index(i,j,k) = idx;
                          } else {
                                idx = index(i,j,k);
                          }
                        output(i,idx,j,k) += backprop(i,idx,j,k);
                    }
                }
            }
        }
};
REGISTER_KERNEL_BUILDER(Name("TopPool").Device(DEVICE_CPU).TypeConstraint<float>("T"), TopPoolOp<CPUDevice, float>);
REGISTER_KERNEL_BUILDER(Name("TopPool").Device(DEVICE_CPU).TypeConstraint<double>("T"), TopPoolOp<CPUDevice, double>);
REGISTER_KERNEL_BUILDER(Name("TopPoolGrad").Device(DEVICE_CPU).TypeConstraint<float>("T"), TopPoolGradOp<CPUDevice, float>);
REGISTER_KERNEL_BUILDER(Name("TopPoolGrad").Device(DEVICE_CPU).TypeConstraint<double>("T"), TopPoolGradOp<CPUDevice, double>);


#include "v4r/registration/MultiSessionModelling.h"
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <v4r/common/miscellaneous.h>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include "v4r/common/visibility_reasoning.h"

template <class PointT>
float v4r::Registration::MultiSessionModelling<PointT>::computeFSV(
    const typename pcl::PointCloud<PointT>::ConstPtr& cloud, pcl::PointCloud<pcl::Normal>::ConstPtr& normals,
    const std::vector<int>& indices, const Eigen::Matrix4f& pose,
    const typename pcl::PointCloud<PointT>::ConstPtr& range_image) {
  v4r::VisibilityReasoning<PointT> vr(Intrinsics::PrimeSense());
  vr.setThresholdTSS(0.01f);

  typename pcl::PointCloud<PointT>::Ptr model(new pcl::PointCloud<PointT>());
  pcl::transformPointCloud(*cloud, indices, *model, pose);

  pcl::PointCloud<pcl::Normal>::Ptr model_normals(new pcl::PointCloud<pcl::Normal>);
  v4r::transformNormals(*normals, *model_normals, indices, pose);

  // Eigen::Matrix4f identity = Eigen::Matrix4f::Identity();

  float fsv = vr.computeFSVWithNormals(range_image, model, model_normals);

  // the more points on the same surface the more reliable this is, so increase fsv if not many points
  int ss = vr.getFSVUsedPoints();
  float ratio = 1.f - ss / static_cast<float>(model->points.size());

  // std::cout << model->points.size() << " " << indices.size() << " fsv:" << fsv << std::endl;

  /*if(fsv > 0.1)
  {*/
  /*    ///debug
      pcl::visualization::PCLVisualizer vis("correspondences");
      //int v1,v2;
      //vis.createViewPort(0,0,0.5,1,v1);
      //vis.createViewPort(0.5,0,1,1,v2);

      pcl::visualization::PointCloudColorHandlerCustom<PointT> handler(model, 255, 0, 0);
      vis.addPointCloud(range_image, "cloud_2");
      vis.addPointCloud(model, handler, "cloud_1");

      vis.spin();
  */
  /*}*/

  return fsv * ratio;
}

template <class PointT>
void v4r::Registration::MultiSessionModelling<PointT>::computeCost(EdgeBetweenPartialModels& edge) {
  // fsv computation
  // take all views of edge_i with respective indices and compute FSV with respect to the views of j_
  // do i need normals?

  float fsv = 0.f;
  int non_valid = 0;
  int total = 0;
  for (int ii = session_ranges_[edge.i_].first; ii <= session_ranges_[edge.i_].second; ii++) {
    typename pcl::PointCloud<PointT>::ConstPtr cloud = clouds_[ii];
    std::vector<int>& indices = getIndices(ii);
    Eigen::Matrix4f pose = edge.transformation_.inverse() * getPose(ii);

    for (int jj = session_ranges_[edge.j_].first; jj <= session_ranges_[edge.j_].second; jj++) {
      Eigen::Matrix4f pose_to_jj = poses_[jj].inverse() * pose;
      float fsv_loc = computeFSV(cloud, normals_[ii], indices, pose_to_jj, clouds_[jj]);
      if (fsv_loc < 0) {
        non_valid++;
      } else {
        fsv += fsv_loc;
      }

      total++;
    }
  }

  if (non_valid == total) {
    std::cout << "No valid edge..." << std::endl;
    edge.cost_ = std::numeric_limits<float>::infinity();
  } else {
    edge.cost_ = fsv / (total - non_valid);
  }
}

template <class PointT>
void v4r::Registration::MultiSessionModelling<PointT>::compute() {
  for (size_t a = 0; a < reg_algos_.size(); a++) {
    reg_algos_[a]->setMSM(this);
    reg_algos_[a]->initialize(session_ranges_);
  }

  std::vector<std::vector<std::vector<EdgeBetweenPartialModels>>> edges;
  edges.resize(session_ranges_.size());
  for (size_t i = 0; i < session_ranges_.size(); i++) {
    edges[i].resize(session_ranges_.size());
  }

  // for each session pair, call the class that registers two partial models and returns a set of poses
  for (size_t i = 0; i < session_ranges_.size(); i++) {
    std::pair<int, int> pair_i = session_ranges_[i];
    for (size_t j = (i + 1); j < session_ranges_.size(); j++) {
      std::pair<int, int> pair_j = session_ranges_[j];

      for (size_t a = 0; a < reg_algos_.size(); a++) {
        reg_algos_[a]->setSessions(pair_i, pair_j);
        reg_algos_[a]->compute(i, j);

        // poses transform the RF of pair_j to the RF of pair_i
        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> poses;
        reg_algos_[a]->getPoses(poses);

        std::cout << "poses between " << j << " and " << i << " :" << poses.size() << std::endl;

        for (size_t p = 0; p < poses.size(); p++) {
          EdgeBetweenPartialModels ebpm;
          ebpm.transformation_ = poses[p];
          ebpm.cost_ = -1;
          ebpm.i_ = i;
          ebpm.j_ = j;

          edges[i][j].push_back(ebpm);
        }
      }
    }
  }

  // for each edge in the graph, compute a score quantifying quality of alignment
  // overlap? number of outliers? fsv average?

  for (size_t i = 0; i < session_ranges_.size(); i++) {
    for (size_t j = (i + 1); j < session_ranges_.size(); j++) {
      if (edges[i][j].size() > 0) {
        std::cout << "Number of edges:" << edges[i][j].size() << std::endl;
        // iterate over the edges and compute cost_ (the higher, the worse)

#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
        for (size_t k = 0; k < edges[i][j].size(); k++) {
          computeCost(edges[i][j][k]);
        }
      }
    }
  }

  // final alignment between partial models? MST
  int final_alignment = 0;  // 0-MST, 1-something fancy?

  switch (final_alignment) {
    case 0: {
      std::vector<typename pcl::PointCloud<PointT>::Ptr> partial_model_clouds(session_ranges_.size());

      for (size_t i = 0; i < session_ranges_.size(); i++) {
        partial_model_clouds[i].reset(new pcl::PointCloud<PointT>);

        for (int t = session_ranges_[i].first; t <= session_ranges_[i].second; t++) {
          typename pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>);
          pcl::copyPointCloud(*getCloud(static_cast<size_t>(t)), getIndices(static_cast<size_t>(t)), *transformed);
          Eigen::Matrix4f pose_inv = getPose(static_cast<size_t>(t));
          pcl::transformPointCloud(*transformed, *transformed, pose_inv);
          *partial_model_clouds[i] += *transformed;
        }
      }

      Graph G;
      for (size_t i = 0; i < session_ranges_.size(); i++) {
        boost::add_vertex((int)i, G);
      }

      for (size_t i = 0; i < session_ranges_.size(); i++) {
        for (size_t j = (i + 1); j < session_ranges_.size(); j++) {
          if (edges[i][j].size() > 0) {
            float min_cost = std::numeric_limits<float>::infinity();
            int min_k = -1;

            for (size_t k = 0; k < edges[i][j].size(); k++) {
              // std::cout << "cost:" << edges[i][j][k].cost_ << std::endl;
              if (edges[i][j][k].cost_ < min_cost) {
                min_cost = edges[i][j][k].cost_;
                min_k = static_cast<int>(k);
              }
            }

            std::cout << j << " => " << i << " " << min_cost << " " << min_k << std::endl;
            // add edge to graph... transformation maps from j to i (otherwise invert)
            myEdge e;
            e.edge_weight = edges[i][j][min_k].cost_;
            e.transformation = edges[i][j][min_k].transformation_;
            e.source_id = j;
            e.target_id = i;
            boost::add_edge((int)j, (int)i, e, G);

            /*std::stringstream title_str;
            title_str << "best pw from " << j << " " << i;
            pcl::visualization::PCLVisualizer vis(title_str.str().c_str());
            vis.addCoordinateSystem(0.1);
            for(size_t k=0; k < edges[i][j].size(); k++)
            {
                if(edges[i][j][k].cost_ * 0.25 <= min_cost)
                {
                    std::cout << "cost:" << edges[i][j][k].cost_ << std::endl;

                    {
                        pcl::visualization::PointCloudColorHandlerRGBField<PointT> handler(partial_model_clouds[i]);
                        vis.addPointCloud(partial_model_clouds[i], handler, "cloud_i");
                    }

                    typename pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>);
                    pcl::transformPointCloud(*partial_model_clouds[j], *transformed, edges[i][j][k].transformation_);

                    {
                        pcl::visualization::PointCloudColorHandlerRGBField<PointT> handler(transformed);
                        vis.addPointCloud(transformed, handler, "cloud_j");
                    }

                    vis.spin();
                    vis.removeAllPointClouds();
                }
            }*/
          }
        }
      }

      boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, G);
      Graph MST;
      for (size_t i = 0; i < session_ranges_.size(); i++)
        boost::add_vertex((int)i, MST);

      // there is a bug in boost 1.54 that causes negative weights error (even though weights are positive)
      /*std::vector < boost::graph_traits<Graph>::vertex_descriptor > p (boost::num_vertices (G));
      boost::prim_minimum_spanning_tree (G, &p[0]);

      for (std::size_t i = 0; i != p.size (); ++i)
      {
          if (p[i] != i)
              std::cout << "parent[" << i << "] = " << p[i] << std::endl;
          else
              std::cout << "parent[" << i << "] = no parent" << std::endl;
      }

      typedef typename Graph::edge_iterator EdgeIterator;
      std::pair<EdgeIterator, EdgeIterator> edges = boost::edges(G);
      EdgeIterator edge;

      for (edge = edges.first; edge != edges.second; edge++)
      {
          typename boost::graph_traits<Graph>::vertex_descriptor s, t;
          s = boost::source(*edge, G);
          t = boost::target(*edge, G);

          if(p[s] == t || p[t] == s)
          {
              //edge in prim
              boost::add_edge ((int)s, (int)t, weightmap[*edge], MST);
          }
      }*/

      std::vector<Edge> spanning_tree;
      boost::kruskal_minimum_spanning_tree(G, std::back_inserter(spanning_tree));

      std::cout << "Print the edges in the MST:" << std::endl;
      for (std::vector<Edge>::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei) {
        std::cout << source(*ei, G) << " " << target(*ei, G) << std::endl;
        boost::add_edge(source(*ei, G), target(*ei, G), weightmap[*ei], MST);
      }

      std::cout << boost::num_edges(MST) << " " << boost::num_vertices(MST) << std::endl;
      output_session_poses_.resize(boost::num_vertices(MST));

      computeAbsolutePoses(MST, output_session_poses_);

      // finally, define output_cloud_poses_
      output_cloud_poses_.resize(clouds_.size());

      for (size_t i = 0; i < output_session_poses_.size(); i++) {
        std::cout << output_session_poses_[i] << std::endl;
        for (int j = session_ranges_[i].first; j <= session_ranges_[i].second; j++) {
          Eigen::Matrix4f trans = output_session_poses_[i] * poses_[j];
          output_cloud_poses_[j] = trans;
        }
      }

      break;
    }

    default: { break; }
  }
}

template class V4R_EXPORTS v4r::Registration::MultiSessionModelling<pcl::PointXYZRGB>;


#pragma once

#include "keyboard_optimizer/layout.hpp"
#include "keyboard_optimizer/utils.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <iostream>
#include <random>
#include <stdexcept>

namespace KeyboardOptimizer {

class InvalidConstraints final : public std::invalid_argument {
public:
  std::unordered_set<Sym> syms;
  std::unordered_set<Key> keys;
  std::unordered_map<Sym, Key> matchedSyms;
  Layer layer;
  constexpr static auto msg =
      "Found set of syms which are matched to a smaller set of keys. \n Note: "
      "some keys could be unavailable due to sym bonds.";
  InvalidConstraints(std::unordered_set<Sym> &&syms,
                     std::unordered_set<Key> &&keys,
                     std::unordered_map<Sym, Key> &&matchedSyms, Layer layer)
      : syms(std::move(syms)), keys(std::move(keys)),
        matchedSyms(std::move(matchedSyms)),
        layer(layer), std::invalid_argument(msg) {
    assert(this->syms.size() > this->keys.size());
  }
};

class UnsatisfiedConstraints : public std::invalid_argument {
public:
  Layout layout;

protected:
  UnsatisfiedConstraints(Layout &&layout)
      : layout(std::move(layout)), std::invalid_argument(""){};
};
class MissingEdge final : public UnsatisfiedConstraints {
public:
  Sym sym;
  Key key;
  MissingEdge(Layout &&layout, Sym sym, Key key)
      : UnsatisfiedConstraints(std::move(layout)), sym(sym), key(key) {}
  char const *what() const {
    return "Found sym-key pair which is restricted by provided constraints.";
  }
};
class MismatchedLayer final : public UnsatisfiedConstraints {
public:
  Sym sym;
  Layer actualLayer;
  Layer constraintsLayer;
  MismatchedLayer(Layout &&layout, Sym sym, Layer actualLayer,
                  Layer constraintsLayer)
      : UnsatisfiedConstraints(std::move(layout)), sym(sym),
        actualLayer(actualLayer), constraintsLayer(constraintsLayer) {}
  char const *what() const {
    return "Found sym which is located on different layer than in provided "
           "constraints.";
  }
};
class UnsatisfiedSymBond final : public UnsatisfiedConstraints {
public:
  Sym sym1;
  Key key1;
  Sym sym2;
  Key key2;
  UnsatisfiedSymBond(Layout &&layout, Sym sym1, Key key1, Sym sym2, Key key2)
      : UnsatisfiedConstraints(std::move(layout)), sym1(sym1), key1(key1),
        sym2(sym2), key2(key2) {}
  char const *what() const {
    return "Found syms which are located on different keys, which is "
           "restricted by provided constraints.";
  }
};

struct GraphTypes final {
  using Capacity = int;
  using Weight = int;
  using Edge = boost::graph_traits<boost::adjacency_list<
      boost::vecS, boost::vecS, boost::directedS>>::edge_descriptor;
  using Vertex = boost::graph_traits<boost::adjacency_list<
      boost::vecS, boost::vecS, boost::directedS>>::vertex_descriptor;
  struct VertexProperties final {
    size_t index;
    std::variant<Sym, Key> symOrKey;
  };
  using EdgeProperties = boost::property<
      boost::edge_capacity_t, Capacity,
      boost::property<boost::edge_index_t, size_t,
                      boost::property<boost::edge_reverse_t, Edge>>>;
  using Graph =
      boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                            VertexProperties, EdgeProperties>;
};

inline auto GetEdgeIndexMap(GraphTypes::Graph const &g) {
  return boost::get(boost::edge_index, g);
}
inline auto GetEdgeIndexMap(GraphTypes::Graph &g) {
  return boost::get(boost::edge_index, g);
}
inline auto GetVertexIndexMap(GraphTypes::Graph const &g) {
  return boost::make_function_property_map<GraphTypes::Vertex>(
      [&](GraphTypes::Vertex v) { return g[v].index; });
}
inline auto GetVertexIndexMap(GraphTypes::Graph &g) {
  return boost::make_function_property_map<GraphTypes::Vertex>(
      [&](GraphTypes::Vertex v) { return g[v].index; });
}
inline auto GetCapacityMap(GraphTypes::Graph const &g) {
  return boost::get(boost::edge_capacity, g);
}
inline auto GetCapacityMap(GraphTypes::Graph &g) {
  return boost::get(boost::edge_capacity, g);
}
inline auto GetReverseEdgeMap(GraphTypes::Graph const &g) {
  return boost::get(boost::edge_reverse, g);
}
inline auto GetReverseEdgeMap(GraphTypes::Graph &g) {
  return boost::get(boost::edge_reverse, g);
}
inline auto
GetResidualCapacityMap(GraphTypes::Graph const &g,
                       std::vector<GraphTypes::Capacity> &residualCapacityVec) {
  return boost::make_iterator_property_map(residualCapacityVec.begin(),
                                           GetEdgeIndexMap(g));
}
inline auto GetColorMap(GraphTypes::Graph const &g,
                        std::vector<boost::default_color_type> &colorVec) {
  return boost::make_iterator_property_map(colorVec.begin(),
                                           GetVertexIndexMap(g));
}
inline auto GetPredcessorMap(GraphTypes::Graph const &g,
                             std::vector<GraphTypes::Edge> &predcessorVec) {
  return boost::make_iterator_property_map(predcessorVec.begin(),
                                           GetVertexIndexMap(g));
}
inline auto GetDistanceMap(GraphTypes::Graph const &g,
                           std::vector<GraphTypes::Capacity> &distanceVec) {
  return boost::make_iterator_property_map(distanceVec.begin(),
                                           GetVertexIndexMap(g));
}
inline auto GetWeightMap(GraphTypes::Graph const &g,
                         std::vector<GraphTypes::Weight> const &weightVec) {
  return boost::make_iterator_property_map(weightVec.begin(),
                                           GetEdgeIndexMap(g));
}

class GraphContainer final {
public:
  using Capacity = GraphTypes::Capacity;
  using Weight = GraphTypes::Weight;
  using Edge = GraphTypes::Edge;
  using Vertex = GraphTypes::Vertex;
  using Graph = GraphTypes::Graph;

private:
  std::unique_ptr<Graph> gptr;

public:
  GraphContainer() : gptr{std::make_unique<Graph>()} {}
  GraphContainer(GraphContainer const &gc) : gptr{Copy(gc)} {}
  GraphContainer(GraphContainer &&gc) noexcept = default;
  GraphContainer &operator=(GraphContainer const &gc) & { gptr = Copy(gc); }
  GraphContainer &operator=(GraphContainer &&gc) &noexcept = default;
  ~GraphContainer() = default;
  Graph &GetGraph() noexcept { return *gptr; }
  operator Graph &() noexcept { return *gptr; }
  Graph const &GetGraph() const noexcept { return *gptr; }
  operator Graph const &() const noexcept { return *gptr; }

private:
  static std::unique_ptr<Graph> Copy(GraphContainer const &gc) {
    // Vertices are value types, but edges behave more like references
    // Thus it requires rebinding in some properties
    auto gptr = std::make_unique<Graph>();
    auto &&g1 = *gptr;
    auto const &g2 = *gc.gptr;
    auto const &vertexIndexMap = GetVertexIndexMap(g2);
    auto props = boost::vertex_index1_map(vertexIndexMap);
    boost::copy_graph(g2, g1, props);

    auto indices1 = std::vector<Edge>(boost::num_edges(g1));
    auto &&edgeIndexMap1 = GetEdgeIndexMap(g1);
    auto const &edgeIndexMap2 = GetEdgeIndexMap(g2);
    auto &&capacityMap1 = GetCapacityMap(g1);
    auto const &capacityMap2 = GetCapacityMap(g2);
    auto &&reverseEdgeMap1 = GetReverseEdgeMap(g1);
    auto const &reverseEdgeMap2 = GetReverseEdgeMap(g2);
    assert(boost::num_edges(g1) == boost::num_edges(g2));
    auto const &edges1 = boost::edges(g1);
    auto const &edges2 = boost::edges(g2);
    auto it1 = edges1.first;
    auto it2 = edges2.first;
    for (auto i = size_t{0}, e = boost::num_edges(g1); i != e; ++i) {
      edgeIndexMap1[*it1] = edgeIndexMap2[*it2];
      capacityMap1[*it1] = capacityMap2[*it2];
      indices1[edgeIndexMap1[*it1]] = *it1;
      ++it1;
      ++it2;
    }
    it1 = edges1.first;
    it2 = edges2.first;
    for (auto i = size_t{0}, e = boost::num_edges(g1); i != e; ++i) {
      reverseEdgeMap1[*it1] = indices1[edgeIndexMap2[reverseEdgeMap2[*it2]]];
      ++it1;
      ++it2;
    }
    return std::move(gptr);
  }
};

class LayoutGraph final {
public:
  using Capacity = GraphTypes::Capacity;
  using Weight = GraphTypes::Weight;
  using Edge = GraphTypes::Edge;
  using Vertex = GraphTypes::Vertex;
  using Graph = GraphTypes::Graph;
  using Weights = std::vector<Weight>;

private:
  using SymVertexMap = std::unordered_map<Sym, Vertex>;
  using KeyVertexMap = std::unordered_map<Key, Vertex>;

  GraphContainer gc;
  Vertex symSource = Vertex{};
  Vertex keySink = Vertex{};
  SymVertexMap symVertices;
  KeyVertexMap keyVertices;
  std::unordered_set<size_t> reverseEdges;
  std::set<size_t> directEdges;
  Graph &G() { return gc.GetGraph(); }
  Graph const &G() const { return gc.GetGraph(); }
  void AddSource() {
    symSource = boost::add_vertex({boost::num_vertices(G()), Sym{}}, G());
  }
  void AddSink() {
    keySink = boost::add_vertex({boost::num_vertices(G()), Sym{}}, G());
  }

public:
  LayoutGraph() {
    AddSource();
    AddSink();
  }

  size_t GetNEdges() const { return boost::num_edges(G()); }
  size_t GetNVertices() const { return boost::num_vertices(G()); }
  auto GetEdges() const {
    return boost::make_iterator_range(boost::edges(G()));
  }
  auto GetVertices() const {
    return boost::make_iterator_range(boost::vertices(G()));
  }
  Vertex GetSource() const { return symSource; }
  Vertex GetSink() const { return keySink; }
  std::optional<Vertex> GetVertex(Sym sym) const {
    if (symVertices.contains(sym))
      return symVertices.at(sym);
    return {};
  }
  std::optional<Vertex> GetVertex(Key key) const {
    if (keyVertices.contains(key))
      return keyVertices.at(key);
    return {};
  }
  Vertex GetSource(Edge e) const { return boost::source(e, G()); }
  Vertex GetTarget(Edge e) const { return boost::target(e, G()); }
  Vertex GetOrAddVertex(Sym sym) {
    if (!symVertices.contains(sym)) {
      auto v = boost::add_vertex({boost::num_vertices(G()), sym}, G());
      symVertices.emplace(sym, v);
      AddEdge(symSource, v);
    }
    return symVertices.at(sym);
  }
  Vertex GetOrAddVertex(Key key) {
    if (!keyVertices.contains(key)) {
      auto v = boost::add_vertex({boost::num_vertices(G()), key}, G());
      keyVertices.emplace(key, v);
      AddEdge(v, keySink);
    }
    return keyVertices.at(key);
  }
  std::optional<Edge> GetEdge(Vertex v1, Vertex v2) const {
    auto const &[e, isExist] = boost::edge(v1, v2, G());
    if (isExist)
      return e;
    return {};
  }
  Edge AddEdge(Vertex v1, Vertex v2) {
#ifndef NDEBUG
    assert(v1 != v2);
    assert(!IsSource(v2));
    assert(!IsSink(v1));
    assert(!IsSource(v1) || IsSym(v2));
    assert(!IsSink(v2) || IsKey(v1));
    assert(!((IsSym(v1) || IsKey(v1)) && (IsSym(v2) || IsKey(v2))) ||
           (IsSym(v1) && IsKey(v2)));
    auto const &[tmp, isExist] = boost::edge(v1, v2, G());
    assert(!isExist);
#endif // !NDEBUG
    auto &&edgeIndexMap = boost::get(boost::edge_index, G());
    auto &&capacityMap = boost::get(boost::edge_capacity, G());
    auto &&reverseEdgeMap = boost::get(boost::edge_reverse, G());
    auto const &[direct, success1] = boost::add_edge(v1, v2, G());
    assert(success1);
    assert(boost::num_edges(G()) > 0);
    edgeIndexMap[direct] = boost::num_edges(G()) - 1;
    capacityMap[direct] = 1;
    auto const &[reverse, success2] = boost::add_edge(v2, v1, G());
    assert(success2);
    edgeIndexMap[reverse] = boost::num_edges(G()) - 1;
    capacityMap[reverse] = 0;
    reverseEdgeMap[direct] = reverse;
    reverseEdgeMap[reverse] = direct;
    reverseEdges.insert(edgeIndexMap[reverse]);
    auto ind = edgeIndexMap[direct];
    directEdges.insert(edgeIndexMap[direct]);
    return direct;
  }
  bool IsSource(Vertex v) const { return v == symSource; }
  bool IsSink(Vertex v) const { return v == keySink; }
  bool IsSym(Vertex v) const {
    return !IsSource(v) && !IsSink(v) &&
           std::holds_alternative<Sym>(G()[v].symOrKey);
  }
  bool IsKey(Vertex v) const {
    return !IsSource(v) && !IsSink(v) &&
           std::holds_alternative<Key>(G()[v].symOrKey);
  }
  bool IsReverse(Edge e) const {
    auto const &edgeIndexMap = boost::get(boost::edge_index, G());
    return reverseEdges.contains(edgeIndexMap[e]);
  }
  Sym GetSym(Vertex v) const {
    assert(IsSym(v));
    return std::get<Sym>(G()[v].symOrKey);
  }
  Key GetKey(Vertex v) const {
    assert(IsKey(v));
    return std::get<Key>(G()[v].symOrKey);
  }

  Weights ScaleDirectWeights(Weights const &userWeights) const {
    assert(userWeights.size() == directEdges.size());
    auto weights = Weights(GetNEdges());
    auto ind = size_t{0};
    for (auto directInd : directEdges)
      weights[directInd] = userWeights[ind++];
    auto const &edgeIndexMap = boost::get(boost::edge_index, G());
    auto const &reverseEdgeMap = boost::get(boost::edge_reverse, G());
    for (auto const &e : GetEdges()) {
      if (!IsReverse(e))
        continue;
      auto i1 = edgeIndexMap[e];
      auto i2 = edgeIndexMap[reverseEdgeMap[e]];
      weights[edgeIndexMap[e]] = -weights[edgeIndexMap[reverseEdgeMap[e]]];
    }
    return weights;
  }

  Weights GenerateDirectWeights(Layout const &layout, Weight min,
                                Weight max) const {
    auto weights = Weights(directEdges.size(), max);
    auto const &edgeIndexMap = boost::get(boost::edge_index, G());
    for (auto sym = Sym{}, e = layout.size(); sym != e; ++sym) {
      auto const &p = layout[sym];
      auto sv = GetVertex(sym);
      auto kv = GetVertex(p.key);
      if (!sv || !kv)
        continue;
      auto edge = GetEdge(sv.value(), kv.value());
      if (!edge)
        continue;
      auto se = GetEdge(GetSource(), sv.value()).value();
      auto te = GetEdge(kv.value(), GetSink()).value();
      weights[edgeIndexMap[edge.value()]] = min;
      weights[edgeIndexMap[se]] = min;
      weights[edgeIndexMap[te]] = min;
    }
  }

  Graph const &GetGraph() const noexcept { return G(); }
  operator Graph const &() const noexcept { return G(); }
};

class GraphProperties final {
  using Graph = GraphTypes::Graph;
  using Capacity = GraphTypes::Capacity;
  using Vertex = GraphTypes::Vertex;
  using Edge = GraphTypes::Edge;

  using EdgeIndexMap = decltype(GetEdgeIndexMap(std::declval<Graph const &>()));
  using VertexIndexMap =
      decltype(GetVertexIndexMap(std::declval<Graph const &>()));
  using CapacityMap = decltype(GetCapacityMap(std::declval<Graph const &>()));
  using ReverseEdgeMap =
      decltype(GetReverseEdgeMap(std::declval<Graph const &>()));
  using ResidualCapacityMap = decltype(GetResidualCapacityMap(
      std::declval<Graph const &>(), std::declval<std::vector<Capacity> &>()));
  using ColorMap = decltype(
      GetColorMap(std::declval<Graph const &>(),
                  std::declval<std::vector<boost::default_color_type> &>()));
  using PredcessorMap = decltype(GetPredcessorMap(
      std::declval<Graph const &>(), std::declval<std::vector<Edge> &>()));
  using DistanceMap = decltype(GetDistanceMap(
      std::declval<Graph const &>(), std::declval<std::vector<Capacity> &>()));

  LayoutGraph const &g;
  size_t nEdges;
  size_t nVertices;
  Vertex source;
  Vertex sink;
  std::vector<Capacity> residualCapacityVec;
  std::vector<boost::default_color_type> colorVec;
  std::vector<Capacity> distanceVec;
  std::vector<Capacity> distanceVec2;
  std::vector<Edge> predcessorVec;
  EdgeIndexMap edgeIndexMap;
  VertexIndexMap vertexIndexMap;
  CapacityMap capacityMap;
  ReverseEdgeMap reverseEdgeMap;
  ResidualCapacityMap residualCapacityMap;
  ColorMap colorMap;
  PredcessorMap predcessorMap;
  DistanceMap distanceMap;
  DistanceMap distanceMap2;

public:
  GraphProperties(LayoutGraph const &layoutGraph, Vertex source, Vertex sink)
      : g(layoutGraph), nEdges(g.GetNEdges()), nVertices(g.GetNVertices()),
        source(source), sink(sink),
        residualCapacityVec(std::vector<Capacity>(nEdges)),
        colorVec(std::vector<boost::default_color_type>(nVertices)),
        distanceVec(std::vector<Capacity>(nVertices)),
        distanceVec2(std::vector<Capacity>(nVertices)),
        predcessorVec(std::vector<Edge>(nVertices)),
        edgeIndexMap(KeyboardOptimizer::GetEdgeIndexMap(g)),
        vertexIndexMap(KeyboardOptimizer::GetVertexIndexMap(g)),
        capacityMap(KeyboardOptimizer::GetCapacityMap(g)),
        reverseEdgeMap(KeyboardOptimizer::GetReverseEdgeMap(g)),
        residualCapacityMap(
            KeyboardOptimizer::GetResidualCapacityMap(g, residualCapacityVec)),
        colorMap(KeyboardOptimizer::GetColorMap(g, colorVec)),
        predcessorMap(KeyboardOptimizer::GetPredcessorMap(g, predcessorVec)),
        distanceMap(KeyboardOptimizer::GetDistanceMap(g, distanceVec)),
        distanceMap2(KeyboardOptimizer::GetDistanceMap(g, distanceVec2)) {}
  // Other ctors are deleted because most of fields in this class are
  // effectively references, which require non-trivial copying
  GraphProperties(GraphProperties const &) = delete;
  GraphProperties(GraphProperties &&) = delete;
  GraphProperties &operator=(GraphProperties const &) = delete;
  GraphProperties &operator=(GraphProperties &&) = delete;
  Vertex GetSource() const { return source; }
  Vertex GetSink() const { return sink; }
  LayoutGraph const &GetLayoutGraph() const { return g; }
  EdgeIndexMap const &GetEdgeIndexMap() const { return edgeIndexMap; }
  VertexIndexMap const &GetVertexIndexMap() const { return vertexIndexMap; }
  CapacityMap const &GetCapacityMap() const { return capacityMap; }
  ReverseEdgeMap const &GetReverseEdgeMap() const { return reverseEdgeMap; }
  ResidualCapacityMap const &GetResidualCapacityMap() const {
    return residualCapacityMap;
  }
  ColorMap const &GetColorMap() const { return colorMap; }
  PredcessorMap const &GetPredcessorMap() const { return predcessorMap; }
  DistanceMap const &GetDistanceMap() const { return distanceMap; }
  DistanceMap const &GetDistanceMap2() const { return distanceMap2; }
  ResidualCapacityMap &GetResidualCapacityMap() {
    return residualCapacityMap;
  }
  ColorMap &GetColorMap() { return colorMap; }
  PredcessorMap &GetPredcessorMap() { return predcessorMap; }
  DistanceMap &GetDistanceMap() { return distanceMap; }
  DistanceMap &GetDistanceMap2() { return distanceMap2; }

  auto GetProperties() {
    return boost::color_map(colorMap)
        .residual_capacity_map(residualCapacityMap)
        .distance_map(distanceMap)
        .distance_map2(distanceMap2)
        .vertex_index_map(vertexIndexMap)
        .predecessor_map(predcessorMap);
  }
};

class Filter final {
public:
  using SymLayers = SymMap<Layer>;
  using SymKeyMap = std::unordered_map<Sym, Key>;
  using FilteredGraph =
      boost::filtered_graph<LayoutGraph::Graph, Filter, Filter>;

private:
  using Vertex = LayoutGraph::Vertex;
  using Edge = LayoutGraph::Edge;

  LayoutGraph const *g;
  SymLayers symLayers;
  SymKeyMap matchedSyms;
  std::unordered_set<Key> matchedKeys;
  Layer layer;
  bool isShowSym(Sym sym) const {
    return (symLayers[sym] == layer) && (!matchedSyms.contains(sym));
  }
  bool isShowKey(Key key) const { return !matchedKeys.contains(key); }
  bool isShow(Vertex v) const {
    if (g->IsSym(v))
      return isShowSym(g->GetSym(v));
    if (g->IsKey(v))
      return isShowKey(g->GetKey(v));
    return true;
  }

public:
  // Default ctor is required for boost::filtered_graph
  Filter() : g{nullptr}, layer{Layer{}} {}
  Filter(LayoutGraph const &g, SymLayers const &symLayers)
      : g{&g}, symLayers{symLayers}, layer{Layer{}} {}
  void Set(Layer layer_, SymKeyMap &&matchedSyms_) {
    auto matchedKeys_ = std::unordered_set<Key>{};
    for (auto &&[sym, key] : matchedSyms_)
      if (symLayers[sym] == layer)
        matchedKeys_.insert(key);
    matchedSyms = std::move(matchedSyms_);
    matchedKeys = std::move(matchedKeys_);
    layer = layer_;
  }
  bool operator()(Edge e) const {
    assert(g);
    return isShow(g->GetSource(e)) && isShow(g->GetTarget(e));
  }
  bool operator()(Vertex v) const {
    assert(g);
    return isShow(v);
  }
  Layer GetLayer() const noexcept { return layer; }
  LayoutGraph const &GetLayoutGraph() const { return *g; }
  FilteredGraph GetFilteredGraph() { return FilteredGraph(*g, *this, *this); }
};

class StandartConstraintsGraph final {
public:
  using Weight = LayoutGraph::Weight;
  using Weights = LayoutGraph::Weights;
  using AllowedKeys = std::unordered_set<Key>;
  using SymConstraints = SymMap<AllowedKeys>;
  using SymLayers = Filter::SymLayers;
  using SymKeyMap = Filter::SymKeyMap;
  using SymBonds = SymVector;

private:
  Keyboard keyboard;
  SymLayers symLayers;
  SymBonds symBonds;
  SymConstraints constraints;
  size_t nLayers;
  LayersMap<Layer> layersComputeOrder;
  LayoutGraph layoutGraph;
  using FilteredGraph = Filter::FilteredGraph;

public:
  StandartConstraintsGraph(SymLayers &&symLayers_, SymBonds &&symBonds_,
                           SymConstraints &&constraints_, Keyboard &&keyboard)
      : keyboard{std::move(keyboard)}, symLayers{std::move(symLayers_)},
        symBonds{std::move(symBonds_)}, constraints{std::move(constraints_)},
        nLayers{EvaluateNLayers(symLayers)} {
    CheckSymKeyNumber();
    RegularizeSymBonds(symBonds);
    layersComputeOrder = GetBestLayersPermutation();
    GenerateGraph(constraints);
  }

  size_t GetRequiredWeightsSize() const {
    auto nEdges = layoutGraph.GetNEdges();
    assert(nEdges % 2 == 0);
    return nEdges / 2;
  }

  Weights GenerateWeights(Layout const &layout, Weight min, Weight max) const {
    return layoutGraph.GenerateDirectWeights(layout, min, max);
  }

  void CheckConstraints(Layout const &layout) const {
    assert(layout.size() == symLayers.size());
    for (auto sym = Sym{}, e = layout.size(); sym != e; ++sym) {
      auto const &p = layout[sym];
      if (symLayers[sym] != p.layer)
        throw MismatchedLayer(Layout(layout), sym, p.layer, symLayers[sym]);
      auto sv = layoutGraph.GetVertex(sym).value();
      auto kv = layoutGraph.GetVertex(p.key);
      if (!kv || !layoutGraph.GetEdge(sv, kv.value()))
        throw MissingEdge(Layout(layout), sym, p.key);
    }
    for (auto sym = Sym{}, e = symBonds.size(); sym != e; ++sym) {
      auto key = layout[sym].key;
      auto sym2 = symBonds[sym];
      auto key2 = layout[sym2].key;
      if (!(key == key2))
        throw UnsatisfiedSymBond(Layout(layout), sym, key, sym2, key2);
    }
  }

  Layout GenerateLayout(Weights const &weights,
                        bool allowBreakingConstraints) const {
    auto matchedSyms = SymKeyMap{};
    auto filter = Filter{layoutGraph, symLayers};
    auto props = GraphProperties{layoutGraph, layoutGraph.GetSource(),
                                 layoutGraph.GetSink()};
    auto adaptedWeigths = layoutGraph.ScaleDirectWeights(weights);
    for (auto layer : layersComputeOrder) {
      filter.Set(layer, SymKeyMap{matchedSyms});
      auto &&g = filter.GetFilteredGraph();
      EvaluateMaxFlow(g, props);
      if (!allowBreakingConstraints)
        CheckMaxFlow(g, props, matchedSyms);
      EvaluateMinCostMaxFlow(g, props, adaptedWeigths);
      UpdateMatchedSyms(g, props, matchedSyms);
    }
    return GenerateLayout_(matchedSyms);
  }

  Keyboard const &GetKeyboard() const { return keyboard; }
  size_t GetNSyms() const {
    assert(symLayers.size() == symBonds.size());
    assert(constraints.size() == symBonds.size());
    return symLayers.size();
  }

private:
  void CheckSymKeyNumber() const {
#ifndef NDEBUG
    auto nSyms = LayersMap<size_t>(nLayers);
    for (auto sym = Sym{}, e = symLayers.size(); sym != e; ++sym)
      ++nSyms[symLayers[sym]];
    for (auto layer = size_t{0}; layer != nLayers; ++layer)
      assert(nSyms[layer] <= keyboard.size());
#endif // !NDEBUG
  }
  static size_t EvaluateNLayers(SymLayers const &symLayers) {
    auto mentioned = LayersMap<bool>{};
    for (auto layer : symLayers) {
      if (mentioned.size() <= layer)
        mentioned.resize(layer + 1, false);
      mentioned[layer] = true;
    }
    assert(std::all_of(mentioned.begin(), mentioned.end(),
                       [](bool m) { return m; }));
    return mentioned.size();
  }
  static void RegularizeSymBonds(SymBonds &symBonds) {
    assert(*std::max_element(symBonds.begin(), symBonds.end()) <
           symBonds.size());
    for (auto &&parent : symBonds)
      while (parent != symBonds[parent])
        parent = symBonds[parent];
  }

  void GenerateGraph(SymConstraints const &constraints) {
    assert(*std::max_element(symBonds.begin(), symBonds.end()) <
           symBonds.size());
    assert(symLayers.size() == symBonds.size());
    assert(symLayers.size() == constraints.size());
#ifndef NDEBUG
    for (auto sym = Sym{}; sym != symBonds.size(); ++sym)
      assert((symBonds[sym] == sym) ==
             (symLayers[symBonds[sym]] == symLayers[sym]));
#endif // !NDEBUG
    for (auto sym = Sym{}; sym != constraints.size(); ++sym) {
      auto const &keys = constraints[sym];
      auto symV = layoutGraph.GetOrAddVertex(sym);
      for (auto const &key : keys) {
        auto keyV = layoutGraph.GetOrAddVertex(key);
        layoutGraph.AddEdge(symV, keyV);
      }
    }
  }

  // Since min-const max-flow problem is solved iteratively
  // This function provides an order of layers
  LayersMap<Layer> GetBestLayersPermutation() const {
    auto layers = LayersMap<Layer>(nLayers);
    std::iota(layers.begin(), layers.end(), 0);
    return layers;
  }

  static void EvaluateMaxFlow(FilteredGraph &g, GraphProperties &p) {
    boost::boykov_kolmogorov_max_flow(g, p.GetSource(), p.GetSink(),
                                      p.GetProperties());
  }

  void CheckMaxFlow(FilteredGraph const &g, GraphProperties const &p,
                    SymKeyMap const &matchedSyms) const {
    auto whiteSyms = std::unordered_set<Sym>{};
    auto keys = std::unordered_set<Key>{};
    for (auto v : boost::make_iterator_range(boost::vertices(g))) {
      if (!layoutGraph.IsSym(v))
        continue;
      if (p.GetColorMap()[v] != boost::default_color_type::white_color)
        continue;
      whiteSyms.insert(layoutGraph.GetSym(v));
      for (auto const &ke :
           boost::make_iterator_range(boost::out_edges(v, g))) {
        auto kv = boost::target(ke, g);
        if (layoutGraph.IsSource(kv))
          continue;
        keys.insert(layoutGraph.GetKey(kv));
      }
    }
    if (whiteSyms.size() == 0)
      return;
    assert(whiteSyms.size() > keys.size());
    auto layer = symLayers[*whiteSyms.begin()];
    throw InvalidConstraints(std::move(whiteSyms), std::move(keys),
                             SymKeyMap(matchedSyms), layer);
  }
  static void EvaluateMinCostMaxFlow(FilteredGraph &g, GraphProperties &p,
                                     Weights const &weights) {
    auto props =
        p.GetProperties().weight_map(GetWeightMap(p.GetLayoutGraph(), weights));
    boost::successive_shortest_path_nonnegative_weights(g, p.GetSource(),
                                                        p.GetSink(), props);
  }
  void UpdateMatchedSyms(FilteredGraph &g, GraphProperties &p,
                         SymKeyMap &matchedSyms) const {
    for (auto const &e : boost::make_iterator_range(boost::edges(g))) {
      if (layoutGraph.IsReverse(e))
        continue;
      auto s = layoutGraph.GetSource(e);
      auto t = layoutGraph.GetTarget(e);
      if (!layoutGraph.IsSym(s))
        continue;
      auto chosen =
          (p.GetCapacityMap()[e] - p.GetResidualCapacityMap()[e]) == 1;
      if (!chosen)
        continue;
      auto sym = layoutGraph.GetSym(s);
      auto key = layoutGraph.GetKey(t);
      matchedSyms.emplace(sym, std::move(key));
    }

    // Now, propagate all new sym matches to other layers
    for (auto sym = Sym{}, e = symBonds.size(); sym != e; ++sym)
      if (matchedSyms.contains(sym))
        matchedSyms.emplace(symBonds[sym], matchedSyms.at(sym));
    for (auto sym = Sym{}, e = matchedSyms.size(); sym != e; ++sym)
      if (matchedSyms.contains(symBonds[sym]))
        matchedSyms.emplace(sym, matchedSyms.at(symBonds[sym]));
  }

  std::vector<Key> GetNonMatchedKeys(SymKeyMap const &matchedSyms) const {
    auto availableKeys = std::vector<Key>{};
    auto matchedKeys = std::unordered_set<Key>{};
    availableKeys.reserve(keyboard.size());
    matchedKeys.reserve(matchedSyms.size());
    for (auto const &[sym, key] : matchedSyms)
      matchedKeys.insert(key);
    for (auto const &key : keyboard)
      if (!matchedKeys.contains(key))
        availableKeys.push_back(key);
    return availableKeys;
  }

  Layout GenerateLayout_(SymKeyMap const &matchedSyms) const {
    using std::swap;
    auto availableKeys = std::optional<std::vector<Key>>{};
    if (symLayers.size() != matchedSyms.size())
      availableKeys = GetNonMatchedKeys(matchedSyms);
    auto layout = Layout{};
    auto &&gen = Utility::GetRandomGenerator();
    auto rand = std::uniform_int_distribution<size_t>();
    layout.reserve(symLayers.size());
    for (auto sym = Sym{}, e = symLayers.size(); sym != e; ++sym) {
      auto layer = symLayers[sym];
      if (matchedSyms.contains(sym))
        layout.emplace_back(matchedSyms.at(sym), layer);
      else {
        auto i = rand(gen) % availableKeys.value().size();
        swap(availableKeys.value()[i], availableKeys.value().back());
        auto key = availableKeys.value().back();
        availableKeys.value().pop_back();
        layout.emplace_back(key, layer);
      }
    }
    return layout;
  }
};
} // namespace KeyboardOptimizer