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