From f1ce2e6384f529dbf9d456c470920fc0a1320faf Mon Sep 17 00:00:00 2001
From: Siarhei Fedartsou <siarhei.fedartsou@gmail.com>
Date: Sun, 28 Jul 2024 12:04:19 +0200
Subject: [PATCH] wip

---
 .../routing_algorithms/routing_base_ch.hpp    | 11 +++++
 .../routing_algorithms/routing_base_mld.hpp   | 25 ++++++++++++
 .../routing_algorithms/map_matching.cpp       | 40 +++++++++++++++++++
 3 files changed, 76 insertions(+)

diff --git a/include/engine/routing_algorithms/routing_base_ch.hpp b/include/engine/routing_algorithms/routing_base_ch.hpp
index e2e3b5400..e39702611 100644
--- a/include/engine/routing_algorithms/routing_base_ch.hpp
+++ b/include/engine/routing_algorithms/routing_base_ch.hpp
@@ -460,6 +460,17 @@ void search(SearchEngineData<Algorithm> &engine_working_data,
                   duration_upper_bound);
 }
 
+inline std::vector<double> getNetworkDistances(SearchEngineData<Algorithm> &,
+                          const DataFacade<ch::Algorithm> &,
+                          SearchEngineData<Algorithm>::QueryHeap &,
+                          SearchEngineData<Algorithm>::QueryHeap &,
+                          const PhantomNode &,
+                          const std::vector<PhantomNode> &,
+                          EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT) {
+    std::vector<double> distances;
+    return distances;
+                          }
+
 // Requires the heaps for be empty
 // If heaps should be adjusted to be initialized outside of this function,
 // the addition of force_step parameters might be required
diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp
index aedbcf75a..e668cbd99 100644
--- a/include/engine/routing_algorithms/routing_base_mld.hpp
+++ b/include/engine/routing_algorithms/routing_base_mld.hpp
@@ -705,6 +705,8 @@ void unpackPath(const FacadeT &facade,
     annotatePath(facade, route_endpoints, unpacked_nodes, unpacked_edges, unpacked_path);
 }
 
+
+
 template <typename Algorithm>
 double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                           const DataFacade<Algorithm> &facade,
@@ -763,6 +765,29 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
     return from_alias<double>(distance);
 }
 
+template <typename Algorithm>
+std::vector<double> getNetworkDistances(SearchEngineData<Algorithm> &engine_working_data,
+                          const DataFacade<Algorithm> &facade,
+ typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
+                          typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &reverse_heap,
+                          const PhantomNode &source_phantom,
+                          const std::vector<PhantomNode> &target_phantoms,
+                          EdgeWeight duration_upper_bound = INVALID_EDGE_WEIGHT) {
+    std::vector<double> distances;
+    for (const auto &target_phantom : target_phantoms)
+    {
+        auto distance = getNetworkDistance(engine_working_data,
+                                           facade,
+                                           forward_heap,
+                                           reverse_heap,
+                                           source_phantom,
+                                           target_phantom,
+                                           duration_upper_bound);
+        distances.push_back(distance);
+    }
+    return distances;
+}
+
 } // namespace osrm::engine::routing_algorithms::mld
 
 #endif // OSRM_ENGINE_ROUTING_BASE_MLD_HPP
diff --git a/src/engine/routing_algorithms/map_matching.cpp b/src/engine/routing_algorithms/map_matching.cpp
index 45133551e..be11d8bc3 100644
--- a/src/engine/routing_algorithms/map_matching.cpp
+++ b/src/engine/routing_algorithms/map_matching.cpp
@@ -225,6 +225,46 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                     continue;
                 }
 
+                std::vector<PhantomNode> target_phantom_nodes;
+                for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
+                {
+                    const double emission_pr = emission_log_probabilities[t][s_prime];
+                    double new_value = prev_viterbi[s] + emission_pr;
+                    if (current_viterbi[s_prime] > new_value)
+                    {
+                        continue;
+                    }
+                    target_phantom_nodes.push_back(current_timestamps_list[s_prime].phantom_node);
+                }
+
+                auto new_distances = getNetworkDistances(engine_working_data,
+                                    facade,
+                                    forward_heap,
+                                    reverse_heap,
+                                    prev_unbroken_timestamps_list[s].phantom_node,
+                                    target_phantom_nodes,
+                                    weight_upper_bound);
+
+                std::vector<double> old_distances;
+
+                for (const auto& pn: target_phantom_nodes)
+                {
+                     double network_distance =
+                        getNetworkDistance(engine_working_data,
+                                           facade,
+                                           forward_heap,
+                                           reverse_heap,
+                                           prev_unbroken_timestamps_list[s].phantom_node,
+                                           pn,
+                                           weight_upper_bound);
+                     old_distances.push_back(network_distance);
+                }
+
+                if (old_distances != new_distances) {
+                    std::cerr << "OOPS " << std::endl;
+                }
+
+
                 for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
                 {
                     const double emission_pr = emission_log_probabilities[t][s_prime];