Equalizer.h

#pragma once
#include <math.h>
 
#include "AudioToolsConfig.h"
#include "AudioTools/CoreAudio/AudioOutput.h"
#include "AudioTools/CoreAudio/AudioStreams.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;
};
 
class Equalizer3Bands : public ModifyingStream {
 public:
  Equalizer3Bands(Print &out) { setOutput(out); }
 
  Equalizer3Bands(Stream &in) { setStream(in); }
 
  Equalizer3Bands(AudioOutput &out) {
    setOutput(out);
    out.addNotifyAudioChange(*this);
  }
 
  Equalizer3Bands(AudioStream &stream) {
    setStream(stream);
    stream.addNotifyAudioChange(*this);
  }
 
  ~Equalizer3Bands() {
    if (state != nullptr) delete[] state;
  }
 
  void setStream(Stream &io) override {
    p_print = &io;
    p_stream = &io;
  };
 
  void setOutput(Print &out) override { p_print = &out; }
 
  ConfigEqualizer3Bands &config() { return cfg; }
 
  ConfigEqualizer3Bands &defaultConfig() { return config(); }
 
  bool begin(ConfigEqualizer3Bands &config) {
    p_cfg = &config;
    return begin();
  }
 
  bool begin(){
    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++) {
      memset(&state[j], 0, sizeof(EQSTATE));
 
      // Calculate filter cutoff frequencies
      state[j].lf =
          2 *
          sin((float)PI * ((float)p_cfg->freq_low / (float)p_cfg->sample_rate));
      state[j].hf = 2 * sin((float)PI * ((float)p_cfg->freq_high /
                                         (float)p_cfg->sample_rate));
    }
    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_print->write(data, len);
  }
 
  int availableForWrite() override { return p_print->availableForWrite(); }
 
  size_t readBytes(uint8_t *data, size_t len) override {
    size_t result = 0;
    if (p_stream != nullptr) {
      result = p_stream->readBytes(data, len);
      filterSamples(data, len);
    }
    return result;
  }
 
  int available() override {
    return p_stream != nullptr ? p_stream->available() : 0;
  }
 
 protected:
  bool is_active = false;                             
  ConfigEqualizer3Bands cfg;                           
  ConfigEqualizer3Bands *p_cfg = &cfg;                 
  const float vsa = (1.0 / 4294967295.0);              
  Print *p_print = nullptr;                            
  Stream *p_stream = nullptr;                          
  int max_state_count = 0;                             
 
  struct EQSTATE {
    // Filter #1 (Low band) - 4-pole low-pass filter
    float lf;    
    float f1p0;  
    float f1p1;  
    float f1p2;  
    float f1p3;  
 
    // Filter #2 (High band) - 4-pole high-pass filter  
    float hf;    
    float f2p0;  
    float f2p1;  
    float f2p2;  
    float f2p3;  
 
    // Sample history buffer for filter calculations
    float sdm1;  
    float sdm2;  
    float 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] = NumberConverter::fromFloat(sample(state[ch], NumberConverter::toFloat(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] = NumberConverter::fromFloat(sample(state[ch], NumberConverter::toFloat(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] = NumberConverter::fromFloat(sample(state[ch], NumberConverter::toFloat(p_dataT[j + ch], 32)), 32);
          }
        }
      } break;
 
      default:
        LOGE("Only 16 bits supported: %d", p_cfg->bits_per_sample);
        break;
    }
  }
 
  float sample(EQSTATE &es, float sample) {
    // Locals
    float 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 *= p_cfg->gain_low;
    m *= p_cfg->gain_medium;
    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);
  }
};
 
class Equalizer3BandsPerChannel : public ModifyingStream {
 public:
  Equalizer3BandsPerChannel(Print &out) { setOutput(out); }
 
  Equalizer3BandsPerChannel(Stream &in) { setStream(in); }
 
  Equalizer3BandsPerChannel(AudioOutput &out) {
    setOutput(out);
    out.addNotifyAudioChange(*this);
  }
 
  Equalizer3BandsPerChannel(AudioStream &stream) {
    setStream(stream);
    stream.addNotifyAudioChange(*this);
  }
 
  ~Equalizer3BandsPerChannel() {
    if (state != nullptr) delete[] state;
  }
 
  void setStream(Stream &io) override {
    p_print = &io;
    p_stream = &io;
  };
 
  void setOutput(Print &out) override { p_print = &out; }
 
  ConfigEqualizer3Bands &config() { return cfg; }
 
  ConfigEqualizer3Bands &defaultConfig() { return config(); }
 
  bool begin(ConfigEqualizer3Bands config) {
    p_cfg = &config;
    return begin();
  }
 
  bool begin(){ 
    // 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++) {
      memset(&state[j], 0, sizeof(EQSTATE));
 
      // Calculate filter cutoff frequencies per channel
      state[j].lf = 2 * sin((float)PI * ((float)freq_low[j] / (float)p_cfg->sample_rate));
      state[j].hf = 2 * sin((float)PI * ((float)freq_high[j] / (float)p_cfg->sample_rate));
    }
    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 = 2 * sin((float)PI * ((float)freq_low_val / (float)p_cfg->sample_rate));
        state[channel].hf = 2 * sin((float)PI * ((float)freq_high_val / (float)p_cfg->sample_rate));
      }
    }
  }
 
  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_print->write(data, len);
  }
 
  int availableForWrite() override { return p_print->availableForWrite(); }
 
  size_t readBytes(uint8_t *data, size_t len) override {
    size_t result = 0;
    if (p_stream != nullptr) {
      result = p_stream->readBytes(data, len);
      filterSamples(data, len);
    }
    return result;
  }
 
  int available() override {
    return p_stream != nullptr ? p_stream->available() : 0;
  }
 
 protected:
  bool is_active = false;
  ConfigEqualizer3Bands cfg;                           
  ConfigEqualizer3Bands *p_cfg = &cfg;                 
  const float vsa = (1.0 / 4294967295.0);              
  Print *p_print = nullptr;                            
  Stream *p_stream = 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)
    float lf;    // Frequency
    float f1p0;  // Poles ...
    float f1p1;
    float f1p2;
    float f1p3;
 
    // Filter #2 (High band)
    float hf;    // Frequency
    float f2p0;  // Poles ...
    float f2p1;
    float f2p2;
    float f2p3;
 
    // Sample history buffer
    float sdm1;  // Sample data minus 1
    float sdm2;  //                   2
    float 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] = NumberConverter::fromFloat(sample(ch, NumberConverter::toFloat(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] = NumberConverter::fromFloat(sample(ch, NumberConverter::toFloat(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] = NumberConverter::fromFloat(sample(ch, NumberConverter::toFloat(p_dataT[j + ch], 32)), 32);
          }
        }
      } break;
 
      default:
        LOGE("Only 16 bits supported: %d", p_cfg->bits_per_sample);
        break;
    }
  }
 
  float sample(int channel, float sample_val) {
    EQSTATE &es = state[channel];
    
    // Locals
    float 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 *= gain_low[channel];
    m *= gain_medium[channel];
    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);
  }
};
 
}  // namespace audio_tools