From cd8ad1584c80889ae3893b45d3764fa06b678762 Mon Sep 17 00:00:00 2001
From: Morten Fyhn Amundsen <morten.fyhn.amundsen@gmail.com>
Date: Mon, 24 Aug 2020 10:49:12 +0200
Subject: [PATCH] Make wrap() work with integer types (#145)

* Make wrap() work with integers
---
 matrix/helper_functions.hpp | 61 ++++++++++++++++++++++++++++++-------
 test/helper.cpp             |  7 +++++
 2 files changed, 57 insertions(+), 11 deletions(-)

diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
index 89f2dbffb95b..a5c3fbc3a563 100644
--- a/matrix/helper_functions.hpp
+++ b/matrix/helper_functions.hpp
@@ -39,25 +39,64 @@ bool isEqualF(const Type x, const Type y, const Type eps = 1e-4f)
            || (isinf(x) && isinf(y) && isnan(x - y));
 }
 
+namespace detail
+{
+
+template<typename Floating>
+Floating wrap_floating(Floating x, Floating low, Floating high) {
+    // already in range
+    if (low <= x && x < high) {
+        return x;
+    }
+
+    const auto range = high - low;
+    const auto inv_range = Floating(1) / range; // should evaluate at compile time, multiplies below at runtime
+    const auto num_wraps = floor((x - low) * inv_range);
+    return x - range * num_wraps;
+}
+
+}  // namespace detail
+
 /**
- * Wrap value to stay in range [low, high)
+ * Wrap single precision floating point value to stay in range [low, high)
  *
  * @param x input possibly outside of the range
  * @param low lower limit of the allowed range
  * @param high upper limit of the allowed range
  * @return wrapped value inside the range
  */
-template<typename Type>
-Type wrap(Type x, Type low, Type high) {
-    // already in range
-    if (low <= x && x < high) {
-        return x;
-    }
+float wrap(float x, float low, float high) {
+    return matrix::detail::wrap_floating(x, low, high);
+}
 
-    const Type range = high - low;
-    const Type inv_range = Type(1) / range; // should evaluate at compile time, multiplies below at runtime
-    const Type num_wraps = floor((x - low) * inv_range);
-    return x - range * num_wraps;
+/**
+ * Wrap double precision floating point value to stay in range [low, high)
+ *
+ * @param x input possibly outside of the range
+ * @param low lower limit of the allowed range
+ * @param high upper limit of the allowed range
+ * @return wrapped value inside the range
+ */
+double wrap(double x, double low, double high) {
+    return matrix::detail::wrap_floating(x, low, high);
+}
+
+/**
+ * Wrap integer value to stay in range [low, high)
+ *
+ * @param x input possibly outside of the range
+ * @param low lower limit of the allowed range
+ * @param high upper limit of the allowed range
+ * @return wrapped value inside the range
+ */
+template<typename Integer>
+Integer wrap(Integer x, Integer low, Integer high) {
+    const auto range = high - low;
+
+    if (x < low)
+        x += range * ((low - x) / range + 1);
+
+    return low + (x - low) % range;
 }
 
 /**
diff --git a/test/helper.cpp b/test/helper.cpp
index c9f42cfdd588..6d8b539f3a1a 100644
--- a/test/helper.cpp
+++ b/test/helper.cpp
@@ -21,6 +21,13 @@ int main()
     TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
     TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
 
+    // integer wraps
+    TEST(wrap(-10, 0, 10) == 0);
+    TEST(wrap(-4, 0, 10) == 6);
+    TEST(wrap(0, 0, 10) == 0)
+    TEST(wrap(4, 0, 10) == 4);
+    TEST(wrap(10, 0, 10) == 0);
+
     // wrap pi
     TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);
     TEST(fabs(wrap_pi(4.) - (4. - M_TWOPI)) < FLT_EPSILON);