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