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