From e99fcdd26a0e3be9bbea672d64c2468d8e707a17 Mon Sep 17 00:00:00 2001
From: Beomki Yeo <beomki.yeo@gmail.com>
Date: Sun, 13 Oct 2024 04:44:01 +0200
Subject: [PATCH 1/3] Wrap the phi of track parameters in the range of [-pi,pi]

---
 core/include/traccc/edm/track_parameters.hpp     | 16 ++++++++++++++++
 .../kalman_filter/gain_matrix_smoother.hpp       |  2 ++
 .../kalman_filter/gain_matrix_updater.hpp        |  2 ++
 3 files changed, 20 insertions(+)

diff --git a/core/include/traccc/edm/track_parameters.hpp b/core/include/traccc/edm/track_parameters.hpp
index c240f397dd..a310591c6b 100644
--- a/core/include/traccc/edm/track_parameters.hpp
+++ b/core/include/traccc/edm/track_parameters.hpp
@@ -31,4 +31,20 @@ using bound_covariance = bound_track_parameters::covariance_type;
 using bound_track_parameters_collection_types =
     collection_types<bound_track_parameters>;
 
+// Wrap the phi of track parameters to [-pi,pi]
+TRACCC_HOST_DEVICE
+inline void wrap_phi(bound_track_parameters& param) {
+
+    traccc::scalar phi = param.phi();
+    static constexpr traccc::scalar TWOPI =
+        2. * traccc::constant<traccc::scalar>::pi;
+    phi = math::fmod(phi, TWOPI);
+    if (phi > traccc::constant<traccc::scalar>::pi) {
+        phi -= TWOPI;
+    } else if (phi < -traccc::constant<traccc::scalar>::pi) {
+        phi += TWOPI;
+    }
+    param.set_phi(phi);
+}
+
 }  // namespace traccc
diff --git a/core/include/traccc/fitting/kalman_filter/gain_matrix_smoother.hpp b/core/include/traccc/fitting/kalman_filter/gain_matrix_smoother.hpp
index cd5a7e7b81..b6e1a71b53 100644
--- a/core/include/traccc/fitting/kalman_filter/gain_matrix_smoother.hpp
+++ b/core/include/traccc/fitting/kalman_filter/gain_matrix_smoother.hpp
@@ -107,6 +107,8 @@ struct gain_matrix_smoother {
 
         cur_state.smoothed().set_vector(smt_vec);
         cur_state.smoothed().set_covariance(smt_cov);
+        // Wrap the phi in the range of [-pi, pi]
+        wrap_phi(cur_state.smoothed());
 
         matrix_type<D, e_bound_size> H = meas.subs.template projector<D>();
 
diff --git a/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp b/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp
index f1e68c493a..b92fbbe17f 100644
--- a/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp
+++ b/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp
@@ -124,6 +124,8 @@ struct gain_matrix_updater {
         // Set the stepper parameter
         bound_params.set_vector(filtered_vec);
         bound_params.set_covariance(filtered_cov);
+        // Wrap the phi in the range of [-pi, pi]
+        wrap_phi(bound_params);
 
         // Set the track state parameters
         trk_state.filtered().set_vector(filtered_vec);

From cc3ef9be771e19902df9a059c281b50c583cbeea Mon Sep 17 00:00:00 2001
From: Beomki Yeo <beomki.yeo@gmail.com>
Date: Sun, 13 Oct 2024 04:51:09 +0200
Subject: [PATCH 2/3] Add header'

---
 core/include/traccc/edm/track_parameters.hpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/core/include/traccc/edm/track_parameters.hpp b/core/include/traccc/edm/track_parameters.hpp
index a310591c6b..e2912237a0 100644
--- a/core/include/traccc/edm/track_parameters.hpp
+++ b/core/include/traccc/edm/track_parameters.hpp
@@ -9,6 +9,7 @@
 
 // traccc include
 #include "traccc/definitions/common.hpp"
+#include "traccc/definitions/math.hpp"
 #include "traccc/definitions/primitives.hpp"
 #include "traccc/definitions/qualifiers.hpp"
 #include "traccc/definitions/track_parametrization.hpp"

From 89e588163553d4ef62549554a92344514d989a75 Mon Sep 17 00:00:00 2001
From: beomki-yeo <63090140+beomki-yeo@users.noreply.github.com>
Date: Mon, 14 Oct 2024 15:20:51 +0200
Subject: [PATCH 3/3] Update core/include/traccc/edm/track_parameters.hpp

Co-authored-by: Joana Niermann <53186085+niermann999@users.noreply.github.com>
---
 core/include/traccc/edm/track_parameters.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/core/include/traccc/edm/track_parameters.hpp b/core/include/traccc/edm/track_parameters.hpp
index e2912237a0..4b62da294a 100644
--- a/core/include/traccc/edm/track_parameters.hpp
+++ b/core/include/traccc/edm/track_parameters.hpp
@@ -38,7 +38,7 @@ inline void wrap_phi(bound_track_parameters& param) {
 
     traccc::scalar phi = param.phi();
     static constexpr traccc::scalar TWOPI =
-        2. * traccc::constant<traccc::scalar>::pi;
+        2.f * traccc::constant<traccc::scalar>::pi;
     phi = math::fmod(phi, TWOPI);
     if (phi > traccc::constant<traccc::scalar>::pi) {
         phi -= TWOPI;