1 files changed, 20 insertions, 161 deletions
diff --git a/src/audio/resample.cpp b/src/audio/resample.cpp
index 430a6a26..bc2c7c51 100644
--- a/src/audio/resample.cpp
+++ b/src/audio/resample.cpp
@@ -23,183 +23,42 @@
 #include "esp_log.h"
 
 #include "sample.hpp"
+#include "speex/speex_resampler.h"
 #include "stream_info.hpp"
 
 namespace audio {
 
-static constexpr double kLowPassRatio = 0.5;
-static constexpr size_t kNumFilters = 64;
-static constexpr size_t kFilterSize = 16;
-
-typedef std::array<float, kFilterSize> Filter;
-static std::array<Filter, kNumFilters + 1> sFilters{};
-static bool sFiltersInitialised = false;
-
-auto InitFilter(int index) -> void;
+static constexpr int kQuality = SPEEX_RESAMPLER_QUALITY_MIN;
 
 Resampler::Resampler(uint32_t source_sample_rate,
                      uint32_t target_sample_rate,
                      uint8_t num_channels)
-    : source_sample_rate_(source_sample_rate),
-      target_sample_rate_(target_sample_rate),
-      factor_(static_cast<double>(target_sample_rate) /
-              static_cast<double>(source_sample_rate)),
+    : err_(0),
+      resampler_(speex_resampler_init(num_channels,
+                                      source_sample_rate,
+                                      target_sample_rate,
+                                      kQuality,
+                                      &err_)),
       num_channels_(num_channels) {
-  channel_buffers_.resize(num_channels);
-  channel_buffer_size_ = kFilterSize * 16;
-
-  for (int i = 0; i < num_channels; i++) {
-    channel_buffers_[i] =
-        static_cast<float*>(calloc(sizeof(float), channel_buffer_size_));
-  }
-
-  output_offset_ = kFilterSize / 2.0f;
-  input_index_ = kFilterSize;
-
-  if (!sFiltersInitialised) {
-    sFiltersInitialised = true;
-    for (int i = 0; i < kNumFilters + 1; i++) {
-      InitFilter(i);
-    }
-  }
+  assert(err_ == 0);
 }
 
-Resampler::~Resampler() {}
+Resampler::~Resampler() {
+  speex_resampler_destroy(resampler_);
+}
 
-auto Resampler::Process(cpp::span<const sample::Sample> input,
+auto Resampler::Process(cpp::span<sample::Sample> input,
                         cpp::span<sample::Sample> output,
                         bool end_of_data) -> std::pair<size_t, size_t> {
-  size_t samples_used = 0;
-  size_t samples_produced = 0;
-
-  size_t input_frames = input.size() / num_channels_;
-  size_t output_frames = output.size() / num_channels_;
-
-  int half_taps = kFilterSize / 2;
-  while (output_frames > 0) {
-    if (output_offset_ >= input_index_ - half_taps) {
-      if (input_frames > 0) {
-        // Check whether the channel buffers will overflow with the addition of
-        // this sample. If so, we need to move the remaining contents back to
-        // the beginning of the buffer.
-        if (input_index_ == channel_buffer_size_) {
-          for (int i = 0; i < num_channels_; ++i) {
-            memmove(channel_buffers_[i],
-                    channel_buffers_[i] + channel_buffer_size_ - kFilterSize,
-                    kFilterSize * sizeof(float));
-          }
-
-          output_offset_ -= channel_buffer_size_ - kFilterSize;
-          input_index_ -= channel_buffer_size_ - kFilterSize;
-        }
-
-        for (int i = 0; i < num_channels_; ++i) {
-          channel_buffers_[i][input_index_] =
-              sample::ToFloat(input[samples_used++]);
-        }
-
-        input_index_++;
-        input_frames--;
-      } else {
-        break;
-      }
-    } else {
-      for (int i = 0; i < num_channels_; i++) {
-        output[samples_produced++] = sample::FromFloat(Subsample(i));
-      }
-
-      // NOTE: floating point division here is potentially slow due to FPU
-      // limitations. Consider explicitly bunding the xtensa libgcc divsion via
-      // reciprocal implementation if we care about portability between
-      // compilers.
-      output_offset_ += 1.0f / factor_;
-      output_frames--;
-    }
-  }
-
-  return {samples_used, samples_produced};
-}
-
-/*
- * Constructs the filter in-place for the given index of sFilters. This only
- * needs to be done once, per-filter. 64-bit math is okay here, because filters
- * will not be initialised within a performance critical path.
- */
-auto InitFilter(int index) -> void {
-  Filter& filter = sFilters[index];
-  std::array<double, kFilterSize> working_buffer{};
+  uint32_t samples_used = input.size() / num_channels_;
+  uint32_t samples_produced = output.size() / num_channels_;
 
-  double fraction = index / static_cast<double>(kNumFilters);
-  double filter_sum = 0.0;
-
-  for (int i = 0; i < kFilterSize; ++i) {
-    // "dist" is the absolute distance from the sinc maximum to the filter tap
-    //  to be calculated, in radians.
-    double dist = fabs((kFilterSize / 2.0 - 1.0) + fraction - i) * M_PI;
-    // "ratio" is that distance divided by half the tap count such that it
-    // reaches π at the window extremes
-    double ratio = dist / (kFilterSize / 2.0);
-
-    double value;
-    if (dist != 0.0) {
-      value = sin(dist * kLowPassRatio) / (dist * kLowPassRatio);
-
-      // Hann window. We could alternatively use a Blackman Harris window,
-      // however our unusually small filter size makes the Hann window's
-      // steeper cutoff more important.
-      value *= 0.5 * (1.0 + cos(ratio));
-    } else {
-      value = 1.0;
-    }
-
-    working_buffer[i] = value;
-    filter_sum += value;
-  }
-
-  // Filter should have unity DC gain
-  double scaler = 1.0 / filter_sum;
-  double error = 0.0;
-
-  for (int i = kFilterSize / 2; i < kFilterSize;
-       i = kFilterSize - i - (i >= kFilterSize / 2)) {
-    working_buffer[i] *= scaler;
-    filter[i] = working_buffer[i] - error;
-    error += static_cast<double>(filter[i]) - working_buffer[i];
-  }
-}
-
-/*
- * Performs sub-sampling with interpolation for the given channel. Assumes that
- * the channel buffer has already been filled with samples.
- */
-auto Resampler::Subsample(int channel) -> float {
-  cpp::span<float> source{channel_buffers_[channel], channel_buffer_size_};
-
-  int offset_integral = std::floor(output_offset_);
-  source = source.subspan(offset_integral);
-  float offset_fractional = output_offset_ - offset_integral;
-
-  offset_fractional *= kNumFilters;
-  int filter_index = std::floor(offset_fractional);
-
-  float sum1 = ApplyFilter(sFilters[filter_index],
-                           {source.data() - kFilterSize / 2 + 1, kFilterSize});
-
-  offset_fractional -= filter_index;
-
-  float sum2 = ApplyFilter(sFilters[filter_index + 1],
-                           {source.data() - kFilterSize / 2 + 1, kFilterSize});
-
-  return (sum2 * offset_fractional) + (sum1 * (1.0f - offset_fractional));
-}
+  int err = speex_resampler_process_interleaved_int(
+      resampler_, input.data(), &samples_used, output.data(),
+      &samples_produced);
+  assert(err == 0);
 
-auto Resampler::ApplyFilter(cpp::span<float> filter, cpp::span<float> input)
-    -> float {
-  float sum = 0.0;
-  for (int i = 0; i < kFilterSize; i++) {
-    sum += filter[i] * input[i];
-  }
-  return sum;
+  return {samples_used * num_channels_, samples_produced * num_channels_};
 }
 
 }  // namespace audio