From 134aedd86d7f9648a95846e0e8714e949d7f9082 Mon Sep 17 00:00:00 2001
From: David Protzman <NotGonnaTell@SeriouslyNo.Fack>
Date: Sun, 9 Oct 2022 19:03:32 -0400
Subject: [PATCH] Added mean_fast function

---
 .../include/gnuradio/dji_droneid/utils.h      |  2 ++
 gnuradio/gr-dji_droneid/lib/qa_utils.cc       | 35 +++++++++++++++++++
 gnuradio/gr-dji_droneid/lib/utils.cc          | 18 ++++++++++
 3 files changed, 55 insertions(+)

diff --git a/gnuradio/gr-dji_droneid/include/gnuradio/dji_droneid/utils.h b/gnuradio/gr-dji_droneid/include/gnuradio/dji_droneid/utils.h
index 2dd756b..d40d265 100644
--- a/gnuradio/gr-dji_droneid/include/gnuradio/dji_droneid/utils.h
+++ b/gnuradio/gr-dji_droneid/include/gnuradio/dji_droneid/utils.h
@@ -81,6 +81,8 @@ public:
      */
     static std::pair<uint16_t, uint16_t> get_cyclic_prefix_lengths(float sample_rate);
 
+    static std::complex<float> mean_fast(const std::complex<float> * samples, uint32_t sample_count);
+
 private:
 };
 
diff --git a/gnuradio/gr-dji_droneid/lib/qa_utils.cc b/gnuradio/gr-dji_droneid/lib/qa_utils.cc
index dcfebe7..f3eea05 100644
--- a/gnuradio/gr-dji_droneid/lib/qa_utils.cc
+++ b/gnuradio/gr-dji_droneid/lib/qa_utils.cc
@@ -113,6 +113,18 @@ struct TestFixture {
             static_cast<uint16_t>(short_cp_len[0])
         };
     }
+
+    static std::complex<float> mean(const std::vector<std::complex<float>> & samples) {
+        matlab_engine->setVariable("samples", factory.createArray({1, samples.size()}, samples.begin(), samples.end()));
+        matlab_engine->eval(u"m = mean(samples);");
+        const matlab::data::TypedArray<std::complex<float>> mean_val = matlab_engine->getVariable("m");
+
+        BOOST_REQUIRE_EQUAL(mean_val.getNumberOfElements(), 1);
+
+        matlab_engine->eval(u"clear samples m");
+
+        return mean_val[0];
+    }
 };
 
 BOOST_FIXTURE_TEST_SUITE(Utils_Test_Suite, TestFixture);
@@ -201,6 +213,29 @@ BOOST_AUTO_TEST_CASE(test_utils__get_cyclic_prefix) {
     }
 }
 
+BOOST_AUTO_TEST_CASE(test_utils__mean_fast) {
+    const uint32_t iters = 100;
+    const uint32_t length = 10000;
+
+    matlab_engine->setVariable("sample_count", factory.createScalar(static_cast<double>(length)));
+    for (uint32_t iter = 0; iter < iters; iter++) {
+        matlab_engine->eval(u"samples = single(complex(randn(1, sample_count), randn(1, sample_count)));");
+        const matlab::data::TypedArray<std::complex<float>> samples_array = matlab_engine->getVariable("samples");
+        const std::vector<std::complex<float>> samples(samples_array.begin(), samples_array.end());
+
+        const auto expected = mean(samples);
+        const auto calculated = utils::mean_fast(&samples[0], samples.size());
+
+        // MATLAB does all of its arithmetic in double precision, and the mean_fast function does everything in single
+        // This means there is going to be a large delta between the values simply due to rounding errors.  The value
+        // below is a percentage (eg 0.2 == a 0.2% delta) that the difference can be before failing the test
+        const auto accuracy = 0.2;
+
+        BOOST_REQUIRE_CLOSE(expected.real(), calculated.real(), accuracy);
+        BOOST_REQUIRE_CLOSE(expected.imag(), calculated.imag(), accuracy);
+    }
+}
+
 BOOST_AUTO_TEST_SUITE_END()
 
 } /* namespace dji_droneid */
diff --git a/gnuradio/gr-dji_droneid/lib/utils.cc b/gnuradio/gr-dji_droneid/lib/utils.cc
index 76b605d..6271dd3 100644
--- a/gnuradio/gr-dji_droneid/lib/utils.cc
+++ b/gnuradio/gr-dji_droneid/lib/utils.cc
@@ -11,6 +11,7 @@
 #include <gnuradio/fft/fft.h>
 #include <gnuradio/fft/fft_shift.h>
 #include <fftw3.h>
+#include <volk/volk.h>
 
 namespace gr {
 namespace dji_droneid {
@@ -130,5 +131,22 @@ std::pair<uint16_t, uint16_t> utils::get_cyclic_prefix_lengths(const float sampl
     );
 }
 
+std::complex<float> utils::mean_fast(const std::complex<float> * const samples, const uint32_t sample_count)
+{
+    // Treat the vector of complex floats as a single vector of float values
+    auto sample_floats = reinterpret_cast<const float *>(samples);
+    float real = 0, imag = 0;
+
+    for (uint32_t idx = 0; idx < sample_count; idx++) {
+        real += *sample_floats++;
+        imag += *sample_floats++;
+    }
+
+    real = real / static_cast<float>(sample_count);
+    imag = imag / static_cast<float>(sample_count);
+
+    return {real, imag};
+}
+
 } /* namespace dji_droneid */
 } /* namespace gr */