Equalizer3Bands.h

Go to the documentation of this file.

#pragma once
#include <math.h>
 
#include "AudioToolsConfig.h"
#include "AudioTools/CoreAudio/AudioOutput.h"
#include "AudioTools/CoreAudio/AudioStreams.h"
#include "AudioTools/CoreAudio/BaseConverter.h"
#include "AudioTools/CoreAudio/AudioBasic/soft_float_t.h"
#include "AudioTools/CoreAudio/AudioBasic/q1_14_t.h"

 
namespace audio_tools {

struct ConfigEqualizer3Bands : public AudioInfo {
  ConfigEqualizer3Bands() {
    channels = 2;
    bits_per_sample = 16;
    sample_rate = 44100;
  }

  int freq_low = 880;

  int freq_high = 5000;

  float gain_low = 1.0;

  float gain_medium = 1.0;

  float gain_high = 1.0;
};


template <typename T>
T eqAssignCoeff(float value, const char* name) {
  T t = value;
  float roundtrip = (float)t;
  if (fabs(roundtrip - value) > fabs(value) * 0.01f + 1e-4f) {
    LOGE(
        "Equalizer coefficient %s=%f does not fit in the range of T (stored "
        "as %f) - the equalizer will be inaccurate",
        name, value, roundtrip);
  }
  return t;
}

template <typename T>
struct EqDenormalGuard {
  static T value() { return T(0); }
};
template <>
struct EqDenormalGuard<float> {
  static float value() { return 1.0f / 4294967295.0f; }
};
template <>
struct EqDenormalGuard<double> {
  static double value() { return 1.0 / 4294967295.0; }
};

template <typename IntT, typename T>
struct EqSampleConverter {
  static T toEqType(IntT sample, int bits) {
    return FilterSampleConverter<IntT, T>::toFilterType(sample);
  }
  static IntT fromEqType(T value, int bits) {
    return FilterSampleConverter<IntT, T>::fromFilterType(value);
  }
};
template <typename IntT>
struct EqSampleConverter<IntT, float> {
  static float toEqType(IntT sample, int bits) {
    return NumberConverter::toFloat((int32_t)sample, bits);
  }
  static IntT fromEqType(float value, int bits) {
    return (IntT)NumberConverter::fromFloat(value, bits);
  }
};


template <typename T = float>
class Equalizer3BandsT : public ModifyingStream {
 public:
  Equalizer3BandsT(Print& out) { setOutput(out); }

  Equalizer3BandsT(Stream& in) { setStream(in); }

  Equalizer3BandsT(AudioOutput& out) {
    setOutput(out);
    addNotifyAudioChange(out);
  }

  Equalizer3BandsT(AudioStream& stream) {
    setStream(stream);
    addNotifyAudioChange(stream);
  }
 
  ~Equalizer3BandsT() {
    if (state != nullptr) delete[] state;
  }

  void setStream(Stream& io) override {
    p_out = &io;
    p_io = &io;
  };

  void setOutput(Print& out) override { p_out = &out; }

  ConfigEqualizer3Bands& config() { return *p_cfg; }

  ConfigEqualizer3Bands defaultConfig() { return cfg; }

  bool begin(ConfigEqualizer3Bands& config) {
    p_cfg = &config;
    return begin();
  }
 
  bool begin() {
    ModifyingStream::setAudioInfo(*p_cfg);
 
    if (p_cfg->channels > max_state_count) {
      if (state != nullptr) delete[] state;
      state = new EQSTATE[p_cfg->channels];
      max_state_count = p_cfg->channels;
    }
 
    // Setup state
    for (int j = 0; j < max_state_count; j++) {
      state[j] = EQSTATE();
 
      // Calculate filter cutoff frequencies
      state[j].lf = eqAssignCoeff<T>(
          2 * sin((float)PI * ((float)p_cfg->freq_low /
                                (float)p_cfg->sample_rate)),
          "lf");
      state[j].hf = eqAssignCoeff<T>(
          2 * sin((float)PI * ((float)p_cfg->freq_high /
                                (float)p_cfg->sample_rate)),
          "hf");
    }
    is_active = true;
    return true;
  }
 
  void end() { is_active = false; }

  virtual void setAudioInfo(AudioInfo info) override {
    p_cfg->sample_rate = info.sample_rate;
    p_cfg->channels = info.channels;
    p_cfg->bits_per_sample = info.bits_per_sample;
    begin(*p_cfg);
  }

  size_t write(const uint8_t* data, size_t len) override {
    filterSamples(data, len);
    return p_out->write(data, len);
  }

  int availableForWrite() override { return p_out->availableForWrite(); }
 
  void flush() override {
    if (p_out != nullptr) p_out->flush();
  }

  size_t readBytes(uint8_t* data, size_t len) override {
    size_t result = 0;
    if (p_io != nullptr) {
      result = p_io->readBytes(data, len);
      filterSamples(data, len);
    }
    return result;
  }

  int available() override {
    return p_io != nullptr ? p_io->available() : 0;
  }
 
 protected:
  bool is_active = false;     
  ConfigEqualizer3Bands cfg;  
  ConfigEqualizer3Bands* p_cfg = &cfg;  
  const T vsa = EqDenormalGuard<T>::value();  
  Print* p_out = nullptr;    
  Stream* p_io = nullptr;  
  int max_state_count = 0;     

  struct EQSTATE {
    // Filter #1 (Low band) - 4-pole low-pass filter
    T lf;    
    T f1p0;  
    T f1p1;  
    T f1p2;  
    T f1p3;  
 
    // Filter #2 (High band) - 4-pole high-pass filter
    T hf;    
    T f2p0;  
    T f2p1;  
    T f2p2;  
    T f2p3;  
 
    // Sample history buffer for filter calculations
    T sdm1;  
    T sdm2;  
    T sdm3;  
 
  }* state = nullptr;

  void filterSamples(const uint8_t* data, size_t len) {
    // no filter if not active
    if (!is_active) return;
 
    // process samples
    switch (p_cfg->bits_per_sample) {
      case 16: {
        int16_t* p_dataT = (int16_t*)data;
        size_t sample_count = len / sizeof(int16_t);
        for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
          for (int ch = 0; ch < p_cfg->channels; ch++) {
            p_dataT[j + ch] = EqSampleConverter<int16_t, T>::fromEqType(
                sample(state[ch], EqSampleConverter<int16_t, T>::toEqType(
                                       p_dataT[j + ch], 16)),
                16);
          }
        }
      } break;
      case 24: {
        int24_t* p_dataT = (int24_t*)data;
        size_t sample_count = len / sizeof(int24_t);
        for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
          for (int ch = 0; ch < p_cfg->channels; ch++) {
            p_dataT[j + ch] = EqSampleConverter<int24_t, T>::fromEqType(
                sample(state[ch], EqSampleConverter<int24_t, T>::toEqType(
                                       p_dataT[j + ch], 24)),
                24);
          }
        }
      } break;
      case 32: {
        int32_t* p_dataT = (int32_t*)data;
        size_t sample_count = len / sizeof(int32_t);
        for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
          for (int ch = 0; ch < p_cfg->channels; ch++) {
            p_dataT[j + ch] = EqSampleConverter<int32_t, T>::fromEqType(
                sample(state[ch], EqSampleConverter<int32_t, T>::toEqType(
                                       p_dataT[j + ch], 32)),
                32);
          }
        }
      } break;
 
      default:
        LOGE("Only 16 bits supported: %d", p_cfg->bits_per_sample);
        break;
    }
  }

  T sample(EQSTATE& es, T sample) {
    // Locals
    T l, m, h;  // Low / Mid / High - Sample Values
    // Filter #1 (lowpass)
    es.f1p0 += (es.lf * (sample - es.f1p0)) + vsa;
    es.f1p1 += (es.lf * (es.f1p0 - es.f1p1));
    es.f1p2 += (es.lf * (es.f1p1 - es.f1p2));
    es.f1p3 += (es.lf * (es.f1p2 - es.f1p3));
 
    l = es.f1p3;
 
    // Filter #2 (highpass)
    es.f2p0 += (es.hf * (sample - es.f2p0)) + vsa;
    es.f2p1 += (es.hf * (es.f2p0 - es.f2p1));
    es.f2p2 += (es.hf * (es.f2p1 - es.f2p2));
    es.f2p3 += (es.hf * (es.f2p2 - es.f2p3));
 
    h = es.sdm3 - es.f2p3;
    // Calculate midrange (signal - (low + high))
    m = es.sdm3 - (h + l);
    // Scale, Combine and store
    l = l * p_cfg->gain_low;
    m = m * p_cfg->gain_medium;
    h = h * p_cfg->gain_high;
 
    // Shuffle history buffer
    es.sdm3 = es.sdm2;
    es.sdm2 = es.sdm1;
    es.sdm1 = sample;
 
    // Return result
    return (l + m + h);
  }
};

using Equalizer3Bands = Equalizer3BandsT<float>;

template <typename T = float>
class Equalizer3BandsPerChannelT : public ModifyingStream {
 public:
  Equalizer3BandsPerChannelT(Print& out) { setOutput(out); }

  Equalizer3BandsPerChannelT(Stream& in) { setStream(in); }

  Equalizer3BandsPerChannelT(AudioOutput& out) {
    setOutput(out);
    out.addNotifyAudioChange(*this);
  }

  Equalizer3BandsPerChannelT(AudioStream& stream) {
    setStream(stream);
    stream.addNotifyAudioChange(*this);
  }
 
  ~Equalizer3BandsPerChannelT() {
    if (state != nullptr) delete[] state;
  }

  void setStream(Stream& io) override {
    p_out = &io;
    p_io = &io;
  };

  void setOutput(Print& out) override { p_out = &out; }
 
  ConfigEqualizer3Bands& config() { return *p_cfg; }
 
  ConfigEqualizer3Bands defaultConfig() { return cfg; }

  bool begin(ConfigEqualizer3Bands& config) {
    p_cfg = &config;
    return begin();
  }

  bool begin() {
    ModifyingStream::setAudioInfo(*p_cfg);
    // Ensure per-channel arrays are allocated
    ensureChannelArraysAllocated();
 
    // Ensure that EQSTATE is allocated
    if (p_cfg->channels > max_state_count) {
      if (state != nullptr) delete[] state;
      state = new EQSTATE[p_cfg->channels];
      max_state_count = p_cfg->channels;
    }
 
    // Setup state for each channel with its own parameters
    for (int j = 0; j < p_cfg->channels; j++) {
      state[j] = EQSTATE();
 
      // Calculate filter cutoff frequencies per channel
      state[j].lf = eqAssignCoeff<T>(
          2 * sin((float)PI *
                  ((float)freq_low[j] / (float)p_cfg->sample_rate)),
          "lf");
      state[j].hf = eqAssignCoeff<T>(
          2 * sin((float)PI *
                  ((float)freq_high[j] / (float)p_cfg->sample_rate)),
          "hf");
    }
    is_active = true;
    return true;
  }
 
  void end() override { is_active = false; }
 
  virtual void setAudioInfo(AudioInfo info) override {
    p_cfg->sample_rate = info.sample_rate;
    p_cfg->channels = info.channels;
    p_cfg->bits_per_sample = info.bits_per_sample;
    begin(*p_cfg);
  }

  void setChannelFrequencies(int channel, int freq_low_val, int freq_high_val) {
    ensureChannelArraysAllocated();
    if (channel >= 0 && channel < p_cfg->channels && !freq_low.empty()) {
      freq_low[channel] = freq_low_val;
      freq_high[channel] = freq_high_val;
 
      // Recalculate filter coefficients for this channel
      if (state != nullptr) {
        state[channel].lf = eqAssignCoeff<T>(
            2 * sin((float)PI *
                    ((float)freq_low_val / (float)p_cfg->sample_rate)),
            "lf");
        state[channel].hf = eqAssignCoeff<T>(
            2 * sin((float)PI *
                    ((float)freq_high_val / (float)p_cfg->sample_rate)),
            "hf");
      }
    }
  }

  void setChannelGains(int channel, float gain_low_val, float gain_medium_val,
                       float gain_high_val) {
    ensureChannelArraysAllocated();
    if (channel >= 0 && channel < p_cfg->channels && !gain_low.empty()) {
      gain_low[channel] = gain_low_val;
      gain_medium[channel] = gain_medium_val;
      gain_high[channel] = gain_high_val;
    }
  }

  bool getChannelFrequencies(int channel, int& freq_low_val,
                             int& freq_high_val) {
    if (channel >= 0 && channel < p_cfg->channels && !freq_low.empty()) {
      freq_low_val = freq_low[channel];
      freq_high_val = freq_high[channel];
      return true;
    }
    return false;
  }

  bool getChannelGains(int channel, float& gain_low_val, float& gain_medium_val,
                       float& gain_high_val) {
    if (channel >= 0 && channel < p_cfg->channels && !gain_low.empty()) {
      gain_low_val = gain_low[channel];
      gain_medium_val = gain_medium[channel];
      gain_high_val = gain_high[channel];
      return true;
    }
    return false;
  }

  size_t write(const uint8_t* data, size_t len) override {
    filterSamples(data, len);
    return p_out->write(data, len);
  }

  int availableForWrite() override { return p_out->availableForWrite(); }
 
  void flush() override {
    if (p_out != nullptr) p_out->flush();
  }

  size_t readBytes(uint8_t* data, size_t len) override {
    size_t result = 0;
    if (p_io != nullptr) {
      result = p_io->readBytes(data, len);
      filterSamples(data, len);
    }
    return result;
  }

  int available() override {
    return p_io != nullptr ? p_io->available() : 0;
  }
 
 protected:
  bool is_active = false;
  ConfigEqualizer3Bands cfg;            
  ConfigEqualizer3Bands* p_cfg = &cfg;  
  const T vsa = EqDenormalGuard<T>::value();  
  Print* p_out = nullptr;    
  Stream* p_io = nullptr;  
  int max_state_count = 0;     
 
  // Per-channel frequency and gain settings using Vector containers
  Vector<int> freq_low;       
  Vector<int> freq_high;      
  Vector<float> gain_low;     
  Vector<float> gain_medium;  
  Vector<float> gain_high;    
 
  struct EQSTATE {
    // Filter #1 (Low band)
    T lf;    // Frequency
    T f1p0;  // Poles ...
    T f1p1;
    T f1p2;
    T f1p3;
 
    // Filter #2 (High band)
    T hf;    // Frequency
    T f2p0;  // Poles ...
    T f2p1;
    T f2p2;
    T f2p3;
 
    // Sample history buffer
    T sdm1;  // Sample data minus 1
    T sdm2;  //                   2
    T sdm3;  //                   3
 
  }* state = nullptr;

  void ensureChannelArraysAllocated() {
    if (freq_low.size() != p_cfg->channels) {
      allocateChannelArrays(p_cfg->channels);
    }
  }

  void allocateChannelArrays(int num_channels) {
    // Resize all vectors to accommodate the number of channels
    freq_low.resize(num_channels);
    freq_high.resize(num_channels);
    gain_low.resize(num_channels);
    gain_medium.resize(num_channels);
    gain_high.resize(num_channels);
 
    // Initialize with config default values
    for (int i = 0; i < num_channels; i++) {
      freq_low[i] = p_cfg->freq_low;
      freq_high[i] = p_cfg->freq_high;
      gain_low[i] = p_cfg->gain_low;
      gain_medium[i] = p_cfg->gain_medium;
      gain_high[i] = p_cfg->gain_high;
    }
  }

  void filterSamples(const uint8_t* data, size_t len) {
    if (!is_active) return;
    switch (p_cfg->bits_per_sample) {
      case 16: {
        int16_t* p_dataT = (int16_t*)data;
        size_t sample_count = len / sizeof(int16_t);
        for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
          for (int ch = 0; ch < p_cfg->channels; ch++) {
            p_dataT[j + ch] = EqSampleConverter<int16_t, T>::fromEqType(
                sample(ch, EqSampleConverter<int16_t, T>::toEqType(
                               p_dataT[j + ch], 16)),
                16);
          }
        }
      } break;
      case 24: {
        int24_t* p_dataT = (int24_t*)data;
        size_t sample_count = len / sizeof(int24_t);
        for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
          for (int ch = 0; ch < p_cfg->channels; ch++) {
            p_dataT[j + ch] = EqSampleConverter<int24_t, T>::fromEqType(
                sample(ch, EqSampleConverter<int24_t, T>::toEqType(
                               p_dataT[j + ch], 24)),
                24);
          }
        }
      } break;
      case 32: {
        int32_t* p_dataT = (int32_t*)data;
        size_t sample_count = len / sizeof(int32_t);
        for (size_t j = 0; j < sample_count; j += p_cfg->channels) {
          for (int ch = 0; ch < p_cfg->channels; ch++) {
            p_dataT[j + ch] = EqSampleConverter<int32_t, T>::fromEqType(
                sample(ch, EqSampleConverter<int32_t, T>::toEqType(
                               p_dataT[j + ch], 32)),
                32);
          }
        }
      } break;
 
      default:
        LOGE("Only 16 bits supported: %d", p_cfg->bits_per_sample);
        break;
    }
  }

  T sample(int channel, T sample_val) {
    EQSTATE& es = state[channel];
 
    // Locals
    T l, m, h;  // Low / Mid / High - Sample Values
 
    // Filter #1 (lowpass)
    es.f1p0 += (es.lf * (sample_val - es.f1p0)) + vsa;
    es.f1p1 += (es.lf * (es.f1p0 - es.f1p1));
    es.f1p2 += (es.lf * (es.f1p1 - es.f1p2));
    es.f1p3 += (es.lf * (es.f1p2 - es.f1p3));
 
    l = es.f1p3;
 
    // Filter #2 (highpass)
    es.f2p0 += (es.hf * (sample_val - es.f2p0)) + vsa;
    es.f2p1 += (es.hf * (es.f2p0 - es.f2p1));
    es.f2p2 += (es.hf * (es.f2p1 - es.f2p2));
    es.f2p3 += (es.hf * (es.f2p2 - es.f2p3));
 
    h = es.sdm3 - es.f2p3;
 
    // Calculate midrange (signal - (low + high))
    m = es.sdm3 - (h + l);
 
    // Scale with per-channel gains
    l = l * gain_low[channel];
    m = m * gain_medium[channel];
    h = h * gain_high[channel];
 
    // Shuffle history buffer
    es.sdm3 = es.sdm2;
    es.sdm2 = es.sdm1;
    es.sdm1 = sample_val;
 
    // Return result
    return (l + m + h);
  }
};

using Equalizer3BandsPerChannel = Equalizer3BandsPerChannelT<float>;
 
}  // namespace audio_tools