PitchShift.h

Go to the documentation of this file.

 
#pragma once
#include <math.h>
#include <stdio.h>
#include <string.h>
 
#include "AudioTools/CoreAudio/AudioOutput.h"
#include "AudioTools/CoreAudio/AudioTypes.h"
#include "AudioToolsConfig.h"
 
namespace audio_tools {
 
struct PitchShiftInfo : public AudioInfo {
  PitchShiftInfo() {
    channels = 2;
    sample_rate = 44100;
    bits_per_sample = 16;
  }
  
  float pitch_shift = 1.4f;
  
  int buffer_size = 1000;
};
 
template <typename T = int16_t>
class VariableSpeedRingBufferSimple : public BaseBuffer<T> {
 public:
  VariableSpeedRingBufferSimple(int size = 0, float increment = 1.0) {
    setIncrement(increment);
    if (size > 0) resize(size);
  }
 
  void setIncrement(float increment) {
#if PREFER_FIXEDPOINT
    read_increment_fixed = (int32_t)roundf(increment * 65536.0f);
#else
    read_increment = increment;
#endif
  }
 
  bool resize(size_t size) {
    buffer_size = size;
    return buffer.resize(size);
  }
 
  bool read(T &result) {
    peek(result);
#if PREFER_FIXEDPOINT
    read_pos_fixed += read_increment_fixed;
    // on buffer overflow reset to beginning. Uses >= (unlike the float
    // path's >): with an integer increment (e.g. 1.0) the position can land
    // exactly on bufferSizeFixed(), and > would leave it there -- an index
    // one past the end of the buffer on the next peek().
    if (read_pos_fixed >= bufferSizeFixed()) {
      read_pos_fixed -= bufferSizeFixed();
    }
#else
    read_pos_float += read_increment;
    // on buffer overflow reset to beginning
    if (read_pos_float > buffer_size) {
      read_pos_float -= buffer_size;
    }
#endif
    return true;
  }
 
  bool peek(T &result) {
    if (buffer.size() == 0) {
      LOGE("buffer has no memory");
      result = 0;
    } else {
#if PREFER_FIXEDPOINT
      result = buffer[(int)(read_pos_fixed >> 16)];
#else
      result = buffer[(int)read_pos_float];
#endif
    }
    return true;
  }
 
  bool write(T sample) {
    if (buffer.size() == 0) {
      LOGE("buffer has no memory");
      return false;
    }
    buffer[write_pos++] = sample;
    // on buffer overflow reset to 0
    if (write_pos >= buffer_size) {
      write_pos = 0;
    }
    return true;
  }
 
  void reset() {
#if PREFER_FIXEDPOINT
    read_pos_fixed = 0;
#else
    read_pos_float = 0;
#endif
    write_pos = 0;
    memset(buffer.data(), 0, sizeof(T) * buffer_size);
  }
 
  virtual bool isFull() { return false; }
  virtual int available() { return buffer_size; }
  virtual int availableForWrite() { return buffer_size; }
  virtual T *address() { return nullptr; }
  size_t size() { return buffer_size; }
 
 protected:
  Vector<T> buffer{0};
  int buffer_size = 0;
  int write_pos = 0;
#if PREFER_FIXEDPOINT
  // Q16.16 fixed-point read position/increment: a plain int32_t add/compare
  // per read() instead of a float add + compare, with the buffer index a
  // shift instead of a float->int truncation -- no FPU needed on FPU-less
  // MCUs, at the same 4-byte width as the float it replaces (int64_t would
  // needlessly double the size of exactly the fields this optimization
  // touches, on exactly the RAM-constrained MCUs it targets). int32_t caps
  // buffer_size at < 32768 (buffer_size << 16 must fit in int32) -- not a
  // real constraint here: 32767 samples is ~743ms of buffer at 44.1kHz,
  // already far beyond a practical pitch-shift buffer (typically hundreds
  // to low thousands of samples), and this path's target platforms don't
  // have the RAM for a buffer that large anyway.
  int32_t read_pos_fixed = 0;
  int32_t read_increment_fixed = 1 << 16;
  int32_t bufferSizeFixed() const { return buffer_size << 16; }
#else
  float read_pos_float = 0.0;
  float read_increment = 1.0;
#endif
};
 
template <typename T = int16_t>
class VariableSpeedRingBuffer180 : public BaseBuffer<T> {
 public:
  VariableSpeedRingBuffer180(int size = 0, float increment = 1.0) {
    setIncrement(increment);
    if (size > 0) resize(size);
  }
 
  void setIncrement(float increment) { pitch_shift = increment; }
 
  bool resize(size_t size) {
    buffer_size = size;
    overlap = buffer_size / 10;
    return buffer.resize(size);
  }
 
  bool read(T &result) {
    result = pitchRead();
    return true;
  }
 
  bool peek(T &result) { return false; }
 
  bool write(T sample) {
    if (buffer.size() == 0) {
      LOGE("buffer has no memory");
      return false;
    }
    // write_pointer value is used in pitchRead()
    write_pointer = write_pos;
    buffer[write_pos++] = sample;
    // on buffer overflow reset to 0
    if (write_pos >= buffer_size) {
      write_pos = 0;
    }
    return true;
  }
 
  void reset() {
    read_pos_float = 0;
    write_pos = 0;
    cross_fade = 1.0f;
    overlap = buffer_size / 10;
    memset(buffer.data(), 0, sizeof(T) * buffer_size);
  }
 
  virtual bool isFull() { return false; }
  virtual int available() { return buffer_size; }
  virtual int availableForWrite() { return buffer_size; }
  virtual T *address() { return nullptr; }
  size_t size() { return buffer_size; }
 
 protected:
  Vector<T> buffer{0};
  float read_pos_float = 0.0;
  float cross_fade = 1.0;
  int write_pos = 0;
  int write_pointer = 0;
  int buffer_size = 0;
  int overlap = 0;
  float pitch_shift = 0;
 
  virtual T pitchRead() {
    TRACED();
    assert(pitch_shift > 0);
    assert(buffer_size > 0);
 
    // read fractional readpointer and generate 0° and 180° read-pointer in
    // integer
    int read_pointer_int = roundf(read_pos_float);
    int read_pointer_int180 = 0;
    if (read_pointer_int >= buffer_size / 2)
      read_pointer_int180 = read_pointer_int - (buffer_size / 2);
    else
      read_pointer_int180 = read_pointer_int + (buffer_size / 2);
 
    // read the two samples...
    float read_sample = (float)buffer[read_pointer_int];
    float read_sample_180 = (float)buffer[read_pointer_int180];
 
    // Check if first readpointer starts overlap with write pointer?
    //  if yes -> do cross-fade to second read-pointer
    if (overlap >= (write_pointer - read_pointer_int) &&
        (write_pointer - read_pointer_int) >= 0 && pitch_shift != 1.0f) {
      int rel = write_pointer - read_pointer_int;
      cross_fade = ((float)rel) / (float)overlap;
    } else if (write_pointer - read_pointer_int == 0)
      cross_fade = 0.0f;
 
    // Check if second readpointer starts overlap with write pointer?
    //  if yes -> do cross-fade to first read-pointer
    if (overlap >= (write_pointer - read_pointer_int180) &&
        (write_pointer - read_pointer_int180) >= 0 && pitch_shift != 1.0f) {
      int rel = write_pointer - read_pointer_int180;
      cross_fade = 1.0f - ((float)rel) / (float)overlap;
    } else if (write_pointer - read_pointer_int180 == 0)
      cross_fade = 1.0f;
 
    // do cross-fading and sum up
    T sum = (read_sample * cross_fade + read_sample_180 * (1.0f - cross_fade));
 
    // increment fractional read-pointer and write-pointer
    read_pos_float += pitch_shift;
    if (roundf(read_pos_float) >= buffer_size) read_pos_float = 0.0f;
 
    return sum;
  }
};
 
template <typename T = int16_t>
class VariableSpeedRingBuffer : public BaseBuffer<T> {
 public:
  VariableSpeedRingBuffer(int size = 0, float increment = 1.0) {
    setIncrement(increment);
    if (size > 0) resize(size);
  }
 
  void setIncrement(float increment) { read_increment = increment; }
 
  bool resize(size_t size) {
    buffer_size = size;
    // prevent an overrun at the start
    read_pos_float = size / 2;
    return buffer.resize(size);
  }
 
  bool read(T &result) {
    assert(read_increment != 0.0f);
    peek(result);
    read_pos_float += read_increment;
    handleReadWriteOverrun(last_value);
    if (read_pos_float > buffer_size) {
      read_pos_float -= buffer_size;
    }
    return true;
  }
 
  bool peek(T &result) {
    if (buffer.size() == 0) {
      result = 0;
    } else {
      result = interpolate(read_pos_float);
    }
    return true;
  }
 
  bool write(T sample) {
    if (buffer.size() == 0) return false;
    handleReadWriteOverrun(last_value);
    buffer[write_pos++] = sample;
    // on buffer overflow reset to 0
    if (write_pos >= buffer_size) {
      write_pos = 0;
    }
    return true;
  }
 
  void reset() {
    read_pos_float = 0;
    write_pos = 0;
    memset(buffer.data(), 0, sizeof(T) * buffer_size);
  }
 
  virtual bool isFull() { return false; }
  virtual int available() { return buffer_size; }
  virtual int availableForWrite() { return buffer_size; }
  virtual T *address() { return nullptr; }
  size_t size() { return buffer_size; }
 
 protected:
  Vector<T> buffer{0};
  int buffer_size;
  float read_pos_float = 0.0f;
  float read_increment = 0.0f;
  int write_pos = 0;
  // used to handle overruns:
  T last_value = 0;   
  bool incrementing;  
 
  T interpolate(float read_pos) {
    int read_pos_int = read_pos;
    T value1 = getValue(read_pos_int);
    T value2 = getValue(read_pos_int + 1);
    incrementing = value2 - value1 >= 0;
 
    // make sure that value1 is smaller then value 2
    if (value2 < value1) {
      T tmp = value2;
      value2 = value1;
      value1 = tmp;
    }
    // the result must be between value 1 and value 2: linear interpolation
    float offset_in = read_pos - read_pos_int;  // calculate fraction: e.g 0.5
    LOGD("read_pos=%f read_pos_int=%d, offset_in=%f", read_pos, read_pos_int,
         offset_in);
    float diff_result =
        abs(value2 - value1);  // differrence between values: e.g. 10
    float offset_result = offset_in * diff_result;  // 0.5 * 10 = 5
    float result = offset_result + value1;
    LOGD("interpolate %d %d -> %f -> %f", value1, value2, offset_result,
         result);
 
    last_value = result;
 
    return result;
  }
 
  T getValue(int pos) { return buffer[pos % buffer_size]; }
 
  bool isMatching(T value1, bool incrementing, T v1, T v2) {
    bool v_incrementing = v2 - v1 >= 0;
    // eff sample was ascending so we need to select a ascending value
    if (incrementing && v_incrementing && value1 >= v1 && value1 <= v2) {
      return true;
    }
    // eff sample was descending so we need to select a descending value
    if (!incrementing && !v_incrementing && value1 <= v1 && value1 >= v2) {
      return true;
    }
    return false;
  }
 
  void handleReadWriteOverrun(T last_value) {
    // handle overflow - we need to allign the phase
    int read_pos_int = read_pos_float;  // round down
    if (write_pos == read_pos_int ||
        write_pos == (buffer_size % (read_pos_int + 1))) {
      LOGD("handleReadWriteOverrun write_pos=%d read_pos_int=%d", write_pos,
           read_pos_int);
      bool found = false;
 
      // find the closest match for the last value
      for (int j = read_increment * 2; j < buffer_size; j++) {
        int pos = read_pos_int + j;
        float v1 = getValue(pos);
        float v2 = getValue(pos + 1);
        // find corresponging matching sample in buffer for last sample
        if (isMatching(last_value, incrementing, v1, v2)) {
          // interpolate new position
          float diff_value = abs(v1 - v2);
          float diff_last_value = abs(v1 - last_value);
          float fraction = 0;
          if (diff_value > 0) {
            fraction = diff_last_value / diff_value;
          }
 
          read_pos_float = fraction + pos;
          // move to next value
          read_pos_float += read_increment;
          // if we are at the end of the buffer we restart from 0
          if (read_pos_float > buffer_size) {
            read_pos_float -= buffer_size;
          }
          LOGD("handleReadWriteOverrun -> read_pos pos=%d  pos_float=%f", pos,
               read_pos_float);
          found = true;
          break;
        }
      }
      if (!found) {
        LOGW("phase allign failed: maybe the buffer is too small")
      }
    }
  }
};
 
template <typename T, class BufferT>
class PitchShiftOutput : public AudioOutput {
 public:
  PitchShiftOutput(Print &out) { p_out = &out; }
 
  PitchShiftInfo defaultConfig() {
    PitchShiftInfo result;
    result.bits_per_sample = sizeof(T) * 8;
    return result;
  }
 
  bool begin(PitchShiftInfo info) {
    TRACED();
    cfg = info;
    return begin();
  }
 
  bool begin() {
    AudioOutput::setAudioInfo(cfg);
    buffer.resize(cfg.buffer_size);
    buffer.reset();
    buffer.setIncrement(cfg.pitch_shift);
    active = true;
    return active;
  }
 
  size_t write(const uint8_t *data, size_t len) override {
    LOGD("PitchShiftOutput::write %d bytes", (int)len);
    if (!active) return 0;
 
    size_t result = 0;
    int channels = cfg.channels;
    T *p_in = (T *)data;
    int sample_count = len / sizeof(T);
 
    for (int j = 0; j < sample_count; j += channels) {
      float value = 0;
      for (int ch = 0; ch < channels; ch++) {
        value += p_in[j + ch];
      }
      // calculate avg sample value
      value /= cfg.channels;
 
      // output values
      T out_value = pitchShift(value);
      LOGD("PitchShiftOutput %f -> %d", value, (int)out_value);
      T out_array[channels];
      for (int ch = 0; ch < channels; ch++) {
        out_array[ch] = out_value;
      }
      result += p_out->write((uint8_t *)out_array, sizeof(T) * channels);
    }
    return result;
  }
 
  void end() { active = false; }
 
 protected:
  BufferT buffer;           
  PitchShiftInfo cfg;       
  Print *p_out = nullptr;   
  bool active = false;      
 
  T pitchShift(T value) {
    TRACED();
    if (!active) return 0;
    buffer.write(value);
    T result = 0;
    buffer.read(result);
    return result;
  }
};
 
}  // namespace audio_tools