Some references for the black magic
[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*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.4
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 < .05f)
510       beta=.05f;
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*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+ NOISE_OVERCOMPENS*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       /* A priori SNR update */
558       st->prior[i] = gamma*max(0.0f,st->post[i]) +
559             (1.f-gamma)* (.8*st->gain[i]*st->gain[i]*st->old_ps[i]/tot_noise + .2*st->prior[i]);
560       if (st->prior[i]>100.f)
561          st->prior[i]=100.f;
562       /*if (st->prior[i]<.01f)
563       st->prior[i]=.01f;*/
564    }
565
566    /*print_vec(st->prior, N+M, "prior");*/
567
568    /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
569    st->zeta[0] = .7f*st->zeta[0] + .3f*st->prior[0];
570    for (i=1;i<N-1;i++)
571       st->zeta[i] = .7f*st->zeta[i] + .3f*(.5f*st->prior[i]+.25f*st->prior[i+1]+.25f*st->prior[i-1]);
572    for (i=N-1;i<N+M;i++)
573       st->zeta[i] = .7f*st->zeta[i] + .3f*st->prior[i];
574
575    /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
576    Zframe = 0;
577    for (i=N;i<N+M;i++)
578       Zframe += st->zeta[i];
579    Zframe /= st->nbands;
580    Pframe = qcurve(Zframe);
581    if (Pframe < .2)
582       Pframe = .2;
583    
584    /*print_vec(&Pframe, 1, "");*/
585    
586    /* Compute gain according to the Ephraim-Malah algorithm */
587    for (i=0;i<N+M;i++)
588    {
589       float MM;
590       float theta;
591       float prior_ratio;
592       float p, q;
593       float P1;
594
595       prior_ratio = st->prior[i]/(1.0001f+st->prior[i]);
596       theta = (1.f+st->post[i])*prior_ratio;
597
598       P1 = qcurve (st->zeta[i]);
599       if (P1 < .2)
600          P1 = .2;
601       /* FIXME: add global prob (P2) */
602       q = 1-Pframe*P1;
603       /*q = 1-P1;*/
604       if (q>.95f)
605          q=.95f;
606       p=1.f/(1.f + (q/(1.f-q))*(1.f+st->prior[i])*exp(-theta));
607       /*p=1;*/
608
609       /* Optimal estimator for loudness domain */
610       MM = hypergeom_gain(theta);
611
612       st->gain[i] = prior_ratio * MM;
613       /*Put some (very arbitraty) limit on the gain*/
614       if (st->gain[i]>2.f)
615       {
616          st->gain[i]=2.f;
617       }
618       
619       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];
620       if (st->denoise_enabled)
621       {
622          /* Compute the gain floor based on different floors for the background noise and residual echo */
623          float gain_floor = (.03f*st->noise[i] + .003*st->echo_noise[i])/(1+st->noise[i] + st->echo_noise[i]);
624          gain_floor = sqrt(gain_floor);
625          
626          /* Take into account speech probability of presence (what's the best rule to use?) */
627          /*st->gain2[i] = p*p*st->gain[i];*/
628          st->gain2[i]=(p*sqrt(st->gain[i])+gain_floor*(1-p)) * (p*sqrt(st->gain[i])+gain_floor*(1-p));
629          /*st->gain2[i] = pow(st->gain[i], p) * pow(gain_floor,1.f-p);*/
630       } else {
631          st->gain2[i]=1.f;
632       }
633    }
634    
635    /* Only use the filterbank gains. This should be improved. */
636    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);
637    
638    if (st->agc_enabled)
639       speex_compute_agc(st);
640
641    /* Apply computed gain */
642    for (i=1;i<N;i++)
643    {
644       st->ft[2*i-1] *= st->gain2[i];
645       st->ft[2*i] *= st->gain2[i];
646    }
647
648    /* Get rid of the DC and very low frequencies */
649    st->ft[0]=0;
650    st->ft[1]=0;
651    st->ft[2]=0;
652    /* Nyquist frequency is mostly useless too */
653    st->ft[2*N-1]=0;
654
655    /* Inverse FFT with 1/N scaling */
656    spx_ifft_float(st->fft_lookup, st->ft, st->frame);
657
658    {
659       float max_sample=0;
660       for (i=0;i<2*N;i++)
661          if (fabs(st->frame[i])>max_sample)
662             max_sample = fabs(st->frame[i]);
663       if (max_sample>28000.f)
664       {
665          float damp = 28000.f/max_sample;
666          for (i=0;i<2*N;i++)
667             st->frame[i] *= damp;
668       }
669    }
670
671    for (i=0;i<2*N;i++)
672       st->frame[i] *= st->window[i];
673
674    /* Perform overlap and add */
675    for (i=0;i<N3;i++)
676       x[i] = st->outbuf[i] + st->frame[i];
677    for (i=0;i<N4;i++)
678       x[N3+i] = st->frame[N3+i];
679    
680    /* Update outbuf */
681    for (i=0;i<N3;i++)
682       st->outbuf[i] = st->frame[st->frame_size+i];
683
684    /* Save old power spectrum */
685    for (i=0;i<N+M;i++)
686       st->old_ps[i] = ps[i];
687
688    return 1;
689 }
690
691 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
692 {
693    int i;
694    int N = st->ps_size;
695    int N3 = 2*N - st->frame_size;
696    int M;
697    float *ps=st->ps;
698
699    M = st->nbands;
700    st->min_count++;
701    
702    preprocess_analysis(st, x);
703
704    update_noise_prob(st);
705    
706    for (i=1;i<N-1;i++)
707    {
708       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
709       {
710          if (echo)
711             st->noise[i] = .95f*st->noise[i] + .1f*max(1.0f,st->ps[i]-1.0*echo[i]);
712          else
713             st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
714       }
715    }
716
717    for (i=0;i<N3;i++)
718       st->outbuf[i] = x[st->frame_size-N3+i]*st->window[st->frame_size+i];
719
720    /* Save old power spectrum */
721    for (i=0;i<N+M;i++)
722       st->old_ps[i] = ps[i];
723
724    for (i=1;i<N;i++)
725       st->reverb_estimate[i] *= st->reverb_decay;
726 }
727
728
729 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
730 {
731    int i;
732    SpeexPreprocessState *st;
733    st=(SpeexPreprocessState*)state;
734    switch(request)
735    {
736    case SPEEX_PREPROCESS_SET_DENOISE:
737       st->denoise_enabled = (*(int*)ptr);
738       break;
739    case SPEEX_PREPROCESS_GET_DENOISE:
740       (*(int*)ptr) = st->denoise_enabled;
741       break;
742
743    case SPEEX_PREPROCESS_SET_AGC:
744       st->agc_enabled = (*(int*)ptr);
745       break;
746    case SPEEX_PREPROCESS_GET_AGC:
747       (*(int*)ptr) = st->agc_enabled;
748       break;
749
750    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
751       st->agc_level = (*(float*)ptr);
752       if (st->agc_level<1)
753          st->agc_level=1;
754       if (st->agc_level>32768)
755          st->agc_level=32768;
756       break;
757    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
758       (*(float*)ptr) = st->agc_level;
759       break;
760
761    case SPEEX_PREPROCESS_SET_VAD:
762       speex_warning("The VAD has been removed pending a complete rewrite");
763       st->vad_enabled = (*(int*)ptr);
764       break;
765    case SPEEX_PREPROCESS_GET_VAD:
766       (*(int*)ptr) = st->vad_enabled;
767       break;
768    
769    case SPEEX_PREPROCESS_SET_DEREVERB:
770       st->dereverb_enabled = (*(int*)ptr);
771       for (i=0;i<st->ps_size;i++)
772          st->reverb_estimate[i]=0;
773       break;
774    case SPEEX_PREPROCESS_GET_DEREVERB:
775       (*(int*)ptr) = st->dereverb_enabled;
776       break;
777
778    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
779       st->reverb_level = (*(float*)ptr);
780       break;
781    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
782       (*(float*)ptr) = st->reverb_level;
783       break;
784    
785    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
786       st->reverb_decay = (*(float*)ptr);
787       break;
788    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
789       (*(float*)ptr) = st->reverb_decay;
790       break;
791
792    case SPEEX_PREPROCESS_SET_PROB_START:
793       st->speech_prob_start = (*(int*)ptr) / 100.0;
794       if ( st->speech_prob_start > 1 || st->speech_prob_start < 0 )
795          st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
796       break;
797    case SPEEX_PREPROCESS_GET_PROB_START:
798       (*(int*)ptr) = st->speech_prob_start * 100;
799       break;
800
801    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
802       st->speech_prob_continue = (*(int*)ptr) / 100.0;
803       if ( st->speech_prob_continue > 1 || st->speech_prob_continue < 0 )
804          st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
805       break;
806    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
807       (*(int*)ptr) = st->speech_prob_continue * 100;
808       break;
809
810       default:
811       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
812       return -1;
813    }
814    return 0;
815 }