Resampling *basically* working? Just cleanup and buffering issues

1 files changed, 134 insertions, 209 deletions

diff --git a/src/audio/resample.cpp b/src/audio/resample.cpp
index 6f7e670e..aa4c8f2a 100644
--- a/src/audio/resample.cpp
+++ b/src/audio/resample.cpp
@@ -4,6 +4,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <algorithm>
+#include <cmath>
 #include <numeric>
 
 #include "esp_log.h"
@@ -13,249 +14,173 @@
 
 namespace audio {
 
-static constexpr size_t kFilterSize = 1536;
+static constexpr char kTag[] = "resample";
 
-constexpr auto calc_deltas(const std::array<int32_t, kFilterSize>& filter)
-    -> std::array<int32_t, kFilterSize> {
-  std::array<int32_t, kFilterSize> deltas;
-  for (size_t n = 0; n < kFilterSize - 1; n++)
-    deltas[n] = filter[n + 1] - filter[n];
-  return deltas;
-}
+static constexpr double kLowPassRatio = 0.5;
+static constexpr size_t kNumFilters = 8;
+static constexpr size_t kTapsPerFilter = 8;
 
-static const std::array<int32_t, kFilterSize> kFilter{
-#include "fir.h"
-};
-
-static const std::array<int32_t, kFilterSize> kFilterDeltas =
-    calc_deltas(kFilter);
-
-class Channel {
- public:
-  Channel(uint32_t src_rate,
-          uint32_t dest_rate,
-          size_t chunk_size,
-          size_t skip);
-  ~Channel();
-
-  auto output_chunk_size() -> size_t { return output_chunk_size_; }
-
-  auto FlushSamples(cpp::span<sample::Sample> out) -> size_t;
-  auto AddSample(sample::Sample, cpp::span<sample::Sample> out) -> std::size_t;
-  auto ApplyFilter() -> sample::Sample;
-
- private:
-  size_t output_chunk_size_;
-  size_t skip_;
-
-  uint32_t factor_; /* factor */
-
-  uint32_t time_; /* time */
-
-  uint32_t time_per_filter_iteration_; /* output step */
-  uint32_t filter_step_;               /* filter step */
-  uint32_t filter_end_;                /* filter end */
-
-  int32_t unity_scale_; /* unity scale */
-
-  int32_t samples_per_filter_wing_;  /* extra samples */
-  int32_t latest_sample_;            /* buffer index */
-  cpp::span<int32_t> sample_buffer_; /* the buffer */
-};
-
-enum {
-  Nl = 8,               /* 2^Nl samples per zero crossing in fir */
-  Nη = 8,               /* phase bits for filter interpolation */
-  kPhaseBits = Nl + Nη, /* phase bits (fract of fixed point) */
-  One = 1 << kPhaseBits,
-};
-
-Channel::Channel(uint32_t irate, uint32_t orate, size_t count, size_t skip)
-    : skip_(skip) {
-  factor_ = ((uint64_t)orate << kPhaseBits) / irate;
-  if (factor_ != One) {
-    time_per_filter_iteration_ = ((uint64_t)irate << kPhaseBits) / orate;
-    filter_step_ = 1 << (Nl + Nη);
-    filter_end_ = kFilterSize << Nη;
-    samples_per_filter_wing_ = 1 + (filter_end_ / filter_step_);
-    unity_scale_ = 13128; /* unity scale factor for fir */
-    if (factor_ < One) {
-      unity_scale_ *= factor_;
-      unity_scale_ >>= kPhaseBits;
-      filter_step_ *= factor_;
-      filter_step_ >>= kPhaseBits;
-      samples_per_filter_wing_ *= time_per_filter_iteration_;
-      samples_per_filter_wing_ >>= kPhaseBits;
-    }
-    latest_sample_ = samples_per_filter_wing_;
-    time_ = latest_sample_ << kPhaseBits;
+typedef std::array<float, kTapsPerFilter> Filter;
+static std::array<Filter, kNumFilters + 1> sFilters{};
+static bool sFiltersInitialised = false;
 
-    size_t buf_size = samples_per_filter_wing_ * 2 + count;
-    int32_t* buf = new int32_t[buf_size];
-    sample_buffer_ = {buf, buf_size};
-    count += buf_size; /* account for buffer accumulation */
-  }
-  output_chunk_size_ = ((uint64_t)count * factor_) >> kPhaseBits;
-}
+auto InitFilter(int index) -> void;
 
-Channel::~Channel() {
-  delete sample_buffer_.data();
-}
+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)),
+      num_channels_(num_channels) {
+  channel_buffers_.resize(num_channels);
+  channel_buffer_size_ = kTapsPerFilter * 16;
 
-auto Channel::ApplyFilter() -> sample::Sample {
-  uint32_t iteration, p, i;
-  int32_t *sample, a;
-
-  int64_t value = 0;
-
-  // I did my best, but I'll be honest with you I've no idea about any of this
-  // maths stuff.
-
-  // Left wing of the filter.
-  sample = &sample_buffer_[time_ >> kPhaseBits];
-  p = time_ & ((1 << kPhaseBits) - 1);
-  iteration = factor_ < One ? (factor_ * p) >> kPhaseBits : p;
-  while (iteration < filter_end_) {
-    i = iteration >> Nη;
-    a = iteration & ((1 << Nη) - 1);
-    iteration += filter_step_;
-    a *= kFilterDeltas[i];
-    a >>= Nη;
-    a += kFilter[i];
-    value += static_cast<int64_t>(*--sample) * a;
+  for (int i = 0; i < num_channels; i++) {
+    channel_buffers_[i] =
+        static_cast<float*>(calloc(sizeof(float), channel_buffer_size_));
   }
 
-  // Right wing of the filter.
-  sample = &sample_buffer_[time_ >> kPhaseBits];
-  p = (One - p) & ((1 << kPhaseBits) - 1);
-  iteration = factor_ < One ? (factor_ * p) >> kPhaseBits : p;
-  if (p == 0) /* skip h[0] as it was already been summed above if p == 0 */
-    iteration += filter_step_;
-  while (iteration < filter_end_) {
-    i = iteration >> Nη;
-    a = iteration & ((1 << Nη) - 1);
-    iteration += filter_step_;
-    a *= kFilterDeltas[i];
-    a >>= Nη;
-    a += kFilter[i];
-    value += static_cast<int64_t>(*sample++) * a;
+  output_offset_ = kTapsPerFilter / 2.0f;
+  input_index_ = kTapsPerFilter;
+
+  if (!sFiltersInitialised) {
+    sFiltersInitialised = true;
+    for (int i = 0; i < kNumFilters + 1; i++) {
+      InitFilter(i);
+    }
   }
+}
 
-  /* scale */
-  value >>= 2;
-  value *= unity_scale_;
-  value >>= 27;
+Resampler::~Resampler() {}
 
-  return sample::Clip(value);
-}
+auto Resampler::Process(cpp::span<const 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 = kTapsPerFilter / 2, i;
+  while (output_frames > 0) {
+    if (output_offset_ >= input_index_ - half_taps) {
+      if (input_frames > 0) {
+        if (input_index_ == channel_buffer_size_) {
+          for (i = 0; i < num_channels_; ++i) {
+            memmove(channel_buffers_[i],
+                    channel_buffers_[i] + channel_buffer_size_ - kTapsPerFilter,
+                    kTapsPerFilter * sizeof(float));
+          }
+
+          output_offset_ -= channel_buffer_size_ - kTapsPerFilter;
+          input_index_ -= channel_buffer_size_ - kTapsPerFilter;
+        }
+
+        for (i = 0; i < num_channels_; ++i) {
+          channel_buffers_[i][input_index_] =
+              sample::ToFloat(input[samples_used++]);
+        }
+
+        input_index_++;
+        input_frames--;
+      } else
+        break;
+    } else {
+      for (i = 0; i < num_channels_; i++) {
+        output[samples_produced++] = sample::FromFloat(Subsample(i));
+      }
 
-auto Channel::FlushSamples(cpp::span<sample::Sample> out) -> size_t {
-  size_t zeroes_needed = (2 * samples_per_filter_wing_) - latest_sample_;
-  size_t produced = 0;
-  while (zeroes_needed > 0) {
-    produced += AddSample(0, out.subspan(produced));
-    zeroes_needed--;
+      output_offset_ += (1.0f / factor_);
+    }
   }
-  return produced;
+
+  return {samples_used, samples_produced};
 }
 
-auto Channel::AddSample(sample::Sample in, cpp::span<sample::Sample> out)
-    -> size_t {
-  // Add the latest sample to our working buffer.
-  sample_buffer_[latest_sample_++] = in;
+auto InitFilter(int index) -> void {
+  const double a0 = 0.35875;
+  const double a1 = 0.48829;
+  const double a2 = 0.14128;
+  const double a3 = 0.01168;
 
-  // If we don't have enough samples to run the filter, then bail out and wait
-  // for more.
-  if (latest_sample_ < 2 * samples_per_filter_wing_) {
-    return 0;
-  }
+  double fraction =
+      static_cast<double>(index) / static_cast<double>(kNumFilters);
+  double filter_sum = 0.0;
 
-  // Apply the filter to the buffered samples. First, we work out how long (in
-  // samples) we can run the filter for before running out. This isn't as
-  // trivial as it might look; e.g. depending on the resampling factor we might
-  // be doubling the number of samples, or halving them.
-  uint32_t max_time = (latest_sample_ - samples_per_filter_wing_) << kPhaseBits;
-  size_t samples_output = 0;
-  while (time_ < max_time) {
-    out[skip_ * samples_output++] = ApplyFilter();
-    time_ += time_per_filter_iteration_;
-  }
+  // "dist" is the absolute distance from the sinc maximum to the filter tap to
+  // be calculated, in radians "ratio" is that distance divided by half the tap
+  // count such that it reaches π at the window extremes
+
+  // Note that with this scaling, the odd terms of the Blackman-Harris
+  // calculation appear to be negated with respect to the reference formula
+  // version.
+
+  Filter& filter = sFilters[index];
+  std::array<double, kTapsPerFilter> working_buffer{};
+  for (int i = 0; i < kTapsPerFilter; ++i) {
+    double dist = fabs((kTapsPerFilter / 2.0 - 1.0) + fraction - i) * M_PI;
+    double ratio = dist / (kTapsPerFilter / 2.0);
+    double value;
+
+    if (dist != 0.0) {
+      value = sin(dist * kLowPassRatio) / (dist * kLowPassRatio);
 
-  // If we are approaching the end of our buffer, we need to shift all the data
-  // in it down to the front to make room for more samples.
-  int32_t current_sample = time_ >> kPhaseBits;
-  if (current_sample >= (sample_buffer_.size() - samples_per_filter_wing_)) {
-    // NB: bit shifting back and forth means we're only modifying `time` by
-    // whole samples.
-    time_ -= current_sample << kPhaseBits;
-    time_ += samples_per_filter_wing_ << kPhaseBits;
-
-    int32_t new_current_sample = time_ >> kPhaseBits;
-    new_current_sample -= samples_per_filter_wing_;
-    current_sample -= samples_per_filter_wing_;
-
-    int32_t samples_to_move = latest_sample_ - current_sample;
-    if (samples_to_move > 0) {
-      auto samples = sample_buffer_.subspan(current_sample, samples_to_move);
-      std::copy_backward(samples.begin(), samples.end(),
-                         sample_buffer_.first(new_current_sample).end());
-      latest_sample_ = new_current_sample + samples_to_move;
+      // Blackman-Harris window
+      value *= a0 + a1 * cos(ratio) + a2 * cos(2 * ratio) + a3 * cos(3 * ratio);
     } else {
-      latest_sample_ = new_current_sample;
+      value = 1.0;
     }
+
+    working_buffer[i] = value;
+    filter_sum += value;
   }
 
-  return samples_output;
-}
+  // filter should have unity DC gain
 
-static const size_t kChunkSizeSamples = 256;
+  double scaler = 1.0 / filter_sum;
+  double error = 0.0;
 
-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_(((uint64_t)target_sample_rate << kPhaseBits) /
-              source_sample_rate),
-      num_channels_(num_channels),
-      channels_() {
-  for (int i = 0; i < num_channels; i++) {
-    channels_.emplace_back(source_sample_rate, target_sample_rate,
-                           kChunkSizeSamples, num_channels);
+  for (int i = kTapsPerFilter / 2; i < kTapsPerFilter;
+       i = kTapsPerFilter - i - (i >= kTapsPerFilter / 2)) {
+    working_buffer[i] *= scaler;
+    filter[i] = working_buffer[i] - error;
+    error += static_cast<double>(filter[i]) - working_buffer[i];
   }
 }
 
-Resampler::~Resampler() {}
+auto Resampler::Subsample(int channel) -> float {
+  float sum1, sum2;
 
-auto Resampler::Process(cpp::span<const sample::Sample> input,
-                        cpp::span<sample::Sample> output,
-                        bool end_of_data) -> std::pair<size_t, size_t> {
-  size_t samples_used = 0;
-  std::vector<size_t> samples_produced = {};
-  samples_produced.resize(num_channels_, 0);
-  size_t total_samples_produced = 0;
+  cpp::span<float> source{channel_buffers_[channel], channel_buffer_size_};
 
-  size_t slop = (factor_ >> kPhaseBits) + 1;
+  int offset_integral = std::floor(output_offset_);
+  source = source.subspan(offset_integral);
+  float offset_fractional = output_offset_ - offset_integral;
 
-  uint_fast8_t cur_channel = 0;
+  int filter_index = offset_fractional * kNumFilters;
+  offset_fractional *= kNumFilters;
 
-  while (input.size() > samples_used &&
-         output.size() > total_samples_produced + slop) {
-    // Work out where the next set of samples should be placed.
-    size_t next_output_index =
-        (samples_produced[cur_channel] * num_channels_) + cur_channel;
+  sum1 = ApplyFilter(sFilters[filter_index],
+                     {source.data() - kTapsPerFilter / 2 + 1, kTapsPerFilter});
 
-    // Generate the next samples
-    size_t new_samples = channels_[cur_channel].AddSample(
-        input[samples_used++], output.subspan(next_output_index));
+  offset_fractional -= filter_index;
 
-    samples_produced[cur_channel] += new_samples;
-    total_samples_produced += new_samples;
+  sum2 = ApplyFilter(sFilters[filter_index + 1],
+                     {source.data() - kTapsPerFilter / 2 + 1, kTapsPerFilter});
 
-    cur_channel = (cur_channel + 1) % num_channels_;
-  }
+  return (sum2 * offset_fractional) + (sum1 * (1.0f - offset_fractional));
+}
 
-  return {samples_used, total_samples_produced};
+auto Resampler::ApplyFilter(cpp::span<float> filter, cpp::span<float> input)
+    -> float {
+  float sum = 0.0;
+  for (int i = 0; i < kTapsPerFilter; i++) {
+    sum += filter[i] * input[i];
+  }
+  return sum;
 }
 
 }  // namespace audio