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