text: #ifndef OSRM_ENGINE_ROUTING_BASE_CH_HPP
  #define OSRM_ENGINE_ROUTING_BASE_CH_HPP
  
  #include "engine/algorithm.hpp"
  #include "engine/datafacade.hpp"
  #include "engine/routing_algorithms/routing_base.hpp"
  #include "engine/search_engine_data.hpp"
  
  #include "util/typedefs.hpp"
  
  #include <boost/assert.hpp>
  
  namespace osrm
  {
  namespace engine
  {
  
  namespace routing_algorithms
  {
  
  namespace ch
  {
  
  // Stalling
  template <bool DIRECTION, typename HeapT>
  bool stallAtNode(const DataFacade<Algorithm> &facade,
                   const typename HeapT::HeapNode &heapNode,
                   const HeapT &query_heap)
  {
      for (auto edge : facade.GetAdjacentEdgeRange(heapNode.node))
      {
          const auto &data = facade.GetEdgeData(edge);
          if (DIRECTION == REVERSE_DIRECTION ? data.forward : data.backward)
          {
              const NodeID to = facade.GetTarget(edge);
              const EdgeWeight edge_weight = data.weight;
              BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
              const auto toHeapNode = query_heap.GetHeapNodeIfWasInserted(to);
              if (toHeapNode)
              {
                  if (toHeapNode->weight + edge_weight < heapNode.weight)
                  {
                      return true;
                  }
              }
          }
      }
      return false;
  }
  
  template <bool DIRECTION>
  void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
                          const SearchEngineData<Algorithm>::QueryHeap::HeapNode &heapNode,
                          SearchEngineData<Algorithm>::QueryHeap &heap)
  {
      for (const auto edge : facade.GetAdjacentEdgeRange(heapNode.node))
      {
          const auto &data = facade.GetEdgeData(edge);
          if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward)
          {
              const NodeID to = facade.GetTarget(edge);
              const EdgeWeight edge_weight = data.weight;
  
              BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
              const EdgeWeight to_weight = heapNode.weight + edge_weight;
  
              const auto toHeapNode = heap.GetHeapNodeIfWasInserted(to);
              // New Node discovered -> Add to Heap + Node Info Storage
              if (!toHeapNode)
              {
                  heap.Insert(to, to_weight, heapNode.node);
              }
              // Found a shorter Path -> Update weight
              else if (to_weight < toHeapNode->weight)
              {
                  // new parent
                  toHeapNode->data.parent = heapNode.node;
                  toHeapNode->weight = to_weight;
                  heap.DecreaseKey(*toHeapNode);
              }
          }
      }
  }
  
  /*
  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.
  */
  static constexpr bool ENABLE_STALLING = true;
  static constexpr bool DISABLE_STALLING = false;
  template <bool DIRECTION, bool STALLING = ENABLE_STALLING>
  void routingStep(const DataFacade<Algorithm> &facade,
                   SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                   SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                   NodeID &middle_node_id,
                   EdgeWeight &upper_bound,
                   EdgeWeight min_edge_offset,
                   const std::vector<NodeID> &force_loop_forward_nodes,
                   const std::vector<NodeID> &force_loop_reverse_nodes)
  {
      auto heapNode = forward_heap.DeleteMinGetHeapNode();
      const auto reverseHeapNode = reverse_heap.GetHeapNodeIfWasInserted(heapNode.node);
  
      if (reverseHeapNode)
      {
          const EdgeWeight new_weight = reverseHeapNode->weight + heapNode.weight;
          if (new_weight < upper_bound)
          {
              if (force_loop(force_loop_forward_nodes, heapNode) ||
                  force_loop(force_loop_reverse_nodes, heapNode) ||
                  // 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(heapNode.node))
                  {
                      const auto &data = facade.GetEdgeData(edge);
                      if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward)
                      {
                          const NodeID to = facade.GetTarget(edge);
                          if (to == heapNode.node)
                          {
                              const EdgeWeight edge_weight = data.weight;
                              const EdgeWeight loop_weight = new_weight + edge_weight;
                              if (loop_weight >= 0 && loop_weight < upper_bound)
                              {
                                  middle_node_id = heapNode.node;
                                  upper_bound = loop_weight;
                              }
                          }
                      }
                  }
              }
              else
              {
                  BOOST_ASSERT(new_weight >= 0);
  
                  middle_node_id = heapNode.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 (heapNode.weight + min_edge_offset > upper_bound)
      {
          forward_heap.DeleteAll();
          return;
      }
  
      // Stalling
      if (STALLING && stallAtNode<DIRECTION>(facade, heapNode, forward_heap))
      {
          return;
      }
  
      relaxOutgoingEdges<DIRECTION>(facade, heapNode, forward_heap);
  }
  
  template <bool UseDuration>
  std::tuple<EdgeWeight, EdgeDistance> getLoopWeight(const DataFacade<Algorithm> &facade, NodeID node)
  {
      EdgeWeight loop_weight = UseDuration ? MAXIMAL_EDGE_DURATION : INVALID_EDGE_WEIGHT;
      EdgeDistance loop_distance = MAXIMAL_EDGE_DISTANCE;
      for (auto edge : facade.GetAdjacentEdgeRange(node))
      {
          const auto &data = facade.GetEdgeData(edge);
          if (data.forward)
          {
              const NodeID to = facade.GetTarget(edge);
              if (to == node)
              {
                  const auto value = UseDuration ? data.duration : data.weight;
                  if (value < loop_weight)
                  {
                      loop_weight = value;
                      loop_distance = data.distance;
                  }
              }
          }
      }
      return std::make_tuple(loop_weight, loop_distance);
  }
  
  /**
   * Given a sequence of connected `NodeID`s in the CH graph, performs a depth-first unpacking of
   * the shortcut
   * edges.  For every "original" edge found, it calls the `callback` with the two NodeIDs for the
   * edge, and the EdgeData
   * for that edge.
   *
   * The primary purpose of this unpacking is to expand a path through the CH into the original
   * route through the
   * pre-contracted graph.
   *
   * Because of the depth-first-search, the `callback` will effectively be called in sequence for
   * the original route
   * from beginning to end.
   *
   * @param packed_path_begin iterator pointing to the start of the NodeID list
   * @param packed_path_end iterator pointing to the end of the NodeID list
   * @param callback void(const std::pair<NodeID, NodeID>, const EdgeID &) called for each
   * original edge found.
   */
  template <typename BidirectionalIterator, typename Callback>
  void unpackPath(const DataFacade<Algorithm> &facade,
                  BidirectionalIterator packed_path_begin,
                  BidirectionalIterator packed_path_end,
                  Callback &&callback)
  {
      // make sure we have at least something to unpack
      if (packed_path_begin == packed_path_end)
          return;
  
      std::stack<std::pair<NodeID, NodeID>> recursion_stack;
  
      // We have to push the path in reverse order onto the stack because it's LIFO.
      for (auto current = std::prev(packed_path_end); current != packed_path_begin;
           current = std::prev(current))
      {
          recursion_stack.emplace(*std::prev(current), *current);
      }
  
      std::pair<NodeID, NodeID> edge;
      while (!recursion_stack.empty())
      {
          edge = recursion_stack.top();
          recursion_stack.pop();
  
          // Look for an edge on the forward CH graph (.forward)
          EdgeID smaller_edge_id = facade.FindSmallestEdge(
              edge.first, edge.second, [](const auto &data) { return data.forward; });
  
          // If we didn't find one there, the we might be looking at a part of the path that
          // was found using the backward search.  Here, we flip the node order (.second, .first)
          // and only consider edges with the `.backward` flag.
          if (SPECIAL_EDGEID == smaller_edge_id)
          {
              smaller_edge_id = facade.FindSmallestEdge(
                  edge.second, edge.first, [](const auto &data) { return data.backward; });
          }
  
          // If we didn't find anything *still*, then something is broken and someone has
          // called this function with bad values.
          BOOST_ASSERT_MSG(smaller_edge_id != SPECIAL_EDGEID, "Invalid smaller edge ID");
  
          const auto &data = facade.GetEdgeData(smaller_edge_id);
          BOOST_ASSERT_MSG(data.weight != std::numeric_limits<EdgeWeight>::max(),
                           "edge weight invalid");
  
          // If the edge is a shortcut, we need to add the two halfs to the stack.
          if (data.shortcut)
          { // unpack
              const NodeID middle_node_id = data.turn_id;
              // Note the order here - we're adding these to a stack, so we
              // want the first->middle to get visited before middle->second
              recursion_stack.emplace(middle_node_id, edge.second);
              recursion_stack.emplace(edge.first, middle_node_id);
          }
          else
          {
              // We found an original edge, call our callback.
              std::forward<Callback>(callback)(edge, smaller_edge_id);
          }
      }
  }
  
  template <typename BidirectionalIterator>
  EdgeDistance calculateEBGNodeAnnotations(const DataFacade<Algorithm> &facade,
                                           BidirectionalIterator packed_path_begin,
                                           BidirectionalIterator packed_path_end)
  {
      // Make sure we have at least something to unpack
      if (packed_path_begin == packed_path_end ||
          std::distance(packed_path_begin, packed_path_end) <= 1)
          return 0;
  
      std::stack<std::tuple<NodeID, NodeID, bool>> recursion_stack;
      std::stack<EdgeDistance> distance_stack;
      // We have to push the path in reverse order onto the stack because it's LIFO.
      for (auto current = std::prev(packed_path_end); current > packed_path_begin;
           current = std::prev(current))
      {
          recursion_stack.emplace(*std::prev(current), *current, false);
      }
  
      std::tuple<NodeID, NodeID, bool> edge;
      while (!recursion_stack.empty())
      {
          edge = recursion_stack.top();
          recursion_stack.pop();
  
          // Have we processed the edge before? tells us if we have values in the durations stack that
          // we can add up
          if (!std::get<2>(edge))
          { // haven't processed edge before, so process it in the body!
  
              std::get<2>(edge) = true; // mark that this edge will now be processed
  
              // Look for an edge on the forward CH graph (.forward)
              EdgeID smaller_edge_id =
                  facade.FindSmallestEdge(std::get<0>(edge), std::get<1>(edge), [](const auto &data) {
                      return data.forward;
                  });
  
              // If we didn't find one there, the we might be looking at a part of the path that
              // was found using the backward search.  Here, we flip the node order (.second,
              // .first) and only consider edges with the `.backward` flag.
              if (SPECIAL_EDGEID == smaller_edge_id)
              {
                  smaller_edge_id =
                      facade.FindSmallestEdge(std::get<1>(edge),
                                              std::get<0>(edge),
                                              [](const auto &data) { return data.backward; });
              }
  
              // If we didn't find anything *still*, then something is broken and someone has
              // called this function with bad values.
              BOOST_ASSERT_MSG(smaller_edge_id != SPECIAL_EDGEID, "Invalid smaller edge ID");
  
              const auto &data = facade.GetEdgeData(smaller_edge_id);
              BOOST_ASSERT_MSG(data.weight != std::numeric_limits<EdgeWeight>::max(),
                               "edge weight invalid");
  
              // If the edge is a shortcut, we need to add the two halfs to the stack.
              if (data.shortcut)
              { // unpack
                  const NodeID middle_node_id = data.turn_id;
                  // Note the order here - we're adding these to a stack, so we
                  // want the first->middle to get visited before middle->second
                  recursion_stack.emplace(edge);
                  recursion_stack.emplace(middle_node_id, std::get<1>(edge), false);
                  recursion_stack.emplace(std::get<0>(edge), middle_node_id, false);
              }
              else
              {
                  // compute the duration here and put it onto the duration stack using method
                  // similar to annotatePath but smaller
                  EdgeDistance distance = computeEdgeDistance(facade, std::get<0>(edge));
                  distance_stack.emplace(distance);
              }
          }
          else
          { // the edge has already been processed. this means that there are enough values in the
              // distances stack
  
              BOOST_ASSERT_MSG(distance_stack.size() >= 2,
                               "There are not enough (at least 2) values on the distance stack");
              EdgeDistance distance1 = distance_stack.top();
              distance_stack.pop();
              EdgeDistance distance2 = distance_stack.top();
              distance_stack.pop();
              EdgeDistance distance = distance1 + distance2;
              distance_stack.emplace(distance);
          }
      }
  
      EdgeDistance total_distance = 0;
      while (!distance_stack.empty())
      {
          total_distance += distance_stack.top();
          distance_stack.pop();
      }
  
      return total_distance;
  }
  
  template <typename RandomIter, typename FacadeT>
  void unpackPath(const FacadeT &facade,
                  RandomIter packed_path_begin,
                  RandomIter packed_path_end,
                  const PhantomEndpoints &route_endpoints,
                  std::vector<PathData> &unpacked_path)
  {
      const auto nodes_number = std::distance(packed_path_begin, packed_path_end);
      BOOST_ASSERT(nodes_number > 0);
  
      std::vector<NodeID> unpacked_nodes;
      std::vector<EdgeID> unpacked_edges;
      unpacked_nodes.reserve(nodes_number);
      unpacked_edges.reserve(nodes_number);
  
      unpacked_nodes.push_back(*packed_path_begin);
      if (nodes_number > 1)
      {
          unpackPath(facade,
                     packed_path_begin,
                     packed_path_end,
                     [&](std::pair<NodeID, NodeID> &edge, const auto &edge_id) {
                         BOOST_ASSERT(edge.first == unpacked_nodes.back());
                         unpacked_nodes.push_back(edge.second);
                         unpacked_edges.push_back(edge_id);
                     });
      }
  
      annotatePath(facade, route_endpoints, unpacked_nodes, unpacked_edges, unpacked_path);
  }
  
  /**
   * 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 DataFacade<Algorithm> &facade,
                  const NodeID from,
                  const NodeID to,
                  std::vector<NodeID> &unpacked_path);
  
  void retrievePackedPathFromHeap(const SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                                  const SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                                  const NodeID middle_node_id,
                                  std::vector<NodeID> &packed_path);
  
  void retrievePackedPathFromSingleHeap(const SearchEngineData<Algorithm>::QueryHeap &search_heap,
                                        const NodeID middle_node_id,
                                        std::vector<NodeID> &packed_path);
  
  void retrievePackedPathFromSingleManyToManyHeap(
      const SearchEngineData<Algorithm>::ManyToManyQueryHeap &search_heap,
      const NodeID middle_node_id,
      std::vector<NodeID> &packed_path);
  
  // 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(SearchEngineData<Algorithm> &engine_working_data,
              const DataFacade<Algorithm> &facade,
              SearchEngineData<Algorithm>::QueryHeap &forward_heap,
              SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
              EdgeWeight &weight,
              std::vector<NodeID> &packed_leg,
              const std::vector<NodeID> &force_loop_forward_node,
              const std::vector<NodeID> &force_loop_reverse_node,
              const EdgeWeight duration_upper_bound = INVALID_EDGE_WEIGHT);
  
  template <typename PhantomEndpointT>
  void search(SearchEngineData<Algorithm> &engine_working_data,
              const DataFacade<Algorithm> &facade,
              SearchEngineData<Algorithm>::QueryHeap &forward_heap,
              SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
              EdgeWeight &weight,
              std::vector<NodeID> &packed_leg,
              const std::vector<NodeID> &force_loop_forward_node,
              const std::vector<NodeID> &force_loop_reverse_node,
              const PhantomEndpointT & /*endpoints*/,
              const EdgeWeight duration_upper_bound = INVALID_EDGE_WEIGHT)
  {
      // Avoid templating the CH search implementations.
      return search(engine_working_data,
                    facade,
                    forward_heap,
                    reverse_heap,
                    weight,
                    packed_leg,
                    force_loop_forward_node,
                    force_loop_reverse_node,
                    duration_upper_bound);
  }
  
  // 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(SearchEngineData<Algorithm> &engine_working_data,
                            const DataFacade<ch::Algorithm> &facade,
                            SearchEngineData<Algorithm>::QueryHeap &forward_heap,
                            SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
                            const PhantomNode &source_phantom,
                            const PhantomNode &target_phantom,
                            int duration_upper_bound = INVALID_EDGE_WEIGHT);
  
  } // namespace ch
  } // namespace routing_algorithms
  } // namespace engine
  } // namespace osrm
  
  #endif // OSRM_ENGINE_ROUTING_BASE_CH_HPP
  