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