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