Fixing warnings (patch by Alfred E. Heggestad)
[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
765    Pframe = qcurve(Zframe);
766
767    /*fprintf (stderr, "%f\n", Pframe);*/
768    /* Compute gain according to the Ephraim-Malah algorithm */
769    for (i=1;i<N;i++)
770    {
771       float MM;
772       float theta;
773       float prior_ratio;
774       float p, q;
775       float zeta1;
776       float P1;
777
778       prior_ratio = st->prior[i]/(1.0001f+st->prior[i]);
779       theta = (1.f+st->post[i])*prior_ratio;
780
781       if (i==1 || i==N-1)
782          zeta1 = st->zeta[i];
783       else
784          zeta1 = .25f*st->zeta[i-1] + .5f*st->zeta[i] + .25f*st->zeta[i+1];
785       P1 = qcurve (zeta1);
786       
787       /* FIXME: add global prob (P2) */
788       q = 1-Pframe*P1;
789       q = 1-P1;
790       if (q>.95f)
791          q=.95f;
792       p=1.f/(1.f + (q/(1.f-q))*(1.f+st->prior[i])*exp(-theta));
793       /*p=1;*/
794
795       /* Optimal estimator for loudness domain */
796       MM = hypergeom_gain(theta);
797
798       st->gain[i] = prior_ratio * MM;
799       /*Put some (very arbitraty) limit on the gain*/
800       if (st->gain[i]>2.f)
801       {
802          st->gain[i]=2.f;
803       }
804       
805       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];
806       if (st->denoise_enabled)
807       {
808          st->gain2[i] = p*p*st->gain[i];
809          /*st->gain2[i]=(p*sqrt(st->gain[i])+.05*(1-p))*(p*sqrt(st->gain[i])+.05*(1-p));*/
810          /*st->gain2[i] = pow(st->gain[i], p) * pow(.2f,1.f-p);*/
811       } else {
812          st->gain2[i]=1.f;
813       }
814    }
815    
816    st->gain2[0]=st->gain[0]=0.f;
817    st->gain2[N-1]=st->gain[N-1]=0.f;
818    /*
819    for (i=30;i<N-2;i++)
820    {
821       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]);
822    }
823    for (i=30;i<N-2;i++)
824       st->gain2[i] = st->gain[i];
825    */
826    if (st->agc_enabled)
827       speex_compute_agc(st, mean_prior);
828
829 #if 0
830    if (!is_speech)
831    {
832       for (i=0;i<N;i++)
833          st->gain2[i] = 0;
834    }
835 #if 0
836  else {
837       for (i=0;i<N;i++)
838          st->gain2[i] = 1;
839    }
840 #endif
841 #endif
842
843    /* Apply computed gain */
844    for (i=1;i<N;i++)
845    {
846       st->frame[2*i-1] *= st->gain2[i];
847       st->frame[2*i] *= st->gain2[i];
848    }
849
850    /* Get rid of the DC and very low frequencies */
851    st->frame[0]=0;
852    st->frame[1]=0;
853    st->frame[2]=0;
854    /* Nyquist frequency is mostly useless too */
855    st->frame[2*N-1]=0;
856
857    /* Inverse FFT with 1/N scaling */
858    spx_drft_backward(st->fft_lookup, st->frame);
859
860    for (i=0;i<2*N;i++)
861       st->frame[i] *= scale;
862
863    {
864       float max_sample=0;
865       for (i=0;i<2*N;i++)
866          if (fabs(st->frame[i])>max_sample)
867             max_sample = fabs(st->frame[i]);
868       if (max_sample>28000.f)
869       {
870          float damp = 28000.f/max_sample;
871          for (i=0;i<2*N;i++)
872             st->frame[i] *= damp;
873       }
874    }
875
876    for (i=0;i<2*N;i++)
877       st->frame[i] *= st->window[i];
878
879    /* Perform overlap and add */
880    for (i=0;i<N3;i++)
881       x[i] = st->outbuf[i] + st->frame[i];
882    for (i=0;i<N4;i++)
883       x[N3+i] = st->frame[N3+i];
884    
885    /* Update outbuf */
886    for (i=0;i<N3;i++)
887       st->outbuf[i] = st->frame[st->frame_size+i];
888
889    /* Save old power spectrum */
890    for (i=1;i<N;i++)
891       st->old_ps[i] = ps[i];
892
893    return is_speech;
894 }
895
896 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, float *echo)
897 {
898    int i;
899    int N = st->ps_size;
900    int N3 = 2*N - st->frame_size;
901
902    float *ps=st->ps;
903
904    preprocess_analysis(st, x);
905
906    update_noise_prob(st);
907
908    st->nb_preprocess++;
909    
910    for (i=1;i<N-1;i++)
911    {
912       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
913       {
914          if (echo)
915             st->noise[i] = .95f*st->noise[i] + .1f*max(1.0f,st->ps[i]-echo[i]);
916          else
917             st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
918       }
919    }
920
921    for (i=0;i<N3;i++)
922       st->outbuf[i] = x[st->frame_size-N3+i]*st->window[st->frame_size+i];
923
924    /* Save old power spectrum */
925    for (i=1;i<N;i++)
926       st->old_ps[i] = ps[i];
927
928    for (i=1;i<N;i++)
929       st->reverb_estimate[i] *= st->reverb_decay;
930 }
931
932
933 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
934 {
935    int i;
936    SpeexPreprocessState *st;
937    st=(SpeexPreprocessState*)state;
938    switch(request)
939    {
940    case SPEEX_PREPROCESS_SET_DENOISE:
941       st->denoise_enabled = (*(int*)ptr);
942       break;
943    case SPEEX_PREPROCESS_GET_DENOISE:
944       (*(int*)ptr) = st->denoise_enabled;
945       break;
946
947    case SPEEX_PREPROCESS_SET_AGC:
948       st->agc_enabled = (*(int*)ptr);
949       break;
950    case SPEEX_PREPROCESS_GET_AGC:
951       (*(int*)ptr) = st->agc_enabled;
952       break;
953
954    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
955       st->agc_level = (*(float*)ptr);
956       if (st->agc_level<1)
957          st->agc_level=1;
958       if (st->agc_level>32768)
959          st->agc_level=32768;
960       break;
961    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
962       (*(float*)ptr) = st->agc_level;
963       break;
964
965    case SPEEX_PREPROCESS_SET_VAD:
966       st->vad_enabled = (*(int*)ptr);
967       break;
968    case SPEEX_PREPROCESS_GET_VAD:
969       (*(int*)ptr) = st->vad_enabled;
970       break;
971    
972    case SPEEX_PREPROCESS_SET_DEREVERB:
973       st->dereverb_enabled = (*(int*)ptr);
974       for (i=0;i<st->ps_size;i++)
975          st->reverb_estimate[i]=0;
976       break;
977    case SPEEX_PREPROCESS_GET_DEREVERB:
978       (*(int*)ptr) = st->dereverb_enabled;
979       break;
980
981    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
982       st->reverb_level = (*(float*)ptr);
983       break;
984    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
985       (*(float*)ptr) = st->reverb_level;
986       break;
987    
988    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
989       st->reverb_decay = (*(float*)ptr);
990       break;
991    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
992       (*(float*)ptr) = st->reverb_decay;
993       break;
994
995       default:
996       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
997       return -1;
998    }
999    return 0;
1000 }