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