some AGC tuning
[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 #include <math.h>
35 #include "speex_preprocess.h"
36 #include <stdio.h>
37 #include "misc.h"
38 #include "smallft.h"
39
40 #define max(a,b) ((a) > (b) ? (a) : (b))
41 #define min(a,b) ((a) < (b) ? (a) : (b))
42
43 #ifndef M_PI
44 #define M_PI 3.14159263
45 #endif
46
47 #define SQRT_M_PI_2 0.88623
48 #define LOUDNESS_EXP 2.5
49
50 #define NB_BANDS 8
51
52 static void conj_window(float *w, int len)
53 {
54    int i;
55    for (i=0;i<len;i++)
56    {
57       float x=4*((float)i)/len;
58       int inv=0;
59       if (x<1)
60       {
61       } else if (x<2)
62       {
63          x=2-x;
64          inv=1;
65       } else if (x<3)
66       {
67          x=x-2;
68          inv=1;
69       } else {
70          x=4-x;
71       }
72       x*=1.9979;
73       w[i]=(.5-.5*cos(x))*(.5-.5*cos(x));
74       if (inv)
75          w[i]=1-w[i];
76       w[i]=sqrt(w[i]);
77    }
78 }
79
80 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
81 {
82    int i;
83    int N, N3, N4;
84
85    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
86    st->frame_size = frame_size;
87
88    /* Round ps_size down to the nearest power of two */
89 #if 0
90    i=1;
91    st->ps_size = st->frame_size;
92    while(1)
93    {
94       if (st->ps_size & ~i)
95       {
96          st->ps_size &= ~i;
97          i<<=1;
98       } else {
99          break;
100       }
101    }
102    
103    
104    if (st->ps_size < 3*st->frame_size/4)
105       st->ps_size = st->ps_size * 3 / 2;
106 #else
107    st->ps_size = st->frame_size;
108 #endif
109
110    N = st->ps_size;
111    N3 = 2*N - st->frame_size;
112    N4 = st->frame_size - N3;
113    
114    st->sampling_rate = sampling_rate;
115    st->denoise_enabled = 1;
116    st->agc_enabled = 0;
117    st->agc_level = 8000;
118    st->vad_enabled = 0;
119
120    st->frame = (float*)speex_alloc(2*N*sizeof(float));
121    st->ps = (float*)speex_alloc(N*sizeof(float));
122    st->gain2 = (float*)speex_alloc(N*sizeof(float));
123    st->window = (float*)speex_alloc(2*N*sizeof(float));
124    st->noise = (float*)speex_alloc(N*sizeof(float));
125    st->old_ps = (float*)speex_alloc(N*sizeof(float));
126    st->gain = (float*)speex_alloc(N*sizeof(float));
127    st->prior = (float*)speex_alloc(N*sizeof(float));
128    st->post = (float*)speex_alloc(N*sizeof(float));
129    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
130    st->inbuf = (float*)speex_alloc(N3*sizeof(float));
131    st->outbuf = (float*)speex_alloc(N3*sizeof(float));
132    st->echo_noise = (float*)speex_alloc(N*sizeof(float));
133
134    st->S = (float*)speex_alloc(N*sizeof(float));
135    st->Smin = (float*)speex_alloc(N*sizeof(float));
136    st->Stmp = (float*)speex_alloc(N*sizeof(float));
137    st->update_prob = (float*)speex_alloc(N*sizeof(float));
138
139    st->noise_bands = (float*)speex_alloc(NB_BANDS*sizeof(float));
140    st->noise_bands2 = (float*)speex_alloc(NB_BANDS*sizeof(float));
141    st->speech_bands = (float*)speex_alloc(NB_BANDS*sizeof(float));
142    st->speech_bands2 = (float*)speex_alloc(NB_BANDS*sizeof(float));
143    st->noise_bandsN = st->speech_bandsN = 1;
144
145    conj_window(st->window, 2*N3);
146    for (i=2*N3;i<2*st->ps_size;i++)
147       st->window[i]=1;
148    
149    if (N4>0)
150    {
151       for (i=N3-1;i>=0;i--)
152       {
153          st->window[i+N3+N4]=st->window[i+N3];
154          st->window[i+N3]=1;
155       }
156    }
157    for (i=0;i<N;i++)
158    {
159       st->noise[i]=1e4;
160       st->old_ps[i]=1e4;
161       st->gain[i]=1;
162       st->post[i]=1;
163       st->prior[i]=1;
164    }
165
166    for (i=0;i<N3;i++)
167    {
168       st->inbuf[i]=0;
169       st->outbuf[i]=0;
170    }
171
172    for (i=0;i<N;i++)
173    {
174       float ff=((float)i)*.5*sampling_rate/((float)N);
175       st->loudness_weight[i] = .35-.35*ff/16000+.73*exp(-.5*(ff-3800)*(ff-3800)/9e5);
176       if (st->loudness_weight[i]<.01)
177          st->loudness_weight[i]=.01;
178       st->loudness_weight[i] *= st->loudness_weight[i];
179    }
180
181    st->speech_prob = 0;
182    st->last_speech = 1000;
183    st->loudness = pow(6000,LOUDNESS_EXP);
184    st->loudness2 = 6000;
185    st->nb_loudness_adapt = 0;
186
187    st->fft_lookup = speex_alloc(sizeof(struct drft_lookup));
188    drft_init(st->fft_lookup,2*N);
189
190    st->nb_adapt=0;
191    st->consec_noise=0;
192    st->nb_preprocess=0;
193    st->nb_min_estimate=0;
194    st->last_update=0;
195    return st;
196 }
197
198 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
199 {
200    speex_free(st->frame);
201    speex_free(st->ps);
202    speex_free(st->gain2);
203    speex_free(st->window);
204    speex_free(st->noise);
205    speex_free(st->old_ps);
206    speex_free(st->gain);
207    speex_free(st->prior);
208    speex_free(st->post);
209    speex_free(st->loudness_weight);
210    speex_free(st->echo_noise);
211
212    speex_free(st->S);
213    speex_free(st->Smin);
214    speex_free(st->Stmp);
215    speex_free(st->update_prob);
216
217    speex_free(st->noise_bands);
218    speex_free(st->noise_bands2);
219    speex_free(st->speech_bands);
220    speex_free(st->speech_bands2);
221
222    speex_free(st->inbuf);
223    speex_free(st->outbuf);
224
225    drft_clear(st->fft_lookup);
226    speex_free(st->fft_lookup);
227
228    speex_free(st);
229 }
230
231 static void update_noise(SpeexPreprocessState *st, float *ps, float *echo)
232 {
233    int i;
234    float beta;
235    st->nb_adapt++;
236    beta=1.0/st->nb_adapt;
237    if (beta < .05)
238       beta=.05;
239    
240    if (!echo)
241    {
242       for (i=0;i<st->ps_size;i++)
243          st->noise[i] = (1-beta)*st->noise[i] + beta*ps[i];   
244    } else {
245       for (i=0;i<st->ps_size;i++)
246          st->noise[i] = (1-beta)*st->noise[i] + beta*max(0,ps[i]-echo[i]); 
247 #if 0
248       for (i=0;i<st->ps_size;i++)
249          st->noise[i] = 0;
250 #endif
251    }
252 }
253
254 static int speex_compute_vad(SpeexPreprocessState *st, float *ps, float mean_prior, float mean_post)
255 {
256    int i, is_speech=0;
257    int N = st->ps_size;
258    float scale=.5/N;
259
260    /* FIXME: Clean this up a bit */
261    {
262       float bands[NB_BANDS];
263       int j;
264       float p0, p1;
265       float tot_loudness=0;
266       float x = sqrt(mean_post);
267
268       for (i=5;i<N-10;i++)
269       {
270          tot_loudness += scale*st->ps[i] * st->loudness_weight[i];
271       }
272
273       for (i=0;i<NB_BANDS;i++)
274       {
275          bands[i]=1e4;
276          for (j=i*N/NB_BANDS;j<(i+1)*N/NB_BANDS;j++)
277          {
278             bands[i] += ps[j];
279          }
280          bands[i]=log(bands[i]);
281       }
282       
283       /*p1 = .0005+.6*exp(-.5*(x-.4)*(x-.4)*11)+.1*exp(-1.2*x);
284       if (x<1.5)
285          p0=.1*exp(2*(x-1.5));
286       else
287          p0=.02+.1*exp(-.2*(x-1.5));
288       */
289
290       p0=1/(1+exp(3*(1.5-x)));
291       p1=1-p0;
292
293       /*fprintf (stderr, "%f %f ", p0, p1);*/
294       /*p0 *= .99*st->speech_prob + .01*(1-st->speech_prob);
295       p1 *= .01*st->speech_prob + .99*(1-st->speech_prob);
296       
297       st->speech_prob = p0/(p1+p0);
298       */
299
300       if (st->noise_bandsN < 50 || st->speech_bandsN < 50)
301       {
302          if (mean_post > 5)
303          {
304             float adapt = 1./st->speech_bandsN++;
305             if (adapt<.005)
306                adapt = .005;
307             for (i=0;i<NB_BANDS;i++)
308             {
309                st->speech_bands[i] = (1-adapt)*st->speech_bands[i] + adapt*bands[i];
310                /*st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*bands[i]*bands[i];*/
311                st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*(bands[i]-st->speech_bands[i])*(bands[i]-st->speech_bands[i]);
312             }
313          } else {
314             float adapt = 1./st->noise_bandsN++;
315             if (adapt<.005)
316                adapt = .005;
317             for (i=0;i<NB_BANDS;i++)
318             {
319                st->noise_bands[i] = (1-adapt)*st->noise_bands[i] + adapt*bands[i];
320                /*st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*bands[i]*bands[i];*/
321                st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*(bands[i]-st->noise_bands[i])*(bands[i]-st->noise_bands[i]);
322             }
323          }
324       }
325       p0=p1=1;
326       for (i=0;i<NB_BANDS;i++)
327       {
328          float noise_var, speech_var;
329          float noise_mean, speech_mean;
330          float tmp1, tmp2, pr;
331
332          /*noise_var = 1.01*st->noise_bands2[i] - st->noise_bands[i]*st->noise_bands[i];
333            speech_var = 1.01*st->speech_bands2[i] - st->speech_bands[i]*st->speech_bands[i];*/
334          noise_var = st->noise_bands2[i];
335          speech_var = st->speech_bands2[i];
336          if (noise_var < .1)
337             noise_var = .1;
338          if (speech_var < .1)
339             speech_var = .1;
340          
341          /*speech_var = sqrt(speech_var*noise_var);
342            noise_var = speech_var;*/
343          if (speech_var < .05*speech_var)
344             noise_var = .05*speech_var; 
345          if (speech_var < .05*noise_var)
346             speech_var = .05*noise_var;
347          
348          if (bands[i] < st->noise_bands[i])
349             speech_var = noise_var;
350          if (bands[i] > st->speech_bands[i])
351             noise_var = speech_var;
352
353          speech_mean = st->speech_bands[i];
354          noise_mean = st->noise_bands[i];
355          if (noise_mean < speech_mean - 5)
356             noise_mean = speech_mean - 5;
357
358          tmp1 = exp(-.5*(bands[i]-speech_mean)*(bands[i]-speech_mean)/speech_var)/sqrt(2*M_PI*speech_var);
359          tmp2 = exp(-.5*(bands[i]-noise_mean)*(bands[i]-noise_mean)/noise_var)/sqrt(2*M_PI*noise_var);
360          /*fprintf (stderr, "%f ", (float)(p0/(.01+p0+p1)));*/
361          /*fprintf (stderr, "%f ", (float)(bands[i]));*/
362          pr = tmp1/(1e-25+tmp1+tmp2);
363          /*if (bands[i] < st->noise_bands[i])
364             pr=.01;
365          if (bands[i] > st->speech_bands[i] && pr < .995)
366          pr=.995;*/
367          if (pr>.999)
368             pr=.999;
369          if (pr<.001)
370             pr=.001;
371          /*fprintf (stderr, "%f ", pr);*/
372          p0 *= pr;
373          p1 *= (1-pr);
374       }
375
376       p0 = pow(p0,.2);
377       p1 = pow(p1,.2);      
378       
379 #if 1
380       p0 *= 2;
381       p0=p0/(p1+p0);
382       if (st->last_speech>20) 
383       {
384          float tmp = sqrt(tot_loudness)/st->loudness2;
385          tmp = 1-exp(-10*tmp);
386          if (p0>tmp)
387             p0=tmp;
388       }
389       p1=1-p0;
390 #else
391       if (sqrt(tot_loudness) < .6*st->loudness2 && p0>15*p1)
392          p0=15*p1;
393       if (sqrt(tot_loudness) < .45*st->loudness2 && p0>7*p1)
394          p0=7*p1;
395       if (sqrt(tot_loudness) < .3*st->loudness2 && p0>3*p1)
396          p0=3*p1;
397       if (sqrt(tot_loudness) < .15*st->loudness2 && p0>p1)
398          p0=p1;
399       /*fprintf (stderr, "%f %f ", (float)(sqrt(tot_loudness) /( .25*st->loudness2)), p0/(p1+p0));*/
400 #endif
401
402       p0 *= .99*st->speech_prob + .01*(1-st->speech_prob);
403       p1 *= .01*st->speech_prob + .99*(1-st->speech_prob);
404       
405       st->speech_prob = p0/(1e-25+p1+p0);
406       /*fprintf (stderr, "%f %f %f ", tot_loudness, st->loudness2, st->speech_prob);*/
407
408       if (st->speech_prob>.35 || (st->last_speech < 20 && st->speech_prob>.1))
409       {
410          is_speech = 1;
411          st->last_speech = 0;
412       } else {
413          st->last_speech++;
414          if (st->last_speech<20)
415            is_speech = 1;
416       }
417
418       if (st->noise_bandsN > 50 && st->speech_bandsN > 50)
419       {
420          if (mean_post > 5)
421          {
422             float adapt = 1./st->speech_bandsN++;
423             if (adapt<.005)
424                adapt = .005;
425             for (i=0;i<NB_BANDS;i++)
426             {
427                st->speech_bands[i] = (1-adapt)*st->speech_bands[i] + adapt*bands[i];
428                /*st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*bands[i]*bands[i];*/
429                st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*(bands[i]-st->speech_bands[i])*(bands[i]-st->speech_bands[i]);
430             }
431          } else {
432             float adapt = 1./st->noise_bandsN++;
433             if (adapt<.005)
434                adapt = .005;
435             for (i=0;i<NB_BANDS;i++)
436             {
437                st->noise_bands[i] = (1-adapt)*st->noise_bands[i] + adapt*bands[i];
438                /*st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*bands[i]*bands[i];*/
439                st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*(bands[i]-st->noise_bands[i])*(bands[i]-st->noise_bands[i]);
440             }
441          }
442       }
443
444
445    }
446
447    return is_speech;
448 }
449
450 static void speex_compute_agc(SpeexPreprocessState *st, float mean_prior)
451 {
452    int i;
453    int N = st->ps_size;
454    float scale=.5/N;
455    float agc_gain;
456    int freq_start, freq_end;
457    float active_bands = 0;
458
459    freq_start = (int)(300.0*2*N/st->sampling_rate);
460    freq_end   = (int)(2000.0*2*N/st->sampling_rate);
461    for (i=freq_start;i<freq_end;i++)
462    {
463       if (st->S[i] > 20*st->Smin[i]+1000)
464          active_bands+=1;
465    }
466    active_bands /= (freq_end-freq_start+1);
467
468    if (active_bands > .2)
469    {
470       float loudness=0;
471       float rate, rate2=.2;
472       st->nb_loudness_adapt++;
473       rate=2.0/(1+st->nb_loudness_adapt);
474       if (rate < .05)
475          rate = .05;
476       if (rate < .1 && pow(loudness, LOUDNESS_EXP) > st->loudness)
477          rate = .1;
478       if (rate < .2 && pow(loudness, LOUDNESS_EXP) > 3*st->loudness)
479          rate = .2;
480       if (rate < .4 && pow(loudness, LOUDNESS_EXP) > 10*st->loudness)
481          rate = .4;
482
483       for (i=2;i<N;i++)
484       {
485          loudness += scale*st->ps[i] * st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
486       }
487       loudness=sqrt(loudness);
488       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
489         loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
490       st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
491       
492       st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0/LOUDNESS_EXP);
493
494       loudness = pow(st->loudness, 1.0/LOUDNESS_EXP);
495
496       /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
497    }
498    
499    agc_gain = st->agc_level/st->loudness2;
500    /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
501    if (agc_gain>200)
502       agc_gain = 200;
503
504    for (i=0;i<N;i++)
505       st->gain2[i] *= agc_gain;
506    
507 }
508
509 static void preprocess_analysis(SpeexPreprocessState *st, float *x)
510 {
511    int i;
512    int N = st->ps_size;
513    int N3 = 2*N - st->frame_size;
514    int N4 = st->frame_size - N3;
515    float *ps=st->ps;
516
517    /* 'Build' input frame */
518    for (i=0;i<N3;i++)
519       st->frame[i]=st->inbuf[i];
520    for (i=0;i<st->frame_size;i++)
521       st->frame[N3+i]=x[i];
522    
523    /* Update inbuf */
524    for (i=0;i<N3;i++)
525       st->inbuf[i]=x[N4+i];
526
527    /* Windowing */
528    for (i=0;i<2*N;i++)
529       st->frame[i] *= st->window[i];
530
531    /* Perform FFT */
532    drft_forward(st->fft_lookup, st->frame);
533
534    /* Power spectrum */
535    ps[0]=1;
536    for (i=1;i<N;i++)
537       ps[i]=1+st->frame[2*i-1]*st->frame[2*i-1] + st->frame[2*i]*st->frame[2*i];
538
539 }
540
541 static void update_noise_prob(SpeexPreprocessState *st)
542 {
543    int i;
544    int N = st->ps_size;
545
546    for (i=1;i<N-1;i++)
547       st->S[i] = 100+ .8*st->S[i] + .05*st->ps[i-1]+.1*st->ps[i]+.05*st->ps[i+1];
548    
549    if (st->nb_preprocess<1)
550    {
551       for (i=1;i<N-1;i++)
552          st->Smin[i] = st->Stmp[i] = st->S[i]+100;
553    }
554
555    if (st->nb_preprocess%80==0)
556    {
557       for (i=1;i<N-1;i++)
558       {
559          st->Smin[i] = min(st->Stmp[i], st->S[i]);
560          st->Stmp[i] = st->S[i];
561       }
562    } else {
563       for (i=1;i<N-1;i++)
564       {
565          st->Smin[i] = min(st->Smin[i], st->S[i]);
566          st->Stmp[i] = min(st->Stmp[i], st->S[i]);      
567       }
568    }
569    for (i=1;i<N-1;i++)
570    {
571       st->update_prob[i] *= .2;
572       if (st->S[i] > 5*st->Smin[i])
573          st->update_prob[i] += .8;
574       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
575       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
576    }
577
578 }
579
580 int speex_preprocess(SpeexPreprocessState *st, float *x, float *echo)
581 {
582    int i;
583    int is_speech=1;
584    float mean_post=0;
585    float mean_prior=0;
586    int N = st->ps_size;
587    int N3 = 2*N - st->frame_size;
588    int N4 = st->frame_size - N3;
589    float scale=.5/N;
590    float *ps=st->ps;
591
592    preprocess_analysis(st, x);
593
594    update_noise_prob(st);
595
596    st->nb_preprocess++;
597
598    /* Noise estimation always updated for the 20 first times */
599    if (st->nb_adapt<10)
600       /*if (st->nb_adapt<25 && st->nb_adapt>15)*/
601    {
602       update_noise(st, ps, echo);
603       st->last_update=0;
604    }
605
606    /* Deal with residual echo if provided */
607    if (echo)
608       for (i=1;i<N;i++)
609          st->echo_noise[i] = (.7*st->echo_noise[i] + .3* echo[i]);
610
611    /* Compute a posteriori SNR */
612    for (i=1;i<N;i++)
613    {
614       st->post[i] = ps[i]/(1+st->noise[i]+st->echo_noise[i]) - 1;
615       if (st->post[i]>100)
616          st->post[i]=100;
617       /*if (st->post[i]<0)
618         st->post[i]=0;*/
619       mean_post+=st->post[i];
620    }
621    mean_post /= N;
622    if (mean_post<0)
623       mean_post=0;
624
625    /* Special case for first frame */
626    if (st->nb_adapt==1)
627       for (i=1;i<N;i++)
628          st->old_ps[i] = ps[i];
629
630    /* Compute a priori SNR */
631    {
632       /* A priori update rate */
633       float gamma;
634       float min_gamma=0.12;
635       gamma = 1.0/st->nb_preprocess;
636
637       /*Make update rate smaller when there's no speech*/
638 #if 0
639       if (mean_post<3.5 && mean_prior < 1)
640          min_gamma *= (mean_post+.5);
641       else
642          min_gamma *= 4.;
643 #else
644       min_gamma = .2*fabs(mean_prior - mean_post)*fabs(mean_prior - mean_post);
645       if (min_gamma>.6)
646          min_gamma = .6;
647       if (min_gamma<.01)
648          min_gamma = .01;
649 #endif
650
651       if (gamma<min_gamma)
652          gamma=min_gamma;
653       
654       for (i=1;i<N;i++)
655       {
656          
657          /* A priori SNR update */
658          st->prior[i] = gamma*max(0.0,st->post[i]) +
659          (1-gamma)*st->gain[i]*st->gain[i]*st->old_ps[i]/(1+st->noise[i]+st->echo_noise[i]);
660          
661          if (st->prior[i]>100)
662             st->prior[i]=100;
663          
664          mean_prior+=st->prior[i];
665       }
666    }
667    mean_prior /= N;
668
669 #if 0
670    for (i=0;i<N;i++)
671    {
672       fprintf (stderr, "%f ", st->prior[i]);
673    }
674    fprintf (stderr, "\n");
675 #endif
676    /*fprintf (stderr, "%f %f\n", mean_prior,mean_post);*/
677
678    if (st->nb_preprocess>=20)
679    {
680       int do_update = 0;
681       float noise_ener=0, sig_ener=0;
682       /* If SNR is low (both a priori and a posteriori), update the noise estimate*/
683       /*if (mean_prior<.23 && mean_post < .5)*/
684       if (mean_prior<.23 && mean_post < .5)
685          do_update = 1;
686       for (i=1;i<N;i++)
687       {
688          noise_ener += st->noise[i];
689          sig_ener += ps[i];
690       }
691       if (noise_ener > 3*sig_ener)
692          do_update = 1;
693       /*do_update = 0;*/
694       if (do_update)
695       {
696          st->consec_noise++;
697       } else {
698          st->consec_noise=0;
699       }
700    }
701
702    if (st->vad_enabled)
703       is_speech = speex_compute_vad(st, ps, mean_prior, mean_post);
704
705
706    if (st->consec_noise>=3)
707    {
708       update_noise(st, st->old_ps, echo);
709       st->last_update=0;
710    } else {
711       st->last_update++;
712       for (i=1;i<N-1;i++)
713       {
714          if (st->update_prob[i]<.5)
715             st->noise[i] = .90*st->noise[i] + .1*st->ps[i];
716       }
717    }
718
719
720    /* Compute gain according to the Ephraim-Malah algorithm */
721    for (i=1;i<N;i++)
722    {
723       float MM;
724       float theta;
725       float prior_ratio;
726
727       prior_ratio = st->prior[i]/(1.0001+st->prior[i]);
728       theta = (1+st->post[i])*prior_ratio;
729
730 #if 0
731       /* Spectral magnitude estimator */
732       /* Approximation of:
733          exp(-theta/2)*((1+theta)*I0(theta/2) + theta.*I1(theta/2))
734          because I don't feel like computing Bessel functions
735       */
736       /*MM = -.22+1.155*sqrt(theta+1.1);*/
737       MM=-.22+1.163*sqrt(theta+1.1)-.0015*theta;
738       st->gain[i] = SQRT_M_PI_2*sqrt(prior_ratio/(1.0001+st->post[i]))*MM;
739       if (st->gain[i]>1)
740       {
741          st->gain[i]=1;
742       }
743 #else
744       /* log-spectral magnitude estimator */
745       if (theta<6)
746          MM = 0.74082*pow(theta+1,.61)/sqrt(.0001+theta);
747       else
748          MM=1;
749       st->gain[i] = prior_ratio * MM;
750       /*Put some (very arbitraty) limit on the gain*/
751       if (st->gain[i]>2)
752       {
753          st->gain[i]=2;
754       }
755 #endif
756       /*st->gain[i] = prior_ratio;*/
757    }
758    st->gain[0]=0;
759    st->gain[N-1]=0;
760
761    if (st->denoise_enabled)
762    {
763       for (i=1;i<N-1;i++)
764       {
765          st->gain2[i]=st->gain[i];
766          /* Limits noise reduction to -26 dB, put prevents some musical noise */
767          if (st->gain2[i]<.05)
768             st->gain2[i]=.05;
769       }
770       st->gain2[N-1]=0;
771    } else {
772       for (i=0;i<N;i++)
773          st->gain2[i] = 1;
774    }
775
776    if (st->agc_enabled)
777       speex_compute_agc(st, mean_prior);
778
779 #if 0
780    if (!is_speech)
781    {
782       for (i=0;i<N;i++)
783          st->gain2[i] = 0;
784    }
785 #if 0
786  else {
787       for (i=0;i<N;i++)
788          st->gain2[i] = 1;
789    }
790 #endif
791 #endif
792
793    /* Apply computed gain */
794    for (i=1;i<N;i++)
795    {
796       st->frame[2*i-1] *= st->gain2[i];
797       st->frame[2*i] *= st->gain2[i];
798    }
799
800    /* Get rid of the DC and very low frequencies */
801    st->frame[0]=0;
802    st->frame[1]=0;
803    st->frame[2]=0;
804    /* Nyquist frequency is mostly useless too */
805    st->frame[2*N-1]=0;
806
807    /* Inverse FFT with 1/N scaling */
808    drft_backward(st->fft_lookup, st->frame);
809
810    for (i=0;i<2*N;i++)
811       st->frame[i] *= scale;
812
813    {
814       float max_sample=0;
815       for (i=0;i<2*N;i++)
816          if (fabs(st->frame[i])>max_sample)
817             max_sample = fabs(st->frame[i]);
818       if (max_sample>28000)
819       {
820          float damp = 28000./max_sample;
821          for (i=0;i<2*N;i++)
822             st->frame[i] *= damp;
823       }
824    }
825
826    for (i=0;i<2*N;i++)
827       st->frame[i] *= st->window[i];
828
829    /* Perform overlap and add */
830    for (i=0;i<N3;i++)
831       x[i] = st->outbuf[i] + st->frame[i];
832    for (i=0;i<N4;i++)
833       x[N3+i] = st->frame[N3+i];
834    
835    /* Update outbuf */
836    for (i=0;i<N3;i++)
837       st->outbuf[i] = st->frame[st->frame_size+i];
838
839    /* Save old power spectrum */
840    for (i=1;i<N;i++)
841       st->old_ps[i] = ps[i];
842
843    return is_speech;
844 }
845
846 void speex_preprocess_estimate_update(SpeexPreprocessState *st, float *x, float *noise)
847 {
848    int i;
849    int N = st->ps_size;
850    int N3 = 2*N - st->frame_size;
851
852    float *ps=st->ps;
853
854    preprocess_analysis(st, x);
855
856    update_noise_prob(st);
857
858    st->nb_preprocess++;
859    
860    for (i=1;i<N-1;i++)
861    {
862       if (st->update_prob[i]<.5)
863          st->noise[i] = .90*st->noise[i] + .1*ps[i];
864    }
865
866    for (i=0;i<N3;i++)
867       st->outbuf[i] = x[st->frame_size-N3+i]*st->window[st->frame_size+i];
868
869    /* Save old power spectrum */
870    for (i=1;i<N;i++)
871       st->old_ps[i] = ps[i];
872
873 }
874
875
876 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
877 {
878    SpeexPreprocessState *st;
879    st=(SpeexPreprocessState*)state;
880    switch(request)
881    {
882    case SPEEX_PREPROCESS_SET_DENOISE:
883       st->denoise_enabled = (*(int*)ptr);
884       break;
885    case SPEEX_PREPROCESS_GET_DENOISE:
886       (*(int*)ptr) = st->denoise_enabled;
887       break;
888
889    case SPEEX_PREPROCESS_SET_AGC:
890       st->agc_enabled = (*(int*)ptr);
891       break;
892    case SPEEX_PREPROCESS_GET_AGC:
893       (*(int*)ptr) = st->agc_enabled;
894       break;
895
896    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
897       st->agc_level = (*(float*)ptr);
898       if (st->agc_level<1)
899          st->agc_level=1;
900       if (st->agc_level>32768)
901          st->agc_level=32768;
902       break;
903    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
904       (*(float*)ptr) = st->agc_level;
905       break;
906
907    case SPEEX_PREPROCESS_SET_VAD:
908       st->vad_enabled = (*(int*)ptr);
909       break;
910    case SPEEX_PREPROCESS_GET_VAD:
911       (*(int*)ptr) = st->vad_enabled;
912       break;
913    default:
914       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
915       return -1;
916    }
917    return 0;
918 }