From dbb3d6d557a7192dc61035571cfb9728122377fb Mon Sep 17 00:00:00 2001
From: Sven Czarnian <devel@svcz.de>
Date: Wed, 15 Dec 2021 16:53:55 +0100
Subject: [PATCH] fix the issues with the predicted path to planned path
 matching

---
 include/aman/types/Inbound.h |  2 +-
 src/types/Inbound.cpp        | 49 ++++++++++++++++++++++++++----------
 2 files changed, 37 insertions(+), 14 deletions(-)

diff --git a/include/aman/types/Inbound.h b/include/aman/types/Inbound.h
index 2a8a31c..f326f86 100644
--- a/include/aman/types/Inbound.h
+++ b/include/aman/types/Inbound.h
@@ -45,7 +45,7 @@ namespace aman {
         Velocity groundSpeed(const Length& altitude, const Velocity& ias, const Angle& heading);
         void createWindTables(const google::protobuf::RepeatedPtrField<aman::WindData>& wind);
         static int matchToPredictedPath(const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions, const GeoCoordinate& position,
-                                        Length& trackmiles);
+                                        const Velocity& groundspeed, Length& trackmiles);
         void predictETA(const EuroScopePlugIn::CRadarTarget& target, const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions);
 
     public:
diff --git a/src/types/Inbound.cpp b/src/types/Inbound.cpp
index e36a205..f2e6ab3 100644
--- a/src/types/Inbound.cpp
+++ b/src/types/Inbound.cpp
@@ -229,30 +229,46 @@ static __inline Angle __normalize(const Angle& angle) {
 }
 
 int Inbound::matchToPredictedPath(const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions, const GeoCoordinate& position,
-                                  Length& trackmiles) {
+                                  const Velocity& groundspeed, Length& trackmiles) {
     trackmiles = 0_m;
 
-    if (0 == predictions.GetPointsNumber())
+    if (1 >= predictions.GetPointsNumber())
         return 0;
 
-    GeoCoordinate lastPosition(__convert(predictions.GetPosition(0)));
+    std::vector<Length> distances;
+    distances.reserve(static_cast<std::size_t>(predictions.GetPointsNumber()) - 1);
 
+    /* calculate the "potential field" for the distances*/
     for (int i = 1; i < predictions.GetPointsNumber(); ++i) {
-        GeoCoordinate coordinate(__convert(predictions.GetPosition(i)));
+        const GeoCoordinate coordinate(__convert(predictions.GetPosition(i)));
+        distances.push_back(coordinate.distanceTo(position));
+    }
 
-        const auto prev = lastPosition.bearingTo(position);
-        const auto next = coordinate.bearingTo(position);
-        const auto delta = __normalize(prev - next);
-        if (90_deg <= delta.abs() || 2.0_nm >= lastPosition.distanceTo(position)) {
-            trackmiles += lastPosition.distanceTo(position);
-            return i - 1;
-        }
+    /* get the index of the minimal value*/
+    std::size_t idx = std::min_element(distances.cbegin(), distances.cend()) - distances.cbegin();
 
+    /* calculate the theoretical maximum distance (assuming one minute steps) and validate the distance */
+    const auto& maxDistance = groundspeed * 1_min;
+    if (gsl::at(distances, idx) > maxDistance)
+        return 0;
+
+    if (idx + 1 < static_cast<std::size_t>(predictions.GetPointsNumber()) && 0 != idx) {
+        const auto bearingCurrent = position.bearingTo(GeoCoordinate(__convert(predictions.GetPosition(idx))));
+        const auto bearingNext = position.bearingTo(GeoCoordinate(__convert(predictions.GetPosition(idx + 1))));
+
+        /* current and next in front of position -> use the previous element */
+        if (90_deg > __normalize(bearingNext - bearingCurrent).abs())
+            idx -= 1;
+    }
+
+    GeoCoordinate lastPosition(__convert(predictions.GetPosition(0)));
+    for (std::size_t i = 1; i <= static_cast<int>(idx + 1); ++i) {
+        const GeoCoordinate coordinate(__convert(predictions.GetPosition(i)));
         trackmiles += lastPosition.distanceTo(coordinate);
         lastPosition = coordinate;
     }
 
-    return predictions.GetPointsNumber();
+    return static_cast<int>(idx);
 }
 
 void Inbound::predictETA(const EuroScopePlugIn::CRadarTarget& target, const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions) {
@@ -261,9 +277,16 @@ void Inbound::predictETA(const EuroScopePlugIn::CRadarTarget& target, const Euro
         return;
     }
 
+    /* get the ground speed */
+    Velocity groundspeed;
+    if (0 == target.GetGS())
+        groundspeed = static_cast<double>(target.GetPosition().GetReportedGS()) * knot;
+    else
+        groundspeed = static_cast<double>(target.GetGS()) * knot;
+
     const auto& destination = gsl::at(this->m_arrivalRoute, this->m_nextStarWaypoint);
     Length distanceToNextWaypoint = 0_m;
-    Inbound::matchToPredictedPath(predictions, destination.position(), distanceToNextWaypoint);
+    Inbound::matchToPredictedPath(predictions, destination.position(), groundspeed, distanceToNextWaypoint);
     this->m_trackmiles = distanceToNextWaypoint;
 
     /* predict the flight and the descend */