Review the C++ implementation of the binary search function

cxx/isce3/core/Utilities.h

@@ -16,6 +16,8 @@
 #include <vector>
 #include <valarray>
 #include <complex>
+#include <cmath>
+#include <stdexcept>
 
 // Macro wrappers to check vector lengths 
 // (adds calling function and variable name information to the exception)
@@ -332,27 +334,80 @@ namespace isce3 { namespace core {
         }
     }
 
-    /** Searches array for index closest to provided value */   
-    inline int binarySearch(const std::valarray<double> & array, double value) {
-
-        // Do the binary search 
+    /** Searches array for index closest to provided value
+     *
+     *  Assumes the array is sorted in ascending order and contains no NaNs.
+     * 
+     * @param array The input array, which must be sorted in ascending order
+     * and contain no NaNs.
+     * @param value The value to search for.
+     * @param always_pick_left If true, and `value` is strictly between two
+     * array elements, the function will return the lower index (left).
+     * If false, the function returns the closest index (left or right).
+     *
+     * @return The index of the array element closest to `value`.
+    */
+    inline int binarySearch(const std::valarray<double> & array, double value,
+                            const bool always_pick_left = false) {
+
+        if (array.size() == 0) {
+                throw std::invalid_argument("input array must contain at least 1 element");
+        }
+        if (array.size() == 1) {
+                return 0;
+        }
+
         int left = 0;
         int right = array.size() - 1;
-        int index;
-        while (left <= right) {
-            const int middle = static_cast<int>(std::round(0.5 * (left + right)));
-            if (left == (right - 1)) {
-                index = left;
-                return index;
+
+        // Test extreme values
+        if (value <= array[left]) {
+            return left;
+        }
+        if (array[right] <= value) {
+            return right;
+        }
+
+        std::size_t count = 0;
+
+        // `max_iter` provides a safeguard against unexpected behavior.
+        // The loop is guaranteed to converge with correct input, but we include
+        // this check as a safe measure for unexpected edge cases.
+        constexpr std::size_t padding = 2;
+        std::size_t max_iter = std::ceil(std::log2(array.size())) + padding;
+        while (left + 1 < right) {
+            const int middle = left + (right - left) / 2;
+            const auto middle_value = array[middle];
+            if (std::isnan(middle_value)) {
+                throw std::invalid_argument("input array may not contain NaN values");
             }
-            if (array[middle] <= value) {
+            if (middle_value < value) {
                 left = middle;
-            } else if (array[middle] > value) {
+            } else {
                 right = middle;
             }
+            if (++count > max_iter) {
+                throw std::runtime_error(
+                    "Binary search failed to converge within the allowed iterations.");
+            }
         }
-        index = left;
-        return index;
+
+        // Test for NaNs
+        if (std::isnan(array[left]) || std::isnan(array[right])) {
+            throw std::invalid_argument("input array may not contain NaN values");
+        }
+
+        // If always pick left and the `right` index is not pointing at value
+        if (always_pick_left && value < array[right]) {
+            return left;
+        }
+
+        // Return the closest of left and right
+        if (std::abs(array[left] - value) <= std::abs(array[right] - value)) {
+            return left;
+        }
+
+        return right;
     }
 
     /** Clip a number between an upper and lower range (implements std::clamp for older GCC) */

cxx/isce3/geometry/Topo.cpp

@@ -812,7 +812,9 @@ setLayoverShadow(TopoLayers& layers, DEMInterpolator& demInterp,
 
             // Compute nearest ctrack index for current ctrackGrid value
             const double crossTrack = ctrackGrid[i];
-            int k = isce3::core::binarySearch(ctrack, crossTrack);
+            const bool always_pick_left = true;
+            int k = isce3::core::binarySearch(ctrack, crossTrack, always_pick_left);
+
             // Adjust edges if necessary
             if (k == (width - 1)) {
                 k = width - 2;

tests/cxx/isce3/Sources.cmake

@@ -23,6 +23,7 @@ core/projections/utm.cpp
 core/serialization/serializeAttitude.cpp
 core/serialization/serializeDoppler.cpp
 core/serialization/serializeOrbit.cpp
+core/utilities/binarySearch.cpp
 fft/fft.cpp
 fft/fftplan.cpp
 fft/fftutil.cpp

tests/cxx/isce3/core/utilities/binarySearch.cpp

@@ -0,0 +1,100 @@
+#include <gtest/gtest.h>
+
+#include <isce3/core/Utilities.h>
+
+using isce3::core::binarySearch;
+
+TEST(BinarySearchTest, SingleElementArray) {
+    std::valarray<double> arr = {2.0};
+    EXPECT_EQ(binarySearch(arr, 0.0), 0);
+    EXPECT_EQ(binarySearch(arr, 100.0), 0);
+}
+
+TEST(BinarySearchTest, SingleElementArrayPickLeft) {
+    const bool always_pick_left = true;
+    std::valarray<double> arr = {2.0};
+    EXPECT_EQ(binarySearch(arr, 0.0, always_pick_left), 0);
+    EXPECT_EQ(binarySearch(arr, 100.0, always_pick_left), 0);
+}
+
+
+TEST(BinarySearchTest, ClosestMatchExact) {
+    std::valarray<double> arr = {1.0, 3.0, 5.0, 7.0};
+    EXPECT_EQ(binarySearch(arr, 3.0), 1);
+    EXPECT_EQ(binarySearch(arr, 7.0), 3);
+}
+
+TEST(BinarySearchTest, ClosestMatchExactPickLeft) {
+    const bool always_pick_left = true;
+    std::valarray<double> arr = {1.0, 3.0, 5.0, 7.0};
+    EXPECT_EQ(binarySearch(arr, 3.0, always_pick_left), 1);
+    EXPECT_EQ(binarySearch(arr, 7.0, always_pick_left), 3);
+}
+
+TEST(BinarySearchTest, ClosestMatchInBetween) {
+    std::valarray<double> arr = {1.0, 3.0, 5.0, 7.0};
+    EXPECT_EQ(binarySearch(arr, 2.1), 1); // closer to `3`
+    EXPECT_EQ(binarySearch(arr, 4.1), 2); // closer to `5`
+    EXPECT_EQ(binarySearch(arr, 6.4), 3); // closer to `7`
+}
+
+TEST(BinarySearchTest, ClosestMatchInBetweenPickLeft) {
+    const bool always_pick_left = true;
+    std::valarray<double> arr = {1.0, 3.0, 5.0, 7.0};
+    EXPECT_EQ(binarySearch(arr, 2.1, always_pick_left), 0);
+    EXPECT_EQ(binarySearch(arr, 4.1, always_pick_left), 1);
+    EXPECT_EQ(binarySearch(arr, 6.4, always_pick_left), 2);
+}
+
+TEST(BinarySearchTest, ValueBeforeFirstElement) {
+    std::valarray<double> arr = {1.0, 2.0, 3.0};
+    EXPECT_EQ(binarySearch(arr, -1.0), 0); // `-1` closest to `1`
+}
+
+TEST(BinarySearchTest, ValueBeforeFirstElementPickLeft) {
+    const bool always_pick_left = true;
+    std::valarray<double> arr = {1.0, 2.0, 3.0};
+    EXPECT_EQ(binarySearch(arr, -1.0, always_pick_left), 0);
+}
+
+TEST(BinarySearchTest, ValueAfterLastElement) {
+    std::valarray<double> arr = {1.0, 2.0, 3.0};
+    EXPECT_EQ(binarySearch(arr, 5.0), 2); // `5` closest to `3`
+}
+
+TEST(BinarySearchTest, ValueAfterLastElementPickLeft) {
+    const bool always_pick_left = true;
+    std::valarray<double> arr = {1.0, 2.0, 3.0};
+    EXPECT_EQ(binarySearch(arr, 5.0, always_pick_left), 2);
+}
+
+TEST(BinarySearchTest, EqualDistanceChoosesLeft) {
+    std::valarray<double> arr = {1.0, 2.0, 3.0, 4.0};
+
+    // `2` and `3` equally distant; picks left (`2`)
+    EXPECT_EQ(binarySearch(arr, 2.5), 1);
+}
+
+TEST(BinarySearchTest, EqualDistanceChoosesLeftPickLeft) {
+    const bool always_pick_left = true;
+    std::valarray<double> arr = {1.0, 2.0, 3.0, 4.0};
+
+    // `2` and `3` equally distant; picks left (`2`)
+    EXPECT_EQ(binarySearch(arr, 2.5, always_pick_left), 1);
+}
+
+TEST(BinarySearchTest, ThrowsOnEmptyArray) {
+    std::valarray<double> arr;
+    EXPECT_THROW(binarySearch(arr, 0.0), std::invalid_argument);
+}
+
+TEST(BinarySearchTest, ThrowsOnNaN) {
+    std::valarray<double> arr = {1.0, 2.0, NAN, 4.0};
+    EXPECT_THROW(binarySearch(arr, 3.0), std::invalid_argument);
+}
+
+int main(int argc, char * argv[])
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}