tuning and cleanup
[speexdsp.git] / libspeex / preprocess.c
1 /* Copyright (C) 2003 Epic Games (written by Jean-Marc Valin)
2    Copyright (C) 2004-2006 Epic Games 
3    
4    File: preprocess.c
5    Preprocessor with denoising based on the algorithm by Ephraim and Malah
6
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions are
9    met:
10
11    1. Redistributions of source code must retain the above copyright notice,
12    this list of conditions and the following disclaimer.
13
14    2. Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17
18    3. The name of the author may not be used to endorse or promote products
19    derived from this software without specific prior written permission.
20
21    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31    POSSIBILITY OF SUCH DAMAGE.
32 */
33
34
35 /*
36    Recommended papers:
37    
38    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
39    short-time spectral amplitude estimator". IEEE Transactions on Acoustics, 
40    Speech and Signal Processing, vol. ASSP-32, no. 6, pp. 1109-1121, 1984.
41    
42    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
43    log-spectral amplitude estimator". IEEE Transactions on Acoustics, Speech and 
44    Signal Processing, vol. ASSP-33, no. 2, pp. 443-445, 1985.
45    
46    I. Cohen and B. Berdugo, "Speech enhancement for non-stationary noise environments".
47    Signal Processing, vol. 81, no. 2, pp. 2403-2418, 2001.
48
49    Stefan Gustafsson, Rainer Martin, Peter Jax, and Peter Vary. "A psychoacoustic 
50    approach to combined acoustic echo cancellation and noise reduction". IEEE 
51    Transactions on Speech and Audio Processing, 2002.
52    
53    J.-M. Valin, J. Rouat, and F. Michaud, "Microphone array post-filter for separation
54    of simultaneous non-stationary sources". In Proceedings IEEE International 
55    Conference on Acoustics, Speech, and Signal Processing, 2004.
56 */
57
58 #ifdef HAVE_CONFIG_H
59 #include "config.h"
60 #endif
61
62 #include <math.h>
63 #include "speex/speex_preprocess.h"
64 #include "misc.h"
65 #include "fftwrap.h"
66 #include "filterbank.h"
67
68 #define max(a,b) ((a) > (b) ? (a) : (b))
69 #define min(a,b) ((a) < (b) ? (a) : (b))
70
71 #ifndef M_PI
72 #define M_PI 3.14159263
73 #endif
74
75 #define LOUDNESS_EXP 2.5
76
77 #define NB_BANDS 24
78
79 #define SPEEX_PROB_START_DEFAULT    0.35f
80 #define SPEEX_PROB_CONTINUE_DEFAULT 0.20f
81
82 /** Speex pre-processor state. */
83 struct SpeexPreprocessState_ {
84    int    frame_size;        /**< Number of samples processed each time */
85    int    ps_size;           /**< Number of points in the power spectrum */
86    int    sampling_rate;     /**< Sampling rate of the input/output */
87    
88    /* parameters */
89    int    denoise_enabled;
90    int    agc_enabled;
91    float  agc_level;
92    int    vad_enabled;
93    int    dereverb_enabled;
94    float  reverb_decay;
95    float  reverb_level;
96    float  speech_prob_start;
97    float  speech_prob_continue;
98    
99    int    nbands;
100    FilterBank *bank;
101    
102    float *frame;             /**< Processing frame (2*ps_size) */
103    float *ft;                /**< Processing frame in freq domain (2*ps_size) */
104    float *ps;                /**< Current power spectrum */
105    float *gain2;             /**< Adjusted gains */
106    float *window;            /**< Analysis/Synthesis window */
107    float *noise;             /**< Noise estimate */
108    float *reverb_estimate;   /**< Estimate of reverb energy */
109    float *old_ps;            /**< Power spectrum for last frame */
110    float *gain;              /**< Ephraim Malah gain */
111    float *prior;             /**< A-priori SNR */
112    float *post;              /**< A-posteriori SNR */
113
114    float *S;                 /**< Smoothed power spectrum */
115    float *Smin;              /**< See Cohen paper */
116    float *Stmp;              /**< See Cohen paper */
117    float *update_prob;       /**< Propability of speech presence for noise update */
118
119    float *zeta;              /**< Smoothed a priori SNR */
120
121    float *loudness_weight;   /**< Perceptual loudness curve */
122
123    float *echo_noise;
124
125    float *inbuf;             /**< Input buffer (overlapped analysis) */
126    float *outbuf;            /**< Output buffer (for overlap and add) */
127
128    float  loudness;          /**< loudness estimate */
129    float  loudness2;         /**< loudness estimate */
130    int    nb_adapt;          /**< Number of frames used for adaptation so far */
131    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
132    int    min_count;         /**< Number of frames processed so far */
133    void *fft_lookup;   /**< Lookup table for the FFT */
134
135 };
136
137
138 static void conj_window(float *w, int len)
139 {
140    int i;
141    for (i=0;i<len;i++)
142    {
143       float x=4*((float)i)/len;
144       int inv=0;
145       if (x<1)
146       {
147       } else if (x<2)
148       {
149          x=2-x;
150          inv=1;
151       } else if (x<3)
152       {
153          x=x-2;
154          inv=1;
155       } else {
156          x=4-x;
157       }
158       x*=1.9979;
159       w[i]=(.5-.5*cos(x))*(.5-.5*cos(x));
160       if (inv)
161          w[i]=1-w[i];
162       w[i]=sqrt(w[i]);
163    }
164 }
165
166 /* This function approximates the gain function 
167    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
168    which multiplied by xi/(1+xi) is the optimal gain
169    in the loudness domain ( sqrt[amplitude] )
170 */
171 static inline float hypergeom_gain(float x)
172 {
173    int ind;
174    float integer, frac;
175    static const float table[21] = {
176       0.82157f, 1.02017f, 1.20461f, 1.37534f, 1.53363f, 1.68092f, 1.81865f,
177       1.94811f, 2.07038f, 2.18638f, 2.29688f, 2.40255f, 2.50391f, 2.60144f,
178       2.69551f, 2.78647f, 2.87458f, 2.96015f, 3.04333f, 3.12431f, 3.20326f};
179       
180    integer = floor(2*x);
181    ind = (int)integer;
182    if (ind<0)
183       return 1;
184    if (ind>19)
185       return 1+.1296/x;
186    frac = 2*x-integer;
187    return ((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001f);
188 }
189
190 static inline float qcurve(float x)
191 {
192    return 1.f/(1.f+.1f/(x));
193 }
194
195 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
196 {
197    int i;
198    int N, N3, N4, M;
199
200    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
201    st->frame_size = frame_size;
202
203    /* Round ps_size down to the nearest power of two */
204 #if 0
205    i=1;
206    st->ps_size = st->frame_size;
207    while(1)
208    {
209       if (st->ps_size & ~i)
210       {
211          st->ps_size &= ~i;
212          i<<=1;
213       } else {
214          break;
215       }
216    }
217    
218    
219    if (st->ps_size < 3*st->frame_size/4)
220       st->ps_size = st->ps_size * 3 / 2;
221 #else
222    st->ps_size = st->frame_size;
223 #endif
224
225    N = st->ps_size;
226    N3 = 2*N - st->frame_size;
227    N4 = st->frame_size - N3;
228    
229    st->sampling_rate = sampling_rate;
230    st->denoise_enabled = 1;
231    st->agc_enabled = 0;
232    st->agc_level = 8000;
233    st->vad_enabled = 0;
234    st->dereverb_enabled = 0;
235    st->reverb_decay = .0;
236    st->reverb_level = .0;
237
238    st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
239    st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
240
241    st->nbands = NB_BANDS;
242    M = st->nbands;
243    st->bank = filterbank_new(M, 4000, 8000, N, 1);
244          
245    st->frame = (float*)speex_alloc(2*N*sizeof(float));
246    st->window = (float*)speex_alloc(2*N*sizeof(float));
247    st->ft = (float*)speex_alloc(2*N*sizeof(float));
248    
249    st->ps = (float*)speex_alloc((N+M)*sizeof(float));
250    st->noise = (float*)speex_alloc((N+M)*sizeof(float));
251    st->echo_noise = (float*)speex_alloc((N+M)*sizeof(float));
252    st->reverb_estimate = (float*)speex_alloc((N+M)*sizeof(float));
253    st->old_ps = (float*)speex_alloc((N+M)*sizeof(float));
254    st->prior = (float*)speex_alloc((N+M)*sizeof(float));
255    st->post = (float*)speex_alloc((N+M)*sizeof(float));
256    st->gain = (float*)speex_alloc((N+M)*sizeof(float));
257    st->gain2 = (float*)speex_alloc((N+M)*sizeof(float));
258    st->zeta = (float*)speex_alloc((N+M)*sizeof(float));
259    
260    st->S = (float*)speex_alloc(N*sizeof(float));
261    st->Smin = (float*)speex_alloc(N*sizeof(float));
262    st->Stmp = (float*)speex_alloc(N*sizeof(float));
263    st->update_prob = (float*)speex_alloc(N*sizeof(float));
264    
265    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
266    st->inbuf = (float*)speex_alloc(N3*sizeof(float));
267    st->outbuf = (float*)speex_alloc(N3*sizeof(float));
268
269    conj_window(st->window, 2*N3);
270    for (i=2*N3;i<2*st->ps_size;i++)
271       st->window[i]=1;
272    
273    if (N4>0)
274    {
275       for (i=N3-1;i>=0;i--)
276       {
277          st->window[i+N3+N4]=st->window[i+N3];
278          st->window[i+N3]=1;
279       }
280    }
281    for (i=0;i<N+M;i++)
282    {
283       st->noise[i]=1.;
284       st->reverb_estimate[i]=0.;
285       st->old_ps[i]=1.;
286       st->gain[i]=1;
287       st->post[i]=1;
288       st->prior[i]=1;
289    }
290
291    for (i=0;i<N;i++)
292       st->update_prob[i] = 1;
293    for (i=0;i<N3;i++)
294    {
295       st->inbuf[i]=0;
296       st->outbuf[i]=0;
297    }
298
299    for (i=0;i<N;i++)
300    {
301       float ff=((float)i)*.5*sampling_rate/((float)N);
302       st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
303       if (st->loudness_weight[i]<.01f)
304          st->loudness_weight[i]=.01f;
305       st->loudness_weight[i] *= st->loudness_weight[i];
306    }
307
308    st->loudness = pow(6000,LOUDNESS_EXP);
309    st->loudness2 = 6000;
310    st->nb_loudness_adapt = 0;
311
312    st->fft_lookup = spx_fft_init(2*N);
313
314    st->nb_adapt=0;
315    st->min_count=0;
316    return st;
317 }
318
319 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
320 {
321    speex_free(st->frame);
322    speex_free(st->ft);
323    speex_free(st->ps);
324    speex_free(st->gain2);
325    speex_free(st->window);
326    speex_free(st->noise);
327    speex_free(st->reverb_estimate);
328    speex_free(st->old_ps);
329    speex_free(st->gain);
330    speex_free(st->prior);
331    speex_free(st->post);
332    speex_free(st->loudness_weight);
333    speex_free(st->echo_noise);
334
335    speex_free(st->S);
336    speex_free(st->Smin);
337    speex_free(st->Stmp);
338    speex_free(st->update_prob);
339    speex_free(st->zeta);
340
341    speex_free(st->inbuf);
342    speex_free(st->outbuf);
343
344    spx_fft_destroy(st->fft_lookup);
345    filterbank_destroy(st->bank);
346    speex_free(st);
347 }
348
349 static void speex_compute_agc(SpeexPreprocessState *st)
350 {
351    int i;
352    int N = st->ps_size;
353    float scale=.5f/N;
354    float agc_gain;
355    int freq_start, freq_end;
356    float active_bands = 0;
357
358    freq_start = (int)(300.0f*2*N/st->sampling_rate);
359    freq_end   = (int)(2000.0f*2*N/st->sampling_rate);
360    for (i=freq_start;i<freq_end;i++)
361    {
362       if (st->S[i] > 20.f*st->Smin[i]+1000.f)
363          active_bands+=1;
364    }
365    active_bands /= (freq_end-freq_start+1);
366
367    if (active_bands > .2f)
368    {
369       float loudness=0.f;
370       float rate, rate2=.2f;
371       st->nb_loudness_adapt++;
372       rate=2.0f/(1+st->nb_loudness_adapt);
373       if (rate < .05f)
374          rate = .05f;
375       if (rate < .1f && pow(loudness, LOUDNESS_EXP) > st->loudness)
376          rate = .1f;
377       if (rate < .2f && pow(loudness, LOUDNESS_EXP) > 3.f*st->loudness)
378          rate = .2f;
379       if (rate < .4f && pow(loudness, LOUDNESS_EXP) > 10.f*st->loudness)
380          rate = .4f;
381
382       for (i=2;i<N;i++)
383       {
384          loudness += scale*st->ps[i] * st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
385       }
386       loudness=sqrt(loudness);
387       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
388         loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
389       st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
390       
391       st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0f/LOUDNESS_EXP);
392
393       loudness = pow(st->loudness, 1.0f/LOUDNESS_EXP);
394
395       /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
396    }
397    
398    agc_gain = st->agc_level/st->loudness2;
399    /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
400    if (agc_gain>200)
401       agc_gain = 200;
402
403    for (i=0;i<N;i++)
404       st->gain2[i] *= agc_gain;
405    
406 }
407
408 static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
409 {
410    int i;
411    int N = st->ps_size;
412    int N3 = 2*N - st->frame_size;
413    int N4 = st->frame_size - N3;
414    float *ps=st->ps;
415
416    /* 'Build' input frame */
417    for (i=0;i<N3;i++)
418       st->frame[i]=st->inbuf[i];
419    for (i=0;i<st->frame_size;i++)
420       st->frame[N3+i]=x[i];
421    
422    /* Update inbuf */
423    for (i=0;i<N3;i++)
424       st->inbuf[i]=x[N4+i];
425
426    /* Windowing */
427    for (i=0;i<2*N;i++)
428       st->frame[i] *= st->window[i];
429
430    /* Perform FFT */
431    spx_fft_float(st->fft_lookup, st->frame, st->ft);
432          
433    /* Power spectrum */
434    ps[0]=1;
435    for (i=1;i<N;i++)
436       ps[i]=1+st->ft[2*i-1]*st->ft[2*i-1] + st->ft[2*i]*st->ft[2*i];
437
438    filterbank_compute_bank(st->bank, ps, ps+N);
439 }
440
441 static void update_noise_prob(SpeexPreprocessState *st)
442 {
443    int i;
444    int min_range;
445    int N = st->ps_size;
446
447    for (i=1;i<N-1;i++)
448       st->S[i] = 100.f+ .8f*st->S[i] + .05f*st->ps[i-1]+.1f*st->ps[i]+.05f*st->ps[i+1];
449    st->S[0] = 100.f+ .8f*st->S[0] + .2*st->ps[0];
450    st->S[N-1] = 100.f+ .8f*st->S[N-1] + .2*st->ps[N-1];
451    
452    if (st->nb_adapt==1)
453    {
454       for (i=0;i<N;i++)
455          st->Smin[i] = st->Stmp[i] = 0.f;
456    }
457
458    if (st->nb_adapt < 100)
459       min_range = 15;
460    else if (st->nb_adapt < 1000)
461       min_range = 50;
462    else if (st->nb_adapt < 10000)
463       min_range = 150;
464    else
465       min_range = 300;
466    if (st->min_count > min_range)
467    {
468       st->min_count = 0;
469       for (i=0;i<N;i++)
470       {
471          st->Smin[i] = min(st->Stmp[i], st->S[i]);
472          st->Stmp[i] = st->S[i];
473       }
474    } else {
475       for (i=0;i<N;i++)
476       {
477          st->Smin[i] = min(st->Smin[i], st->S[i]);
478          st->Stmp[i] = min(st->Stmp[i], st->S[i]);      
479       }
480    }
481    for (i=0;i<N;i++)
482    {
483       st->update_prob[i] *= .2f;
484       if (st->S[i] > 2.5*st->Smin[i])
485          st->update_prob[i] += .8f;
486       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
487       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
488    }
489
490 }
491
492 #define NOISE_OVERCOMPENS 1.
493
494 int speex_preprocess(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
495 {
496    int i;
497    int M;
498    int N = st->ps_size;
499    int N3 = 2*N - st->frame_size;
500    int N4 = st->frame_size - N3;
501    float *ps=st->ps;
502    float Zframe=0, Pframe;
503    float beta, beta_1;
504    
505    st->nb_adapt++;
506    st->min_count++;
507    
508    beta =1.0f/st->nb_adapt;
509    if (beta < .03f)
510       beta=.03f;
511    beta_1 = 1.0f-beta;
512    M = st->nbands;
513    /* Deal with residual echo if provided */
514    if (echo)
515    {
516       for (i=0;i<N;i++)
517          st->echo_noise[i] = MAX32(.6f*st->echo_noise[i], echo[i]);
518       filterbank_compute_bank(st->bank, st->echo_noise, st->echo_noise+N);
519    } else {
520       for (i=0;i<N+M;i++)
521          st->echo_noise[i] = 0;
522    }
523    preprocess_analysis(st, x);
524
525    update_noise_prob(st);
526
527    /* Noise estimation always updated for the 10 first frames */
528    /*if (st->nb_adapt<10)
529    {
530       for (i=1;i<N-1;i++)
531          st->update_prob[i] = 0;
532    }
533    */
534    
535    /* Update the noise estimate for the frequencies where it can be */
536    for (i=0;i<N;i++)
537    {
538       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
539          st->noise[i] = beta_1*st->noise[i] + beta*NOISE_OVERCOMPENS*st->ps[i];
540    }
541    filterbank_compute_bank(st->bank, st->noise, st->noise+N);
542
543    /* Special case for first frame */
544    if (st->nb_adapt==1)
545       for (i=0;i<N+M;i++)
546          st->old_ps[i] = ps[i];
547
548    /* Compute a posteriori SNR */
549    for (i=0;i<N+M;i++)
550    {
551       float gamma = .1;
552       float tot_noise = 1.f+ st->noise[i] + st->echo_noise[i] + st->reverb_estimate[i];
553       st->post[i] = ps[i]/tot_noise - 1.f;
554       if (st->post[i]>100.f)
555          st->post[i]=100.f;
556       /*gamma = .15+.85*st->prior[i]*st->prior[i]/((1+st->prior[i])*(1+st->prior[i]));*/
557       gamma = .1+.9*(st->old_ps[i]/(1+st->old_ps[i]+tot_noise))*(st->old_ps[i]/(1+st->old_ps[i]+tot_noise));
558       /* A priori SNR update */
559       st->prior[i] = gamma*max(0.0f,st->post[i]) + (1.f-gamma)*st->old_ps[i]/tot_noise;
560       if (st->prior[i]>100.f)
561          st->prior[i]=100.f;
562    }
563
564    /*print_vec(st->post, N+M, "");*/
565
566    /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
567    st->zeta[0] = .7f*st->zeta[0] + .3f*st->prior[0];
568    for (i=1;i<N-1;i++)
569       st->zeta[i] = .7f*st->zeta[i] + .3f*(.5f*st->prior[i]+.25f*st->prior[i+1]+.25f*st->prior[i-1]);
570    for (i=N-1;i<N+M;i++)
571       st->zeta[i] = .7f*st->zeta[i] + .3f*st->prior[i];
572
573    /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
574    Zframe = 0;
575    for (i=N;i<N+M;i++)
576       Zframe += st->zeta[i];
577    Zframe /= st->nbands;
578    Pframe = .1+.9*qcurve(Zframe);
579    
580    /*print_vec(&Pframe, 1, "");*/
581    /*for (i=N;i<N+M;i++)
582       st->gain2[i] = qcurve (st->zeta[i]);
583    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);*/
584    
585    for (i=N;i<N+M;i++)
586    {
587       float theta, MM;
588       float prior_ratio;
589       float q;
590       float P1;
591
592       prior_ratio = st->prior[i]/(1.0001f+st->prior[i]);
593       theta = (1.f+st->post[i])*prior_ratio;
594
595       MM = hypergeom_gain(theta);
596       st->gain[i] = prior_ratio * MM;
597
598       P1 = .1+.9*qcurve (st->zeta[i]);
599       q = 1-Pframe*P1;
600       st->gain2[i]=1.f/(1.f + (q/(1.f-q))*(1.f+st->prior[i])*exp(-theta));
601    }
602    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);
603    filterbank_compute_psd(st->bank,st->gain+N, st->gain);
604    
605    /* Compute gain according to the Ephraim-Malah algorithm */
606    for (i=0;i<N+M;i++)
607    {
608       float MM;
609       float theta;
610       float prior_ratio;
611       float p;
612       float g;
613       prior_ratio = st->prior[i]/(1.0001f+st->prior[i]);
614       theta = (1.f+st->post[i])*prior_ratio;
615       p = st->gain2[i];
616
617       /* Optimal estimator for loudness domain */
618       MM = hypergeom_gain(theta);
619
620       g = prior_ratio * MM;
621       if (g > 1.5*st->gain[i])
622          g = 1.5*st->gain[i];
623       if (g < .66*st->gain[i])
624          g = .66*st->gain[i];
625       st->gain[i] = g;
626       /* Limit on the gain */
627       if (st->gain[i]>1.f)
628          st->gain[i]=1.f;
629       
630       st->reverb_estimate[i] = st->reverb_decay*st->reverb_estimate[i] + st->reverb_decay*st->reverb_level*st->gain[i]*st->gain[i]*st->ps[i];
631       if (st->denoise_enabled)
632       {
633          /* Compute the gain floor based on different floors for the background noise and residual echo */
634          float gain_floor = (.003f*st->noise[i] + .00003*st->echo_noise[i])/(1+st->noise[i] + st->echo_noise[i]);
635          gain_floor = sqrt(gain_floor);
636
637          /* Take into account speech probability of presence (what's the best rule to use?) */
638          if (st->gain[i] < gain_floor)
639             st->gain[i] = gain_floor;
640          st->gain2[i]=(p*sqrt(st->gain[i])+sqrt(gain_floor)*(1-p)) * (p*sqrt(st->gain[i])+sqrt(gain_floor)*(1-p));
641          /*st->gain2[i] = pow(st->gain[i], p) * pow(gain_floor,1.f-p);*/
642       } else {
643          st->gain2[i]=1.f;
644       }
645    }
646    
647    /* Enable this to only use the filterbank gains */
648 #if 0
649    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);
650 #endif
651
652    if (st->agc_enabled)
653       speex_compute_agc(st);
654
655    /* Apply computed gain */
656    for (i=1;i<N;i++)
657    {
658       st->ft[2*i-1] *= st->gain2[i];
659       st->ft[2*i] *= st->gain2[i];
660    }
661    st->ft[0] *= st->gain2[0];
662    st->ft[2*N-1] *= st->gain2[N-1];
663
664    /* Inverse FFT with 1/N scaling */
665    spx_ifft_float(st->fft_lookup, st->ft, st->frame);
666
667    {
668       float max_sample=0;
669       for (i=0;i<2*N;i++)
670          if (fabs(st->frame[i])>max_sample)
671             max_sample = fabs(st->frame[i]);
672       if (max_sample>28000.f)
673       {
674          float damp = 28000.f/max_sample;
675          for (i=0;i<2*N;i++)
676             st->frame[i] *= damp;
677       }
678    }
679
680    for (i=0;i<2*N;i++)
681       st->frame[i] *= st->window[i];
682
683    /* Perform overlap and add */
684    for (i=0;i<N3;i++)
685       x[i] = st->outbuf[i] + st->frame[i];
686    for (i=0;i<N4;i++)
687       x[N3+i] = st->frame[N3+i];
688    
689    /* Update outbuf */
690    for (i=0;i<N3;i++)
691       st->outbuf[i] = st->frame[st->frame_size+i];
692
693    /* Save old power spectrum */
694    for (i=0;i<N+M;i++)
695       st->old_ps[i] = .2*st->old_ps[i] + .8*st->gain[i]*st->gain[i]*ps[i];
696
697    return 1;
698 }
699
700 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
701 {
702    int i;
703    int N = st->ps_size;
704    int N3 = 2*N - st->frame_size;
705    int M;
706    float *ps=st->ps;
707
708    M = st->nbands;
709    st->min_count++;
710    
711    preprocess_analysis(st, x);
712
713    update_noise_prob(st);
714    
715    for (i=1;i<N-1;i++)
716    {
717       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
718       {
719          if (echo)
720             st->noise[i] = .95f*st->noise[i] + .1f*max(1.0f,st->ps[i]-1.0*echo[i]);
721          else
722             st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
723       }
724    }
725
726    for (i=0;i<N3;i++)
727       st->outbuf[i] = x[st->frame_size-N3+i]*st->window[st->frame_size+i];
728
729    /* Save old power spectrum */
730    for (i=0;i<N+M;i++)
731       st->old_ps[i] = ps[i];
732
733    for (i=1;i<N;i++)
734       st->reverb_estimate[i] *= st->reverb_decay;
735 }
736
737
738 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
739 {
740    int i;
741    SpeexPreprocessState *st;
742    st=(SpeexPreprocessState*)state;
743    switch(request)
744    {
745    case SPEEX_PREPROCESS_SET_DENOISE:
746       st->denoise_enabled = (*(int*)ptr);
747       break;
748    case SPEEX_PREPROCESS_GET_DENOISE:
749       (*(int*)ptr) = st->denoise_enabled;
750       break;
751
752    case SPEEX_PREPROCESS_SET_AGC:
753       st->agc_enabled = (*(int*)ptr);
754       break;
755    case SPEEX_PREPROCESS_GET_AGC:
756       (*(int*)ptr) = st->agc_enabled;
757       break;
758
759    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
760       st->agc_level = (*(float*)ptr);
761       if (st->agc_level<1)
762          st->agc_level=1;
763       if (st->agc_level>32768)
764          st->agc_level=32768;
765       break;
766    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
767       (*(float*)ptr) = st->agc_level;
768       break;
769
770    case SPEEX_PREPROCESS_SET_VAD:
771       speex_warning("The VAD has been removed pending a complete rewrite");
772       st->vad_enabled = (*(int*)ptr);
773       break;
774    case SPEEX_PREPROCESS_GET_VAD:
775       (*(int*)ptr) = st->vad_enabled;
776       break;
777    
778    case SPEEX_PREPROCESS_SET_DEREVERB:
779       st->dereverb_enabled = (*(int*)ptr);
780       for (i=0;i<st->ps_size;i++)
781          st->reverb_estimate[i]=0;
782       break;
783    case SPEEX_PREPROCESS_GET_DEREVERB:
784       (*(int*)ptr) = st->dereverb_enabled;
785       break;
786
787    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
788       st->reverb_level = (*(float*)ptr);
789       break;
790    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
791       (*(float*)ptr) = st->reverb_level;
792       break;
793    
794    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
795       st->reverb_decay = (*(float*)ptr);
796       break;
797    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
798       (*(float*)ptr) = st->reverb_decay;
799       break;
800
801    case SPEEX_PREPROCESS_SET_PROB_START:
802       st->speech_prob_start = (*(int*)ptr) / 100.0;
803       if ( st->speech_prob_start > 1 || st->speech_prob_start < 0 )
804          st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
805       break;
806    case SPEEX_PREPROCESS_GET_PROB_START:
807       (*(int*)ptr) = st->speech_prob_start * 100;
808       break;
809
810    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
811       st->speech_prob_continue = (*(int*)ptr) / 100.0;
812       if ( st->speech_prob_continue > 1 || st->speech_prob_continue < 0 )
813          st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
814       break;
815    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
816       (*(int*)ptr) = st->speech_prob_continue * 100;
817       break;
818
819       default:
820       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
821       return -1;
822    }
823    return 0;
824 }