osrm-backend/include/engine/routing_algorithms/routing_base.hpp

#ifndef ROUTING_BASE_HPP
#define ROUTING_BASE_HPP

#include "extractor/guidance/turn_instruction.hpp"
#include "engine/edge_unpacker.hpp"
#include "engine/internal_route_result.hpp"
#include "engine/search_engine_data.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/guidance/turn_bearing.hpp"
#include "util/typedefs.hpp"

#include <boost/assert.hpp>

#include <cstddef>
#include <cstdint>

#include <algorithm>
#include <iterator>
#include <numeric>
#include <stack>
#include <utility>
#include <vector>

namespace osrm
{
namespace engine
{

namespace routing_algorithms
{

template <class DataFacadeT, class Derived> class BasicRoutingInterface
{
  private:
    using EdgeData = typename DataFacadeT::EdgeData;

  public:
    /*
    min_edge_offset is needed in case we use multiple
    nodes as start/target nodes with different (even negative) offsets.
    In that case the termination criterion is not correct
    anymore.

    Example:
    forward heap: a(-100), b(0),
    reverse heap: c(0), d(100)

    a --- d
      \ /
      / \
    b --- c

    This is equivalent to running a bi-directional Dijkstra on the following graph:

        a --- d
       /  \ /  \
      y    x    z
       \  / \  /
        b --- c

    The graph is constructed by inserting nodes y and z that are connected to the initial nodes
    using edges (y, a) with weight -100, (y, b) with weight 0 and,
    (d, z) with weight 100, (c, z) with weight 0 corresponding.
    Since we are dealing with a graph that contains _negative_ edges,
    we need to add an offset to the termination criterion.
    */
    void RoutingStep(const DataFacadeT &facade,
                     SearchEngineData::QueryHeap &forward_heap,
                     SearchEngineData::QueryHeap &reverse_heap,
                     NodeID &middle_node_id,
                     std::int32_t &upper_bound,
                     std::int32_t min_edge_offset,
                     const bool forward_direction,
                     const bool stalling,
                     const bool force_loop_forward,
                     const bool force_loop_reverse) const
    {
        const NodeID node = forward_heap.DeleteMin();
        const std::int32_t weight = forward_heap.GetKey(node);

        if (reverse_heap.WasInserted(node))
        {
            const std::int32_t new_weight = reverse_heap.GetKey(node) + weight;
            if (new_weight < upper_bound)
            {
                // if loops are forced, they are so at the source
                if ((force_loop_forward && forward_heap.GetData(node).parent == node) ||
                    (force_loop_reverse && reverse_heap.GetData(node).parent == node) ||
                    // in this case we are looking at a bi-directional way where the source
                    // and target phantom are on the same edge based node
                    new_weight < 0)
                {
                    // check whether there is a loop present at the node
                    for (const auto edge : facade.GetAdjacentEdgeRange(node))
                    {
                        const EdgeData &data = facade.GetEdgeData(edge);
                        bool forward_directionFlag =
                            (forward_direction ? data.forward : data.backward);
                        if (forward_directionFlag)
                        {
                            const NodeID to = facade.GetTarget(edge);
                            if (to == node)
                            {
                                const EdgeWeight edge_weight = data.weight;
                                const std::int32_t loop_weight = new_weight + edge_weight;
                                if (loop_weight >= 0 && loop_weight < upper_bound)
                                {
                                    middle_node_id = node;
                                    upper_bound = loop_weight;
                                }
                            }
                        }
                    }
                }
                else
                {
                    BOOST_ASSERT(new_weight >= 0);

                    middle_node_id = node;
                    upper_bound = new_weight;
                }
            }
        }

        // make sure we don't terminate too early if we initialize the weight
        // for the nodes in the forward heap with the forward/reverse offset
        BOOST_ASSERT(min_edge_offset <= 0);
        if (weight + min_edge_offset > upper_bound)
        {
            forward_heap.DeleteAll();
            return;
        }

        // Stalling
        if (stalling)
        {
            for (const auto edge : facade.GetAdjacentEdgeRange(node))
            {
                const EdgeData &data = facade.GetEdgeData(edge);
                const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
                if (reverse_flag)
                {
                    const NodeID to = facade.GetTarget(edge);
                    const EdgeWeight edge_weight = data.weight;

                    BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");

                    if (forward_heap.WasInserted(to))
                    {
                        if (forward_heap.GetKey(to) + edge_weight < weight)
                        {
                            return;
                        }
                    }
                }
            }
        }

        for (const auto edge : facade.GetAdjacentEdgeRange(node))
        {
            const EdgeData &data = facade.GetEdgeData(edge);
            bool forward_directionFlag = (forward_direction ? data.forward : data.backward);
            if (forward_directionFlag)
            {
                const NodeID to = facade.GetTarget(edge);
                const EdgeWeight edge_weight = data.weight;

                BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
                const int to_weight = weight + edge_weight;

                // New Node discovered -> Add to Heap + Node Info Storage
                if (!forward_heap.WasInserted(to))
                {
                    forward_heap.Insert(to, to_weight, node);
                }
                // Found a shorter Path -> Update weight
                else if (to_weight < forward_heap.GetKey(to))
                {
                    // new parent
                    forward_heap.GetData(to).parent = node;
                    forward_heap.DecreaseKey(to, to_weight);
                }
            }
        }
    }

    inline EdgeWeight GetLoopWeight(const DataFacadeT &facade, NodeID node) const
    {
        EdgeWeight loop_weight = INVALID_EDGE_WEIGHT;
        for (auto edge : facade.GetAdjacentEdgeRange(node))
        {
            const auto &data = facade.GetEdgeData(edge);
            if (data.forward)
            {
                const NodeID to = facade.GetTarget(edge);
                if (to == node)
                {
                    loop_weight = std::min(loop_weight, data.weight);
                }
            }
        }
        return loop_weight;
    }

    template <typename RandomIter>
    void UnpackPath(const DataFacadeT &facade,
                    RandomIter packed_path_begin,
                    RandomIter packed_path_end,
                    const PhantomNodes &phantom_node_pair,
                    std::vector<PathData> &unpacked_path) const
    {
        BOOST_ASSERT(std::distance(packed_path_begin, packed_path_end) > 0);

        const bool start_traversed_in_reverse =
            (*packed_path_begin != phantom_node_pair.source_phantom.forward_segment_id.id);
        const bool target_traversed_in_reverse =
            (*std::prev(packed_path_end) != phantom_node_pair.target_phantom.forward_segment_id.id);

        BOOST_ASSERT(*packed_path_begin == phantom_node_pair.source_phantom.forward_segment_id.id ||
                     *packed_path_begin == phantom_node_pair.source_phantom.reverse_segment_id.id);
        BOOST_ASSERT(
            *std::prev(packed_path_end) == phantom_node_pair.target_phantom.forward_segment_id.id ||
            *std::prev(packed_path_end) == phantom_node_pair.target_phantom.reverse_segment_id.id);

        UnpackCHPath(
            facade,
            packed_path_begin,
            packed_path_end,
            [this,
             &facade,
             &unpacked_path,
             &phantom_node_pair,
             &start_traversed_in_reverse,
             &target_traversed_in_reverse](std::pair<NodeID, NodeID> & /* edge */,
                                           const EdgeData &edge_data) {

                BOOST_ASSERT_MSG(!edge_data.shortcut, "original edge flagged as shortcut");
                const auto name_index = facade.GetNameIndexFromEdgeID(edge_data.id);
                const auto turn_instruction = facade.GetTurnInstructionForEdgeID(edge_data.id);
                const extractor::TravelMode travel_mode =
                    (unpacked_path.empty() && start_traversed_in_reverse)
                        ? phantom_node_pair.source_phantom.backward_travel_mode
                        : facade.GetTravelModeForEdgeID(edge_data.id);

                const auto geometry_index = facade.GetGeometryIndexForEdgeID(edge_data.id);
                std::vector<NodeID> id_vector;
                std::vector<EdgeWeight> weight_vector;
                std::vector<DatasourceID> datasource_vector;
                if (geometry_index.forward)
                {
                    id_vector = facade.GetUncompressedForwardGeometry(geometry_index.id);
                    weight_vector = facade.GetUncompressedForwardWeights(geometry_index.id);
                    datasource_vector = facade.GetUncompressedForwardDatasources(geometry_index.id);
                }
                else
                {
                    id_vector = facade.GetUncompressedReverseGeometry(geometry_index.id);
                    weight_vector = facade.GetUncompressedReverseWeights(geometry_index.id);
                    datasource_vector = facade.GetUncompressedReverseDatasources(geometry_index.id);
                }
                BOOST_ASSERT(id_vector.size() > 0);
                BOOST_ASSERT(weight_vector.size() > 0);
                BOOST_ASSERT(datasource_vector.size() > 0);

                const auto total_weight =
                    std::accumulate(weight_vector.begin(), weight_vector.end(), 0);

                BOOST_ASSERT(weight_vector.size() == id_vector.size() - 1);
                const bool is_first_segment = unpacked_path.empty();

                const std::size_t start_index =
                    (is_first_segment
                         ? ((start_traversed_in_reverse)
                                ? weight_vector.size() -
                                      phantom_node_pair.source_phantom.fwd_segment_position - 1
                                : phantom_node_pair.source_phantom.fwd_segment_position)
                         : 0);
                const std::size_t end_index = weight_vector.size();

                BOOST_ASSERT(start_index >= 0);
                BOOST_ASSERT(start_index < end_index);
                for (std::size_t segment_idx = start_index; segment_idx < end_index; ++segment_idx)
                {
                    unpacked_path.push_back(
                        PathData{id_vector[segment_idx + 1],
                                 name_index,
                                 weight_vector[segment_idx],
                                 extractor::guidance::TurnInstruction::NO_TURN(),
                                 {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
                                 travel_mode,
                                 INVALID_ENTRY_CLASSID,
                                 datasource_vector[segment_idx],
                                 util::guidance::TurnBearing(0),
                                 util::guidance::TurnBearing(0)});
                }
                BOOST_ASSERT(unpacked_path.size() > 0);
                if (facade.hasLaneData(edge_data.id))
                    unpacked_path.back().lane_data = facade.GetLaneData(edge_data.id);

                unpacked_path.back().entry_classid = facade.GetEntryClassID(edge_data.id);
                unpacked_path.back().turn_instruction = turn_instruction;
                unpacked_path.back().duration_until_turn += (edge_data.weight - total_weight);
                unpacked_path.back().pre_turn_bearing = facade.PreTurnBearing(edge_data.id);
                unpacked_path.back().post_turn_bearing = facade.PostTurnBearing(edge_data.id);
            });

        std::size_t start_index = 0, end_index = 0;
        std::vector<unsigned> id_vector;
        std::vector<EdgeWeight> weight_vector;
        std::vector<DatasourceID> datasource_vector;
        const bool is_local_path = (phantom_node_pair.source_phantom.packed_geometry_id ==
                                    phantom_node_pair.target_phantom.packed_geometry_id) &&
                                   unpacked_path.empty();

        if (target_traversed_in_reverse)
        {
            id_vector = facade.GetUncompressedReverseGeometry(
                phantom_node_pair.target_phantom.packed_geometry_id);

            weight_vector = facade.GetUncompressedReverseWeights(
                phantom_node_pair.target_phantom.packed_geometry_id);

            datasource_vector = facade.GetUncompressedReverseDatasources(
                phantom_node_pair.target_phantom.packed_geometry_id);

            if (is_local_path)
            {
                start_index = weight_vector.size() -
                              phantom_node_pair.source_phantom.fwd_segment_position - 1;
            }
            end_index =
                weight_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position - 1;
        }
        else
        {
            if (is_local_path)
            {
                start_index = phantom_node_pair.source_phantom.fwd_segment_position;
            }
            end_index = phantom_node_pair.target_phantom.fwd_segment_position;

            id_vector = facade.GetUncompressedForwardGeometry(
                phantom_node_pair.target_phantom.packed_geometry_id);

            weight_vector = facade.GetUncompressedForwardWeights(
                phantom_node_pair.target_phantom.packed_geometry_id);

            datasource_vector = facade.GetUncompressedForwardDatasources(
                phantom_node_pair.target_phantom.packed_geometry_id);
        }

        // Given the following compressed geometry:
        // U---v---w---x---y---Z
        //    s           t
        // s: fwd_segment 0
        // t: fwd_segment 3
        // -> (U, v), (v, w), (w, x)
        // note that (x, t) is _not_ included but needs to be added later.
        for (std::size_t segment_idx = start_index; segment_idx != end_index;
             (start_index < end_index ? ++segment_idx : --segment_idx))
        {
            BOOST_ASSERT(segment_idx < id_vector.size() - 1);
            BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0);
            unpacked_path.push_back(PathData{
                id_vector[start_index < end_index ? segment_idx + 1 : segment_idx - 1],
                phantom_node_pair.target_phantom.name_id,
                weight_vector[segment_idx],
                extractor::guidance::TurnInstruction::NO_TURN(),
                {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
                target_traversed_in_reverse ? phantom_node_pair.target_phantom.backward_travel_mode
                                            : phantom_node_pair.target_phantom.forward_travel_mode,
                INVALID_ENTRY_CLASSID,
                datasource_vector[segment_idx],
                util::guidance::TurnBearing(0),
                util::guidance::TurnBearing(0)});
        }

        if (unpacked_path.size() > 0)
        {
            const auto source_weight = start_traversed_in_reverse
                                           ? phantom_node_pair.source_phantom.reverse_weight
                                           : phantom_node_pair.source_phantom.forward_weight;
            // The above code will create segments for (v, w), (w,x), (x, y) and (y, Z).
            // However the first segment duration needs to be adjusted to the fact that the source
            // phantom is in the middle of the segment. We do this by subtracting v--s from the
            // duration.

            // Since it's possible duration_until_turn can be less than source_weight here if
            // a negative enough turn penalty is used to modify this edge weight during
            // osrm-contract, we clamp to 0 here so as not to return a negative duration
            // for this segment.

            // TODO this creates a scenario where it's possible the duration from a phantom
            // node to the first turn would be the same as from end to end of a segment,
            // which is obviously incorrect and not ideal...
            unpacked_path.front().duration_until_turn =
                std::max(unpacked_path.front().duration_until_turn - source_weight, 0);
        }

        // there is no equivalent to a node-based node in an edge-expanded graph.
        // two equivalent routes may start (or end) at different node-based edges
        // as they are added with the offset how much "weight" on the edge
        // has already been traversed. Depending on offset one needs to remove
        // the last node.
        if (unpacked_path.size() > 1)
        {
            const std::size_t last_index = unpacked_path.size() - 1;
            const std::size_t second_to_last_index = last_index - 1;

            if (unpacked_path[last_index].turn_via_node ==
                unpacked_path[second_to_last_index].turn_via_node)
            {
                unpacked_path.pop_back();
            }
            BOOST_ASSERT(!unpacked_path.empty());
        }
    }

    /**
     * Unpacks a single edge (NodeID->NodeID) from the CH graph down to it's original non-shortcut
     * route.
     * @param from the node the CH edge starts at
     * @param to the node the CH edge finishes at
     * @param unpacked_path the sequence of original NodeIDs that make up the expanded CH edge
     */
    void UnpackEdge(const DataFacadeT &facade,
                    const NodeID from,
                    const NodeID to,
                    std::vector<NodeID> &unpacked_path) const
    {
        std::array<NodeID, 2> path{{from, to}};
        UnpackCHPath(
            facade,
            path.begin(),
            path.end(),
            [&unpacked_path](const std::pair<NodeID, NodeID> &edge, const EdgeData & /* data */) {
                unpacked_path.emplace_back(edge.first);
            });
        unpacked_path.emplace_back(to);
    }

    void RetrievePackedPathFromHeap(const SearchEngineData::QueryHeap &forward_heap,
                                    const SearchEngineData::QueryHeap &reverse_heap,
                                    const NodeID middle_node_id,
                                    std::vector<NodeID> &packed_path) const
    {
        RetrievePackedPathFromSingleHeap(forward_heap, middle_node_id, packed_path);
        std::reverse(packed_path.begin(), packed_path.end());
        packed_path.emplace_back(middle_node_id);
        RetrievePackedPathFromSingleHeap(reverse_heap, middle_node_id, packed_path);
    }

    void RetrievePackedPathFromSingleHeap(const SearchEngineData::QueryHeap &search_heap,
                                          const NodeID middle_node_id,
                                          std::vector<NodeID> &packed_path) const
    {
        NodeID current_node_id = middle_node_id;
        // all initial nodes will have itself as parent, or a node not in the heap
        // in case of a core search heap. We need a distinction between core entry nodes
        // and start nodes since otherwise start node specific code that assumes
        // node == node.parent (e.g. the loop code) might get actived.
        while (current_node_id != search_heap.GetData(current_node_id).parent &&
               search_heap.WasInserted(search_heap.GetData(current_node_id).parent))
        {
            current_node_id = search_heap.GetData(current_node_id).parent;
            packed_path.emplace_back(current_node_id);
        }
    }

    // assumes that heaps are already setup correctly.
    // ATTENTION: This only works if no additional offset is supplied next to the Phantom Node
    // Offsets.
    // In case additional offsets are supplied, you might have to force a loop first.
    // A forced loop might be necessary, if source and target are on the same segment.
    // If this is the case and the offsets of the respective direction are larger for the source
    // than the target
    // then a force loop is required (e.g. source_phantom.forward_segment_id ==
    // target_phantom.forward_segment_id
    // && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
    // requires
    // a force loop, if the heaps have been initialized with positive offsets.
    void Search(const DataFacadeT &facade,
                SearchEngineData::QueryHeap &forward_heap,
                SearchEngineData::QueryHeap &reverse_heap,
                std::int32_t &weight,
                std::vector<NodeID> &packed_leg,
                const bool force_loop_forward,
                const bool force_loop_reverse,
                const int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        NodeID middle = SPECIAL_NODEID;
        weight = duration_upper_bound;

        // get offset to account for offsets on phantom nodes on compressed edges
        const auto min_edge_offset = std::min(0, forward_heap.MinKey());
        BOOST_ASSERT(min_edge_offset <= 0);
        // we only every insert negative offsets for nodes in the forward heap
        BOOST_ASSERT(reverse_heap.MinKey() >= 0);

        // run two-Target Dijkstra routing step.
        const constexpr bool STALLING_ENABLED = true;
        while (0 < (forward_heap.Size() + reverse_heap.Size()))
        {
            if (!forward_heap.Empty())
            {
                RoutingStep(facade,
                            forward_heap,
                            reverse_heap,
                            middle,
                            weight,
                            min_edge_offset,
                            true,
                            STALLING_ENABLED,
                            force_loop_forward,
                            force_loop_reverse);
            }
            if (!reverse_heap.Empty())
            {
                RoutingStep(facade,
                            reverse_heap,
                            forward_heap,
                            middle,
                            weight,
                            min_edge_offset,
                            false,
                            STALLING_ENABLED,
                            force_loop_reverse,
                            force_loop_forward);
            }
        }

        // No path found for both target nodes?
        if (duration_upper_bound <= weight || SPECIAL_NODEID == middle)
        {
            weight = INVALID_EDGE_WEIGHT;
            return;
        }

        // Was a paths over one of the forward/reverse nodes not found?
        BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != weight),
                         "no path found");

        // make sure to correctly unpack loops
        if (weight != forward_heap.GetKey(middle) + reverse_heap.GetKey(middle))
        {
            // self loop makes up the full path
            packed_leg.push_back(middle);
            packed_leg.push_back(middle);
        }
        else
        {
            RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
        }
    }

    // assumes that heaps are already setup correctly.
    // A forced loop might be necessary, if source and target are on the same segment.
    // If this is the case and the offsets of the respective direction are larger for the source
    // than the target
    // then a force loop is required (e.g. source_phantom.forward_segment_id ==
    // target_phantom.forward_segment_id
    // && source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
    // requires
    // a force loop, if the heaps have been initialized with positive offsets.
    void SearchWithCore(const DataFacadeT &facade,
                        SearchEngineData::QueryHeap &forward_heap,
                        SearchEngineData::QueryHeap &reverse_heap,
                        SearchEngineData::QueryHeap &forward_core_heap,
                        SearchEngineData::QueryHeap &reverse_core_heap,
                        int &weight,
                        std::vector<NodeID> &packed_leg,
                        const bool force_loop_forward,
                        const bool force_loop_reverse,
                        int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        NodeID middle = SPECIAL_NODEID;
        weight = duration_upper_bound;

        using CoreEntryPoint = std::tuple<NodeID, EdgeWeight, NodeID>;
        std::vector<CoreEntryPoint> forward_entry_points;
        std::vector<CoreEntryPoint> reverse_entry_points;

        // get offset to account for offsets on phantom nodes on compressed edges
        const auto min_edge_offset = std::min(0, forward_heap.MinKey());
        // we only every insert negative offsets for nodes in the forward heap
        BOOST_ASSERT(reverse_heap.MinKey() >= 0);

        const constexpr bool STALLING_ENABLED = true;
        // run two-Target Dijkstra routing step.
        while (0 < (forward_heap.Size() + reverse_heap.Size()))
        {
            if (!forward_heap.Empty())
            {
                if (facade.IsCoreNode(forward_heap.Min()))
                {
                    const NodeID node = forward_heap.DeleteMin();
                    const int key = forward_heap.GetKey(node);
                    forward_entry_points.emplace_back(node, key, forward_heap.GetData(node).parent);
                }
                else
                {
                    RoutingStep(facade,
                                forward_heap,
                                reverse_heap,
                                middle,
                                weight,
                                min_edge_offset,
                                true,
                                STALLING_ENABLED,
                                force_loop_forward,
                                force_loop_reverse);
                }
            }
            if (!reverse_heap.Empty())
            {
                if (facade.IsCoreNode(reverse_heap.Min()))
                {
                    const NodeID node = reverse_heap.DeleteMin();
                    const int key = reverse_heap.GetKey(node);
                    reverse_entry_points.emplace_back(node, key, reverse_heap.GetData(node).parent);
                }
                else
                {
                    RoutingStep(facade,
                                reverse_heap,
                                forward_heap,
                                middle,
                                weight,
                                min_edge_offset,
                                false,
                                STALLING_ENABLED,
                                force_loop_reverse,
                                force_loop_forward);
                }
            }
        }

        const auto insertInCoreHeap = [](const CoreEntryPoint &p,
                                         SearchEngineData::QueryHeap &core_heap) {
            NodeID id;
            EdgeWeight weight;
            NodeID parent;
            // TODO this should use std::apply when we get c++17 support
            std::tie(id, weight, parent) = p;
            core_heap.Insert(id, weight, parent);
        };

        forward_core_heap.Clear();
        for (const auto &p : forward_entry_points)
        {
            insertInCoreHeap(p, forward_core_heap);
        }

        reverse_core_heap.Clear();
        for (const auto &p : reverse_entry_points)
        {
            insertInCoreHeap(p, reverse_core_heap);
        }

        // get offset to account for offsets on phantom nodes on compressed edges
        int min_core_edge_offset = 0;
        if (forward_core_heap.Size() > 0)
        {
            min_core_edge_offset = std::min(min_core_edge_offset, forward_core_heap.MinKey());
        }
        if (reverse_core_heap.Size() > 0 && reverse_core_heap.MinKey() < 0)
        {
            min_core_edge_offset = std::min(min_core_edge_offset, reverse_core_heap.MinKey());
        }
        BOOST_ASSERT(min_core_edge_offset <= 0);

        // run two-target Dijkstra routing step on core with termination criterion
        const constexpr bool STALLING_DISABLED = false;
        while (0 < forward_core_heap.Size() && 0 < reverse_core_heap.Size() &&
               weight > (forward_core_heap.MinKey() + reverse_core_heap.MinKey()))
        {
            RoutingStep(facade,
                        forward_core_heap,
                        reverse_core_heap,
                        middle,
                        weight,
                        min_core_edge_offset,
                        true,
                        STALLING_DISABLED,
                        force_loop_forward,
                        force_loop_reverse);

            RoutingStep(facade,
                        reverse_core_heap,
                        forward_core_heap,
                        middle,
                        weight,
                        min_core_edge_offset,
                        false,
                        STALLING_DISABLED,
                        force_loop_reverse,
                        force_loop_forward);
        }

        // No path found for both target nodes?
        if (duration_upper_bound <= weight || SPECIAL_NODEID == middle)
        {
            weight = INVALID_EDGE_WEIGHT;
            return;
        }

        // Was a paths over one of the forward/reverse nodes not found?
        BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != weight),
                         "no path found");

        // we need to unpack sub path from core heaps
        if (facade.IsCoreNode(middle))
        {
            if (weight != forward_core_heap.GetKey(middle) + reverse_core_heap.GetKey(middle))
            {
                // self loop
                BOOST_ASSERT(forward_core_heap.GetData(middle).parent == middle &&
                             reverse_core_heap.GetData(middle).parent == middle);
                packed_leg.push_back(middle);
                packed_leg.push_back(middle);
            }
            else
            {
                std::vector<NodeID> packed_core_leg;
                RetrievePackedPathFromHeap(
                    forward_core_heap, reverse_core_heap, middle, packed_core_leg);
                BOOST_ASSERT(packed_core_leg.size() > 0);
                RetrievePackedPathFromSingleHeap(forward_heap, packed_core_leg.front(), packed_leg);
                std::reverse(packed_leg.begin(), packed_leg.end());
                packed_leg.insert(packed_leg.end(), packed_core_leg.begin(), packed_core_leg.end());
                RetrievePackedPathFromSingleHeap(reverse_heap, packed_core_leg.back(), packed_leg);
            }
        }
        else
        {
            if (weight != forward_heap.GetKey(middle) + reverse_heap.GetKey(middle))
            {
                // self loop
                BOOST_ASSERT(forward_heap.GetData(middle).parent == middle &&
                             reverse_heap.GetData(middle).parent == middle);
                packed_leg.push_back(middle);
                packed_leg.push_back(middle);
            }
            else
            {
                RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
            }
        }
    }

    bool NeedsLoopForward(const PhantomNode &source_phantom,
                          const PhantomNode &target_phantom) const
    {
        return source_phantom.forward_segment_id.enabled &&
               target_phantom.forward_segment_id.enabled &&
               source_phantom.forward_segment_id.id == target_phantom.forward_segment_id.id &&
               source_phantom.GetForwardWeightPlusOffset() >
                   target_phantom.GetForwardWeightPlusOffset();
    }

    bool NeedsLoopBackwards(const PhantomNode &source_phantom,
                            const PhantomNode &target_phantom) const
    {
        return source_phantom.reverse_segment_id.enabled &&
               target_phantom.reverse_segment_id.enabled &&
               source_phantom.reverse_segment_id.id == target_phantom.reverse_segment_id.id &&
               source_phantom.GetReverseWeightPlusOffset() >
                   target_phantom.GetReverseWeightPlusOffset();
    }

    double GetPathDistance(const DataFacadeT &facade,
                           const std::vector<NodeID> &packed_path,
                           const PhantomNode &source_phantom,
                           const PhantomNode &target_phantom) const
    {
        std::vector<PathData> unpacked_path;
        PhantomNodes nodes;
        nodes.source_phantom = source_phantom;
        nodes.target_phantom = target_phantom;
        UnpackPath(facade, packed_path.begin(), packed_path.end(), nodes, unpacked_path);

        using util::coordinate_calculation::detail::DEGREE_TO_RAD;
        using util::coordinate_calculation::detail::EARTH_RADIUS;

        double distance = 0;
        double prev_lat =
            static_cast<double>(toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
        double prev_lon =
            static_cast<double>(toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD;
        double prev_cos = std::cos(prev_lat);
        for (const auto &p : unpacked_path)
        {
            const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node);

            const double current_lat =
                static_cast<double>(toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
            const double current_lon =
                static_cast<double>(toFloating(current_coordinate.lon)) * DEGREE_TO_RAD;
            const double current_cos = std::cos(current_lat);

            const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
            const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);

            const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
            const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
            distance += EARTH_RADIUS * charv;

            prev_lat = current_lat;
            prev_lon = current_lon;
            prev_cos = current_cos;
        }

        const double current_lat =
            static_cast<double>(toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
        const double current_lon =
            static_cast<double>(toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD;
        const double current_cos = std::cos(current_lat);

        const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
        const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);

        const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
        const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
        distance += EARTH_RADIUS * charv;

        return distance;
    }

    // Requires the heaps for be empty
    // If heaps should be adjusted to be initialized outside of this function,
    // the addition of force_loop parameters might be required
    double GetNetworkDistanceWithCore(const DataFacadeT &facade,
                                      SearchEngineData::QueryHeap &forward_heap,
                                      SearchEngineData::QueryHeap &reverse_heap,
                                      SearchEngineData::QueryHeap &forward_core_heap,
                                      SearchEngineData::QueryHeap &reverse_core_heap,
                                      const PhantomNode &source_phantom,
                                      const PhantomNode &target_phantom,
                                      int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        BOOST_ASSERT(forward_heap.Empty());
        BOOST_ASSERT(reverse_heap.Empty());

        if (source_phantom.forward_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.forward_segment_id.id,
                                -source_phantom.GetForwardWeightPlusOffset(),
                                source_phantom.forward_segment_id.id);
        }
        if (source_phantom.reverse_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.reverse_segment_id.id,
                                -source_phantom.GetReverseWeightPlusOffset(),
                                source_phantom.reverse_segment_id.id);
        }

        if (target_phantom.forward_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.forward_segment_id.id,
                                target_phantom.GetForwardWeightPlusOffset(),
                                target_phantom.forward_segment_id.id);
        }
        if (target_phantom.reverse_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.reverse_segment_id.id,
                                target_phantom.GetReverseWeightPlusOffset(),
                                target_phantom.reverse_segment_id.id);
        }

        const bool constexpr DO_NOT_FORCE_LOOPS =
            false; // prevents forcing of loops, since offsets are set correctly

        int duration = INVALID_EDGE_WEIGHT;
        std::vector<NodeID> packed_path;
        SearchWithCore(facade,
                       forward_heap,
                       reverse_heap,
                       forward_core_heap,
                       reverse_core_heap,
                       duration,
                       packed_path,
                       DO_NOT_FORCE_LOOPS,
                       DO_NOT_FORCE_LOOPS,
                       duration_upper_bound);

        double distance = std::numeric_limits<double>::max();
        if (duration != INVALID_EDGE_WEIGHT)
        {
            return GetPathDistance(facade, packed_path, source_phantom, target_phantom);
        }
        return distance;
    }

    // Requires the heaps for be empty
    // If heaps should be adjusted to be initialized outside of this function,
    // the addition of force_loop parameters might be required
    double GetNetworkDistance(const DataFacadeT &facade,
                              SearchEngineData::QueryHeap &forward_heap,
                              SearchEngineData::QueryHeap &reverse_heap,
                              const PhantomNode &source_phantom,
                              const PhantomNode &target_phantom,
                              int duration_upper_bound = INVALID_EDGE_WEIGHT) const
    {
        BOOST_ASSERT(forward_heap.Empty());
        BOOST_ASSERT(reverse_heap.Empty());

        if (source_phantom.forward_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.forward_segment_id.id,
                                -source_phantom.GetForwardWeightPlusOffset(),
                                source_phantom.forward_segment_id.id);
        }
        if (source_phantom.reverse_segment_id.enabled)
        {
            forward_heap.Insert(source_phantom.reverse_segment_id.id,
                                -source_phantom.GetReverseWeightPlusOffset(),
                                source_phantom.reverse_segment_id.id);
        }

        if (target_phantom.forward_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.forward_segment_id.id,
                                target_phantom.GetForwardWeightPlusOffset(),
                                target_phantom.forward_segment_id.id);
        }
        if (target_phantom.reverse_segment_id.enabled)
        {
            reverse_heap.Insert(target_phantom.reverse_segment_id.id,
                                target_phantom.GetReverseWeightPlusOffset(),
                                target_phantom.reverse_segment_id.id);
        }

        const bool constexpr DO_NOT_FORCE_LOOPS =
            false; // prevents forcing of loops, since offsets are set correctly

        int duration = INVALID_EDGE_WEIGHT;
        std::vector<NodeID> packed_path;
        Search(facade,
               forward_heap,
               reverse_heap,
               duration,
               packed_path,
               DO_NOT_FORCE_LOOPS,
               DO_NOT_FORCE_LOOPS,
               duration_upper_bound);

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

        return GetPathDistance(facade, packed_path, source_phantom, target_phantom);
    }
};
}
}
}

#endif // ROUTING_BASE_HPP