MDFEchoCancellation.h

Go to the documentation of this file.

/* Copyright (C) 2003-2008 Jean-Marc Valin
 * Copyright (C) 2024 Phil Schatzmann (Header-only adaptation)
 *
 * Echo canceller based on the MDF algorithm
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * 3. The name of the author may not be used to endorse or promote products
 * derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
#pragma once
 
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <type_traits>
 
#include "MDFEchoCancellationConfig.h"
#include "PseudoFloat.h"
#include "AudioTools/FFT/AudioFFT.h"
#include "AudioTools/CoreAudio/AudioBasic/Collections/Allocator.h"
#include "AudioTools/CoreAudio/AudioTypes.h"
 
// Control requests
#define ECHO_GET_FRAME_SIZE 3
#define ECHO_SET_SAMPLING_RATE 24
#define ECHO_GET_SAMPLING_RATE 25
#define ECHO_GET_IMPULSE_RESPONSE_SIZE 27
#define ECHO_GET_IMPULSE_RESPONSE 29
 
namespace audio_tools {
 
// Type definitions for the fixed-width PCM/control values that are always
// concrete integers, regardless of the internal DSP numeric representation
// chosen via SampleType (see MDFFloat / MDFFixedPoint below).
using echo_int16_t = int16_t;
using echo_uint16_t = uint16_t;
using echo_int32_t = int32_t;
using echo_uint32_t = uint32_t;
 
struct MDFFloat {
  using word16_t = float;
  using word32_t = float;
  using float_t = float;
};
 
struct MDFFixedPoint {
  using word16_t = PseudoFloat;
  using word32_t = PseudoFloat;
  using float_t = PseudoFloat;
};
 
template <typename T, typename SampleType>
struct EchoState {
  using Word16 = typename SampleType::word16_t;
  using Word32 = typename SampleType::word32_t;
  using Num = typename SampleType::float_t;
  using Mem = Word32;
 
  int frame_size; 
  int window_size;
  int M;
  int cancel_count;
  int adapted;
  int saturated;
  int screwed_up;
  int C; 
  int K; 
  echo_int32_t sampling_rate;
  Word16 spec_average;
  Word16 beta0;
  Word16 beta_max;
  Word32 sum_adapt;
  Word16 leak_estimate;
 
  Word16* e;     /* scratch */
  Word16* x;     /* Far-end input buffer (2N) */
  Word16* X;     /* Far-end buffer (M+1 frames) in frequency domain */
  Word16* input; /* scratch */
  Word16* y;     /* scratch */
  Word16* last_y;
  Word16* Y; /* scratch */
  Word16* E;
  Word32* PHI; /* scratch */
  Word32* W;   /* (Background) filter weights */
#ifdef TWO_PATH
  Word16* foreground; /* Foreground filter weights */
  Word32
      Davg1; /* 1st recursive average of the residual power difference */
  Word32
      Davg2; /* 2nd recursive average of the residual power difference */
  Num Dvar1; /* Estimated variance of 1st estimator */
  Num Dvar2; /* Estimated variance of 2nd estimator */
#endif
  Word32* power;  /* Power of the far-end signal */
  Num* power_1; /* Inverse power of far-end */
  Word16* wtmp;   /* scratch */
  Word32* Rf;     /* scratch */
  Word32* Yf;     /* scratch */
  Word32* Xf;     /* scratch */
  Word32* Eh;
  Word32* Yh;
  Num Pey;
  Num Pyy;
  Word16* window;
  Word16* prop;
  void* fft_table;
  Word16 *memX, *memD, *memE;
  Word16 preemph;
  Word16 notch_radius;
  Mem* notch_mem;
 
  T* play_buf;
  int play_buf_pos;
  int play_buf_started;
};
 
struct fft_state {
  // The MDF algorithm needs synchronous per-sample setValue()/fft()/getValue()
  // access at a window size (2x frame size) that is independent of whatever
  // AudioFFTBase::config().length the caller set up. That low-level, single-
  // shot API only exists on FFTDriver (e.g. FFTDriverRealFFT), not on the
  // higher-level AudioFFTBase stream wrapper, so we operate on the raw driver
  // obtained via AudioFFTBase::driver().
  FFTDriver* driver; 
  int N; 
  fft_state(int size, FFTDriver* drv) : driver(drv), N(size) {}
};
 
// Forward declarations: these are defined at the bottom of this file but
// referenced from inside MDFEchoCancellation below. echo_fft/echo_ifft are
// templated on the sample element type (T = SampleType::word16_t, i.e.
// float or PseudoFloat) since they convert to/from the FFT driver's plain
// float bins at the boundary.
inline void* echo_fft_init(int size, FFTDriver* driver);
inline void echo_fft_destroy(void* table);
template <typename T>
inline void echo_fft(void* table, T* in, T* out);
template <typename T>
inline void echo_ifft(void* table, T* in, T* out);
 
template <typename T = int16_t, typename SampleType = MDFFloat>
class MDFEchoCancellation : public AudioStream {
 public:
  using Word16 = typename SampleType::word16_t;
  using Word32 = typename SampleType::word32_t;
  using Num = typename SampleType::float_t;
  using Mem = Word32;
  using State = EchoState<T, SampleType>;
 
  MDFEchoCancellation(int filterLength, AudioFFTBase& fftDriver, Allocator& alloc = DefaultAllocator)
      : fft_driver(&fftDriver), allocator(alloc),
        filter_length(filterLength), nb_mic(1), nb_speakers(1) {}
 
  MDFEchoCancellation(int filterLength, int nbMic, int nbSpeakers,
                AudioFFTBase& fftDriver, Allocator& alloc = DefaultAllocator)
      : fft_driver(&fftDriver), allocator(alloc),
        filter_length(filterLength), nb_mic(nbMic), nb_speakers(nbSpeakers) {}
 
  ~MDFEchoCancellation() {
    if (!state) return;
 
    if (state->fft_table) echo_fft_destroy(state->fft_table);
 
    int N = state->window_size;
    int M = state->M;
    int C = state->C;
    int K = state->K;
    int frame_size = state->frame_size;
 
    echoFree(state->e, C * N);
    echoFree(state->x, K * N);
    echoFree(state->input, C * frame_size);
    echoFree(state->y, C * N);
    echoFree(state->last_y, C * N);
    echoFree(state->Yf, frame_size + 1);
    echoFree(state->Rf, frame_size + 1);
    echoFree(state->Xf, frame_size + 1);
    echoFree(state->Yh, frame_size + 1);
    echoFree(state->Eh, frame_size + 1);
    echoFree(state->X, K * (M + 1) * N);
    echoFree(state->Y, C * N);
    echoFree(state->E, C * N);
    echoFree(state->W, C * K * M * N);
#ifdef TWO_PATH
    echoFree(state->foreground, M * N * C * K);
#endif
    echoFree(state->PHI, N);
    echoFree(state->power, frame_size + 1);
    echoFree(state->power_1, frame_size + 1);
    echoFree(state->window, N);
    echoFree(state->prop, M);
    echoFree(state->wtmp, N);
    echoFree(state->memX, K);
    echoFree(state->memD, C);
    echoFree(state->memE, C);
    echoFree(state->notch_mem, 2 * C);
    echoFree(state->play_buf, K * (PLAYBACK_DELAY + 1) * frame_size);
    delete state;
    state = nullptr;
  }
 
  void cancel(const T* rec, const T* play,
                    T* out) {
    ensureInitialized();
    echoCancellationImpl(state, rec, play, out);
  }
 
  void capture(const T* rec, T* out) {
    ensureInitialized();
    state->play_buf_started = 1;
    if (state->play_buf_pos >= state->frame_size) {
      echoCancellationImpl(state, rec, state->play_buf, out);
      state->play_buf_pos -= state->frame_size;
      std::memmove(state->play_buf,
                   &state->play_buf[state->frame_size],
                   state->play_buf_pos * sizeof(T));
    } else {
      echoWarning("No playback frame available");
      if (state->play_buf_pos != 0) {
        echoWarning("Internal playback buffer corruption");
        state->play_buf_pos = 0;
      }
      for (int i = 0; i < state->frame_size; i++) out[i] = rec[i];
    }
  }
 
  void playback(const T* play) {
    ensureInitialized();
    if (!state->play_buf_started) {
      echoWarning("Discarded first playback frame");
      return;
    }
    if (state->play_buf_pos <= PLAYBACK_DELAY * state->frame_size) {
      for (int i = 0; i < state->frame_size; i++)
        state->play_buf[state->play_buf_pos + i] = play[i];
      state->play_buf_pos += state->frame_size;
      if (state->play_buf_pos <= (PLAYBACK_DELAY - 1) * state->frame_size) {
        echoWarning("Auto-filling buffer");
        for (int i = 0; i < state->frame_size; i++)
          state->play_buf[state->play_buf_pos + i] = play[i];
        state->play_buf_pos += state->frame_size;
      }
    } else {
      echoWarning("Had to discard playback frame");
    }
  }
 
  void reset() {
    ensureInitialized();
    int N = state->window_size;
    int M = state->M;
    int C = state->C;
    int K = state->K;
 
    state->cancel_count = 0;
    state->screwed_up = 0;
 
    for (int i = 0; i < N * M; i++) state->W[i] = 0;
#ifdef TWO_PATH
    for (int i = 0; i < N * M; i++) state->foreground[i] = 0;
#endif
    for (int i = 0; i < N * (M + 1); i++) state->X[i] = 0;
    for (int i = 0; i <= state->frame_size; i++) {
      state->power[i] = 0;
      state->power_1[i] = 1.0f;
      state->Eh[i] = 0;
      state->Yh[i] = 0;
    }
    for (int i = 0; i < state->frame_size; i++) state->last_y[i] = 0;
    for (int i = 0; i < N * C; i++) state->E[i] = 0;
    for (int i = 0; i < N * K; i++) state->x[i] = 0;
    for (int i = 0; i < 2 * C; i++) state->notch_mem[i] = 0;
    for (int i = 0; i < C; i++) state->memD[i] = state->memE[i] = 0;
    for (int i = 0; i < K; i++) state->memX[i] = 0;
 
    state->saturated = 0;
    state->adapted = 0;
    state->sum_adapt = 0;
    state->Pey = state->Pyy = 1.0f;
#ifdef TWO_PATH
    state->Davg1 = state->Davg2 = 0;
    state->Dvar1 = state->Dvar2 = 0.0f;
#endif
    for (int i = 0; i < 3 * state->frame_size; i++) state->play_buf[i] = 0;
    state->play_buf_pos = PLAYBACK_DELAY * state->frame_size;
    state->play_buf_started = 0;
  }
 
  int control(int request, void* ptr) {
    switch (request) {
      case ECHO_GET_FRAME_SIZE:
        (*(int*)ptr) = state->frame_size;
        break;
      case ECHO_SET_SAMPLING_RATE:
        state->sampling_rate = (*(int*)ptr);
        state->spec_average = state->frame_size / (float)state->sampling_rate;
        state->beta0 = (2.0f * state->frame_size) / state->sampling_rate;
        state->beta_max = (.5f * state->frame_size) / state->sampling_rate;
        if (state->sampling_rate < 12000)
          state->notch_radius = .9f;
        else if (state->sampling_rate < 24000)
          state->notch_radius = .982f;
        else
          state->notch_radius = .992f;
        break;
      case ECHO_GET_SAMPLING_RATE:
        (*(int*)ptr) = state->sampling_rate;
        break;
      case ECHO_GET_IMPULSE_RESPONSE_SIZE:
        *((echo_int32_t*)ptr) = state->M * state->frame_size;
        break;
      case ECHO_GET_IMPULSE_RESPONSE: {
        int M = state->M, N = state->window_size, n = state->frame_size;
        echo_int32_t* filt = (echo_int32_t*)ptr;
        for (int j = 0; j < M; j++) {
          echo_ifft(state->fft_table, &state->W[j * N], state->wtmp);
          for (int i = 0; i < n; i++) filt[j * n + i] = 32767 * state->wtmp[i];
        }
      } break;
      default:
        return -1;
    }
    return 0;
  }
 
  int getFrameSize() { return state->frame_size; }
 
  void setSamplingRate(int rate) { control(ECHO_SET_SAMPLING_RATE, &rate); }
 
  int getSamplingRate() { return state->sampling_rate; }
 
  int getImpulseResponseSize() { return state->M * state->frame_size; }
 
  void getImpulseResponse(echo_int32_t* response) {
    control(ECHO_GET_IMPULSE_RESPONSE, response);
  }
 
  void setFilterLength(int len) {
    if (initialized) {
      echoWarning("Cannot change filter length after initialization");
      return;
    }
    filter_length = len;
  }
 
  int getFilterLength() { return filter_length; }
 
  void setMicChannels(int num) {
    if (initialized) {
      echoWarning("Cannot change mic channels after initialization");
      return;
    }
    nb_mic = num;
  }
 
  int getMicChannels() { return nb_mic; }
 
  void setSpeakerChannels(int num) {
    if (initialized) {
      echoWarning("Cannot change speaker channels after initialization");
      return;
    }
    nb_speakers = num;
  }
 
  int getSpeakerChannels() { return nb_speakers; }
 
  void setFFTDriver(AudioFFTBase& fftDriver) {
    if (initialized) {
      echoWarning("Cannot change FFT driver after initialization");
      return;
    }
    fft_driver = &fftDriver;
  }
 
  State* getState() { return state; }
 
 protected:
  State* state = nullptr; 
  AudioFFTBase* fft_driver; 
  Allocator& allocator; 
  int filter_length; 
  int nb_mic; 
  int nb_speakers; 
  bool initialized = false; 
  void ensureInitialized() {
    if (initialized) return;
 
    int frameSize = fft_driver->config().length;
    state = echoStateInitMc(frameSize, filter_length, nb_mic, nb_speakers);
    if (state && fft_driver) {
      state->fft_table = echo_fft_init(state->window_size, fft_driver->driver());
    }
    initialized = true;
  }
 
  template <typename A>
  A* echoAlloc(size_t count) {
    return allocator.createArray<A>(count);
  }
 
  template <typename A>
  void echoFree(A* ptr, size_t count) {
    allocator.removeArray<A>(ptr, count);
  }
 
  inline void echoWarning(const char* str) {
    LOGW("EchoCanceller Warning: %s", str);
  }
 
  inline void echoFatal(const char* str) {
    LOGE("EchoCanceller Error: %s", str);
  }
 
  template <typename A>
  inline A spxSqrt(A x) { return A(sqrtf((float)x)); }
 
  inline echo_int16_t spxIlog2(echo_uint32_t x) {
    int r = 0;
    if (x >= (echo_int32_t)65536) {
      x >>= 16;
      r += 16;
    }
    if (x >= 256) {
      x >>= 8;
      r += 8;
    }
    if (x >= 16) {
      x >>= 4;
      r += 4;
    }
    if (x >= 4) {
      x >>= 2;
      r += 2;
    }
    if (x >= 2) {
      r += 1;
    }
    return r;
  }
 
  inline float spxExp(float x) { return expf(x); }
 
  inline float spxCos(float x) { return cosf(x); }
 
  inline void filterDcNotch16(const T* in, Word16 radius,
                              Word16* out, int len, Mem* mem,
                              int stride) {
    Word16 den2 = radius * radius + .7f * (1 - radius) * (1 - radius);
    for (int i = 0; i < len; i++) {
      Word16 vin = in[i * stride];
      Word32 vout = mem[0] + vin;
      mem[0] = mem[1] + 2 * (-vin + radius * vout);
      mem[1] = vin - den2 * vout;
      out[i] = radius * vout;
    }
  }
 
  inline Word32 mdfInnerProd(const Word16* x, const Word16* y,
                                   int len) {
    float sum = 0;
    for (int i = 0; i < len; i++) {
      sum += x[i] * y[i];
    }
    return sum;
  }
 
  inline void powerSpectrum(const Word16* X, Word32* ps, int N) {
    ps[0] = X[0] * X[0];
    for (int i = 1, j = 1; i < N - 1; i += 2, j++) {
      ps[j] = X[i] * X[i] + X[i + 1] * X[i + 1];
    }
    ps[N / 2] = X[N - 1] * X[N - 1];
  }
 
  inline void powerSpectrumAccum(const Word16* X, Word32* ps,
                                 int N) {
    ps[0] += X[0] * X[0];
    for (int i = 1, j = 1; i < N - 1; i += 2, j++) {
      ps[j] += X[i] * X[i] + X[i + 1] * X[i + 1];
    }
    ps[N / 2] += X[N - 1] * X[N - 1];
  }
 
  inline void spectralMulAccum(const Word16* X, const Word32* Y,
                               Word16* acc, int N, int M) {
    for (int i = 0; i < N; i++) acc[i] = 0;
 
    for (int j = 0; j < M; j++) {
      acc[0] += X[0] * Y[0];
      for (int i = 1; i < N - 1; i += 2) {
        acc[i] += (X[i] * Y[i] - X[i + 1] * Y[i + 1]);
        acc[i + 1] += (X[i + 1] * Y[i] + X[i] * Y[i + 1]);
      }
      acc[N - 1] += X[N - 1] * Y[N - 1];
      X += N;
      Y += N;
    }
  }
 
  inline void weightedSpectralMulConj(const Num* w, const Num p,
                                      const Word16* X,
                                      const Word16* Y, Word32* prod,
                                      int N) {
    // NOTE: the (int32_t) casts this code originally had here were only
    // correct in true fixed-point mode (X/Y as int16_t, needing widening
    // before multiplying to avoid 16-bit overflow). With Word16 /
    // Word32 now float (see the aliases above), those casts instead
    // truncated every frequency-domain sample to an integer before ever
    // multiplying it by anything -- e.g. a bin magnitude of 0.03 became 0,
    // silently zeroing out the filter's gradient. Removed.
    Num W;
    W = p * w[0];
    prod[0] = W * (X[0] * Y[0]);
    for (int i = 1, j = 1; i < N - 1; i += 2, j++) {
      W = p * w[j];
      prod[i] = W * (X[i] * Y[i] + X[i + 1] * Y[i + 1]);
      prod[i + 1] = W * (-X[i + 1] * Y[i] + X[i] * Y[i + 1]);
    }
    W = p * w[N / 2];
    prod[N - 1] = W * (X[N - 1] * Y[N - 1]);
  }
 
  inline void mdfAdjustProp(const Word32* W, int N, int M, int P,
                            Word16* prop) {
    Word16 max_sum = 1;
    Word32 prop_sum = 1;
 
    for (int i = 0; i < M; i++) {
      Word32 tmp = 1;
      for (int p = 0; p < P; p++) {
        for (int j = 0; j < N; j++) {
          float val = W[p * N * M + i * N + j];
          tmp += val * val;
        }
      }
      prop[i] = spxSqrt(tmp);
      if (prop[i] > max_sum) max_sum = prop[i];
    }
 
    for (int i = 0; i < M; i++) {
      prop[i] += 0.1f * max_sum;
      prop_sum += prop[i];
    }
 
    for (int i = 0; i < M; i++) {
      prop[i] = 0.99f * prop[i] / prop_sum;
    }
  }
 
  State* echoStateInitMc(int frame_size, int filter_length, int nb_mic,
                                  int nb_speakers) {
    int N = frame_size * 2;
    int M = (filter_length + frame_size - 1) / frame_size;
    int C = nb_mic;
    int K = nb_speakers;
 
    State* st = new State();
    if (!st) return nullptr;
 
    st->K = K;
    st->C = C;
    st->frame_size = frame_size;
    st->window_size = N;
    st->M = M;
    st->cancel_count = 0;
    st->sum_adapt = 0;
    st->saturated = 0;
    st->screwed_up = 0;
    st->sampling_rate = 8000;
    st->spec_average = st->frame_size / (float)st->sampling_rate;
    st->beta0 = (2.0f * st->frame_size) / st->sampling_rate;
    st->beta_max = (.5f * st->frame_size) / st->sampling_rate;
    st->leak_estimate = 0;
 
    // Allocate buffers
    st->e = echoAlloc<Word16>(C * N);
    st->x = echoAlloc<Word16>(K * N);
    st->input = echoAlloc<Word16>(C * st->frame_size);
    st->y = echoAlloc<Word16>(C * N);
    st->last_y = echoAlloc<Word16>(C * N);
    st->Yf = echoAlloc<Word32>(st->frame_size + 1);
    st->Rf = echoAlloc<Word32>(st->frame_size + 1);
    st->Xf = echoAlloc<Word32>(st->frame_size + 1);
    st->Yh = echoAlloc<Word32>(st->frame_size + 1);
    st->Eh = echoAlloc<Word32>(st->frame_size + 1);
    st->X = echoAlloc<Word16>(K * (M + 1) * N);
    st->Y = echoAlloc<Word16>(C * N);
    st->E = echoAlloc<Word16>(C * N);
    st->W = echoAlloc<Word32>(C * K * M * N);
 
#ifdef TWO_PATH
    st->foreground = echoAlloc<Word16>(M * N * C * K);
#endif
 
    st->PHI = echoAlloc<Word32>(N);
    st->power = echoAlloc<Word32>(frame_size + 1);
    st->power_1 = echoAlloc<Num>(frame_size + 1);
    st->window = echoAlloc<Word16>(N);
    st->prop = echoAlloc<Word16>(M);
    st->wtmp = echoAlloc<Word16>(N);
 
    // Initialize window
    for (int i = 0; i < N; i++)
      st->window[i] = .5f - .5f * cosf(2 * M_PI * i / N);
 
    // Initialize power_1
    for (int i = 0; i <= st->frame_size; i++) st->power_1[i] = 1.0f;
 
    // Initialize W
    for (int i = 0; i < N * M * K * C; i++) st->W[i] = 0;
 
    // Initialize prop
    {
      Word32 sum = 0;
      float decay = expf(-2.4f / M);
      st->prop[0] = .7f;
      sum = st->prop[0];
      for (int i = 1; i < M; i++) {
        st->prop[i] = st->prop[i - 1] * decay;
        sum += st->prop[i];
      }
      for (int i = M - 1; i >= 0; i--) {
        st->prop[i] = .8f * st->prop[i] / sum;
      }
    }
 
    st->memX = echoAlloc<Word16>(K);
    st->memD = echoAlloc<Word16>(C);
    st->memE = echoAlloc<Word16>(C);
    st->preemph = .9f;
 
    if (st->sampling_rate < 12000)
      st->notch_radius = .9f;
    else if (st->sampling_rate < 24000)
      st->notch_radius = .982f;
    else
      st->notch_radius = .992f;
 
    st->notch_mem = echoAlloc<Mem>(2 * C);
    st->adapted = 0;
    st->Pey = st->Pyy = 1.0f;
 
#ifdef TWO_PATH
    st->Davg1 = st->Davg2 = 0;
    st->Dvar1 = st->Dvar2 = 0.0f;
#endif
 
    st->play_buf =
        echoAlloc<T>(K * (PLAYBACK_DELAY + 1) * st->frame_size);
    st->play_buf_pos = PLAYBACK_DELAY * st->frame_size;
    st->play_buf_started = 0;
 
    st->fft_table = nullptr;
 
    return st;
  }
 
  inline T saturateToSample(Num x) const {
    float v = (float)x;
    float mn = NumberConverter::minValueT<T>();
    float mx = NumberConverter::maxValueT<T>();
    if (v < mn + 0.5f) return (T)mn;
    if (v > mx - 0.5f) return (T)mx;
    return (T)floorf(0.5f + v);
  }
 
  inline void echoCancellationImpl(State* st, const T* in,
                                   const T* far_end,
                                   T* out) {
    int N = st->window_size;
    int M = st->M;
    int C = st->C;
    int K = st->K;
 
    // Wide-dynamic-range tuning constants, as Num (float or PseudoFloat)
    // so they multiply/compare directly against Num-typed state below.
    const Num min_leak(0.005f);        // Minimum leak estimate for the adaptive filter
    const Num var1_smooth(0.36f);      // Smoothing coefficient, 1st variance estimator
    const Num var2_smooth(0.7225f);    // Smoothing coefficient, 2nd variance estimator
    const Num var1_update(0.5f);       // Update threshold, 1st variance estimator
    const Num var2_update(0.25f);      // Update threshold, 2nd variance estimator
    const Num var_backtrack(4.0f);     // Backtrack threshold for filter reset
 
    st->cancel_count++;
    float ss = .35f / M;
    float ss_1 = 1 - ss;
 
    // Apply notch filter and pre-emphasis to input
    for (int chan = 0; chan < C; chan++) {
      filterDcNotch16(in + chan, st->notch_radius,
                      st->input + chan * st->frame_size, st->frame_size,
                      st->notch_mem + 2 * chan, C);
 
      for (int i = 0; i < st->frame_size; i++) {
        Word32 tmp32 =
            st->input[chan * st->frame_size + i] - st->preemph * st->memD[chan];
        st->memD[chan] = st->input[chan * st->frame_size + i];
        st->input[chan * st->frame_size + i] = tmp32;
      }
    }
 
    // Process far-end signal
    for (int speak = 0; speak < K; speak++) {
      std::memmove(&st->x[speak * N], 
                   &st->x[speak * N + st->frame_size],
                   st->frame_size * sizeof(Word16));
      for (int i = 0; i < st->frame_size; i++) {
        Word32 tmp32 =
            far_end[i * K + speak] - st->preemph * st->memX[speak];
        st->x[speak * N + i + st->frame_size] = tmp32;
        st->memX[speak] = far_end[i * K + speak];
      }
    }
 
    // Shift memory and compute FFT of far-end
    for (int speak = 0; speak < K; speak++) {
      for (int j = M - 1; j >= 0; j--) {
        std::memmove(&st->X[(j + 1) * N * K + speak * N],
                     &st->X[j * N * K + speak * N],
                     N * sizeof(Word16));
      }
      echo_fft(st->fft_table, st->x + speak * N, &st->X[speak * N]);
    }
 
    // Compute power spectrum of far-end
    Word32 Sxx = 0;
    for (int speak = 0; speak < K; speak++) {
      Sxx += mdfInnerProd(st->x + speak * N + st->frame_size,
                          st->x + speak * N + st->frame_size, st->frame_size);
      powerSpectrumAccum(st->X + speak * N, st->Xf, N);
    }
 
    // Compute foreground filter output and residual
    Word32 Sff = 0;
    for (int chan = 0; chan < C; chan++) {
#ifdef TWO_PATH
      spectralMulAccum(st->X, st->foreground + chan * N * K * M,
                       st->Y + chan * N, N, M * K);
      echo_ifft(st->fft_table, st->Y + chan * N, st->e + chan * N);
      for (int i = 0; i < st->frame_size; i++)
        st->e[chan * N + i] = st->input[chan * st->frame_size + i] -
                              st->e[chan * N + i + st->frame_size];
      Sff += mdfInnerProd(st->e + chan * N, st->e + chan * N, st->frame_size);
#endif
    }
 
    // Adjust proportional adaptation
    if (st->adapted) mdfAdjustProp(st->W, N, M, C * K, st->prop);
 
    // Compute weight gradient
    if (st->saturated == 0) {
      for (int chan = 0; chan < C; chan++) {
        for (int speak = 0; speak < K; speak++) {
          for (int j = M - 1; j >= 0; j--) {
            weightedSpectralMulConj(st->power_1, st->prop[j],
                                    &st->X[(j + 1) * N * K + speak * N],
                                    st->E + chan * N, st->PHI, N);
            for (int i = 0; i < N; i++)
              st->W[chan * N * K * M + j * N * K + speak * N + i] += st->PHI[i];
          }
        }
      }
    } else {
      st->saturated--;
    }
 
    // Update weights (AUMDF)
    for (int chan = 0; chan < C; chan++) {
      for (int speak = 0; speak < K; speak++) {
        for (int j = 0; j < M; j++) {
          if (j == 0 || st->cancel_count % (M - 1) == j - 1) {
            echo_ifft(
                st->fft_table, &st->W[chan * N * K * M + j * N * K + speak * N],
                st->wtmp);
            for (int i = st->frame_size; i < N; i++) st->wtmp[i] = 0;
            echo_fft(
                st->fft_table, st->wtmp,
                &st->W[chan * N * K * M + j * N * K + speak * N]);
          }
        }
      }
    }
 
    // Initialize spectrum buffers
    for (int i = 0; i <= st->frame_size; i++)
      st->Rf[i] = st->Yf[i] = st->Xf[i] = 0;
 
    Word32 Dbf = 0;
    Word32 See = 0;
 
#ifdef TWO_PATH
    // Compute background filter output
    for (int chan = 0; chan < C; chan++) {
      spectralMulAccum(st->X, st->W + chan * N * K * M, st->Y + chan * N, N,
                       M * K);
      echo_ifft(st->fft_table, st->Y + chan * N, st->y + chan * N);
      for (int i = 0; i < st->frame_size; i++)
        st->e[chan * N + i] = st->e[chan * N + i + st->frame_size] -
                              st->y[chan * N + i + st->frame_size];
      Dbf +=
          10 + mdfInnerProd(st->e + chan * N, st->e + chan * N, st->frame_size);
      for (int i = 0; i < st->frame_size; i++)
        st->e[chan * N + i] = st->input[chan * st->frame_size + i] -
                              st->y[chan * N + i + st->frame_size];
      See += mdfInnerProd(st->e + chan * N, st->e + chan * N, st->frame_size);
    }
#endif
 
#ifndef TWO_PATH
    Sff = See;
#endif
 
#ifdef TWO_PATH
    // Two-path filter logic
    st->Davg1 = .6f * st->Davg1 + .4f * (Sff - See);
    st->Davg2 = .85f * st->Davg2 + .15f * (Sff - See);
    st->Dvar1 = var1_smooth * st->Dvar1 + (.4f * Sff) * (.4f * Dbf);
    st->Dvar2 = var2_smooth * st->Dvar2 + (.15f * Sff) * (.15f * Dbf);
 
    int update_foreground = 0;
    if ((Sff - See) * fabsf(Sff - See) > Sff * Dbf)
      update_foreground = 1;
    else if (st->Davg1 * fabsf((float)st->Davg1) > var1_update * st->Dvar1)
      update_foreground = 1;
    else if (st->Davg2 * fabsf((float)st->Davg2) > var2_update * st->Dvar2)
      update_foreground = 1;
 
    if (update_foreground) {
      st->Davg1 = st->Davg2 = 0;
      st->Dvar1 = st->Dvar2 = 0.0f;
      std::memcpy(st->foreground, st->W, N * M * C * K * sizeof(Word16));
      for (int chan = 0; chan < C; chan++)
        for (int i = 0; i < st->frame_size; i++)
          st->e[chan * N + i + st->frame_size] =
              st->window[i + st->frame_size] *
                  st->e[chan * N + i + st->frame_size] +
              st->window[i] * st->y[chan * N + i + st->frame_size];
    } else {
      int reset_background = 0;
      if ((-(Sff - See)) * fabsf((float)(Sff - See)) >
          var_backtrack * (Sff * Dbf))
        reset_background = 1;
      if ((-st->Davg1) * fabsf((float)st->Davg1) > var_backtrack * st->Dvar1)
        reset_background = 1;
      if ((-st->Davg2) * fabsf((float)st->Davg2) > var_backtrack * st->Dvar2)
        reset_background = 1;
 
      if (reset_background) {
        std::memcpy(st->W, st->foreground, N * M * C * K * sizeof(Word32));
        for (int chan = 0; chan < C; chan++) {
          for (int i = 0; i < st->frame_size; i++)
            st->y[chan * N + i + st->frame_size] =
                st->e[chan * N + i + st->frame_size];
          for (int i = 0; i < st->frame_size; i++)
            st->e[chan * N + i] = st->input[chan * st->frame_size + i] -
                                  st->y[chan * N + i + st->frame_size];
        }
        See = Sff;
        st->Davg1 = st->Davg2 = 0;
        st->Dvar1 = st->Dvar2 = 0.0f;
      }
    }
#endif
 
    Word32 Sey = 0, Syy = 0, Sdd = 0;
    const float sat_threshold = NumberConverter::maxValueT<T>() * (32000.0f / 32767.0f);
    for (int chan = 0; chan < C; chan++) {
      // Compute output with de-emphasis
      for (int i = 0; i < st->frame_size; i++) {
        Word32 tmp_out;
#ifdef TWO_PATH
        tmp_out = st->input[chan * st->frame_size + i] -
                  st->e[chan * N + i + st->frame_size];
#else
        tmp_out = st->input[chan * st->frame_size + i] -
                  st->y[chan * N + i + st->frame_size];
#endif
        tmp_out = tmp_out + st->preemph * st->memE[chan];
        if (in[i * C + chan] <= -sat_threshold || in[i * C + chan] >= sat_threshold) {
          if (st->saturated == 0) st->saturated = 1;
        }
        out[i * C + chan] = saturateToSample(tmp_out);
        st->memE[chan] = tmp_out;
      }
 
      // Prepare error signal for filter update
      for (int i = 0; i < st->frame_size; i++) {
        st->e[chan * N + i + st->frame_size] = st->e[chan * N + i];
        st->e[chan * N + i] = 0;
      }
 
      // Compute correlations
      Sey += mdfInnerProd(st->e + chan * N + st->frame_size,
                          st->y + chan * N + st->frame_size, st->frame_size);
      Syy += mdfInnerProd(st->y + chan * N + st->frame_size,
                          st->y + chan * N + st->frame_size, st->frame_size);
      Sdd += mdfInnerProd(st->input + chan * st->frame_size,
                          st->input + chan * st->frame_size, st->frame_size);
 
      // Convert error to frequency domain
      echo_fft(st->fft_table, st->e + chan * N, st->E + chan * N);
 
      for (int i = 0; i < st->frame_size; i++) st->y[i + chan * N] = 0;
      echo_fft(st->fft_table, st->y + chan * N, st->Y + chan * N);
 
      // Compute power spectra
      powerSpectrumAccum(st->E + chan * N, st->Rf, N);
      powerSpectrumAccum(st->Y + chan * N, st->Yf, N);
    }
 
    // Sanity checks
    if (!(Syy >= 0 && Sxx >= 0 && See >= 0) ||
        !(Sff < N * 1e9f && Syy < N * 1e9f && Sxx < N * 1e9f)) {
      st->screwed_up += 50;
      for (int i = 0; i < st->frame_size * C; i++) out[i] = 0;
    } else if (Sff > Sdd + N * 10000.0f) {
      st->screwed_up++;
    } else {
      st->screwed_up = 0;
    }
 
    if (st->screwed_up >= 50) {
      echoWarning("Echo canceller reset");
      reset();
      return;
    }
 
    if (See < N * 100.0f) See = N * 100.0f;
 
    for (int speak = 0; speak < K; speak++) {
      Sxx += mdfInnerProd(st->x + speak * N + st->frame_size,
                          st->x + speak * N + st->frame_size, st->frame_size);
      powerSpectrumAccum(st->X + speak * N, st->Xf, N);
    }
 
    // Smooth far-end energy
    for (int j = 0; j <= st->frame_size; j++)
      st->power[j] = ss_1 * st->power[j] + 1 + ss * st->Xf[j];
 
    // Compute filtered spectra and correlations
    Num Pey = 0.0f, Pyy = 0.0f;
    for (int j = st->frame_size; j >= 0; j--) {
      Num Eh = st->Rf[j] - st->Eh[j];
      Num Yh = st->Yf[j] - st->Yh[j];
      Pey = Pey + Eh * Yh;
      Pyy = Pyy + Yh * Yh;
      st->Eh[j] =
          (1 - st->spec_average) * st->Eh[j] + st->spec_average * st->Rf[j];
      st->Yh[j] =
          (1 - st->spec_average) * st->Yh[j] + st->spec_average * st->Yf[j];
    }
 
    Pyy = spxSqrt(Pyy);
    // Pyy is 0 on the very first frames (no echo spectrum has built up yet),
    // which would make Pey/Pyy evaluate to 0/0 = NaN and permanently poison
    // st->Pey / leak_estimate (a NaN comparison is always false, so the
    // clamps below can never recover from it). Floor it at 1.0f, same
    // as the clamp already applied to st->Pyy a few lines down.
    if (Pyy < 1.0f) Pyy = 1.0f;
    Pey = Pey / Pyy;
 
    // Compute correlation update rate
    Word32 tmp32 = st->beta0 * Syy;
    if (tmp32 > st->beta_max * See) tmp32 = st->beta_max * See;
    Num alpha = tmp32 / See;
    Num alpha_1 = 1.0f - alpha;
 
    st->Pey = alpha_1 * st->Pey + alpha * Pey;
    st->Pyy = alpha_1 * st->Pyy + alpha * Pyy;
    if (st->Pyy < 1.0f) st->Pyy = 1.0f;
    if (st->Pey < min_leak * st->Pyy) st->Pey = min_leak * st->Pyy;
    if (st->Pey > st->Pyy) st->Pey = st->Pyy;
 
    st->leak_estimate = st->Pey / st->Pyy;
    if (st->leak_estimate > 16383)
      st->leak_estimate = 32767;
    else
      st->leak_estimate = st->leak_estimate * 2;
 
    // Compute RER
    Word16 RER;
    RER = (.0001f * Sxx + 3.f * st->leak_estimate * Syy) / See;
    if (RER < Sey * Sey / (1 + See * Syy)) RER = Sey * Sey / (1 + See * Syy);
    if (RER > .5f) RER = .5f;
 
    if (!st->adapted && st->sum_adapt > M &&
        st->leak_estimate * Syy > .03f * Syy) {
      st->adapted = 1;
    }
 
    if (st->adapted) {
      for (int i = 0; i <= st->frame_size; i++) {
        Word32 r = st->leak_estimate * st->Yf[i];
        Word32 e = st->Rf[i] + 1;
        if (r > .5f * e) r = .5f * e;
        r = .7f * r + .3f * (RER * e);
        st->power_1[i] = r / (e * (st->power[i] + 10));
      }
    } else {
      Word16 adapt_rate = 0;
      if (Sxx > N * 1000.0f) {
        tmp32 = .25f * Sxx;
        if (tmp32 > .25f * See) tmp32 = .25f * See;
        adapt_rate = tmp32 / See;
      }
      for (int i = 0; i <= st->frame_size; i++)
        st->power_1[i] = adapt_rate / (st->power[i] + 10);
      st->sum_adapt = st->sum_adapt + adapt_rate;
    }
 
    std::memmove(st->last_y, 
                 &st->last_y[st->frame_size],
                 st->frame_size * sizeof(Word16));
    if (st->adapted) {
      for (int i = 0; i < st->frame_size; i++)
        st->last_y[st->frame_size + i] = in[i] - out[i];
    }
  }
};
 
// ============================================================================
// FFT Implementation
// ============================================================================
inline void* echo_fft_init(int size, FFTDriver* driver) {
  if (!driver) {
    return nullptr;
  }
 
  // Re-initialize the raw driver at the window size the MDF algorithm
  // actually needs (independent of the caller's AudioFFTBase::config()).
  if (!driver->begin(size)) {
    return nullptr;
  }
  return new fft_state(size, driver);
}
 
inline void echo_fft_destroy(void* table) {
  if (table) {
    auto* st = static_cast<fft_state*>(table);
    st->driver->end();
    delete st;
  }
}
 
template <typename T>
inline void echo_fft(void* table, T* in, T* out) {
  auto* st = static_cast<fft_state*>(table);
  if (!st || !st->driver) return;
 
  // Set input values
  for (int i = 0; i < st->N; i++) {
    st->driver->setValue(i, (float)in[i] / (float)st->N);
  }
 
  // Perform FFT
  st->driver->fft();
 
  // Get output in packed format: out[0]=real[0], out[1]=real[1],
  // out[2]=img[1],
  // ...
  // Note: getValue() only reflects the frequency domain after rfft()
  // (inverse); right after fft() (forward), it is still the raw input, so
  // DC/Nyquist must come from getBin() too (its imaginary part is always 0
  // for those two bins).
  FFTBin dc_bin;
  st->driver->getBin(0, dc_bin);
  out[0] = dc_bin.real;  // DC component
  for (int i = 1; i < st->N - 1; i += 2) {
    int bin = (i + 1) / 2;
    FFTBin fft_bin;
    st->driver->getBin(bin, fft_bin);
    out[i] = fft_bin.real;
    out[i + 1] = fft_bin.img;
  }
  FFTBin nyquist_bin;
  st->driver->getBin(st->N / 2, nyquist_bin);
  out[st->N - 1] = nyquist_bin.real;  // Nyquist
}
 
template <typename T>
inline void echo_ifft(void* table, T* in, T* out) {
  auto* st = static_cast<fft_state*>(table);
  if (!st || !st->driver || !st->driver->isReverseFFT()) return;
 
  // Set bins from packed format
  st->driver->setBin(0, (float)in[0], 0);
  for (int i = 1; i < st->N - 1; i += 2) {
    int bin = (i + 1) / 2;
    st->driver->setBin(bin, (float)in[i], (float)in[i + 1]);
  }
  st->driver->setBin(st->N / 2, (float)in[st->N - 1], 0);
 
  // Perform inverse FFT
  st->driver->rfft();
 
  // Get output
  for (int i = 0; i < st->N; i++) {
    out[i] = st->driver->getValue(i);
  }
}
 
}  // namespace audio_tools