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