Stage 5. Routing using.

4 files changed, 256 insertions, 3 deletions

diff --git a/routing/CMakeLists.txt b/routing/CMakeLists.txt
index 50ed852816..6e3672f51e 100644
--- a/routing/CMakeLists.txt
+++ b/routing/CMakeLists.txt
@@ -13,6 +13,7 @@ set(
   base/astar_progress.cpp
   base/astar_progress.hpp
   base/astar_weight.hpp
+  base/bfs.hpp
   base/followed_polyline.cpp
   base/followed_polyline.hpp
   base/routing_result.hpp
@@ -67,6 +68,8 @@ set(
   joint_index.hpp
   joint_segment.cpp
   joint_segment.hpp
+  leaps_postprocessor.cpp
+  leaps_postprocessor.hpp
   loaded_path_segment.hpp
   maxspeeds.cpp
   maxspeeds.hpp
diff --git a/routing/index_router.cpp b/routing/index_router.cpp
index 66df863395..e609775819 100644
--- a/routing/index_router.cpp
+++ b/routing/index_router.cpp
@@ -1,6 +1,7 @@
 #include "routing/index_router.hpp"
 
 #include "routing/base/astar_progress.hpp"
+#include "routing/base/bfs.hpp"
 
 #include "routing/bicycle_directions.hpp"
 #include "routing/fake_ending.hpp"
@@ -9,6 +10,7 @@
 #include "routing/index_graph_serialization.hpp"
 #include "routing/index_graph_starter.hpp"
 #include "routing/index_road_graph.hpp"
+#include "routing/leaps_postprocessor.hpp"
 #include "routing/pedestrian_directions.hpp"
 #include "routing/restriction_loader.hpp"
 #include "routing/route.hpp"
@@ -636,12 +638,17 @@ RouterResultCode IndexRouter::CalculateSubroute(Checkpoints const & checkpoints,
     if (result != RouterResultCode::NoError)
       return result;
 
-    RouterResultCode const leapsResult = ProcessLeapsJoints(routingResult.m_path, delegate,
-                                                            starter.GetGraph().GetMode(), 
-                                                            starter, progress, subroute);
+    vector<Segment> subrouteWithoutPostprocessing;
+    RouterResultCode const leapsResult = ProcessLeapsJoints(routingResult.m_path, delegate, 
+                                                            starter.GetGraph().GetMode(),
+                                                            starter, progress,
+                                                            subrouteWithoutPostprocessing);
 
     if (leapsResult != RouterResultCode::NoError)
       return leapsResult;
+
+    LeapsPostProcessor leapsPostProcessor(subrouteWithoutPostprocessing, starter);
+    subroute = leapsPostProcessor.GetProcessedPath();
   }
 
   LOG(LINFO, ("Time for routing in mode:", starter.GetGraph().GetMode(), "is",
diff --git a/routing/leaps_postprocessor.cpp b/routing/leaps_postprocessor.cpp
new file mode 100644
index 0000000000..d57d012574
--- /dev/null
+++ b/routing/leaps_postprocessor.cpp
@@ -0,0 +1,177 @@
+#include "routing/leaps_postprocessor.hpp"
+
+#include "base/assert.hpp"
+#include "base/logging.hpp"
+#include "base/non_intersecting_intervals.hpp"
+#include "base/scope_guard.hpp"
+
+#include <algorithm>
+#include <iterator>
+#include <sstream>
+#include <string>
+#include <utility>
+
+namespace
+{
+using namespace routing;
+
+class NonIntersectingSegmentPaths
+{
+public:
+  void AddInterval(LeapsPostProcessor::PathInterval const & pathInterval)
+  {
+    if (!m_nonIntersectingIntervals.AddInterval(pathInterval.m_left, pathInterval.m_right))
+      return;
+
+    m_intervals.emplace_back(pathInterval);
+  }
+
+  std::vector<LeapsPostProcessor::PathInterval> && StealIntervals()
+  {
+    return std::move(m_intervals);
+  }
+
+private:
+  base::NonIntersectingIntervals<size_t> m_nonIntersectingIntervals;
+  std::vector<LeapsPostProcessor::PathInterval> m_intervals;
+};
+}  // namespace
+
+namespace routing
+{
+LeapsPostProcessor::LeapsPostProcessor(std::vector<Segment> const & path, IndexGraphStarter & starter)
+  : m_path(path),
+    m_starter(starter),
+    m_bfs(starter),
+    m_prefixSumETA(path.size(), 0.0)
+{
+  for (size_t i = 1; i < path.size(); ++i)
+  {
+    auto const & segment = path[i];
+    m_prefixSumETA[i] = m_prefixSumETA[i - 1] + starter.CalcSegmentETA(segment);
+
+    CHECK_EQUAL(m_segmentToIndex.count(segment), 0, ());
+    m_segmentToIndex[segment] = i;
+  }
+}
+
+std::vector<Segment> LeapsPostProcessor::GetProcessedPath()
+{
+  auto const intervalsToRelax = CalculateIntervalsToRelax();
+
+  NonIntersectingSegmentPaths toReplace;
+  for (auto const & interval : intervalsToRelax)
+    toReplace.AddInterval(interval);
+
+  std::vector<PathInterval> intervals = toReplace.StealIntervals();
+  std::sort(intervals.begin(), intervals.end());
+
+  std::vector<Segment> output;
+  size_t prevIndex = 0;
+  for (auto & interval : intervals)
+  {
+    CHECK(m_path.begin() + prevIndex <= m_path.begin() + interval.m_left, ());
+    output.insert(output.end(), m_path.begin() + prevIndex, m_path.begin() + interval.m_left);
+
+    auto leftIt = std::make_move_iterator(interval.m_path.begin());
+    auto rightIt = std::make_move_iterator(interval.m_path.end());
+    output.insert(output.end(), leftIt, rightIt);
+
+    prevIndex = interval.m_right + 1;
+  }
+
+  output.insert(output.end(), m_path.begin() + prevIndex, m_path.end());
+  return output;
+}
+
+auto LeapsPostProcessor::CalculateIntervalsToRelax() -> std::set<PathInterval, PathInterval::GreaterByWeight>
+{
+  std::set<PathInterval, PathInterval::GreaterByWeight> result;
+  for (size_t right = kMaxStep; right < m_path.size(); ++right)
+  {
+    std::map<Segment, SegmentData> segmentsData;
+    auto const & segment = m_path[right];
+    segmentsData.emplace(segment, SegmentData(0, m_starter.CalcSegmentETA(segment)));
+
+    FillIngoingPaths(segment, segmentsData);
+
+    for (auto const & item : segmentsData)
+    {
+      Segment const & visited = item.first;
+      SegmentData const & data = item.second;
+
+      auto const it = m_segmentToIndex.find(visited);
+      if (it == m_segmentToIndex.cend())
+        continue;
+
+      size_t const left = it->second;
+      if (left >= right || left == 0)
+        continue;
+
+      auto const prevWeight = m_prefixSumETA[right] - m_prefixSumETA[left - 1];
+      auto const curWeight = data.m_summaryETA;
+      if (prevWeight - PathInterval::kWeightEps <= curWeight)
+        continue;
+
+      double const winWeight = prevWeight - curWeight;
+      result.emplace(winWeight, left, right, m_bfs.ReconstructPath(visited, true /* reverse */));
+    }
+  }
+
+  return result;
+}
+
+void LeapsPostProcessor::FillIngoingPaths(
+    Segment const & start, std::map<Segment, LeapsPostProcessor::SegmentData> & segmentsData)
+{
+  m_bfs.Run(start, false /* isOutgoing */,
+            [&segmentsData, this](BFS<IndexGraphStarter>::State const & state)
+            {
+              if (segmentsData.count(state.m_vertex) != 0)
+                return false;
+
+              auto const & parent = segmentsData[state.m_parent];
+              ASSERT_LESS_OR_EQUAL(parent.m_steps, kMaxStep, ());
+              if (parent.m_steps == kMaxStep)
+                return false;
+
+              auto & current = segmentsData[state.m_vertex];
+              current.m_summaryETA =
+                  parent.m_summaryETA + m_starter.CalcSegmentETA(state.m_vertex);
+
+              current.m_steps = parent.m_steps + 1;
+
+              return true;
+            });
+}
+
+LeapsPostProcessor::SegmentData::SegmentData(size_t steps, double eta)
+  : m_steps(steps), m_summaryETA(eta) {}
+
+LeapsPostProcessor::PathInterval::PathInterval(double weight, size_t left, size_t right,
+                                               std::vector<Segment> && path)
+  : m_winWeight(weight), m_left(left), m_right(right), m_path(std::move(path)) {}
+
+bool LeapsPostProcessor::PathInterval::GreaterByWeight::operator()(PathInterval const & lhs,
+                                                                   PathInterval const & rhs) const
+{
+  return lhs.m_winWeight > rhs.m_winWeight;
+}
+
+bool LeapsPostProcessor::PathInterval::operator<(PathInterval const & rhs) const
+{
+  CHECK(m_left > rhs.m_right || m_right < rhs.m_left,
+        ("Intervals shouldn't intersect.", *this, rhs));
+
+  return m_right < rhs.m_left;
+}
+
+std::string DebugPrint(LeapsPostProcessor::PathInterval const & interval)
+{
+  std::stringstream ss;
+  ss << "[" << interval.m_left << ", " << interval.m_right << "], weight = "
+     << interval.m_winWeight;
+
+  return ss.str();
+}
+}  // namespace routing
diff --git a/routing/leaps_postprocessor.hpp b/routing/leaps_postprocessor.hpp
new file mode 100644
index 0000000000..cfdea5b95e
--- /dev/null
+++ b/routing/leaps_postprocessor.hpp
@@ -0,0 +1,66 @@
+#pragma once
+
+#include "routing/base/bfs.hpp"
+
+#include "routing/index_graph_starter.hpp"
+#include "routing/segment.hpp"
+
+#include "base/math.hpp"
+
+#include <cstdint>
+#include <map>
+#include <set>
+#include <vector>
+
+namespace routing
+{
+class LeapsPostProcessor
+{
+public:
+  LeapsPostProcessor(std::vector<Segment> const & path, IndexGraphStarter & starter);
+
+  struct PathInterval
+  {
+    static double constexpr kWeightEps = 1.0;
+
+    PathInterval(double weight, size_t left, size_t right, std::vector<Segment> && path);
+
+    struct GreaterByWeight
+    {
+      bool operator()(PathInterval const & lhs, PathInterval const & rhs) const;
+    };
+
+    bool operator<(PathInterval const & rhs) const;
+
+    double m_winWeight = 0.0;
+    size_t m_left = 0;
+    size_t m_right = 0;
+    std::vector<Segment> m_path;
+  };
+
+  std::vector<Segment> GetProcessedPath();
+
+private:
+  static size_t constexpr kMaxStep = 5;
+
+  struct SegmentData
+  {
+    SegmentData() = default;
+    SegmentData(size_t steps, double eta);
+    size_t m_steps = 0;
+    double m_summaryETA = 0.0;
+  };
+
+  std::set<PathInterval, PathInterval::GreaterByWeight> CalculateIntervalsToRelax();
+  void FillIngoingPaths(Segment const & start, std::map<Segment, SegmentData> & segmentsData);
+
+  std::vector<Segment> const & m_path;
+  IndexGraphStarter & m_starter;
+
+  BFS<IndexGraphStarter> m_bfs;
+  std::map<Segment, size_t> m_segmentToIndex;
+  std::vector<double> m_prefixSumETA;
+};
+
+std::string DebugPrint(LeapsPostProcessor::PathInterval const & interval);
+}  // namespace routing