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