From 19661fe98144f34deb6092eba158450368af91eb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 14 Feb 2022 12:32:34 +1100
Subject: [PATCH] AP_Common: added uint16_t sorting code

also added test suite
---
 libraries/AP_Common/sorting.cpp            | 109 ++++++++++++++
 libraries/AP_Common/sorting.h              |  44 ++++++
 libraries/AP_Common/tests/test_sorting.cpp | 156 +++++++++++++++++++++
 3 files changed, 309 insertions(+)
 create mode 100644 libraries/AP_Common/sorting.cpp
 create mode 100644 libraries/AP_Common/sorting.h
 create mode 100644 libraries/AP_Common/tests/test_sorting.cpp

diff --git a/libraries/AP_Common/sorting.cpp b/libraries/AP_Common/sorting.cpp
new file mode 100644
index 0000000000..cc68563696
--- /dev/null
+++ b/libraries/AP_Common/sorting.cpp
@@ -0,0 +1,109 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include "sorting.h"
+
+/*
+  in-place insertion sort for small arrays of data. This is O(n) if
+  already sorted and O(n^2) for worst case (elements are reversed)
+  sort order is smallest first
+ */
+void insertion_sort_uint16(uint16_t *data, uint16_t n)
+{
+    for (uint16_t i=1; i<n; i++) {
+        uint16_t temp = data[i];
+        int16_t j = i - 1;
+
+        while (j >= 0 && data[j] > temp) {
+            data[j+1] = data[j];
+            j--;
+		}
+        data[j+1] = temp;
+    }
+}
+
+/*
+  remove duplicates from a sorted uint16_t array, returning the new
+  count
+ */
+uint16_t remove_duplicates_uint16(uint16_t *data, uint16_t n)
+{
+    uint16_t removed = 0;
+    for (uint16_t i=1; i<n; i++) {
+        if (data[i-(1+removed)] == data[i]) {
+            removed++;
+        } else if (removed != 0) {
+            data[i-removed] = data[i];
+        }
+    }
+    return n - removed;
+}
+
+/*
+  bisection search on a sorted uint16_t array to find an element
+  return true if found
+*/
+bool bisect_search_uint16(const uint16_t *data, uint16_t n, uint16_t value)
+{
+    if (n == 0) {
+        return false;
+    }
+    uint16_t low=0, high=n-1;
+    while (low < high) {
+        uint16_t mid = (low+high)/2;
+        if (value < data[mid]) {
+            high = mid;
+            continue;
+        }
+        if (value > data[mid]) {
+            low = mid+1;
+            continue;
+        }
+        return true;
+    }
+    return data[low] == value;
+}
+
+/*
+  remove elements in a 2nd sorted array from a sorted uint16_t array
+  return the number of remaining elements
+ */
+uint16_t remove_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2)
+{
+    uint16_t removed = 0;
+    for (uint16_t i=0; i<n; i++) {
+        if (bisect_search_uint16(rem, n2, data[i])) {
+            removed++;
+        } else if (removed != 0) {
+            data[i-removed] = data[i];
+        }
+    }
+    return n - removed;
+}
+
+/*
+  return number of common elements between two sorted uint16_t lists
+ */
+uint16_t common_list_uint16(uint16_t *data, uint16_t n, const uint16_t *data2, uint16_t n2)
+{
+    uint16_t common = 0;
+    for (uint8_t i=0; i<n2; i++) {
+        if (bisect_search_uint16(data, n, data2[i])) {
+            common++;
+        }
+    }
+    return common;
+}
diff --git a/libraries/AP_Common/sorting.h b/libraries/AP_Common/sorting.h
new file mode 100644
index 0000000000..2a2ca5a601
--- /dev/null
+++ b/libraries/AP_Common/sorting.h
@@ -0,0 +1,44 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  in-place insertion sort for small arrays of data. This is O(n) if
+  already sorted and O(n^2) for worst case (elements are reversed)
+  sort order is smallest first
+ */
+void insertion_sort_uint16(uint16_t *data, uint16_t n);
+
+/*
+  remove duplicates from a sorted uint16_t array, returning the new
+  count
+ */
+uint16_t remove_duplicates_uint16(uint16_t *data, uint16_t n);
+
+/*
+  bisection search on a sorted uint16_t array to find an element
+  return true if found
+*/
+bool bisect_search_uint16(const uint16_t *data, uint16_t n, uint16_t value);
+
+/*
+  remove elements in a 2nd sorted array from a sorted uint16_t array
+  return the number of remaining elements
+ */
+uint16_t remove_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2);
+
+/*
+  return number of common elements between two sorted uint16_t lists
+ */
+uint16_t common_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2);
diff --git a/libraries/AP_Common/tests/test_sorting.cpp b/libraries/AP_Common/tests/test_sorting.cpp
new file mode 100644
index 0000000000..d44f3b14dc
--- /dev/null
+++ b/libraries/AP_Common/tests/test_sorting.cpp
@@ -0,0 +1,156 @@
+#include <AP_gtest.h>
+
+/*
+  tests for AP_Common/sorting.cpp
+ */
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Common/sorting.h>
+#include <stdlib.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+static int comp16(const uint16_t *v1, const uint16_t *v2) {
+    return int32_t(*v1) - int32_t(*v2);
+}
+
+static void check_equal(const uint16_t *a1, const uint16_t *a2, uint16_t n)
+{
+    for (uint8_t j=0; j<n; j++) {
+        EXPECT_EQ(a1[j], a2[j]);
+    }
+}
+
+TEST(Sorting, sort)
+{
+    for (uint16_t i=0; i<10000; i++) {
+        const uint8_t maxval = 100;
+        uint16_t n = 1 + (unsigned(random()) % 100);
+        uint16_t a1[n];
+        uint16_t a2[n];
+        for (uint8_t j=0; j<n; j++) {
+            a1[j] = a2[j] = unsigned(random()) % maxval;
+        }
+        insertion_sort_uint16(a1, n);
+        qsort(a2, n, sizeof(uint16_t), (__compar_fn_t)comp16);
+        check_equal(a1, a2, n);
+    }
+}
+
+// a dumb version of remove_duplicates_uint16() for testing
+static uint16_t dumb_unique(uint16_t *data, uint16_t n)
+{
+    uint16_t a2[n];
+    uint16_t ret = 0;
+    a2[0] = data[0];
+    for (uint16_t i=1; i<n; i++) {
+        if (data[i] != a2[ret]) {
+            a2[++ret] = data[i];
+        }
+    }
+    ret++;
+    memcpy(data, a2, ret*sizeof(uint16_t));
+    return ret;
+}
+
+TEST(Sorting, unique)
+{
+    for (uint16_t i=0; i<10000; i++) {
+        const uint8_t maxval = 30;
+        uint16_t n = 1 + (unsigned(random()) % 100);
+        uint16_t a1[n];
+        uint16_t a2[n];
+        for (uint8_t j=0; j<n; j++) {
+            a1[j] = a2[j] = unsigned(random()) % maxval;
+        }
+        insertion_sort_uint16(a1, n);
+        insertion_sort_uint16(a2, n);
+        uint16_t n1 = remove_duplicates_uint16(a1, n);
+        uint16_t n2 = dumb_unique(a2, n);
+        EXPECT_EQ(n1, n2);
+        check_equal(a1, a2, n1);
+    }
+}
+
+// a dumb version of bisect_search_uint16()
+static bool dumb_search(uint16_t *data, uint16_t n, uint16_t value)
+{
+    for (uint16_t i=0; i<n; i++) {
+        if (data[i] == value) {
+            return true;
+        }
+    }
+    return false;
+}
+
+TEST(Sorting, bisect)
+{
+    for (uint16_t i=0; i<1000; i++) {
+        const uint8_t maxval = 100;
+        uint16_t n = 1 + (unsigned(random()) % 100);
+        uint16_t a1[n];
+        for (uint8_t j=0; j<n; j++) {
+            a1[j] = unsigned(random()) % maxval;
+        }
+        insertion_sort_uint16(a1, n);
+        for (uint8_t j=0; j<10; j++) {
+            uint16_t v = unsigned(random()) % maxval;
+            bool b1 = dumb_search(a1, n, v);
+            bool b2 = bisect_search_uint16(a1, n, v);
+            EXPECT_EQ(b1, b2);
+        }
+    }
+}
+
+// a dumb version of bisect_search_uint16()
+static uint16_t dumb_remove_list(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2)
+{
+    uint16_t a[n];
+    uint16_t ret = 0;
+    for (uint16_t i=0; i<n; i++) {
+        bool found = false;
+        for (uint16_t j=0; j<n2; j++) {
+            if (rem[j] == data[i]) {
+                found = true;
+                break;
+            }
+        }
+        if (!found) {
+            a[ret] = data[i];
+            ret++;
+        }
+    }
+    memcpy(data, a, ret*sizeof(uint16_t));
+    return ret;
+}
+
+TEST(Sorting, remove)
+{
+    for (uint16_t i=0; i<1000; i++) {
+        const uint8_t maxval = 100;
+        uint16_t n = 1 + (unsigned(random()) % 100);
+        uint16_t n2 = 1 + (unsigned(random()) % 100);
+        uint16_t a1[n];
+        uint16_t a2[n];
+        uint16_t a3[n2];
+        for (uint8_t j=0; j<n; j++) {
+            a2[j] = a1[j] = unsigned(random()) % maxval;
+        }
+        for (uint8_t j=0; j<n2; j++) {
+            a3[j] = unsigned(random()) % maxval;
+        }
+        insertion_sort_uint16(a1, n);
+        insertion_sort_uint16(a2, n);
+        insertion_sort_uint16(a3, n2);
+        uint16_t r1 = remove_list_uint16(a1, n, a3, n2);
+        uint16_t r2 = dumb_remove_list(a2, n, a3, n2);
+        EXPECT_EQ(r1, r2);
+        for (uint8_t j=0; j<r1; j++) {
+            EXPECT_EQ(a1[j], a2[j]);
+        }
+    }
+}
+
+AP_GTEST_MAIN()
+
+#endif // HAL_SITL or HAL_LINUX