#include "engine/routing_algorithms/alternative_path.hpp"
#include "engine/routing_algorithms/routing_base_mld.hpp"

#include "util/static_assert.hpp"

#include <boost/assert.hpp>

#include <algorithm>
#include <iterator>
#include <memory>
#include <type_traits>
#include <unordered_set>
#include <utility>
#include <vector>

#include <boost/iterator/function_output_iterator.hpp>

namespace osrm
{
namespace engine
{
namespace routing_algorithms
{

// Unqualified calls below are from the mld namespace.
// This alternative implementation works only for mld.
using namespace mld;

using Heap = SearchEngineData<Algorithm>::QueryHeap;
using Partition = partitioner::MultiLevelPartitionView;
using Facade = DataFacade<Algorithm>;

// Implementation details
namespace
{

struct Parameters
{
    // Alternative paths candidate via nodes are taken from overlapping search spaces.
    // Overlapping by a third guarantees us taking candidate nodes "from the middle".
    double kSearchSpaceOverlapFactor = 1.33;
    // Unpack n-times more candidate paths to run high-quality checks on.
    // Unpacking paths yields higher chance to find good alternatives but is also expensive.
    unsigned kAlternativesToUnpackFactor = 2;
    // Alternative paths length requirement (stretch).
    // At most 25% longer then the shortest path.
    double kAtMostLongerBy = 0.25;
    // Alternative paths similarity requirement (sharing).
    // At least 25% different than the shortest path.
    double kAtMostSameBy = 0.75;
    // Alternative paths are still reasonable around the via node candidate (local optimality).
    // At least optimal around 10% sub-paths around the via node candidate.
    double kAtLeastOptimalAroundViaBy = 0.1;
    // Alternative paths similarity requirement (sharing) based on cells.
    // At least 5% different than the shortest path.
    double kCellsAtMostSameBy = 0.95;
};

// Represents a via middle node where forward (from s) and backward (from t)
// search spaces overlap and the weight a path (made up of s,via and via,t) has.
struct WeightedViaNode
{
    NodeID node;
    EdgeWeight weight;
};

// Represents a complete packed path (made up of s,via and via,t)
// its total weight and the via node used to construct the path.
struct WeightedViaNodePackedPath
{
    WeightedViaNode via;
    PackedPath path;
};

// Represents a high-detail unpacked path (s, .., via, .., t)
// its total weight and the via node used to construct the path.
struct WeightedViaNodeUnpackedPath
{
    double sharing;
    WeightedViaNode via;
    UnpackedNodes nodes;
    UnpackedEdges edges;
};

// Scale the maximum allowed weight increase based on its magnitude:
//  - Shortest path 10 minutes, alternative 13 minutes => Factor of 0.30 ok
//  - Shortest path 10 hours, alternative 13 hours     => Factor of 0.30 unreasonable
double getLongerByFactorBasedOnDuration(const EdgeWeight duration)
{
    BOOST_ASSERT(duration != INVALID_EDGE_WEIGHT);

    // We only have generic weights here and no durations without unpacking.
    // We also have restricted way penalties which are huge and will screw scaling here.
    //
    // Users can pass us generic weights not based on durations; we can't do anything about
    // it here other than either generating too many or no alternatives in these cases.
    //
    // We scale the weights with a step function based on some rough guestimates, so that
    // they match tens of minutes, in the low hours, tens of hours, etc.

    // Computed using a least-squares curve fitiing on the following values:
    //
    // def func(xs, a, b, c, d):
    //       return a + b/(xs-d) + c/(xs-d)**3
    //
    // xs = np.array([5 * 60, 10 * 60, 30 * 60, 60 * 60, 3 * 60 * 60, 10 * 60 * 60])
    // ys = np.array([1.0,    0.75,    0.5,     0.4,     0.3,          0.2])
    //
    // xs_interp = np.arange(5*60, 10*60*60, 5*60)
    // ys_interp = np.interp(xs_interp, xs, ys)
    //
    // params, _ = scipy.optimize.curve_fit(func, xs_interp, ys_interp)
    //
    // The hyperbolic shape was chosen because it interpolated well between
    // the given datapoints and drops off for large durations.
    const constexpr auto a = 1.91578463e-01;
    const constexpr auto b = 1.35118442e+03;
    const constexpr auto c = 2.45437877e+09;
    const constexpr auto d = -2.07944571e+03;

    if (duration < EdgeWeight(5 * 60))
    {
        return 1.0;
    }
    else if (duration > EdgeWeight(10 * 60 * 60))
    {
        return 0.20;
    }

    // Bigger than 10 minutes but smaller than 10 hours
    BOOST_ASSERT(duration >= 5 * 60 && duration <= 10 * 60 * 60);

    return a + b / (duration - d) + c / std::pow(duration - d, 3);
}

Parameters parametersFromRequest(const PhantomNodes &phantom_node_pair)
{
    Parameters parameters;

    const auto distance = util::coordinate_calculation::haversineDistance(
        phantom_node_pair.source_phantom.location, phantom_node_pair.target_phantom.location);

    // 10km
    if (distance < 10000.)
    {
        parameters.kAlternativesToUnpackFactor = 10.0;
        parameters.kCellsAtMostSameBy = 1.0;
        parameters.kAtLeastOptimalAroundViaBy = 0.2;
        parameters.kAtMostSameBy = 0.50;
    }
    // 20km
    else if (distance < 20000.)
    {
        parameters.kAlternativesToUnpackFactor = 8.0;
        parameters.kCellsAtMostSameBy = 1.0;
        parameters.kAtLeastOptimalAroundViaBy = 0.2;
        parameters.kAtMostSameBy = 0.60;
    }
    // 50km
    else if (distance < 50000.)
    {
        parameters.kAlternativesToUnpackFactor = 6.0;
        parameters.kCellsAtMostSameBy = 0.95;
        parameters.kAtMostSameBy = 0.65;
    }
    // 100km
    else if (distance < 100000.)
    {
        parameters.kAlternativesToUnpackFactor = 4.0;
        parameters.kCellsAtMostSameBy = 0.95;
        parameters.kAtMostSameBy = 0.70;
    }

    return parameters;
}

// Filters candidates which are on not unique.
// Returns an iterator to the uniquified range's new end.
// Note: mutates the range in-place invalidating iterators.
template <typename RandIt> RandIt filterViaCandidatesByUniqueNodeIds(RandIt first, RandIt last)
{
    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, WeightedViaNode>();

    std::sort(first, last, [](auto lhs, auto rhs) { return lhs.node < rhs.node; });
    return std::unique(first, last, [](auto lhs, auto rhs) { return lhs.node == rhs.node; });
}

// Filters candidates which are on un-important roads.
// Returns an iterator to the filtered range's new end.
template <typename RandIt>
RandIt filterViaCandidatesByRoadImportance(RandIt first, RandIt last, const Facade &facade)
{
    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, WeightedViaNode>();

    // Todo: the idea here is to filter out alternatives where the via candidate is not on a
    // high-priority road. We should experiment if this is really needed or if the boundary
    // nodes the mld search space gives us already provides us with reasonable via candidates.
    //
    // Implementation: we already have `RoadClassification` from guidance. We need to serialize
    // it to disk and then in the facades read it in again providing `IsImportantRoad(NodeID)`.
    // Note: serialize out bit vector keyed by node id with 0/1 <=> unimportant/important.
    (void)first;
    (void)last;
    (void)facade;

    return last;
}

// Filters candidates with much higher weight than the primary route. Mutates range in-place.
// Returns an iterator to the filtered range's new end.
template <typename RandIt>
RandIt filterViaCandidatesByStretch(RandIt first,
                                    RandIt last,
                                    const EdgeWeight weight,
                                    const Parameters &parameters)
{
    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, WeightedViaNode>();

    // Assumes weight roughly corresponds to duration-ish. If this is not the case e.g.
    // because users are setting weight to be distance in the profiles, then we might
    // either generate more candidates than we have to or not enough. But is okay.
    const auto stretch_weight_limit = (1. + parameters.kAtMostLongerBy) * weight;

    const auto over_weight_limit = [=](const auto via) {
        return via.weight > stretch_weight_limit;
    };

    return std::remove_if(first, last, over_weight_limit);
}

// Filters candidates that are on the path. Mutates range in-place.
// Returns an iterator to the filtered range's new end.
template <typename RandIt>
RandIt
filterViaCandidatesByViaNotOnPath(const WeightedViaNodePackedPath &path, RandIt first, RandIt last)
{
    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, WeightedViaNode>();

    if (path.path.empty())
        return last;

    std::unordered_set<NodeID> nodes;
    nodes.reserve(path.path.size() + 1);

    nodes.insert(std::get<0>(path.path.front()));
    for (const auto &edge : path.path)
        nodes.insert(std::get<1>(edge));

    const auto via_on_path = [&](const auto via) { return nodes.count(via.node) > 0; };

    return std::remove_if(first, last, via_on_path);
}

// Filters packed paths with similar cells between each other. Mutates range in-place.
// Returns an iterator to the filtered range's new end.
template <typename RandIt>
RandIt filterPackedPathsByCellSharing(RandIt first,
                                      RandIt last,
                                      const Partition &partition,
                                      const Parameters &parameters)
{
    // In this case we don't need to calculate sharing, because it would not filter anything
    if (parameters.kCellsAtMostSameBy >= 1.0)
        return last;

    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, WeightedViaNodePackedPath>();

    // Todo: we could scale cell sharing with edge weights. Also basing sharing on
    // cells could be problematic e.g. think of parallel ways in grid cities.

    const auto size = static_cast<std::size_t>(last - first);

    if (size < 2)
        return last;

    const auto number_of_levels = partition.GetNumberOfLevels();
    (void)number_of_levels;
    BOOST_ASSERT(number_of_levels >= 1);

    // Todo: sharing could be a linear combination based on level and sharing on each level.
    // Experimental evaluation shows using the lowest level works surprisingly well already.
    const auto level = 1;
    const auto get_cell = [&](auto node) { return partition.GetCell(level, node); };

    const auto shortest_path = *first;

    if (shortest_path.path.empty())
        return last;

    std::unordered_set<CellID> cells;
    cells.reserve(size * (shortest_path.path.size() + 1) * (1 + parameters.kAtMostLongerBy));

    cells.insert(get_cell(std::get<0>(shortest_path.path.front())));
    for (const auto &edge : shortest_path.path)
        cells.insert(get_cell(std::get<1>(edge)));

    const auto over_sharing_limit = [&](const auto &packed) {
        if (packed.path.empty())
        { // don't remove routes with single-node (empty) path
            return false;
        }

        const auto not_seen = [&](const PackedEdge edge) {
            const auto source_cell = get_cell(std::get<0>(edge));
            const auto target_cell = get_cell(std::get<1>(edge));
            return cells.count(source_cell) < 1 && cells.count(target_cell) < 1;
        };

        const auto different = std::count_if(begin(packed.path), end(packed.path), not_seen);

        const auto difference = different / static_cast<double>(packed.path.size() + 1);
        BOOST_ASSERT(difference >= 0.);
        BOOST_ASSERT(difference <= 1.);

        const auto sharing = 1. - difference;

        if (sharing > parameters.kCellsAtMostSameBy)
        {
            return true;
        }
        else
        {
            cells.insert(get_cell(std::get<0>(packed.path.front())));
            for (const auto &edge : packed.path)
                cells.insert(get_cell(std::get<1>(edge)));

            return false;
        }
    };

    return std::remove_if(first + 1, last, over_sharing_limit);
}

// Filters packed paths based on local optimality. Mutates range in-place.
// Returns an iterator to the filtered range's new end.
template <typename RandIt>
RandIt filterPackedPathsByLocalOptimality(const WeightedViaNodePackedPath &path,
                                          const Heap &forward_heap,
                                          const Heap &reverse_heap,
                                          RandIt first,
                                          RandIt last,
                                          const Parameters &parameters)
{
    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, WeightedViaNodePackedPath>();

    if (path.path.empty())
        return last;

    // Check sub-path optimality on alternative path crossing the via node candidate.
    //
    //   s - - - v - - - t  our packed path made up of (from, to) edges
    //        |--|--|       sub-paths "left" and "right" of v based on threshold
    //        f  v  l       nodes in [v,f] and in [v, l] have to match predecessor in heaps
    //
    // Todo: this approach is efficient but works on packed paths only. Do we need to do a
    // thorough check on the unpacked paths instead? Or do we even need to introduce two
    // new thread-local heaps for the mld SearchEngineData and do proper s-t routing here?

    BOOST_ASSERT(path.via.weight != INVALID_EDGE_WEIGHT);

    // node == parent_in_main_heap(parent_in_side_heap(v)) -> plateaux at `node`
    const auto has_plateaux_at_node = [&](const NodeID node, const Heap &fst, const Heap &snd) {
        BOOST_ASSERT(fst.WasInserted(node));
        auto const parent = fst.GetData(node).parent;
        return snd.WasInserted(parent) && snd.GetData(parent).parent == node;
    };

    // A plateaux is defined as a segment in which the search tree from s and the search
    // tree from t overlap. An edge is part of such a plateaux around `v` if:
    // v == parent_in_reverse_search(parent_in_forward_search(v)).
    // Here we calculate the last node on the plateaux in either direction.
    const auto plateaux_end = [&](NodeID node, const Heap &fst, const Heap &snd) {
        BOOST_ASSERT(node != SPECIAL_NODEID);
        BOOST_ASSERT(fst.WasInserted(node));
        BOOST_ASSERT(snd.WasInserted(node));

        // Check plateaux edges towards the target. Terminates at the source / target
        // at the latest, since parent(target)==target for the reverse heap and
        // parent(target) != target in the forward heap (and vice versa).
        while (node != fst.GetData(node).parent && has_plateaux_at_node(node, fst, snd))
            node = fst.GetData(node).parent;

        return node;
    };

    const auto is_not_locally_optimal = [&](const auto &packed) {
        BOOST_ASSERT(packed.via.node != path.via.node);
        BOOST_ASSERT(packed.via.weight != INVALID_EDGE_WEIGHT);
        BOOST_ASSERT(packed.via.node != SPECIAL_NODEID);

        if (packed.path.empty())
        { // the edge case when packed.via.node is both source and target node
            return false;
        }

        const NodeID via = packed.via.node;

        // Plateaux iff via == parent_in_reverse_search(parent_in_forward_search(via))
        //
        // Forward search starts from s, reverse search starts from t:
        //  - parent_in_forward_search(via) = a
        //  - parent_in_reverse_search(a) = b  != via and therefore not local optimal
        //
        //        via
        //      .'   '.
        // s - a - - - b - t
        //
        // Care needs to be taken for the edge case where the via node is on the border
        // of the search spaces and therefore parent pointers may not be valid in heaps.
        // In these cases we know we can't have local optimality around the via already.

        const auto first_on_plateaux = plateaux_end(via, forward_heap, reverse_heap);
        const auto last_on_plateaux = plateaux_end(via, reverse_heap, forward_heap);

        //        fop - - via - - lop
        //      .'                    '.
        // s - a - - - - - - - - - - - - b - t
        //
        // Lenth of plateaux is given by the weight vetween fop and via as well as via and lop.
        const auto plateaux_length =
            forward_heap.GetKey(last_on_plateaux) - forward_heap.GetKey(first_on_plateaux);

        // Find a/b as the first location where packed and path differ
        const auto a = std::get<0>(*std::mismatch(packed.path.begin(), //
                                                  packed.path.end(),
                                                  path.path.begin(),
                                                  path.path.end())
                                        .first);
        const auto b = std::get<1>(*std::mismatch(packed.path.rbegin(), //
                                                  packed.path.rend(),
                                                  path.path.rbegin(),
                                                  path.path.rend())
                                        .first);

        BOOST_ASSERT(forward_heap.WasInserted(a));
        BOOST_ASSERT(reverse_heap.WasInserted(b));
        const auto detour_length = forward_heap.GetKey(via) - forward_heap.GetKey(a) +
                                   reverse_heap.GetKey(via) - reverse_heap.GetKey(b);

        return plateaux_length < parameters.kAtLeastOptimalAroundViaBy * detour_length;
    };

    return std::remove_if(first, last, is_not_locally_optimal);
}

// Filters unpacked paths compared to all other paths. Mutates range in-place.
// Returns an iterator to the filtered range's new end.
template <typename RandIt>
RandIt filterUnpackedPathsBySharing(RandIt first,
                                    RandIt last,
                                    const Facade &facade,
                                    const Parameters &parameters)
{
    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, WeightedViaNodeUnpackedPath>();

    const auto size = static_cast<std::size_t>(last - first);

    if (size < 2)
        return last;

    const auto &shortest_path = *first;

    if (shortest_path.edges.empty())
        return last;

    std::unordered_set<NodeID> nodes;
    nodes.reserve(size * shortest_path.nodes.size() * (1.25));

    nodes.insert(begin(shortest_path.nodes), end(shortest_path.nodes));

    const auto over_sharing_limit = [&](auto &unpacked) {
        if (unpacked.edges.empty())
        { // don't remove routes with single-node (empty) path
            return false;
        }

        EdgeWeight total_duration = 0;
        const auto add_if_seen = [&](const EdgeWeight duration, const NodeID node) {
            auto node_duration = facade.GetNodeDuration(node);
            total_duration += node_duration;
            if (nodes.count(node) > 0)
            {
                return duration + node_duration;
            }
            return duration;
        };

        const auto shared_duration = std::accumulate(
            begin(unpacked.nodes), end(unpacked.nodes), EdgeDuration{0}, add_if_seen);

        unpacked.sharing = shared_duration / static_cast<double>(total_duration);
        BOOST_ASSERT(unpacked.sharing >= 0.);
        BOOST_ASSERT(unpacked.sharing <= 1.);

        if (unpacked.sharing > parameters.kAtMostSameBy)
        {
            return true;
        }
        else
        {
            nodes.insert(begin(unpacked.nodes), end(unpacked.nodes));
            return false;
        }
    };

    auto end = std::remove_if(first + 1, last, over_sharing_limit);
    std::sort(
        first + 1, end, [](const auto &lhs, const auto &rhs) { return lhs.sharing < rhs.sharing; });
    return end;
}

// Filters annotated routes by stretch based on duration. Mutates range in-place.
// Returns an iterator to the filtered range's new end.
template <typename RandIt>
RandIt filterAnnotatedRoutesByStretch(RandIt first,
                                      RandIt last,
                                      const InternalRouteResult &shortest_route,
                                      const Parameters &parameters)
{
    util::static_assert_iter_category<RandIt, std::random_access_iterator_tag>();
    util::static_assert_iter_value<RandIt, InternalRouteResult>();

    BOOST_ASSERT(shortest_route.is_valid());

    const auto shortest_route_duration = shortest_route.duration();
    const auto stretch_duration_limit = (1. + parameters.kAtMostLongerBy) * shortest_route_duration;

    const auto over_duration_limit = [=](const auto &route) {
        return route.duration() > stretch_duration_limit;
    };

    return std::remove_if(first, last, over_duration_limit);
}

// Unpacks a range of WeightedViaNodePackedPaths into a range of WeightedViaNodeUnpackedPaths.
// Note: destroys search engine heaps for recursive unpacking. Extract heap data you need before.
template <typename InputIt, typename OutIt>
void unpackPackedPaths(InputIt first,
                       InputIt last,
                       OutIt out,
                       SearchEngineData<Algorithm> &search_engine_data,
                       const Facade &facade,
                       const PhantomNodes &phantom_node_pair)
{
    util::static_assert_iter_category<InputIt, std::input_iterator_tag>();
    util::static_assert_iter_category<OutIt, std::output_iterator_tag>();
    util::static_assert_iter_value<InputIt, WeightedViaNodePackedPath>();

    const Partition &partition = facade.GetMultiLevelPartition();

    Heap &forward_heap = *search_engine_data.forward_heap_1;
    Heap &reverse_heap = *search_engine_data.reverse_heap_1;

    for (auto it = first; it != last; ++it, ++out)
    {
        const auto packed_path_weight = it->via.weight;
        const auto packed_path_via = it->via.node;

        const auto &packed_path = it->path;

        //
        // Todo: dup. code with mld::search except for level entry: we run a slight mld::search
        //       adaption here and then dispatch to mld::search for recursively descending down.
        //

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

        // Beware the edge case when start, via, end are all the same.
        // In this case we return a single node, no edges. We also don't unpack.
        if (packed_path.empty())
        {
            const auto source_node = packed_path_via;
            unpacked_nodes.push_back(source_node);
        }
        else
        {
            const auto source_node = std::get<0>(packed_path.front());
            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, phantom_node_pair); // XXX
                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});

                BOOST_ASSERT(!facade.ExcludeNode(source));
                BOOST_ASSERT(!facade.ExcludeNode(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(search_engine_data,
                                                                                facade,
                                                                                forward_heap,
                                                                                reverse_heap,
                                                                                DO_NOT_FORCE_LOOPS,
                                                                                DO_NOT_FORCE_LOOPS,
                                                                                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());
            }
        }

        WeightedViaNodeUnpackedPath unpacked_path{
            0.0,
            WeightedViaNode{packed_path_via, packed_path_weight},
            std::move(unpacked_nodes),
            std::move(unpacked_edges)};

        out = std::move(unpacked_path);
    }
}

// Generates via candidate nodes from the overlap of the two search spaces from s and t.
// Returns via node candidates in no particular order; they're not guaranteed to be unique.
// Note: heaps are modified in-place, after the function returns they're valid and can be used.
inline std::vector<WeightedViaNode>
makeCandidateVias(SearchEngineData<Algorithm> &search_engine_data,
                  const Facade &facade,
                  const PhantomNodes &phantom_node_pair,
                  const Parameters &parameters)
{
    Heap &forward_heap = *search_engine_data.forward_heap_1;
    Heap &reverse_heap = *search_engine_data.reverse_heap_1;

    insertNodesInHeaps(forward_heap, reverse_heap, phantom_node_pair);
    if (forward_heap.Empty() || reverse_heap.Empty())
    {
        return {};
    }

    // The single via node in the shortest paths s,via and via,t sub-paths and
    // the weight for the shortest path s,t we return and compare alternatives to.
    EdgeWeight shortest_path_weight = INVALID_EDGE_WEIGHT;

    // The current via node during search spaces overlap stepping and an artificial
    // weight (overlap factor * shortest path weight) we use as a termination criteria.
    NodeID overlap_via = SPECIAL_NODEID;
    EdgeWeight overlap_weight = INVALID_EDGE_WEIGHT;

    // All via nodes in the overlapping search space (except the shortest path via node).
    // Will be filtered and ranked and then used for s,via and via,t alternative paths.
    std::vector<WeightedViaNode> candidate_vias;

    // The logic below is a bit weird - here's why: we want to re-use the MLD routingStep for
    // stepping our search space from s and from t. We don't know how far to overlap until we have
    // the shortest path. Once we have the shortest path we can use its weight to terminate when
    // we're over factor * weight. We have to set the weight for routingStep to INVALID_EDGE_WEIGHT
    // so that stepping will continue even after we reached the shortest path upper bound.

    EdgeWeight forward_heap_min = forward_heap.MinKey();
    EdgeWeight reverse_heap_min = reverse_heap.MinKey();

    while (forward_heap.Size() + reverse_heap.Size() > 0)
    {
        if (shortest_path_weight != INVALID_EDGE_WEIGHT)
            overlap_weight = shortest_path_weight * parameters.kSearchSpaceOverlapFactor;

        // Termination criteria - when we have a shortest path this will guarantee for our overlap.
        const bool keep_going = forward_heap_min + reverse_heap_min < overlap_weight;

        if (!keep_going)
            break;

        // Force forward step to not break early when we reached the middle, continue for overlap.
        // Note: only invalidate the via node, the weight upper bound is still correct!
        overlap_via = SPECIAL_NODEID;

        if (!forward_heap.Empty())
        {
            routingStep<FORWARD_DIRECTION>(facade,
                                           forward_heap,
                                           reverse_heap,
                                           overlap_via,
                                           overlap_weight,
                                           DO_NOT_FORCE_LOOPS,
                                           DO_NOT_FORCE_LOOPS,
                                           phantom_node_pair);

            if (!forward_heap.Empty())
                forward_heap_min = forward_heap.MinKey();

            if (overlap_weight != INVALID_EDGE_WEIGHT && overlap_via != SPECIAL_NODEID)
            {
                candidate_vias.push_back(WeightedViaNode{overlap_via, overlap_weight});
            }
        }

        // Adjusting upper bound for forward search
        shortest_path_weight = std::min(shortest_path_weight, overlap_weight);
        // Force reverse step to not break early when we reached the middle, continue for overlap.
        // Note: only invalidate the via node, the weight upper bound is still correct!
        overlap_via = SPECIAL_NODEID;

        if (!reverse_heap.Empty())
        {
            routingStep<REVERSE_DIRECTION>(facade,
                                           reverse_heap,
                                           forward_heap,
                                           overlap_via,
                                           overlap_weight,
                                           DO_NOT_FORCE_LOOPS,
                                           DO_NOT_FORCE_LOOPS,
                                           phantom_node_pair);

            if (!reverse_heap.Empty())
                reverse_heap_min = reverse_heap.MinKey();

            if (overlap_weight != INVALID_EDGE_WEIGHT && overlap_via != SPECIAL_NODEID)
            {
                candidate_vias.push_back(WeightedViaNode{overlap_via, overlap_weight});
            }
        }

        // Adjusting upper bound for reverse search
        shortest_path_weight = std::min(shortest_path_weight, overlap_weight);
    }

    return candidate_vias;
}

} // namespace

// Alternative Routes for MLD.
//
// Start search from s and continue "for a while" when middle was found. Save vertices.
// Start search from t and continue "for a while" when middle was found. Save vertices.
// Intersect both vertex sets: these are the candidate vertices.
// For all candidate vertices c a (potentially arbitrarily bad) alternative route is (s, c, t).
// Apply heuristic to evaluate alternative route based on stretch, overlap, how reasonable it is.
//
// For MLD specifically we can pull off some tricks to make evaluating alternatives fast:
//   Only consider (s, c, t) with c border vertex: re-use MLD search steps.
//   Add meta data to border vertices: consider (s, c, t) only when c is e.g. on a highway.
//   Prune based on vertex cell id
//
// https://github.com/Project-OSRM/osrm-backend/issues/3905
InternalManyRoutesResult alternativePathSearch(SearchEngineData<Algorithm> &search_engine_data,
                                               const Facade &facade,
                                               const PhantomNodes &phantom_node_pair,
                                               unsigned number_of_alternatives)
{
    Parameters parameters = parametersFromRequest(phantom_node_pair);

    const auto max_number_of_alternatives = number_of_alternatives;
    const auto max_number_of_alternatives_to_unpack =
        parameters.kAlternativesToUnpackFactor * max_number_of_alternatives;
    BOOST_ASSERT(max_number_of_alternatives > 0);
    BOOST_ASSERT(max_number_of_alternatives_to_unpack >= max_number_of_alternatives);

    const Partition &partition = facade.GetMultiLevelPartition();

    // Prepare heaps for usage below. The searches will modify them in-place.
    search_engine_data.InitializeOrClearFirstThreadLocalStorage(facade.GetNumberOfNodes(),
                                                                facade.GetMaxBorderNodeID() + 1);

    Heap &forward_heap = *search_engine_data.forward_heap_1;
    Heap &reverse_heap = *search_engine_data.reverse_heap_1;

    // Do forward and backward search, save search space overlap as via candidates.
    auto candidate_vias =
        makeCandidateVias(search_engine_data, facade, phantom_node_pair, parameters);

    const auto by_weight = [](const auto &lhs, const auto &rhs) { return lhs.weight < rhs.weight; };
    auto shortest_path_via_it =
        std::min_element(begin(candidate_vias), end(candidate_vias), by_weight);

    const auto has_shortest_path = shortest_path_via_it != end(candidate_vias) &&
                                   shortest_path_via_it->node != SPECIAL_NODEID &&
                                   shortest_path_via_it->weight != INVALID_EDGE_WEIGHT;

    // Care needs to be taken to meet the call sites post condition.
    // We must return at least one route, even if it's an invalid one.
    if (!has_shortest_path)
    {
        InternalRouteResult invalid;
        invalid.shortest_path_weight = INVALID_EDGE_WEIGHT;
        invalid.segment_end_coordinates = {phantom_node_pair};
        return invalid;
    }

    NodeID shortest_path_via = shortest_path_via_it->node;
    EdgeWeight shortest_path_weight = shortest_path_via_it->weight;

    const double duration_estimation = shortest_path_weight / facade.GetWeightMultiplier();
    parameters.kAtMostLongerBy = getLongerByFactorBasedOnDuration(duration_estimation);

    // Filters via candidate nodes with heuristics

    // Note: filter pipeline below only makes range smaller; no need to erase items
    // from the vector when we can mutate in-place and for filtering adjust iterators.
    auto it = end(candidate_vias);

    it = filterViaCandidatesByUniqueNodeIds(begin(candidate_vias), it);
    it = filterViaCandidatesByRoadImportance(begin(candidate_vias), it, facade);
    it = filterViaCandidatesByStretch(begin(candidate_vias), it, shortest_path_weight, parameters);

    // Pre-rank by weight; sharing filtering below then discards by similarity.
    std::sort(begin(candidate_vias), it, [](const auto lhs, const auto rhs) {
        return lhs.weight < rhs.weight;
    });

    // Filtered and ranked candidate range
    const auto candidate_vias_first = begin(candidate_vias);
    const auto candidate_vias_last = it;
    const auto number_of_candidate_vias = candidate_vias_last - candidate_vias_first;

    // Reconstruct packed paths from the heaps.
    // The recursive path unpacking below destructs heaps.
    // We need to save all packed paths from the heaps upfront.

    const auto extract_packed_path_from_heaps = [&](WeightedViaNode via) {
        auto packed_path = retrievePackedPathFromHeap(forward_heap, reverse_heap, via.node);

        return WeightedViaNodePackedPath{std::move(via), std::move(packed_path)};
    };

    std::vector<WeightedViaNodePackedPath> weighted_packed_paths;
    weighted_packed_paths.reserve(1 + number_of_candidate_vias);

    // Store shortest path
    WeightedViaNode shortest_path_weighted_via{shortest_path_via, shortest_path_weight};
    weighted_packed_paths.push_back(extract_packed_path_from_heaps(shortest_path_weighted_via));

    const auto last_filtered = filterViaCandidatesByViaNotOnPath(
        weighted_packed_paths[0], candidate_vias_first + 1, candidate_vias_last);

    // Store all alternative packed paths (if there are any).
    auto into = std::back_inserter(weighted_packed_paths);
    std::transform(begin(candidate_vias) + 1, last_filtered, into, extract_packed_path_from_heaps);

    // Filter packed paths with heuristics

    auto alternative_paths_last = end(weighted_packed_paths);

    alternative_paths_last = filterPackedPathsByLocalOptimality(weighted_packed_paths[0],
                                                                forward_heap, // paths for s, via
                                                                reverse_heap, // paths for via, t
                                                                begin(weighted_packed_paths) + 1,
                                                                alternative_paths_last,
                                                                parameters);
    alternative_paths_last = filterPackedPathsByCellSharing(
        begin(weighted_packed_paths), alternative_paths_last, partition, parameters);

    BOOST_ASSERT(weighted_packed_paths.size() >= 1);

    const auto number_of_filtered_alternative_paths = std::min(
        static_cast<std::size_t>(max_number_of_alternatives_to_unpack),
        static_cast<std::size_t>(alternative_paths_last - (begin(weighted_packed_paths) + 1)));

    const auto paths_first = begin(weighted_packed_paths);
    const auto paths_last = begin(weighted_packed_paths) + 1 + number_of_filtered_alternative_paths;
    const auto number_of_packed_paths = paths_last - paths_first;

    std::vector<WeightedViaNodeUnpackedPath> unpacked_paths;
    unpacked_paths.reserve(number_of_packed_paths);

    // Note: re-uses (read: destroys) heaps; we don't need them from here on anyway.
    unpackPackedPaths(paths_first,
                      paths_last,
                      std::back_inserter(unpacked_paths),
                      search_engine_data,
                      facade,
                      phantom_node_pair);

    //
    // Filter and rank a second time. This time instead of being fast and doing
    // heuristics on the packed path only we now have the detailed unpacked path.
    //

    auto unpacked_paths_last = end(unpacked_paths);

    unpacked_paths_last = filterUnpackedPathsBySharing(
        begin(unpacked_paths), end(unpacked_paths), facade, parameters);

    const auto unpacked_paths_first = begin(unpacked_paths);
    const auto number_of_unpacked_paths =
        std::min(static_cast<std::size_t>(max_number_of_alternatives) + 1,
                 static_cast<std::size_t>(unpacked_paths_last - unpacked_paths_first));
    BOOST_ASSERT(number_of_unpacked_paths >= 1);
    unpacked_paths_last = unpacked_paths_first + number_of_unpacked_paths;

    //
    // Annotate the unpacked path and transform to proper internal route result.
    //

    std::vector<InternalRouteResult> routes;
    routes.reserve(number_of_unpacked_paths);

    const auto unpacked_path_to_route = [&](const WeightedViaNodeUnpackedPath &path) {
        return extractRoute(facade, path.via.weight, phantom_node_pair, path.nodes, path.edges);
    };

    std::transform(unpacked_paths_first,
                   unpacked_paths_last,
                   std::back_inserter(routes),
                   unpacked_path_to_route);

    BOOST_ASSERT(routes.size() >= 1);

    // Only now that we annotated the routes do we have their actual duration.

    const auto routes_first = begin(routes);
    auto routes_last = end(routes);

    if (routes.size() > 1)
    {
        parameters.kAtMostLongerBy = getLongerByFactorBasedOnDuration(routes_first->duration());
        routes_last = filterAnnotatedRoutesByStretch(
            routes_first + 1, routes_last, *routes_first, parameters);
        routes.erase(routes_last, end(routes));
    }

    BOOST_ASSERT(routes.size() >= 1);
    return InternalManyRoutesResult{std::move(routes)};
}

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


#include "DeterministicNoiseSimulator.hpp"
#include "StochasticNoiseSimulator.hpp"
#include "nlohmann/json.hpp"

#include <boost/program_options.hpp>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <memory>
#include <string>

namespace nl = nlohmann;

int main(int argc, char** argv) {
    namespace po = boost::program_options;
    unsigned long long seed;

    po::options_description description("JKQ DDSIM by https://iic.jku.at/eda/ -- Allowed options");
    // clang-format off
    description.add_options()
        ("help,h", "produce help message")
        ("seed", po::value<unsigned long long>(&seed)->default_value(0), "seed for random number generator (default zero is possibly directly used as seed!)")
        ("pm", "print measurements")
        ("ps", "print simulation stats (applied gates, sim. time, and maximal size of the DD)")
        ("verbose", "Causes some simulators to print additional information to STDERR")

        ("simulate_file", po::value<std::string>(), "simulate a quantum circuit given by file (detection by the file extension)")
        ("step_fidelity", po::value<double>()->default_value(1.0), "target fidelity for each approximation run (>=1 = disable approximation)")
        ("steps", po::value<unsigned int>()->default_value(1), "number of approximation steps")

        ("noise_effects", po::value<std::string>()->default_value("APD"), "Noise effects (A (=amplitude damping),D (=depolarization),P (=phase flip)) in the form of a character string describing the noise effects (default=\"APD\")")
        ("noise_prob", po::value<double>()->default_value(0.001), "Probability for applying noise (default=0.001)")
        ("confidence", po::value<double>()->default_value(0.05), "Confidence in the error bound of the stochastic simulation (default= 0.05)")
        ("error_bound", po::value<double>()->default_value(0.1), "Error bound of the stochastic simulation (default=0.1)")
        ("stoch_runs", po::value<long>()->default_value(0), "Number of stochastic runs. When the value is 0 the value is calculated using the confidence, error_bound and number of tracked properties. When the value is -1, the deterministic simulator is started. When the value is -2 the old method for deterministic noise application is used) (default = 0)")
        ("properties", po::value<std::string>()->default_value("-3-1000"), R"(Comma separated list of tracked properties. Note that -1 is the fidelity and "-" can be used to specify a range.  (default="-3-1000"))")
    ;
    // clang-format on
    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, description), vm);

        if (vm.count("help")) {
            std::cout << description;
            return 0;
        }
        po::notify(vm);
    } catch (const po::error& e) {
        std::cerr << "[ERROR] " << e.what() << "! Try option '--help' for available commandline options.\n";
        std::exit(1);
    }

    std::unique_ptr<qc::QuantumComputation> quantumComputation;

    if (vm.count("simulate_file")) {
        const std::string fname = vm["simulate_file"].as<std::string>();
        quantumComputation      = std::make_unique<qc::QuantumComputation>(fname);
    } else {
        std::cerr << "Did not find anything to simulate. See help below.\n"
                  << description << "\n";
        std::exit(1);
    }

    if (quantumComputation && quantumComputation->getNqubits() > 100) {
        std::clog << "[WARNING] Quantum computation contains quite many qubits. You're jumping into the deep end.\n";
    }

    if (vm["stoch_runs"].as<long>() >= 0) {
        std::unique_ptr<StochasticNoiseSimulator> ddsim = std::make_unique<StochasticNoiseSimulator>(quantumComputation,
                                                                                vm["steps"].as<unsigned int>(),
                                                                                vm["step_fidelity"].as<double>(),
                                                                                seed);

        ddsim->setNoiseEffects(vm["noise_effects"].as<std::string>());
        ddsim->setAmplitudeDampingProbability(vm["noise_prob"].as<double>());
        ddsim->stoch_confidence = vm["confidence"].as<double>();
        ddsim->setRecordedProperties(vm["properties"].as<std::string>());
        ddsim->stoch_error_margin = vm["error_bound"].as<double>();
        ddsim->stochastic_runs    = vm["stoch_runs"].as<long>();

        auto t1 = std::chrono::steady_clock::now();

        const std::map<std::string, double> measurement_results = ddsim->StochSimulate();

        auto t2 = std::chrono::steady_clock::now();

        std::chrono::duration<float> duration_simulation = t2 - t1;

        nl::json output_obj;

        if (vm.count("ps")) {
            output_obj["statistics"] = {
                    {"simulation_time", duration_simulation.count()},
                    {"benchmark", ddsim->getName()},
                    {"stoch_runs", ddsim->stochastic_runs},
                    {"threads", ddsim->max_instances},
                    {"n_qubits", +ddsim->getNumberOfQubits()},
                    {"applied_gates", ddsim->getNumberOfOps()},
                    {"max_nodes", ddsim->getMaxNodeCount()},
                    {"seed", ddsim->getSeed()},
            };

            for (const auto& item: ddsim->AdditionalStatistics()) {
                output_obj["statistics"][item.first] = item.second;
            }
        }

        if (vm.count("pm")) {
            output_obj["measurement_results"] = measurement_results;
        }
        std::cout << std::setw(2) << output_obj << std::endl;
    } else if (vm["stoch_runs"].as<long>() < 0) {
        std::unique_ptr<DeterministicNoiseSimulator> ddsim = std::make_unique<DeterministicNoiseSimulator>(quantumComputation, seed);

        if (vm["stoch_runs"].as<long>() == -2) {
            ddsim->noiseApplicationWithKrausMatrices = true;
        }

        ddsim->setNoiseEffects(vm["noise_effects"].as<std::string>());
        ddsim->setAmplitudeDampingProbability(vm["noise_prob"].as<double>());

        auto t1 = std::chrono::steady_clock::now();

        const std::map<std::string, double> measurement_results = ddsim->DeterministicSimulate();

        auto t2 = std::chrono::steady_clock::now();

        std::chrono::duration<float> duration_simulation = t2 - t1;

        nl::json output_obj;

        if (vm.count("ps")) {
            output_obj["statistics"] = {
                    {"simulation_time", duration_simulation.count()},
                    {"benchmark", ddsim->getName()},
                    {"n_qubits", +ddsim->getNumberOfQubits()},
                    {"applied_gates", ddsim->getNumberOfOps()},
                    {"max_nodes", ddsim->getMaxNodeCount()},
                    {"seed", ddsim->getSeed()},
                    {"active_nodes", ddsim->getActiveNodeCount()},
            };

            for (const auto& item: ddsim->AdditionalStatistics()) {
                output_obj["statistics"][item.first] = item.second;
            }
        }

        if (vm.count("pm")) {
            output_obj["measurement_results"] = measurement_results;
        }
        std::cout << std::setw(2) << output_obj << std::endl;
    }
}


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

// authors: Hugo Théveniaut and Fabien Alet

#ifndef NETKET_CUSTOM_LOCAL_KERNEL_HPP
#define NETKET_CUSTOM_LOCAL_KERNEL_HPP

#include <Eigen/Core>
#include <set>
#include "Operator/local_operator.hpp"

#include "Machine/abstract_machine.hpp"
#include "Utils/exceptions.hpp"
#include "Utils/messages.hpp"
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"

namespace netket {

// Metropolis sampling using custom moves provided by user
class CustomLocalKernel {
  LocalOperator move_operators_;
  std::vector<double> operatorsweights_;

  std::vector<std::vector<int>> tochange_;
  std::vector<std::vector<double>> newconfs_;
  std::vector<Complex> mel_;

  Index nstates_;

  std::vector<double> localstates_;

  std::discrete_distribution<Index> disc_dist_;

 public:
  CustomLocalKernel(const AbstractMachine &psi,
                    const LocalOperator &move_operators,
                    const std::vector<double> &move_weights = {});

  CustomLocalKernel(const LocalOperator &move_operators,
                    const std::vector<double> &move_weights = {});

  void operator()(Eigen::Ref<const RowMatrix<double>> v,
                  Eigen::Ref<RowMatrix<double>> vnew,

                  Eigen::Ref<Eigen::ArrayXd> log_acceptance_correction);

 private:
  void Init(const AbstractMachine &psi,
            const std::vector<double> &move_weights);
  void Init(const AbstractHilbert &hilb,
            const std::vector<double> &move_weights);
  void CheckMoveOperators(const LocalOperator &move_operators);
};
}  // namespace netket

#endif


#include <boost/numeric/odeint/integrate/integrate_const.hpp>


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

  @copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ALGORITHM_FIND_IF_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_FIND_IF_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/detail/is_aligned.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/pack.hpp>
#include <type_traits>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns an iterator to the first element in the range [first,last)
    for which std::forward<Pred>(pred) returns true. If no such element is found, the function returns last.

    @param first  Beginning of the range of elements
    @param last   End of the range of elements
    @param std::forward<Pred>(pred)   Predicate to apply to each element in the range.

    @par Requirement

      - @c first, @c last and @c out must be pointers to Vectorizable type.

    @par Example:

      @snippet find_if.cpp find_if

    @par possible output:

      @snippet find_if.txt find_if

  **/
  template<typename T, typename Pred>
  T const* find_if(T const* first, T const* last, Pred const& pred)
  {
    auto pr = segmented_aligned_range(first,last);
    auto safe_pred = [&pred](T x){ return pred(x); };

    auto r = std::find_if(pr.head.begin(), pr.head.end(), safe_pred);
    if (r != pr.head.end()) return r;

    auto rv = std::find_if( pr.body.begin(), pr.body.end()
                          , [&pred](const pack<T>& x){ return any(pred(x)); }
                          );

    if (rv != pr.body.end())
    {
      auto z = std::find_if(rv->begin(), rv->end(), safe_pred);
      return  pr.head.end()                                             //   base
            + std::distance(pr.body.begin(), rv)*pack<T>::static_size   // + jump to pack
            + std::distance(rv->begin(), z);                            // + jump to element
    }

    return std::find_if(pr.tail.begin(), pr.tail.end(), safe_pred);
  }
} }

#endif


// Copyright (c) 2005 - 2015 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#ifndef _CODE_LIBS_TWODLIB_MASTERODEINT_INCLUDE_GUARD
#define _CODE_LIBS_TWODLIB_MASTERODEINT_INCLUDE_GUARD

#include <string>
#include <boost/numeric/odeint.hpp>
#include "CSRMatrix.hpp"
#include "TransitionMatrix.hpp"
#include "Ode2DSystemGroup.hpp"
#include "MasterParameter.hpp"

typedef boost::numeric::odeint::runge_kutta_cash_karp54< std::vector<double> > error_stepper_type;

namespace TwoDLib {

	// Solver for the Poisson Master equation, using BOOST::Odeint as backend
	class MasterOdeint {
	public:

		MasterOdeint
		(
			Ode2DSystemGroup&  sys,                                     //!< The systemgroup keeps track of the density and the mapping that is updated using evolution
			const std::vector< vector<TransitionMatrix> >& vec_vec_mat, //!< TransitionMatrix matrix. First index labels the Mesh that covered by a vector of TransitionMatrix elements, second index labels the different TransitionMatrix objects operating on this mesh, which differ in the efficacy they represent
			const MasterParameter& par                                  //!< Parmeter for configuring the numerical solver
		);

		MasterOdeint(const MasterOdeint&);

		//! Communicate the firing rates to the sparse matrix implementation
		void Apply
			(
				double                                   time_step,      //!< mesh time step
				const std::vector<std::vector<double> >& rate_matrix,	 //!< rate matrix
				const vector<MPILib::Index>&	         weight_order    //!< reference to the mapping from NodeId to weight order
			);

		//! Communicate the firing rates to the sparse matrix implementation for individual neurons
		void ApplyFinitePoisson
		(
			double                                   time_step,      //!< mesh time step
			const std::vector<std::vector<double> >& rate_matrix,	 //!< rate matrix
			const vector<MPILib::Index>& weight_order    //!< reference to the mapping from NodeId to weight order
		);

		//! Number of meshes
		MPILib::Number  NrMeshes() const { return _vec_vec_csr.size(); }

		//! Number of weight matrices associated with Mesh i
		MPILib::Number  NrMatrices(MPILib::Index i) const { return _vec_vec_csr[i].size(); }

		//! Efficacy associated with matrix matrix_index from Mesh mesh_index
		double Efficacy
		(
				MPILib::Index mesh_index,	//!< Index labeling mesh
				MPILib::Index matrix_index
		) const {return _vec_vec_csr[mesh_index][matrix_index].Efficacy(); }

		//! Solution step
		void operator()
				(
					const vector<double>&,
					vector<double>&,
					const double t = 0
				);

	private:

		MasterOdeint& operator=(const MasterOdeint&);

		std::vector<std::vector<CSRMatrix> >
			InitializeCSR
			(
				const std::vector< std::vector<TransitionMatrix> >&,
				const Ode2DSystemGroup&
			);

		Ode2DSystemGroup& _sys;

		const std::vector< std::vector<TransitionMatrix> >& _vec_vec_mat; // After initialization, the original object is allowed to go out of scope; it will not be referred anymore
		const std::vector< std::vector<CSRMatrix> >         _vec_vec_csr;

		const MasterParameter	_par;
		vector<double>			_dydt;
		double					_rate;

		const std::vector<MPILib::Index>*              _p_vec_map;     // place holder for current mapping, needed in operator()
		const std::vector<std::vector<MPILib::Rate> >* _p_vec_rates;   // place holder for rate vector

	};
}

#endif // include guard


#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>
#include <cmath>
#include <tr1/memory>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "verbose.h"
#include "hg.h"
#include "prob.h"
#include "inside_outside.h"
#include "ff_register.h"
#include "decoder.h"
#include "filelib.h"
#include "online_optimizer.h"
#include "fdict.h"
#include "weights.h"
#include "sparse_vector.h"
#include "sampler.h"

#ifdef HAVE_MPI
#include <boost/mpi/timer.hpp>
#include <boost/mpi.hpp>
namespace mpi = boost::mpi;
#endif

using namespace std;
namespace po = boost::program_options;

struct FComp {
  const vector<double>& w_;
  FComp(const vector<double>& w) : w_(w) {}
  bool operator()(int a, int b) const {
    return fabs(w_[a]) > fabs(w_[b]);
  }
};

void ShowFeatures(const vector<double>& w) {
  vector<int> fnums(w.size());
  for (int i = 0; i < w.size(); ++i)
    fnums[i] = i;
  sort(fnums.begin(), fnums.end(), FComp(w));
  for (vector<int>::iterator i = fnums.begin(); i != fnums.end(); ++i) {
    if (w[*i]) cout << FD::Convert(*i) << ' ' << w[*i] << endl;
  }
}

void ReadConfig(const string& ini, vector<string>* out) {
  ReadFile rf(ini);
  istream& in = *rf.stream();
  while(in) {
    string line;
    getline(in, line);
    if (!in) continue;
    out->push_back(line);
  }
}

void StoreConfig(const vector<string>& cfg, istringstream* o) {
  ostringstream os;
  for (int i = 0; i < cfg.size(); ++i) { os << cfg[i] << endl; }
  o->str(os.str());
}

bool InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("input,i",po::value<string>(),"Corpus of source language sentences")
        ("weights,w",po::value<string>(),"Input feature weights file")
        ("decoder_config,c",po::value<string>(), "cdec.ini file");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || !conf->count("input") || !conf->count("decoder_config")) {
    cerr << dcmdline_options << endl;
    return false;
  }
  return true;
}

void ReadTrainingCorpus(const string& fname, int rank, int size, vector<string>* c, vector<int>* order) {
  ReadFile rf(fname);
  istream& in = *rf.stream();
  string line;
  int id = 0;
  while(in) {
    getline(in, line);
    if (!in) break;
    if (id % size == rank) {
      c->push_back(line);
      order->push_back(id);
    }
    ++id;
  }
}

static const double kMINUS_EPSILON = -1e-6;

struct TrainingObserver : public DecoderObserver {
  void Reset() {
    acc_exp.clear();
    total_complete = 0;
  }

  virtual void NotifyDecodingStart(const SentenceMetadata& smeta) {
    cur_model_exp.clear();
    state = 1;
  }

  // compute model expectations, denominator of objective
  virtual void NotifyTranslationForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    assert(state == 1);
    state = 2;
    const prob_t z = InsideOutside<prob_t,
                                   EdgeProb,
                                   SparseVector<prob_t>,
                                   EdgeFeaturesAndProbWeightFunction>(*hg, &cur_model_exp);
    cur_model_exp /= z;
    acc_exp += cur_model_exp;
  }

  virtual void NotifyAlignmentForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    cerr << "IGNORING ALIGNMENT FOREST!\n";
  }

  virtual void NotifyDecodingComplete(const SentenceMetadata& smeta) {
    if (state == 2) {
      ++total_complete;
    }
  }

  void GetExpectations(SparseVector<double>* g) const {
    g->clear();
    for (SparseVector<prob_t>::const_iterator it = acc_exp.begin(); it != acc_exp.end(); ++it)
      g->set_value(it->first, it->second);
  }

  int total_complete;
  SparseVector<prob_t> cur_model_exp;
  SparseVector<prob_t> acc_exp;
  int state;
};

#ifdef HAVE_MPI
namespace boost { namespace mpi {
  template<>
  struct is_commutative<std::plus<SparseVector<double> >, SparseVector<double> >
    : mpl::true_ { };
} } // end namespace boost::mpi
#endif

int main(int argc, char** argv) {
#ifdef HAVE_MPI
  mpi::environment env(argc, argv);
  mpi::communicator world;
  const int size = world.size();
  const int rank = world.rank();
#else
  const int size = 1;
  const int rank = 0;
#endif
  if (size > 1) SetSilent(true);  // turn off verbose decoder output
  register_feature_functions();

  po::variables_map conf;
  if (!InitCommandLine(argc, argv, &conf))
    return 1;

  // load initial weights
  Weights weights;
  if (conf.count("weights"))
    weights.InitFromFile(conf["weights"].as<string>());

  vector<string> corpus;
  vector<int> ids;
  ReadTrainingCorpus(conf["input"].as<string>(), rank, size, &corpus, &ids);
  assert(corpus.size() > 0);

  vector<string> cdec_ini;
  ReadConfig(conf["decoder_config"].as<string>(), &cdec_ini);
  istringstream ini;
  StoreConfig(cdec_ini, &ini);
  Decoder decoder(&ini);
  if (decoder.GetConf()["input"].as<string>() != "-") {
    cerr << "cdec.ini must not set an input file\n";
    return 1;
  }

  SparseVector<double> x;
  weights.InitSparseVector(&x);
  TrainingObserver observer;

  weights.InitFromVector(x);
  vector<double> lambdas;
  weights.InitVector(&lambdas);
  decoder.SetWeights(lambdas);
  observer.Reset();
  for (unsigned i = 0; i < corpus.size(); ++i) {
    int id = ids[i];
    decoder.SetId(id);
    decoder.Decode(corpus[i], &observer);
  }
  SparseVector<double> local_exps, exps;
  observer.GetExpectations(&local_exps);
#ifdef HAVE_MPI
  reduce(world, local_exps, exps, std::plus<SparseVector<double> >(), 0);
#else
  exps.swap(local_exps);
#endif

  weights.InitFromVector(exps);
  weights.InitVector(&lambdas);
  ShowFeatures(lambdas);

  return 0;
}


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_data_hpp__
#define __pinocchio_data_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/container/aligned-vector.hpp"

#include <iostream>
#include <Eigen/Cholesky>

namespace pinocchio
{

  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct DataTpl
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef _Scalar Scalar;
    enum { Options = _Options };

    typedef JointCollectionTpl<Scalar,Options> JointCollection;

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;

    typedef SE3Tpl<Scalar,Options> SE3;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef ForceTpl<Scalar,Options> Force;
    typedef InertiaTpl<Scalar,Options> Inertia;
    typedef FrameTpl<Scalar,Options> Frame;

    typedef pinocchio::Index Index;
    typedef pinocchio::JointIndex JointIndex;
    typedef pinocchio::GeomIndex GeomIndex;
    typedef pinocchio::FrameIndex FrameIndex;
    typedef std::vector<Index> IndexVector;

    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;

    typedef container::aligned_vector<JointModel> JointModelVector;
    typedef container::aligned_vector<JointData> JointDataVector;

    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;

    /// \brief Dense vectorized version of a joint configuration vector.
    typedef VectorXs ConfigVectorType;

    /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc).
    ///        It also handles the notion of co-tangent vector (e.g. torque, etc).
    typedef VectorXs TangentVectorType;

    /// \brief The 6d jacobian type (temporary)
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
    /// \brief The 3d jacobian type (temporary)
    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;

    typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;

    /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model,
    /// encapsulated in JointDataAccessor.
    JointDataVector joints;

    /// \brief Vector of joint accelerations expressed at the centers of the joints frames.
    container::aligned_vector<Motion> a;

    /// \brief Vector of joint accelerations expressed at the origin of the world.
    container::aligned_vector<Motion> oa;

    /// \brief Vector of joint accelerations due to the gravity field.
    container::aligned_vector<Motion> a_gf;

    /// \brief Vector of joint accelerations expressed at the origin of the world including gravity contribution.
    container::aligned_vector<Motion> oa_gf;

    /// \brief Vector of joint velocities expressed at the centers of the joints.
    container::aligned_vector<Motion> v;

    /// \brief Vector of joint velocities expressed at the origin.
    container::aligned_vector<Motion> ov;

    /// \brief Vector of body forces expressed in the local frame of the joint. For each body, the force represents the sum of
    ///        all external forces acting on the body.
    container::aligned_vector<Force> f;

    /// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of
    ///        all external forces acting on the body.
    container::aligned_vector<Force> of;

    /// \brief Vector of spatial momenta expressed in the local frame of the joint.
    container::aligned_vector<Force> h;

    /// \brief Vector of spatial momenta expressed in the world frame.
    container::aligned_vector<Force> oh;

    /// \brief Vector of absolute joint placements (wrt the world).
    container::aligned_vector<SE3> oMi;

    /// \brief Vector of relative joint placements (wrt the body parent).
    container::aligned_vector<SE3> liMi;

    /// \brief Vector of joint torques (dim model.nv).
    TangentVectorType tau;

    /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis, centrifugal and gravitational effects.
    /// \note  In the multibody dynamics equation \f$ M\ddot{q} + b(q, \dot{q}) = \tau \f$,
    ///        the non linear effects are associated to the term \f$b\f$.
    VectorXs nle;

    /// \brief Vector of generalized gravity (dim model.nv).
    /// \note  In the multibody dynamics equation \f$ M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$,
    ///        the gravity effect is associated to the \f$g\f$ term.
    VectorXs g;

    /// \brief Vector of absolute operationnel frame placements (wrt the world).
    container::aligned_vector<SE3> oMf;

    /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint
    ///        and expressed in the local frame of the joint..
    container::aligned_vector<Inertia> Ycrb;

    /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
    container::aligned_vector<typename Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6

    /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
    MatrixXs M;

    /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
    RowMatrixXs Minv;

    /// \brief The Coriolis matrix (a square matrix of dim model.nv).
    MatrixXs C;

    /// \brief Variation of the forceset with respect to the joint configuration.
    Matrix6x dFdq;

    /// \brief Variation of the forceset with respect to the joint velocity.
    Matrix6x dFdv;

    /// \brief Variation of the forceset with respect to the joint acceleration.
    Matrix6x dFda;

    /// \brief Used in computeMinverse
    Matrix6x SDinv;

    /// \brief Used in computeMinverse
    Matrix6x UDinv;

    /// \brief Used in computeMinverse
    Matrix6x IS;

    /// \brief Right variation of the inertia matrix
    container::aligned_vector<typename Inertia::Matrix6> vxI;

    /// \brief Left variation of the inertia matrix
    container::aligned_vector<typename Inertia::Matrix6> Ivx;

    /// \brief Inertia quantities expressed in the world frame
    container::aligned_vector<Inertia> oYcrb;

    /// \brief Time variation of the inertia quantities expressed in the world frame
    container::aligned_vector<typename Inertia::Matrix6> doYcrb;

    /// \brief Temporary for derivative algorithms
    typename Inertia::Matrix6 Itmp;

    /// \brief Temporary for derivative algorithms
    RowMatrix6 M6tmpR;
    RowMatrix6 M6tmpR2;

    /// \brief The joint accelerations computed from ABA
    TangentVectorType ddq;

    // ABA internal data
    /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
    container::aligned_vector<typename Inertia::Matrix6> Yaba;  // TODO: change with dense symmetric matrix6

    /// \brief Intermediate quantity corresponding to apparent torque [ABA]
    TangentVectorType u;                  // Joint Inertia

    // CCRBA return quantities
    /// \brief Centroidal Momentum Matrix
    /// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
    Matrix6x Ag;

    // dCCRBA return quantities
    /// \brief Centroidal Momentum Matrix Time Variation
    /// \note \f$ \dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}\f$ maps the joint velocity and acceleration vectors to the time variation of the centroidal momentum.
    Matrix6x dAg;

    /// \brief Centroidal momentum quantity.
    /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
    /// \note \f$ h_g = \left( m\dot{c}, L_{g} \right); \f$. \f$ h_g \f$ is the stack of the linear momentum and the angular momemtum vectors.
    ///
    Force hg;

    /// \brief Centroidal momentum time derivative.
    /// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
    /// \note \f$ \dot{h_g} = \left( m\ddot{c}, \dot{L}_{g} \right); \f$. \f$ \dot{h_g} \f$ is the stack of the linear momentum variation and the angular momemtum variation.
    ///
    Force dhg;

    /// \brief Centroidal Composite Rigid Body Inertia.
    /// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.
    Inertia Ig;

    /// \brief Spatial forces set, used in CRBA and CCRBA
    container::aligned_vector<Matrix6x> Fcrb;

    /// \brief Index of the last child (for CRBA)
    std::vector<int> lastChild;
    /// \brief Dimension of the subtree motion space (for CRBA)
    std::vector<int> nvSubtree;

    /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
    MatrixXs U;

    /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
    VectorXs D;

    /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
    VectorXs Dinv;

    /// \brief Temporary of size NV used in Cholesky Decomposition.
    VectorXs tmp;

    /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
    std::vector<int> parents_fromRow;

    /// \brief Subtree of the current row index (used in Cholesky Decomposition).
    std::vector<int> nvSubtree_fromRow;

    /// \brief Jacobian of joint placements.
    /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call pinocchio::getJointJacobian
    Matrix6x J;

    /// \brief Derivative of the Jacobian with respect to the time.
    Matrix6x dJ;

    /// \brief Variation of the spatial velocity set with respect to the joint configuration.
    Matrix6x dVdq;

    /// \brief Variation of the spatial acceleration set with respect to the joint configuration.
    Matrix6x dAdq;

    /// \brief Variation of the spatial acceleration set with respect to the joint velocity.
    Matrix6x dAdv;

    /// \brief Partial derivative of the joint torque vector with respect to the joint configuration.
    MatrixXs dtau_dq;

    /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
    MatrixXs dtau_dv;

    /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
    MatrixXs ddq_dq;

    /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
    MatrixXs ddq_dv;

    /// \brief Vector of joint placements wrt to algorithm end effector.
    container::aligned_vector<SE3> iMf;

    /// \brief Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.
    container::aligned_vector<Vector3> com;

    /// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame.
    container::aligned_vector<Vector3> vcom;

    /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
    container::aligned_vector<Vector3> acom;

    /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corresponds to the total mass of the model.
    std::vector<Scalar> mass;

    /// \brief Jacobien of center of mass.
    /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
    Matrix3x Jcom;

    /// \brief Kinetic energy of the model.
    Scalar kinetic_energy;

    /// \brief Potential energy of the model.
    Scalar potential_energy;

    // Temporary variables used in forward dynamics

    /// \brief Inverse of the operational-space inertia matrix
    MatrixXs JMinvJt;

    /// \brief Cholesky decompostion of \f$\JMinvJt\f$.
    Eigen::LLT<MatrixXs> llt_JMinvJt;

    /// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
    VectorXs lambda_c;

    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
    MatrixXs sDUiJt;

    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
    VectorXs torque_residual;

    /// \brief Generalized velocity after impact.
    TangentVectorType dq_after;

    /// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
    VectorXs impulse_c;

    // data related to regressor
    Matrix3x staticRegressor;

    ///
    /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
    ///
    /// \param[in] model The model structure of the rigid body system.
    ///
    explicit DataTpl(const Model & model);

  private:
    void computeLastChild(const Model & model);
    void computeParents_fromRow(const Model & model);

  };

} // namespace pinocchio

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/data.hxx"

#endif // ifndef __pinocchio_data_hpp__