diff --git a/src/finer/Peak.h b/src/finer/Peak.h
index 87a224f..7ede11c 100644
--- a/src/finer/Peak.h
+++ b/src/finer/Peak.h
@@ -98,9 +98,11 @@ public:
             int np = i;
             if (j < nPeaks) {
                 np = m_locations[j];
+            } else if (nPeaks > 0) {
+                np = m_locations[nPeaks-1];
             }
             if (next) {
-                if (pp == i) {
+                if (pp == i || j >= nPeaks) {
                     next[i] = i;
                 } else {
                     next[i] = np;
diff --git a/src/test/TestSignalBits.cpp b/src/test/TestSignalBits.cpp
index 5610a79..73dc467 100644
--- a/src/test/TestSignalBits.cpp
+++ b/src/test/TestSignalBits.cpp
@@ -29,19 +29,10 @@
 
 using namespace RubberBand;
 using namespace std;
+namespace tt = boost::test_tools;
 
 BOOST_AUTO_TEST_SUITE(TestSignalBits)
 
-#define COMPARE_N(a, b, n)						\
-    for (int cmp_i = 0; cmp_i < n; ++cmp_i) {                           \
-        BOOST_CHECK_SMALL(a[cmp_i] - b[cmp_i], 1e-14);			\
-    }
-
-#define COMPARE_INT_N(a, b, n)						\
-    for (int cmp_i = 0; cmp_i < n; ++cmp_i) {                           \
-        BOOST_CHECK_EQUAL(a[cmp_i], b[cmp_i]);                          \
-    }
-
 // NB our moving median has different lag behaviour from bsq - we
 // begin padded with zeros, while bsq begins with an empty vector. The
 // bsq behaviour is imho more correct, and this really shows up in the
@@ -55,7 +46,7 @@ BOOST_AUTO_TEST_CASE(moving_median_simple_3)
     vector<double> arr { 1.0, 2.0, 3.0 };
     vector<double> expected { 1.0, 2.0, 2.0 };
     MovingMedian<double>::filter(mm, arr);
-    COMPARE_N(arr, expected, 3);
+    BOOST_TEST(arr == expected, tt::per_element());
 }
 
 BOOST_AUTO_TEST_CASE(moving_median_simple_4)
@@ -64,7 +55,7 @@ BOOST_AUTO_TEST_CASE(moving_median_simple_4)
     vector<double> arr { 1.0, 2.0, 3.0, 4.0 };
     vector<double> expected { 2.0, 3.0, 3.0, 3.0 };
     MovingMedian<double>::filter(mm, arr);
-    COMPARE_N(arr, expected, 4);
+    BOOST_TEST(arr == expected, tt::per_element());
 }
 
 BOOST_AUTO_TEST_CASE(moving_median_simple_3_4)
@@ -73,7 +64,7 @@ BOOST_AUTO_TEST_CASE(moving_median_simple_3_4)
     vector<double> arr { 1.2, 0.6, 1.0e-6, 1.0 };
     vector<double> expected { 0.6, 0.6, 0.6, 1.0e-6 };
     MovingMedian<double>::filter(mm, arr);
-    COMPARE_N(arr, expected, 4);
+    BOOST_TEST(arr == expected, tt::per_element());
 }
 
 BOOST_AUTO_TEST_CASE(moving_median_simple_5_4)
@@ -82,7 +73,7 @@ BOOST_AUTO_TEST_CASE(moving_median_simple_5_4)
     vector<double> arr { 1.2, 0.6, 1.0e-6, 1.0 };
     vector<double> expected { 1.0e-6, 0.6, 0.6, 1.0e-6 };
     MovingMedian<double>::filter(mm, arr);
-    COMPARE_N(arr, expected, 4);
+    BOOST_TEST(arr == expected, tt::per_element());
 }
   
 BOOST_AUTO_TEST_CASE(moving_median_order_1)
@@ -91,7 +82,7 @@ BOOST_AUTO_TEST_CASE(moving_median_order_1)
     vector<double> arr { 1.2, 0.6, 1.0e-6, 1.0e-6 };
     vector<double> expected = arr;
     MovingMedian<double>::filter(mm, arr);
-    COMPARE_N(arr, expected, 4);
+    BOOST_TEST(arr == expected, tt::per_element());
 }
   
 BOOST_AUTO_TEST_CASE(moving_median_n_1)
@@ -100,7 +91,7 @@ BOOST_AUTO_TEST_CASE(moving_median_n_1)
     vector<double> arr { 1.0 };
     vector<double> expected { 0.0 };
     MovingMedian<double>::filter(mm, arr);
-    COMPARE_N(arr, expected, 1);
+    BOOST_TEST(arr == expected, tt::per_element());
 }
 
 BOOST_AUTO_TEST_CASE(peakpick_nearest_2_1)
@@ -110,7 +101,7 @@ BOOST_AUTO_TEST_CASE(peakpick_nearest_2_1)
     vector<int> out(1);
     vector<int> expected { 0 };
     pp.findNearestAndNextPeaks(in.data(), 2, out.data(), nullptr);
-    COMPARE_INT_N(out, expected, 1);
+    BOOST_TEST(out == expected, tt::per_element());
 }
 
 BOOST_AUTO_TEST_CASE(peakpick_nearest_2_5)
@@ -120,7 +111,7 @@ BOOST_AUTO_TEST_CASE(peakpick_nearest_2_5)
     vector<int> out(5);
     vector<int> expected { 3, 3, 3, 3, 3 };
     pp.findNearestAndNextPeaks(in.data(), 2, out.data(), nullptr);
-    COMPARE_INT_N(out, expected, 5);
+    BOOST_TEST(out == expected, tt::per_element());
 }
 
 BOOST_AUTO_TEST_CASE(peakpick_nearest_2_12)
@@ -131,7 +122,100 @@ BOOST_AUTO_TEST_CASE(peakpick_nearest_2_12)
     vector<int> out(12);
     vector<int> expected { 3, 3, 3, 3, 3, 3, 8, 8, 8, 8, 8, 8 };
     pp.findNearestAndNextPeaks(in.data(), 2, out.data(), nullptr);
-    COMPARE_INT_N(out, expected, 12);
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(peakpick_next_2_1)
+{
+    Peak<double> pp(1);
+    vector<double> in { -0.1 };
+    vector<int> out(1);
+    vector<int> expected { 0 };
+    pp.findNearestAndNextPeaks(in.data(), 2, nullptr, out.data());
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(peakpick_next_2_5)
+{
+    Peak<double> pp(5);
+    vector<double> in { -0.3, -0.1, -0.2, 1.0, -0.3 };
+    vector<int> out(5);
+    vector<int> expected { 3, 3, 3, 3, 4 };
+    pp.findNearestAndNextPeaks(in.data(), 2, nullptr, out.data());
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(peakpick_next_2_12)
+{
+    Peak<double> pp(12);
+    vector<double> in { -0.3, -0.1, 0.2, -1.0, -0.3, -0.5,
+                        -0.5, -0.4, -0.1, -0.1, -0.2, -0.3 };
+    vector<int> out(12);
+    vector<int> expected { 2, 2, 2, 8, 8, 8, 8, 8, 8, 9, 10, 11 };
+    pp.findNearestAndNextPeaks(in.data(), 2, nullptr, out.data());
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(troughpick_nearest_2_1)
+{
+    Peak<double, std::less<double>> pp(1);
+    vector<double> in { -0.1 };
+    vector<int> out(1);
+    vector<int> expected { 0 };
+    pp.findNearestAndNextPeaks(in.data(), 2, out.data(), nullptr);
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(troughpick_nearest_2_5)
+{
+    Peak<double, std::less<double>> pp(5);
+    vector<double> in { -0.3, -0.1, -0.4, 0.1, -0.3 };
+    vector<int> out(5);
+    vector<int> expected { 2, 2, 2, 2, 2 };
+    pp.findNearestAndNextPeaks(in.data(), 2, out.data(), nullptr);
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(troughpick_nearest_2_12)
+{
+    Peak<double, std::less<double>> pp(12);
+    vector<double> in { -0.3, -0.1, -0.2, 1.0, -0.3, -0.5,
+                        -0.5, -0.4, -0.1, -0.1, -0.2, -0.3 };
+    vector<int> out(12);
+    vector<int> expected { 0, 0, 0, 5, 5, 5, 5, 5, 11, 11, 11, 11 };
+    pp.findNearestAndNextPeaks(in.data(), 2, out.data(), nullptr);
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(troughpick_next_2_1)
+{
+    Peak<double, std::less<double>> pp(1);
+    vector<double> in { -0.1 };
+    vector<int> out(1);
+    vector<int> expected { 0 };
+    pp.findNearestAndNextPeaks(in.data(), 2, nullptr, out.data());
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(troughpick_next_2_5)
+{
+    Peak<double, std::less<double>> pp(5);
+    vector<double> in { -0.3, -0.1, -0.4, 0.1, -0.3 };
+    vector<int> out(5);
+    vector<int> expected { 2, 2, 2, 3, 4 };
+    pp.findNearestAndNextPeaks(in.data(), 2, nullptr, out.data());
+    BOOST_TEST(out == expected, tt::per_element());
+}
+
+BOOST_AUTO_TEST_CASE(troughpick_next_2_12)
+{
+    Peak<double, std::less<double>> pp(12);
+    vector<double> in { -0.3, -0.1, 0.2, 1.0, -0.3, -0.5,
+                        -0.5, -0.4, -0.1, -0.1, -0.2, -0.3 };
+    vector<int> out(12);
+    vector<int> expected { 0, 5, 5, 5, 5, 5, 11, 11, 11, 11, 11, 11 };
+    pp.findNearestAndNextPeaks(in.data(), 2, nullptr, out.data());
+    BOOST_TEST(out == expected, tt::per_element());
 }
 
 BOOST_AUTO_TEST_SUITE_END()