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