Big update in the multi-channel AEC to bring it up-to-date with the single
authorJean-Marc Valin <Jean-Marc.Valin@csiro.au>
Fri, 4 May 2007 05:11:18 +0000 (15:11 +1000)
committerJean-Marc Valin <Jean-Marc.Valin@csiro.au>
Mon, 19 May 2008 04:53:14 +0000 (14:53 +1000)
    channel AEC. Mainly this means:
    1) dual-path adaptive filter
    2) Adaptive (pseudo-proportional) learning rate for different taps
    3) API change
    4) Other minor details

Merge commit 'd2cddf7e2f3c1a75265c43cabaa391037c830745' into stereo

Conflicts:

include/speex/speex_echo.h
libspeex/mdf.c
libspeex/testecho.c

1  2 
include/speex/speex_echo.h
libspeex/mdf.c
libspeex/testecho.c

@@@ -48,29 -51,59 +48,32 @@@ extern "C" 
  /** Get sampling rate */
  #define SPEEX_ECHO_GET_SAMPLING_RATE 25
  
 -/** Internal echo canceller state. Should never be accessed directly. */
 -struct SpeexEchoState_;
  
 -/** @class SpeexEchoState
 - * This holds the state of the echo canceller. You need one per channel. 
 -*/
 +/*struct drft_lookup;*/
 +struct SpeexEchoState_;
  
 -/** Internal echo canceller state. Should never be accessed directly. */
  typedef struct SpeexEchoState_ SpeexEchoState;
  
 -/** Creates a new echo canceller state
 - * @param frame_size Number of samples to process at one time (should correspond to 10-20 ms)
 - * @param filter_length Number of samples of echo to cancel (should generally correspond to 100-500 ms)
 - * @return Newly-created echo canceller state
 - */
 -SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length);
 +/** Creates a new echo canceller state */
- SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length, int nb_mic, int nb_speakers);
++SpeexEchoState *mc_echo_state_init(int frame_size, int filter_length, int nb_mic, int nb_speakers);
  
 -/** Destroys an echo canceller state 
 - * @param st Echo canceller state
 -*/
 -void speex_echo_state_destroy(SpeexEchoState *st);
 +/** Destroys an echo canceller state */
 +void mc_echo_state_destroy(SpeexEchoState *st);
  
 -/** Performs echo cancellation a frame, based on the audio sent to the speaker (no delay is added
 - * to playback ni this form)
 - *
 - * @param st Echo canceller state
 - * @param rec signal from the microphone (near end + far end echo)
 - * @param play Signal played to the speaker (received from far end)
 - * @param out Returns near-end signal with echo removed
 - */
 -void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *rec, const spx_int16_t *play, spx_int16_t *out);
 +/** Performs echo cancellation a frame */
++void mc_echo_cancellation(SpeexEchoState *st, const spx_int16_t *rec, const spx_int16_t *play, spx_int16_t *out);
 -/** Performs echo cancellation a frame (deprecated) */
 -void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *rec, const spx_int16_t *play, spx_int16_t *out, spx_int32_t *Yout);
++/** Performs echo cancellation a frame */
 +void mc_echo_cancel(SpeexEchoState *st, const spx_int16_t *rec, const spx_int16_t *play, spx_int16_t *out, spx_int32_t *Yout);
  
 -/** Perform echo cancellation using internal playback buffer, which is delayed by two frames
 - * to account for the delay introduced by most soundcards (but it could be off!)
 - * @param st Echo canceller state
 - * @param rec signal from the microphone (near end + far end echo)
 - * @param out Returns near-end signal with echo removed
 -*/
 -void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out);
 +/** Perform echo cancellation using internal playback buffer */
- void mc_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out, spx_int32_t *Yout);
++void mc_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out);
  
 -/** Let the echo canceller know that a frame was just queued to the soundcard
 - * @param st Echo canceller state
 - * @param play Signal played to the speaker (received from far end)
 -*/
 -void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play);
 +/** Let the echo canceller know that a frame was just played */
 +void mc_echo_playback(SpeexEchoState *st, const spx_int16_t *play);
  
 -/** Reset the echo canceller to its original state 
 - * @param st Echo canceller state
 - */
 -void speex_echo_state_reset(SpeexEchoState *st);
 +/** Reset the echo canceller state */
 +void mc_echo_state_reset(SpeexEchoState *st);
  
  /** Used like the ioctl function to control the echo canceller parameters
   *
diff --cc libspeex/mdf.c
  #define WEIGHT_SHIFT 0
  #endif
  
 +/* If enabled, the transition between blocks is smooth, so there isn't any blocking
 +aftifact when adapting. The cost is an extra FFT and a matrix-vector multiply */
 +#define SMOOTH_BLOCKS
+ /* If enabled, the AEC will use a foreground filter and a background filter to be more robust to double-talk
+    and difficult signals in general. The cost is an extra FFT and a matrix-vector multiply */
+ #define TWO_PATH
  
  #ifdef FIXED_POINT
- static const spx_float_t MIN_LEAK = {16777, -19};
+ static const spx_float_t MIN_LEAK = {20972, -22};
+ /* Constants for the two-path filter */
+ static const spx_float_t VAR1_SMOOTH = {23593, -16};
+ static const spx_float_t VAR2_SMOOTH = {23675, -15};
+ static const spx_float_t VAR1_UPDATE = {16384, -15};
+ static const spx_float_t VAR2_UPDATE = {16384, -16};
+ static const spx_float_t VAR_BACKTRACK = {16384, -12};
  #define TOP16(x) ((x)>>16)
  #else
- static const spx_float_t MIN_LEAK = .032f;
 -static const spx_float_t MIN_LEAK = .005f;
++static const spx_float_t MIN_LEAK = .0032f;
+ /* Constants for the two-path filter */
+ static const spx_float_t VAR1_SMOOTH = .36f;
+ static const spx_float_t VAR2_SMOOTH = .7225f;
+ static const spx_float_t VAR1_UPDATE = .5f;
+ static const spx_float_t VAR2_UPDATE = .25f;
+ static const spx_float_t VAR_BACKTRACK = 4.f;
  #define TOP16(x) (x)
  #endif
  
@@@ -111,8 -129,7 +132,9 @@@ struct SpeexEchoState_ 
     int cancel_count;
     int adapted;
     int saturated;
+    int screwed_up;
 +   int C;                    /** Number of input channels (microphones) */
 +   int K;                    /** Number of output channels (loudspeakers) */
     spx_int32_t sampling_rate;
     spx_word16_t spec_average;
     spx_word16_t beta0;
     /* NOTE: If you only use speex_echo_cancel() and want to save some memory, remove this */
     spx_int16_t *play_buf;
     int play_buf_pos;
+    int play_buf_started;
  };
  
 -static inline void filter_dc_notch16(const spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem)
 +static inline void filter_dc_notch16(const spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem, int stride)
  {
     int i;
     spx_word16_t den2;
@@@ -269,29 -313,83 +330,88 @@@ static inline void spectral_mul_accum(c
  #endif
  
  /** Compute weighted cross-power spectrum of a half-complex (packed) vector with conjugate */
- static inline void weighted_spectral_mul_conj(const spx_float_t *w, const spx_word16_t *X, const spx_word16_t *Y, spx_word32_t *prod, int N)
+ static inline void weighted_spectral_mul_conj(const spx_float_t *w, const spx_float_t p, const spx_word16_t *X, const spx_word16_t *Y, spx_word32_t *prod, int N)
  {
     int i, j;
-    prod[0] = FLOAT_MUL32(w[0],MULT16_16(X[0],Y[0]));
+    spx_float_t W;
+    W = FLOAT_AMULT(p, w[0]);
+    prod[0] = FLOAT_MUL32(W,MULT16_16(X[0],Y[0]));
     for (i=1,j=1;i<N-1;i+=2,j++)
     {
-       prod[i] = FLOAT_MUL32(w[j],MAC16_16(MULT16_16(X[i],Y[i]), X[i+1],Y[i+1]));
-       prod[i+1] = FLOAT_MUL32(w[j],MAC16_16(MULT16_16(-X[i+1],Y[i]), X[i],Y[i+1]));
+       W = FLOAT_AMULT(p, w[j]);
+       prod[i] = FLOAT_MUL32(W,MAC16_16(MULT16_16(X[i],Y[i]), X[i+1],Y[i+1]));
+       prod[i+1] = FLOAT_MUL32(W,MAC16_16(MULT16_16(-X[i+1],Y[i]), X[i],Y[i+1]));
+    }
+    W = FLOAT_AMULT(p, w[j]);
+    prod[i] = FLOAT_MUL32(W,MULT16_16(X[i],Y[i]));
+ }
 -static inline void mdf_adjust_prop(const spx_word32_t *W, int N, int M, spx_word16_t *prop)
++static inline void mdf_adjust_prop(const spx_word32_t *W, int N, int M, int P, spx_word16_t *prop)
+ {
 -   int i, j;
++   int i, j, p;
+    spx_word16_t max_sum = 1;
+    spx_word32_t prop_sum = 1;
+    for (i=0;i<M;i++)
+    {
+       spx_word32_t tmp = 1;
 -      for (j=0;j<N;j++)
 -         tmp += MULT16_16(EXTRACT16(SHR32(W[i*N+j],18)), EXTRACT16(SHR32(W[i*N+j],18)));
++      for (p=0;p<P;p++)
++         for (j=0;j<N;j++)
++            tmp += MULT16_16(EXTRACT16(SHR32(W[p*N*M + i*N+j],18)), EXTRACT16(SHR32(W[p*N*M + i*N+j],18)));
+ #ifdef FIXED_POINT
+       /* Just a security in case an overflow were to occur */
+       tmp = MIN32(ABS32(tmp), 536870912);
+ #endif
+       prop[i] = spx_sqrt(tmp);
+       if (prop[i] > max_sum)
+          max_sum = prop[i];
+    }
+    for (i=0;i<M;i++)
+    {
 -      prop[i] += MULT16_16_Q15(QCONST16(.1f,15),max_sum);
++      prop[i] += MULT16_16_Q15(QCONST16(.03f,15),max_sum);
+       prop_sum += EXTEND32(prop[i]);
+    }
+    for (i=0;i<M;i++)
+    {
+       prop[i] = DIV32(MULT16_16(QCONST16(.99f,15), prop[i]),prop_sum);
+       /*printf ("%f ", prop[i]);*/
     }
-    prod[i] = FLOAT_MUL32(w[j],MULT16_16(X[i],Y[i]));
+    /*printf ("\n");*/
  }
  
+ #ifdef DUMP_ECHO_CANCEL_DATA
+ #include <stdio.h>
+ static FILE *rFile=NULL, *pFile=NULL, *oFile=NULL;
+ static void dump_audio(const spx_int16_t *rec, const spx_int16_t *play, const spx_int16_t *out, int len)
+ {
+    if (!(rFile && pFile && oFile))
+    {
+       speex_error("Dump files not open");
+    }
+    fwrite(rec, sizeof(spx_int16_t), len, rFile);
+    fwrite(play, sizeof(spx_int16_t), len, pFile);
+    fwrite(out, sizeof(spx_int16_t), len, oFile);
+ }
+ #endif
  
  /** Creates a new echo canceller state */
 -SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length)
 +SpeexEchoState *mc_echo_state_init(int frame_size, int filter_length, int nb_mic, int nb_speakers)
  {
 -   int i,N,M;
 +   int i,N,M, C, K;
     SpeexEchoState *st = (SpeexEchoState *)speex_alloc(sizeof(SpeexEchoState));
  
 +   st->K = nb_speakers;
 +   st->C = nb_mic;
 +   C=st->C;
 +   K=st->K;
+ #ifdef DUMP_ECHO_CANCEL_DATA
+    if (rFile || pFile || oFile)
+       speex_error("Opening dump files twice");
+    rFile = fopen("aec_rec.sw", "w");
+    pFile = fopen("aec_play.sw", "w");
+    oFile = fopen("aec_out.sw", "w");
+ #endif
+    
     st->frame_size = frame_size;
     st->window_size = 2*frame_size;
     N = st->window_size;
  
     st->fft_table = spx_fft_init(N);
     
 -   st->e = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
 -   st->x = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
 -   st->input = (spx_word16_t*)speex_alloc(st->frame_size*sizeof(spx_word16_t));
 -   st->y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
 -   st->last_y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
 +   st->e = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
 +   st->x = (spx_word16_t*)speex_alloc(K*N*sizeof(spx_word16_t));
-    st->d = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
++   st->input = (spx_word16_t*)speex_alloc(C*st->frame_size*sizeof(spx_word16_t));
 +   st->y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
-    st->Yps = (spx_word32_t*)speex_alloc(C*N*sizeof(spx_word32_t));
 +   st->last_y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
     st->Yf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
     st->Rf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
     st->Xf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
     st->Yh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
     st->Eh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
  
 -   st->X = (spx_word16_t*)speex_alloc((M+1)*N*sizeof(spx_word16_t));
 -   st->Y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
 -   st->E = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
 -   st->W = (spx_word32_t*)speex_alloc(M*N*sizeof(spx_word32_t));
 +   st->X = (spx_word16_t*)speex_alloc(K*(M+1)*N*sizeof(spx_word16_t));
 +   st->Y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
 +   st->E = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
 +   st->W = (spx_word32_t*)speex_alloc(C*K*M*N*sizeof(spx_word32_t));
+ #ifdef TWO_PATH
 -   st->foreground = (spx_word16_t*)speex_alloc(M*N*sizeof(spx_word16_t));
++   st->foreground = (spx_word16_t*)speex_alloc(M*N*C*K*sizeof(spx_word16_t));
+ #endif
     st->PHI = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
     st->power = (spx_word32_t*)speex_alloc((frame_size+1)*sizeof(spx_word32_t));
     st->power_1 = (spx_float_t*)speex_alloc((frame_size+1)*sizeof(spx_float_t));
  #endif
     for (i=0;i<=st->frame_size;i++)
        st->power_1[i] = FLOAT_ONE;
 -   for (i=0;i<N*M;i++)
 +   for (i=0;i<N*M*K*C;i++)
        st->W[i] = 0;
-    for (i=0;i<N;i++)
-       st->PHI[i] = 0;
     {
        spx_word32_t sum = 0;
        /* Ratio of ~10 between adaptation rate of first and last block */
        }
        for (i=M-1;i>=0;i--)
        {
-          st->prop[i] = DIV32(SHL32(EXTEND32(st->prop[i]),15),sum);
 -         st->prop[i] = DIV32(MULT16_16(QCONST16(.8,15), st->prop[i]),sum);
++         st->prop[i] = DIV32(MULT16_16(QCONST16(.99f,15), st->prop[i]),sum);
        }
     }
     
     st->adapted = 0;
     st->Pey = st->Pyy = FLOAT_ONE;
     
-    st->play_buf = (spx_int16_t*)speex_alloc(K*2*st->frame_size*sizeof(spx_int16_t));
-    st->play_buf_pos = 0;
+ #ifdef TWO_PATH
+    st->Davg1 = st->Davg2 = 0;
+    st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
+ #endif
+    
 -   st->play_buf = (spx_int16_t*)speex_alloc((PLAYBACK_DELAY+1)*st->frame_size*sizeof(spx_int16_t));
++   st->play_buf = (spx_int16_t*)speex_alloc(K*(PLAYBACK_DELAY+1)*st->frame_size*sizeof(spx_int16_t));
+    st->play_buf_pos = PLAYBACK_DELAY*st->frame_size;
+    st->play_buf_started = 0;
+    
     return st;
  }
  
  /** Resets echo canceller state */
  void speex_echo_state_reset(SpeexEchoState *st)
  {
 -   int i, M, N;
 +   int i, M, N, C, K;
     st->cancel_count=0;
+    st->screwed_up = 0;
     N = st->window_size;
     M = st->M;
 +   C=st->C;
 +   K=st->K;
     for (i=0;i<N*M;i++)
        st->W[i] = 0;
+ #ifdef TWO_PATH
+    for (i=0;i<N*M;i++)
+       st->foreground[i] = 0;
+ #endif
     for (i=0;i<N*(M+1);i++)
        st->X[i] = 0;
     for (i=0;i<=st->frame_size;i++)
+    {
        st->power[i] = 0;
-    for (i=0;i<N;i++)
+       st->power_1[i] = FLOAT_ONE;
+       st->Eh[i] = 0;
+       st->Yh[i] = 0;
+    }
+    for (i=0;i<st->frame_size;i++)
+    {
+       st->last_y[i] = 0;
+    }
 -   for (i=0;i<N;i++)
++   for (i=0;i<N*C;i++)
+    {
        st->E[i] = 0;
 -   st->notch_mem[0] = st->notch_mem[1] = 0;
 -   st->memX=st->memD=st->memE=0;
++   }
++   for (i=0;i<N*K;i++)
++   {
+       st->x[i] = 0;
+    }
-   
 +   for (i=0;i<2*C;i++)
 +      st->notch_mem[i] = 0;
++   for (i=0;i<C;i++)
++      st->memD[i]=st->memE[i]=0;
++   for (i=0;i<K;i++)
++      st->memX[i]=0;
     st->saturated = 0;
     st->adapted = 0;
     st->sum_adapt = 0;
@@@ -449,19 -577,28 +610,29 @@@ void mc_echo_state_destroy(SpeexEchoSta
  #endif
     speex_free(st->play_buf);
     speex_free(st);
+    
+ #ifdef DUMP_ECHO_CANCEL_DATA
+    fclose(rFile);
+    fclose(pFile);
+    fclose(oFile);
+    rFile = pFile = oFile = NULL;
+ #endif
  }
  
- void mc_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out, spx_int32_t *Yout)
 -void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out)
++
++void mc_echo_capture2(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out)
  {
     int i;
+    /*speex_warning_int("capture with fill level ", st->play_buf_pos/st->frame_size);*/
+    st->play_buf_started = 1;
     if (st->play_buf_pos>=st->frame_size)
     {
-       mc_echo_cancel(st, rec, st->play_buf, out, Yout);
 -      speex_echo_cancellation(st, rec, st->play_buf, out);
++      mc_echo_cancellation(st, rec, st->play_buf, out);
        st->play_buf_pos -= st->frame_size;
-       for (i=0;i<st->frame_size;i++)
+       for (i=0;i<st->play_buf_pos;i++)
           st->play_buf[i] = st->play_buf[i+st->frame_size];
     } else {
-       speex_warning("no playback frame available");
+       speex_warning("No playback frame available (your application is buggy and/or got xruns)");
        if (st->play_buf_pos!=0)
        {
           speex_warning("internal playback buffer corruption?");
     }
  }
  
 -void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play)
 +void mc_echo_playback(SpeexEchoState *st, const spx_int16_t *play)
  {
-    if (st->play_buf_pos<=st->frame_size)
+    /*speex_warning_int("playback with fill level ", st->play_buf_pos/st->frame_size);*/
+    if (!st->play_buf_started)
+    {
+       speex_warning("discarded first playback frame");
+       return;
+    }
+    if (st->play_buf_pos<=PLAYBACK_DELAY*st->frame_size)
     {
        int i;
        for (i=0;i<st->frame_size;i++)
  }
  
  /** Performs echo cancellation on a frame */
- void mc_echo_cancel(SpeexEchoState *st, const spx_int16_t *ref, const spx_int16_t *echo, spx_int16_t *out, spx_int32_t *Yout)
 -void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out, spx_int32_t *Yout)
++void mc_echo_cancel(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out, spx_int32_t *Yout)
+ {
 -   speex_echo_cancellation(st, in, far_end, out);
++   mc_echo_cancellation(st, in, far_end, out);
+ }
+ /** Performs echo cancellation on a frame (deprecated, last arg now ignored) */
 -void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out)
++void mc_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out)
  {
 -   int i,j;
 -   int N,M;
 +   int i,j, chan, speak;
 +   int N,M, C, K;
-    spx_word16_t leak_estimate;
+    spx_word32_t Syy,See,Sxx,Sdd, Sff;
+ #ifdef TWO_PATH
+    spx_word32_t Dbf;
+    int update_foreground;
+ #endif
+    spx_word32_t Sey;
     spx_word16_t ss, ss_1;
     spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE;
     spx_float_t alpha, alpha_1;
     
     N = st->window_size;
     M = st->M;
-    spx_word32_t Syy=0,See=0,Sxx=0;
 +   C = st->C;
 +   K = st->K;
 +
     st->cancel_count++;
  #ifdef FIXED_POINT
     ss=DIV32_16(11469,M);
     ss_1 = 1-ss;
  #endif
  
 -   /* Apply a notch filter to make sure DC doesn't end up causing problems */
 -   filter_dc_notch16(in, st->notch_radius, st->input, st->frame_size, st->notch_mem);
 -   /* Copy input data to buffer and apply pre-emphasis */
 -   for (i=0;i<st->frame_size;i++)
 +   for (chan = 0; chan < C; chan++)
     {
-       filter_dc_notch16(ref+chan, st->notch_radius, st->d+chan*N, st->frame_size, st->notch_mem+2*chan, C);
 -      spx_word32_t tmp32;
 -      tmp32 = SUB32(EXTEND32(far_end[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memX)));
 -#ifdef FIXED_POINT
 -      /* If saturation occurs here, we need to freeze adaptation for M+1 frames (not just one) */
 -      if (tmp32 > 32767)
++      /* Apply a notch filter to make sure DC doesn't end up causing problems */
++      filter_dc_notch16(in+chan, st->notch_radius, st->input+chan*st->frame_size, st->frame_size, st->notch_mem+2*chan, C);
++      /* Copy input data to buffer and apply pre-emphasis */
 +      /* Copy input data to buffer */
 +      for (i=0;i<st->frame_size;i++)
        {
-          spx_word16_t tmp;
 -         tmp32 = 32767;
 -         st->saturated = M+1;
 +         spx_word32_t tmp32;
-          tmp = st->d[chan*N+i];
-          st->d[chan*N+i] = st->d[chan*N+i+st->frame_size];
-          tmp32 = SUB32(EXTEND32(tmp), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan])));
++         /* FIXME: This core has changed a bit, need to merge properly */
++         tmp32 = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan])));
 +#ifdef FIXED_POINT
 +         if (tmp32 > 32767)
 +         {
 +            tmp32 = 32767;
-             st->saturated = 1;
++            if (st->saturated == 0)
++               st->saturated = 1;
 +         }      
 +         if (tmp32 < -32767)
 +         {
 +            tmp32 = -32767;
-             st->saturated = 1;
++            if (st->saturated == 0)
++               st->saturated = 1;
 +         }
 +#endif
-          st->d[chan*N+i+st->frame_size] = tmp32;
-          st->memD[chan] = tmp;
++         st->memD[chan] = st->input[chan*st->frame_size+i];
++         st->input[chan*st->frame_size+i] = EXTRACT16(tmp32);
        }
 -      if (tmp32 < -32767)
 +   }
 +
 +   for (speak = 0; speak < K; speak++)
 +   {
 +      for (i=0;i<st->frame_size;i++)
        {
-          spx_word16_t tmp;
 -         tmp32 = -32767;
 -         st->saturated = M+1;
 -      }      
 -#endif
 -      st->x[i+st->frame_size] = EXTRACT16(tmp32);
 -      st->memX = far_end[i];
 -      
 -      tmp32 = SUB32(EXTEND32(st->input[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD)));
 +         spx_word32_t tmp32;
 +         st->x[speak*N+i] = st->x[speak*N+i+st->frame_size];
-          tmp32 = SUB32(EXTEND32(echo[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak])));
++         tmp32 = SUB32(EXTEND32(far_end[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak])));
  #ifdef FIXED_POINT
 -      if (tmp32 > 32767)
 -      {
 -         tmp32 = 32767;
 -         if (st->saturated == 0)
 -            st->saturated = 1;
 -      }      
 -      if (tmp32 < -32767)
 +         /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */
 +         if (tmp32 > 32767)
 +         {
 +            tmp32 = 32767;
-             st->saturated = 1;
++            st->saturated = M+1;
 +         }      
 +         if (tmp32 < -32767)
 +         {
 +            tmp32 = -32767;
-             st->saturated = 1;
++            st->saturated = M+1;
 +         }      
 +#endif
 +         st->x[speak*N+i+st->frame_size] = EXTRACT16(tmp32);
-          st->memX[speak] = echo[i*K+speak];
++         st->memX[speak] = far_end[i*K+speak];
 +      }
 +   }   
 +   
 +   for (speak = 0; speak < K; speak++)
 +   {
 +      /* Shift memory: this could be optimized eventually*/
 +      for (j=M-1;j>=0;j--)
        {
 -         tmp32 = -32767;
 -         if (st->saturated == 0)
 -            st->saturated = 1;
 +         for (i=0;i<N;i++)
 +            st->X[(j+1)*N*K+speak*N+i] = st->X[j*N*K+speak*N+i];
        }
 -#endif
 -      st->memD = st->input[i];
 -      st->input[i] = tmp32;
 +      /* Convert x (echo input) to frequency domain */
 +      spx_fft(st->fft_table, st->x+speak*N, &st->X[speak*N]);
     }
 -
 -   /* Shift memory: this could be optimized eventually*/
 -   for (j=M-1;j>=0;j--)
 +   
++   Sxx = 0;
++   for (speak = 0; speak < K; speak++)
+    {
 -      for (i=0;i<N;i++)
 -         st->X[(j+1)*N+i] = st->X[j*N+i];
++      Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size);
++      power_spectrum_accum(st->X+speak*N, st->Xf, N);
+    }
 -
 -   /* Convert x (far end) to frequency domain */
 -   spx_fft(st->fft_table, st->x, &st->X[0]);
 -   for (i=0;i<N;i++)
 -      st->last_y[i] = st->x[i];
 -   Sxx = mdf_inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size);
 -   for (i=0;i<st->frame_size;i++)
 -      st->x[i] = st->x[i+st->frame_size];
 -   /* From here on, the top part of x is used as scratch space */
+    
++   Sff = 0;  
 +   for (chan = 0; chan < C; chan++)
 +   {
- #ifdef SMOOTH_BLOCKS
-       spectral_mul_accum(st->X, st->W+chan*N*K*M, st->Y+chan*N, N, M*K);
+ #ifdef TWO_PATH
 -   /* Compute foreground filter */
 -   spectral_mul_accum16(st->X, st->foreground, st->Y, N, M);   
 -   spx_ifft(st->fft_table, st->Y, st->e);
 -   for (i=0;i<st->frame_size;i++)
 -      st->e[i] = SUB16(st->input[i], st->e[i+st->frame_size]);
 -   Sff = mdf_inner_prod(st->e, st->e, st->frame_size);
++      /* Compute foreground filter */
++      spectral_mul_accum16(st->X, st->foreground+chan*N*K*M, st->Y+chan*N, N, M*K);
 +      spx_ifft(st->fft_table, st->Y+chan*N, st->e+chan*N);
++      for (i=0;i<st->frame_size;i++)
++         st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->e[chan*N+i+st->frame_size]);
++      Sff += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
  #endif
 +   }
     
 -   mdf_adjust_prop (st->W, N, M, st->prop);
+    /* Adjust proportional adaption rate */
++   /* FIXME: Adjust that for C, K*/
++   if (st->adapted)
++      mdf_adjust_prop (st->W, N, M, C*K, st->prop);
     /* Compute weight gradient */
-    if (!st->saturated)
+    if (st->saturated == 0)
     {
 -      for (j=M-1;j>=0;j--)
 +      for (chan = 0; chan < C; chan++)
        {
 -         weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N], st->E, st->PHI, N);
 -         for (i=0;i<N;i++)
 -            st->W[j*N+i] = ADD32(st->W[j*N+i], st->PHI[i]);
 -         
 +         for (speak = 0; speak < K; speak++)
 +         {
 +            for (j=M-1;j>=0;j--)
 +            {
-                weighted_spectral_mul_conj(st->power_1, &st->X[(j+1)*N*K+speak*N], st->E+chan*N, st->PHI, N);
++               weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N*K+speak*N], st->E+chan*N, st->PHI, N);
 +               for (i=0;i<N;i++)
-                   st->W[chan*N*K*M + j*N*K + speak*N + i] += MULT16_32_Q15(st->prop[j], st->PHI[i]);
++                  st->W[chan*N*K*M + j*N*K + speak*N + i] += st->PHI[i];
 +            }
 +         }
-       }   
+       }
+    } else {
+       st->saturated--;
     }
     
-    st->saturated = 0;
-    
 +   /* FIXME: MC conversion required */ 
     /* Update weight to prevent circular convolution (MDF / AUMDF) */
 -   for (j=0;j<M;j++)
 +   for (chan = 0; chan < C; chan++)
     {
 -      /* This is a variant of the Alternatively Updated MDF (AUMDF) */
 -      /* Remove the "if" to make this an MDF filter */
 -      if (j==0 || st->cancel_count%(M-1) == j-1)
 +      for (speak = 0; speak < K; speak++)
        {
 -#ifdef FIXED_POINT
 -         for (i=0;i<N;i++)
 -            st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16));
 -         spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
 -         for (i=0;i<st->frame_size;i++)
 -         {
 -            st->wtmp[i]=0;
 -         }
 -         for (i=st->frame_size;i<N;i++)
 +         for (j=0;j<M;j++)
           {
 -            st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
 -         }
 -         spx_fft(st->fft_table, st->wtmp, st->wtmp2);
 -         /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
 -         for (i=0;i<N;i++)
 -            st->W[j*N+i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
 +            /* This is a variant of the Alternatively Updated MDF (AUMDF) */
 +            /* Remove the "if" to make this an MDF filter */
 +            if (j==0 || st->cancel_count%(M-1) == j-1)
 +            {
 +#ifdef FIXED_POINT
 +               for (i=0;i<N;i++)
 +                  st->wtmp2[i] = EXTRACT16(PSHR32(st->W[chan*N*K*M + j*N*K + speak*N + i],NORMALIZE_SCALEDOWN+16));
 +               spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
 +               for (i=0;i<st->frame_size;i++)
 +               {
 +                  st->wtmp[i]=0;
 +               }
 +               for (i=st->frame_size;i<N;i++)
 +               {
 +                  st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
 +               }
 +               spx_fft(st->fft_table, st->wtmp, st->wtmp2);
 +               /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
 +               for (i=0;i<N;i++)
 +                  st->W[chan*N*K*M + j*N*K + speak*N + i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
  #else
 -         spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
 -         for (i=st->frame_size;i<N;i++)
 -         {
 -            st->wtmp[i]=0;
 -         }
 -         spx_fft(st->fft_table, st->wtmp, &st->W[j*N]);
 +               spx_ifft(st->fft_table, &st->W[chan*N*K*M + j*N*K + speak*N], st->wtmp);
 +               for (i=st->frame_size;i<N;i++)
 +               {
 +                  st->wtmp[i]=0;
 +               }
 +               spx_fft(st->fft_table, st->wtmp, &st->W[chan*N*K*M + j*N*K + speak*N]);
  #endif
 +            }
 +         }
        }
     }
 -
 -   /* Compute filter response Y */
 -   spectral_mul_accum(st->X, st->W, st->Y, N, M);
 -   spx_ifft(st->fft_table, st->Y, st->y);
 -
 +   
 +   /* So we can use power_spectrum_accum */ 
 +   for (i=0;i<=st->frame_size;i++)
 +      st->Rf[i] = st->Yf[i] = st->Xf[i] = 0;
-    
++      
++   Dbf = 0;
++   See = 0;    
+ #ifdef TWO_PATH
+    /* Difference in response, this is used to estimate the variance of our residual power estimate */
 -   for (i=0;i<st->frame_size;i++)
 -      st->e[i] = SUB16(st->e[i+st->frame_size], st->y[i+st->frame_size]);
 -   Dbf = 10+mdf_inner_prod(st->e, st->e, st->frame_size);
 +   for (chan = 0; chan < C; chan++)
 +   {
-       /* Compute filter response Y */
 +      spectral_mul_accum(st->X, st->W+chan*N*K*M, st->Y+chan*N, N, M*K);
 +      spx_ifft(st->fft_table, st->Y+chan*N, st->y+chan*N);
++      for (i=0;i<st->frame_size;i++)
++         st->e[chan*N+i] = SUB16(st->e[chan*N+i+st->frame_size], st->y[chan*N+i+st->frame_size]);
++      Dbf += 10+mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
++      for (i=0;i<st->frame_size;i++)
++         st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]);
++      See += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
++   }
+ #endif
 -   for (i=0;i<st->frame_size;i++)
 -      st->e[i] = SUB16(st->input[i], st->y[i+st->frame_size]);
 -   See = mdf_inner_prod(st->e, st->e, st->frame_size);
+ #ifndef TWO_PATH
+    Sff = See;
+ #endif
  
+ #ifdef TWO_PATH
+    /* Logic for updating the foreground filter */
+    
+    /* For two time windows, compute the mean of the energy difference, as well as the variance */
+    st->Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),st->Davg1), MULT16_32_Q15(QCONST16(.4f,15),SUB32(Sff,See)));
+    st->Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),st->Davg2), MULT16_32_Q15(QCONST16(.15f,15),SUB32(Sff,See)));
+    st->Dvar1 = FLOAT_ADD(FLOAT_MULT(VAR1_SMOOTH, st->Dvar1), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), MULT16_32_Q15(QCONST16(.4f,15),Dbf)));
+    st->Dvar2 = FLOAT_ADD(FLOAT_MULT(VAR2_SMOOTH, st->Dvar2), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), MULT16_32_Q15(QCONST16(.15f,15),Dbf)));
+    
+    /* Equivalent float code:
+    st->Davg1 = .6*st->Davg1 + .4*(Sff-See);
+    st->Davg2 = .85*st->Davg2 + .15*(Sff-See);
+    st->Dvar1 = .36*st->Dvar1 + .16*Sff*Dbf;
+    st->Dvar2 = .7225*st->Dvar2 + .0225*Sff*Dbf;
+    */
+    
+    update_foreground = 0;
+    /* Check if we have a statistically significant reduction in the residual echo */
+    /* Note that this is *not* Gaussian, so we need to be careful about the longer tail */
+    if (FLOAT_GT(FLOAT_MUL32U(SUB32(Sff,See),ABS32(SUB32(Sff,See))), FLOAT_MUL32U(Sff,Dbf)))
+       update_foreground = 1;
+    else if (FLOAT_GT(FLOAT_MUL32U(st->Davg1, ABS32(st->Davg1)), FLOAT_MULT(VAR1_UPDATE,(st->Dvar1))))
+       update_foreground = 1;
+    else if (FLOAT_GT(FLOAT_MUL32U(st->Davg2, ABS32(st->Davg2)), FLOAT_MULT(VAR2_UPDATE,(st->Dvar2))))
+       update_foreground = 1;
     
 -      for (i=0;i<N*M;i++)
+    /* Do we update? */
+    if (update_foreground)
+    {
+       st->Davg1 = st->Davg2 = 0;
+       st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
+       /* Copy background filter to foreground filter */
 -      for (i=0;i<st->frame_size;i++)
 -         st->e[i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[i+st->frame_size]);
++      for (i=0;i<N*M*C*K;i++)
+          st->foreground[i] = EXTRACT16(PSHR32(st->W[i],16));
+       /* Apply a smooth transition so as to not introduce blocking artifacts */
 -         for (i=0;i<N*M;i++)
++      for (chan = 0; chan < C; chan++)
++         for (i=0;i<st->frame_size;i++)
++            st->e[chan*N+i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[chan*N+i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[chan*N+i+st->frame_size]);
+    } else {
+       int reset_background=0;
+       /* Otherwise, check if the background filter is significantly worse */
+       if (FLOAT_GT(FLOAT_MUL32U(NEG32(SUB32(Sff,See)),ABS32(SUB32(Sff,See))), FLOAT_MULT(VAR_BACKTRACK,FLOAT_MUL32U(Sff,Dbf))))
+          reset_background = 1;
+       if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg1), ABS32(st->Davg1)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar1)))
+          reset_background = 1;
+       if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg2), ABS32(st->Davg2)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar2)))
+          reset_background = 1;
+       if (reset_background)
+       {
+          /* Copy foreground filter to background filter */
 -         for (i=0;i<st->frame_size;i++)
 -            st->y[i+st->frame_size] = st->e[i+st->frame_size];
 -         for (i=0;i<st->frame_size;i++)
 -            st->e[i] = SUB16(st->input[i], st->y[i+st->frame_size]);
++         for (i=0;i<N*M*C*K;i++)
+             st->W[i] = SHL32(EXTEND32(st->foreground[i]),16);
+          /* We also need to copy the output so as to get correct adaptation */
 -   /* Compute error signal (for the output with de-emphasis) */ 
 -   for (i=0;i<st->frame_size;i++)
 -   {
 -      spx_word32_t tmp_out;
++         for (chan = 0; chan < C; chan++)
++         {        
++            for (i=0;i<st->frame_size;i++)
++               st->y[chan*N+i+st->frame_size] = st->e[chan*N+i+st->frame_size];
++            for (i=0;i<st->frame_size;i++)
++               st->e[chan*N+i] = SUB16(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 = FLOAT_ZERO;
+       }
+    }
+ #endif
- #ifdef SMOOTH_BLOCKS
-          spx_word16_t y = MULT16_16_Q15(st->window[i+st->frame_size],st->e[chan*N+i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[chan*N+i+st->frame_size]);
-          tmp_out = SUB32(EXTEND32(st->d[chan*N+i+st->frame_size]), EXTEND32(y));
++   Sey = Syy = Sdd = 0;  
++   for (chan = 0; chan < C; chan++)
++   {    
 +      /* Compute error signal (for the output with de-emphasis) */ 
 +      for (i=0;i<st->frame_size;i++)
 +      {
 +         spx_word32_t tmp_out;
 -      tmp_out = SUB32(EXTEND32(st->input[i]), EXTEND32(st->e[i+st->frame_size]));
+ #ifdef TWO_PATH
++         tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->e[chan*N+i+st->frame_size]));
  #else
-          tmp_out = SUB32(EXTEND32(st->d[chan*N+i+st->frame_size]), EXTEND32(st->y[chan*N+i+st->frame_size]));
 -      tmp_out = SUB32(EXTEND32(st->input[i]), EXTEND32(st->y[i+st->frame_size]));
++         tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->y[chan*N+i+st->frame_size]));
  #endif
 -      /* Saturation */
 -      if (tmp_out>32767)
 -         tmp_out = 32767;
 -      else if (tmp_out<-32768)
 -         tmp_out = -32768;
 -      tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE)));
 +         /* Saturation */
 +         if (tmp_out>32767)
 +            tmp_out = 32767;
 +         else if (tmp_out<-32768)
 +            tmp_out = -32768;
 +         tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE[chan])));
-          /* This is an arbitrary test for saturation */
-          if (ref[i*C+chan] <= -32000 || ref[i*C+chan] >= 32000)
+       /* This is an arbitrary test for saturation in the microphone signal */
 -      if (in[i] <= -32000 || in[i] >= 32000)
 -      {
 -         tmp_out = 0;
++         if (in[i*C+chan] <= -32000 || in[i*C+chan] >= 32000)
 +         {
 +            tmp_out = 0;
+          if (st->saturated == 0)
              st->saturated = 1;
 +         }
 +         out[i*C+chan] = (spx_int16_t)tmp_out;
 +         st->memE[chan] = tmp_out;
        }
-       
 -      out[i] = (spx_int16_t)tmp_out;
 -      st->memE = tmp_out;
 -   }
 -   
++
+ #ifdef DUMP_ECHO_CANCEL_DATA
 -   dump_audio(in, far_end, out, st->frame_size);
++      dump_audio(in, far_end, out, st->frame_size);
+ #endif
+    
 -   /* Compute error signal (filter update version) */ 
 -   for (i=0;i<st->frame_size;i++)
 -   {
 -      st->e[i+st->frame_size] = st->e[i];
 -      st->e[i] = 0;
 +      /* Compute error signal (filter update version) */ 
 +      for (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;
-          st->e[chan*N+i+st->frame_size] = st->d[chan*N+i+st->frame_size] - st->y[chan*N+i+st->frame_size];
 +      }
 +      
 +      /* Compute a bunch of correlations */
-       See += mdf_inner_prod(st->e+chan*N+st->frame_size, st->e+chan*N+st->frame_size, st->frame_size);
++      /* FIXME: bad merge */
++      Sey += mdf_inner_prod(st->e+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size);
 +      Syy += mdf_inner_prod(st->y+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size);
-    
++      Sdd += mdf_inner_prod(st->input+chan*st->frame_size, st->input+chan*st->frame_size, st->frame_size);
++      
 +      /* Convert error to frequency domain */
 +      spx_fft(st->fft_table, st->e+chan*N, st->E+chan*N);
 +      for (i=0;i<st->frame_size;i++)
 +         st->y[i+chan*N] = 0;
 +      spx_fft(st->fft_table, st->y+chan*N, st->Y+chan*N);
 +   
 +      /* Compute power spectrum of echo (X), error (E) and filter response (Y) */
 +      power_spectrum_accum(st->E+chan*N, st->Rf, N);
 +      power_spectrum_accum(st->Y+chan*N, st->Yf, N);
++    
     }
-    See = ADD32(See, SHR32(EXTEND32(10000),6));
 -
 -   /* Compute a bunch of correlations */
 -   Sey = mdf_inner_prod(st->e+st->frame_size, st->y+st->frame_size, st->frame_size);
 -   Syy = mdf_inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);
 -   Sdd = mdf_inner_prod(st->input, st->input, st->frame_size);
     
 -      for (i=0;i<st->frame_size;i++)
+    /*printf ("%f %f %f %f\n", Sff, See, Syy, Sdd, st->update_cond);*/
+    
+    /* Do some sanity check */
+    if (!(Syy>=0 && Sxx>=0 && See >= 0)
+ #ifndef FIXED_POINT
+        || !(Sff < N*1e9 && Syy < N*1e9 && Sxx < N*1e9)
+ #endif
+       )
+    {
+       /* Things have gone really bad */
+       st->screwed_up += 50;
 -   /* Convert error to frequency domain */
 -   spx_fft(st->fft_table, st->e, st->E);
 -   for (i=0;i<st->frame_size;i++)
 -      st->y[i] = 0;
 -   spx_fft(st->fft_table, st->y, st->Y);
 -
 -   /* Compute power spectrum of far end (X), error (E) and filter response (Y) */
 -   power_spectrum(st->E, st->Rf, N);
 -   power_spectrum(st->Y, st->Yf, N);
 -   power_spectrum(st->X, st->Xf, N);
++      for (i=0;i<st->frame_size*C;i++)
+          out[i] = 0;
+    } else if (SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6)))
+    {
+       /* AEC seems to add lots of echo instead of removing it, let's see if it will improve */
+       st->screwed_up++;
+    } else {
+       /* Everything's fine */
+       st->screwed_up=0;
+    }
+    if (st->screwed_up>=50)
+    {
+       speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now.");
+       speex_echo_state_reset(st);
+       return;
+    }
+    /* Add a small noise floor to make sure not to have problems when dividing */
+    See = MAX32(See, SHR32(MULT16_16(N, 100),6));
++     
 +   for (speak = 0; speak < K; speak++)
 +   {
 +      Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size);
 +      power_spectrum_accum(st->X+speak*N, st->Xf, N);
 +   }
     
-    /* Smooth echo energy estimate over time */
+    /* Smooth far end energy estimate over time */
     for (j=0;j<=st->frame_size;j++)
        st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]);
 -   
 -   /* Enable this to compute the power based only on the tail (would need to compute more 
 -      efficiently to make this really useful */
 -   if (0)
 -   {
 -      float scale2 = .5f/M;
 -      for (j=0;j<=st->frame_size;j++)
 -         st->power[j] = 100;
 -      for (i=0;i<M;i++)
 -      {
 -         power_spectrum(&st->X[i*N], st->Xf, N);
 -         for (j=0;j<=st->frame_size;j++)
 -            st->power[j] += scale2*st->Xf[j];
 -      }
 -   }
  
     /* Compute filtered spectra and (cross-)correlations */
     for (j=st->frame_size;j>=0;j--)
        st->sum_adapt = ADD32(st->sum_adapt,adapt_rate);
     }
  
 -   /* Save residual echo so it can be used by the nonlinear processor */
 +   /* FIXME: MC conversion required */ 
-    /* Compute spectrum of estimated echo for use in an echo post-filter (if necessary)*/
-    if (Yout)
-    {
-       spx_word16_t leak2;
 +      for (i=0;i<st->frame_size;i++)
 +         st->last_y[i] = st->last_y[st->frame_size+i];
-       if (st->adapted)
-       {
-          /* If the filter is adapted, take the filtered echo */
-          for (i=0;i<st->frame_size;i++)
-             st->last_y[st->frame_size+i] = ref[i]-out[i];
-       } else {
-          /* If filter isn't adapted yet, all we can do is take the echo signal directly */
-          for (i=0;i<st->frame_size;i++)
-             st->last_y[st->frame_size+i] = echo[i];
-       }
-       
-       /* Apply hanning window (should pre-compute it)*/
-       for (i=0;i<N;i++)
-          st->y[i] = MULT16_16_Q15(st->window[i],st->last_y[i]);
+    if (st->adapted)
+    {
+       /* If the filter is adapted, take the filtered echo */
+       for (i=0;i<st->frame_size;i++)
 -         st->last_y[i] = st->last_y[st->frame_size+i];
 -      for (i=0;i<st->frame_size;i++)
+          st->last_y[st->frame_size+i] = in[i]-out[i];
+    } else {
+       /* If filter isn't adapted yet, all we can do is take the far end signal directly */
+       /* moved earlier: for (i=0;i<N;i++)
+       st->last_y[i] = st->x[i];*/
+    }
+ }
+ /* Compute spectrum of estimated echo for use in an echo post-filter */
+ void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *residual_echo, int len)
+ {
+    int i;
+    spx_word16_t leak2;
+    int N;
+    
+    N = st->window_size;
+    /* Apply hanning window (should pre-compute it)*/
+    for (i=0;i<N;i++)
+       st->y[i] = MULT16_16_Q15(st->window[i],st->last_y[i]);
        
-       /* Compute power spectrum of the echo */
-       spx_fft(st->fft_table, st->y, st->Y);
-       power_spectrum(st->Y, st->Yps, N);
+    /* Compute power spectrum of the echo */
+    spx_fft(st->fft_table, st->y, st->Y);
+    power_spectrum(st->Y, residual_echo, N);
        
  #ifdef FIXED_POINT
-       if (leak_estimate > 16383)
-          leak2 = 32767;
-       else
-          leak2 = SHL16(leak_estimate, 1);
+    if (st->leak_estimate > 16383)
+       leak2 = 32767;
+    else
+       leak2 = SHL16(st->leak_estimate, 1);
  #else
-       if (leak_estimate>.5)
-          leak2 = 1;
-       else
-          leak2 = 2*leak_estimate;
+    if (st->leak_estimate>.5)
+       leak2 = 1;
+    else
+       leak2 = 2*st->leak_estimate;
  #endif
-       /* Estimate residual echo */
-       for (i=0;i<=st->frame_size;i++)
-          Yout[i] = (spx_int32_t)MULT16_32_Q15(leak2,st->Yps[i]);
-    }
+    /* Estimate residual echo */
+    for (i=0;i<=st->frame_size;i++)
+       residual_echo[i] = (spx_int32_t)MULT16_32_Q15(leak2,residual_echo[i]);
+    
  }
  
 -int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr)
 +int mc_echo_ctl(SpeexEchoState *st, int request, void *ptr)
  {
     switch(request)
     {
@@@ -32,19 -31,20 +31,20 @@@ int main(int argc, char **argv
     ref_fd  = open (argv[1],  O_RDONLY);
     e_fd    = open (argv[3], O_WRONLY | O_CREAT | O_TRUNC, 0644);
  
 -   st = speex_echo_state_init(NN, TAIL);
 +   st = mc_echo_state_init(NN, TAIL, 1, 1);
     den = speex_preprocess_state_init(NN, 8000);
     int tmp = 8000;
 -   speex_echo_ctl(st, SPEEX_ECHO_SET_SAMPLING_RATE, &tmp);
 +   mc_echo_ctl(st, SPEEX_ECHO_SET_SAMPLING_RATE, &tmp);
+    speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_ECHO_STATE, st);
  
     while (read(ref_fd, ref_buf, NN*2))
     {
        read(echo_fd, echo_buf, NN*2);
-       mc_echo_cancel(st, ref_buf, echo_buf, e_buf, noise);
-       /*speex_preprocess(den, e_buf, noise);*/
 -      speex_echo_cancellation(st, ref_buf, echo_buf, e_buf);
++      mc_echo_cancellation(st, ref_buf, echo_buf, e_buf);
+       speex_preprocess_run(den, e_buf);
        write(e_fd, e_buf, NN*2);
     }
 -   speex_echo_state_destroy(st);
 +   mc_echo_state_destroy(st);
     speex_preprocess_state_destroy(den);
     close(e_fd);
     close(echo_fd);