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