Fixed some bad DTX/packet-loss/wideband interactions.
[speexdsp.git] / libspeex / nb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: nb_celp.c
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #include <math.h>
33 #include "nb_celp.h"
34 #include "lpc.h"
35 #include "lsp.h"
36 #include "ltp.h"
37 #include "quant_lsp.h"
38 #include "cb_search.h"
39 #include "filters.h"
40 #include "stack_alloc.h"
41 #include "vq.h"
42 #include "speex_bits.h"
43 #include "vbr.h"
44 #include "misc.h"
45 #include "speex_callbacks.h"
46
47 #ifdef SLOW_TRIG
48 #include "math_approx.h"
49 #define cos speex_cos
50 #endif
51
52 #ifndef M_PI
53 #define M_PI           3.14159265358979323846  /* pi */
54 #endif
55
56 #ifndef NULL
57 #define NULL 0
58 #endif
59
60 #define SUBMODE(x) st->submodes[st->submodeID]->x
61
62 float exc_gain_quant_scal3[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
63
64 float exc_gain_quant_scal1[2]={-0.35, 0.05};
65
66 #define sqr(x) ((x)*(x))
67
68 void *nb_encoder_init(SpeexMode *m)
69 {
70    EncState *st;
71    SpeexNBMode *mode;
72    int i;
73
74    mode=(SpeexNBMode *)m->mode;
75    st = (EncState*)speex_alloc(sizeof(EncState)+8000*sizeof(float));
76    if (!st)
77       return NULL;
78
79    st->stack = ((char*)st) + sizeof(EncState);
80    
81    st->mode=m;
82
83    st->frameSize = mode->frameSize;
84    st->windowSize = st->frameSize*3/2;
85    st->nbSubframes=mode->frameSize/mode->subframeSize;
86    st->subframeSize=mode->subframeSize;
87    st->lpcSize = mode->lpcSize;
88    st->bufSize = mode->bufSize;
89    st->gamma1=mode->gamma1;
90    st->gamma2=mode->gamma2;
91    st->min_pitch=mode->pitchStart;
92    st->max_pitch=mode->pitchEnd;
93    st->lag_factor=mode->lag_factor;
94    st->lpc_floor = mode->lpc_floor;
95    st->preemph = mode->preemph;
96   
97    st->submodes=mode->submodes;
98    st->submodeID=st->submodeSelect=mode->defaultSubmode;
99    st->pre_mem=0;
100    st->pre_mem2=0;
101    st->bounded_pitch = 1;
102
103    /* Allocating input buffer */
104    st->inBuf = PUSH(st->stack, st->bufSize, float);
105    st->frame = st->inBuf + st->bufSize - st->windowSize;
106    /* Allocating excitation buffer */
107    st->excBuf = PUSH(st->stack, st->bufSize, float);
108    st->exc = st->excBuf + st->bufSize - st->windowSize;
109    st->swBuf = PUSH(st->stack, st->bufSize, float);
110    st->sw = st->swBuf + st->bufSize - st->windowSize;
111
112    st->exc2Buf = PUSH(st->stack, st->bufSize, float);
113    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
114
115    st->innov = PUSH(st->stack, st->frameSize, float);
116
117    /* Asymetric "pseudo-Hamming" window */
118    {
119       int part1, part2;
120       part1 = st->subframeSize*7/2;
121       part2 = st->subframeSize*5/2;
122       st->window = PUSH(st->stack, st->windowSize, float);
123       for (i=0;i<part1;i++)
124          st->window[i]=.54-.46*cos(M_PI*i/part1);
125       for (i=0;i<part2;i++)
126          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
127    }
128    /* Create the window for autocorrelation (lag-windowing) */
129    st->lagWindow = PUSH(st->stack, st->lpcSize+1, float);
130    for (i=0;i<st->lpcSize+1;i++)
131       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
132
133    st->autocorr = PUSH(st->stack, st->lpcSize+1, float);
134
135    st->buf2 = PUSH(st->stack, st->windowSize, float);
136
137    st->lpc = PUSH(st->stack, st->lpcSize+1, float);
138    st->interp_lpc = PUSH(st->stack, st->lpcSize+1, float);
139    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, float);
140    st->bw_lpc1 = PUSH(st->stack, st->lpcSize+1, float);
141    st->bw_lpc2 = PUSH(st->stack, st->lpcSize+1, float);
142
143    st->lsp = PUSH(st->stack, st->lpcSize, float);
144    st->qlsp = PUSH(st->stack, st->lpcSize, float);
145    st->old_lsp = PUSH(st->stack, st->lpcSize, float);
146    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
147    st->interp_lsp = PUSH(st->stack, st->lpcSize, float);
148    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
149    st->rc = PUSH(st->stack, st->lpcSize, float);
150    st->first = 1;
151    for (i=0;i<st->lpcSize;i++)
152    {
153       st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
154    }
155
156    st->mem_sp = PUSH(st->stack, st->lpcSize, float);
157    st->mem_sw = PUSH(st->stack, st->lpcSize, float);
158    st->mem_sw_whole = PUSH(st->stack, st->lpcSize, float);
159    st->mem_exc = PUSH(st->stack, st->lpcSize, float);
160
161    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
162
163    st->pitch = PUSH(st->stack, st->nbSubframes, int);
164
165    st->vbr = PUSHS(st->stack, VBRState);
166    vbr_init(st->vbr);
167    st->vbr_quality = 8;
168    st->vbr_enabled = 0;
169    st->vad_enabled = 0;
170    st->dtx_enabled = 0;
171    st->abr_enabled = 0;
172    st->abr_drift = 0;
173
174    st->complexity=2;
175    st->sampling_rate=8000;
176    st->dtx_count=0;
177
178    return st;
179 }
180
181 void nb_encoder_destroy(void *state)
182 {
183    EncState *st=(EncState *)state;
184    /* Free all allocated memory */
185
186    vbr_destroy(st->vbr);
187
188    /*Free state memory... should be last*/
189    speex_free(st);
190 }
191
192 int nb_encode(void *state, float *in, SpeexBits *bits)
193 {
194    EncState *st;
195    int i, sub, roots;
196    int ol_pitch;
197    float ol_pitch_coef;
198    float ol_gain;
199    float *res, *target, *mem;
200    char *stack;
201    float *syn_resp;
202    float lsp_dist=0;
203    float *orig;
204
205    st=(EncState *)state;
206    stack=st->stack;
207
208    /* Copy new data in input buffer */
209    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
210    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
211    for (i=1;i<st->frameSize;i++)
212       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
213    st->pre_mem = in[st->frameSize-1];
214
215    /* Move signals 1 frame towards the past */
216    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
217    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
218    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
219
220
221    /* Window for analysis */
222    for (i=0;i<st->windowSize;i++)
223       st->buf2[i] = st->frame[i] * st->window[i];
224
225    /* Compute auto-correlation */
226    _spx_autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
227
228    st->autocorr[0] += 10;        /* prevents NANs */
229    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
230
231    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
232    for (i=0;i<st->lpcSize+1;i++)
233       st->autocorr[i] *= st->lagWindow[i];
234
235    /* Levinson-Durbin */
236    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
237    st->lpc[0]=1;
238
239    /* LPC to LSPs (x-domain) transform */
240    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
241    /* Check if we found all the roots */
242    if (roots==st->lpcSize)
243    {
244       /* LSP x-domain to angle domain*/
245       for (i=0;i<st->lpcSize;i++)
246          st->lsp[i] = acos(st->lsp[i]);
247    } else {
248       /* Search again if we can afford it */
249       if (st->complexity>1)
250          roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.05, stack);
251       if (roots==st->lpcSize) 
252       {
253          /* LSP x-domain to angle domain*/
254          for (i=0;i<st->lpcSize;i++)
255             st->lsp[i] = acos(st->lsp[i]);
256       } else {
257          /*If we can't find all LSP's, do some damage control and use previous filter*/
258          for (i=0;i<st->lpcSize;i++)
259          {
260             st->lsp[i]=st->old_lsp[i];
261          }
262       }
263    }
264
265
266    lsp_dist=0;
267    for (i=0;i<st->lpcSize;i++)
268       lsp_dist += (st->old_lsp[i] - st->lsp[i])*(st->old_lsp[i] - st->lsp[i]);
269
270    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
271    {
272       if (st->first)
273          for (i=0;i<st->lpcSize;i++)
274             st->interp_lsp[i] = st->lsp[i];
275       else
276          for (i=0;i<st->lpcSize;i++)
277             st->interp_lsp[i] = .375*st->old_lsp[i] + .625*st->lsp[i];
278
279       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
280
281       /* Compute interpolated LPCs (unquantized) for whole frame*/
282       for (i=0;i<st->lpcSize;i++)
283          st->interp_lsp[i] = cos(st->interp_lsp[i]);
284       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
285
286
287       /*Open-loop pitch*/
288       if (!st->submodes[st->submodeID] || st->vbr_enabled || st->vad_enabled || SUBMODE(forced_pitch_gain) ||
289           SUBMODE(lbr_pitch) != -1)
290       {
291          int nol_pitch[6];
292          float nol_pitch_coef[6];
293          
294          bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
295          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
296          
297          filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);
298
299          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
300                                nol_pitch, nol_pitch_coef, 6, stack);
301          ol_pitch=nol_pitch[0];
302          ol_pitch_coef = nol_pitch_coef[0];
303          /*Try to remove pitch multiples*/
304          for (i=1;i<6;i++)
305          {
306             if ((nol_pitch_coef[i]>.85*ol_pitch_coef) && 
307                 (fabs(nol_pitch[i]-ol_pitch/2.0)<=1 || fabs(nol_pitch[i]-ol_pitch/3.0)<=1 || 
308                  fabs(nol_pitch[i]-ol_pitch/4.0)<=1 || fabs(nol_pitch[i]-ol_pitch/5.0)<=1))
309             {
310                /*ol_pitch_coef=nol_pitch_coef[i];*/
311                ol_pitch = nol_pitch[i];
312             }
313          }
314          /*if (ol_pitch>50)
315            ol_pitch/=2;*/
316          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
317       } else {
318          ol_pitch=0;
319          ol_pitch_coef=0;
320       }
321       /*Compute "real" excitation*/
322       fir_mem2(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc);
323
324       /* Compute open-loop excitation gain */
325       ol_gain=0;
326       for (i=0;i<st->frameSize;i++)
327          ol_gain += st->exc[i]*st->exc[i];
328       
329       ol_gain=sqrt(1+ol_gain/st->frameSize);
330    }
331
332    /*VBR stuff*/
333    if (st->vbr && (st->vbr_enabled||st->vad_enabled))
334    {
335       
336       if (st->abr_enabled)
337       {
338          float qual_change=0;
339          if (st->abr_drift2 * st->abr_drift > 0)
340          {
341             /* Only adapt if long-term and short-term drift are the same sign */
342             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
343             if (qual_change>.05)
344                qual_change=.05;
345             if (qual_change<-.05)
346                qual_change=-.05;
347          }
348          st->vbr_quality += qual_change;
349          if (st->vbr_quality>10)
350             st->vbr_quality=10;
351          if (st->vbr_quality<0)
352             st->vbr_quality=0;
353       }
354
355       st->relative_quality = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
356       /*if (delta_qual<0)*/
357       /*  delta_qual*=.1*(3+st->vbr_quality);*/
358       if (st->vbr_enabled) 
359       {
360          int mode;
361          mode = 7;
362          while (mode)
363          {
364             int v1;
365             float thresh;
366             v1=(int)floor(st->vbr_quality);
367             if (v1==10)
368                thresh = vbr_nb_thresh[mode][v1];
369             else
370                thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
371             if (st->relative_quality > thresh)
372                break;
373             mode--;
374          }
375          
376          if (mode==0)
377          {
378             if (st->dtx_count==0 || lsp_dist>.05 || st->dtx_count>20)
379             {
380                mode=1;
381                st->dtx_count=1;
382             } else {
383                mode=0;
384                st->dtx_count++;
385             }
386          } else {
387             st->dtx_count=0;
388          }
389
390          speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
391
392          if (st->abr_enabled)
393          {
394             int bitrate;
395             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
396             st->abr_drift+=(bitrate-st->abr_enabled);
397             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
398             st->abr_count += 1.0;
399          }
400
401       } else {
402          /*VAD only case*/
403          int mode;
404          if (st->relative_quality<2)
405          {
406             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
407             {
408                st->dtx_count=1;
409                mode=1;
410             } else {
411                mode=0;
412                st->dtx_count++;
413             }
414          } else {
415             st->dtx_count = 0;
416             mode=st->submodeSelect;
417          }
418          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
419          st->submodeID=mode;
420       } 
421    } else {
422       st->relative_quality = -1;
423    }
424
425    /* First, transmit a zero for narrowband */
426    speex_bits_pack(bits, 0, 1);
427
428    /* Transmit the sub-mode we use for this frame */
429    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
430
431
432    /* If null mode (no transmission), just set a couple things to zero*/
433    if (st->submodes[st->submodeID] == NULL)
434    {
435       for (i=0;i<st->frameSize;i++)
436          st->exc[i]=st->exc2[i]=st->sw[i]=0;
437
438       for (i=0;i<st->lpcSize;i++)
439          st->mem_sw[i]=0;
440       st->first=1;
441       st->bounded_pitch = 1;
442
443       /* Final signal synthesis from excitation */
444       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
445
446       in[0] = st->frame[0] + st->preemph*st->pre_mem2;
447       for (i=1;i<st->frameSize;i++)
448          in[i]=st->frame[i] + st->preemph*in[i-1];
449       st->pre_mem2=in[st->frameSize-1];
450
451       return 0;
452
453    }
454
455    /* LSP Quantization */
456    if (st->first)
457    {
458       for (i=0;i<st->lpcSize;i++)
459          st->old_lsp[i] = st->lsp[i];
460    }
461
462
463    /*Quantize LSPs*/
464 #if 1 /*0 for unquantized*/
465    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
466 #else
467    for (i=0;i<st->lpcSize;i++)
468      st->qlsp[i]=st->lsp[i];
469 #endif
470
471    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
472    if (SUBMODE(lbr_pitch)!=-1)
473    {
474       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
475    } 
476    
477    if (SUBMODE(forced_pitch_gain))
478    {
479       int quant;
480       quant = (int)floor(.5+15*ol_pitch_coef);
481       if (quant>15)
482          quant=0;
483       if (quant<0)
484          quant=0;
485       speex_bits_pack(bits, quant, 4);
486       ol_pitch_coef=0.066667*quant;
487    }
488    
489    
490    /*Quantize and transmit open-loop excitation gain*/
491    {
492       int qe = (int)(floor(3.5*log(ol_gain)));
493       if (qe<0)
494          qe=0;
495       if (qe>31)
496          qe=31;
497       ol_gain = exp(qe/3.5);
498       speex_bits_pack(bits, qe, 5);
499    }
500
501    /* Special case for first frame */
502    if (st->first)
503    {
504       for (i=0;i<st->lpcSize;i++)
505          st->old_qlsp[i] = st->qlsp[i];
506    }
507
508    /* Filter response */
509    res = PUSH(stack, st->subframeSize, float);
510    /* Target signal */
511    target = PUSH(stack, st->subframeSize, float);
512    syn_resp = PUSH(stack, st->subframeSize, float);
513    mem = PUSH(stack, st->lpcSize, float);
514    orig = PUSH(stack, st->frameSize, float);
515    for (i=0;i<st->frameSize;i++)
516       orig[i]=st->frame[i];
517
518    /* Loop on sub-frames */
519    for (sub=0;sub<st->nbSubframes;sub++)
520    {
521       float tmp;
522       int   offset;
523       float *sp, *sw, *exc, *exc2;
524       int pitch;
525
526       /* Offset relative to start of frame */
527       offset = st->subframeSize*sub;
528       /* Original signal */
529       sp=st->frame+offset;
530       /* Excitation */
531       exc=st->exc+offset;
532       /* Weighted signal */
533       sw=st->sw+offset;
534
535       exc2=st->exc2+offset;
536
537
538       /* LSP interpolation (quantized and unquantized) */
539       tmp = (1.0 + sub)/st->nbSubframes;
540       for (i=0;i<st->lpcSize;i++)
541          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
542       for (i=0;i<st->lpcSize;i++)
543          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
544
545       /* Make sure the filters are stable */
546       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
547       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
548
549       /* Compute interpolated LPCs (quantized and unquantized) */
550       for (i=0;i<st->lpcSize;i++)
551          st->interp_lsp[i] = cos(st->interp_lsp[i]);
552       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
553
554       for (i=0;i<st->lpcSize;i++)
555          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
556       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
557
558       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
559       tmp=1;
560       st->pi_gain[sub]=0;
561       for (i=0;i<=st->lpcSize;i++)
562       {
563          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
564          tmp = -tmp;
565       }
566
567       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
568       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
569       if (st->gamma2>=0)
570          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
571       else
572       {
573          st->bw_lpc2[0]=1;
574          st->bw_lpc2[1]=-st->preemph;
575          for (i=2;i<=st->lpcSize;i++)
576             st->bw_lpc2[i]=0;
577       }
578
579       /* Compute impulse response of A(z/g1) / ( A(z)*A(z/g2) )*/
580       for (i=0;i<st->subframeSize;i++)
581          exc[i]=0;
582       exc[0]=1;
583       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
584
585       /* Reset excitation */
586       for (i=0;i<st->subframeSize;i++)
587          exc[i]=0;
588       for (i=0;i<st->subframeSize;i++)
589          exc2[i]=0;
590
591       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
592       for (i=0;i<st->lpcSize;i++)
593          mem[i]=st->mem_sp[i];
594       iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
595       
596       for (i=0;i<st->lpcSize;i++)
597          mem[i]=st->mem_sw[i];
598       filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
599       
600       /* Compute weighted signal */
601       for (i=0;i<st->lpcSize;i++)
602          mem[i]=st->mem_sw[i];
603       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
604       
605       /* Compute target signal */
606       for (i=0;i<st->subframeSize;i++)
607          target[i]=sw[i]-res[i];
608
609       for (i=0;i<st->subframeSize;i++)
610          exc[i]=exc2[i]=0;
611
612       /* If we have a long-term predictor (otherwise, something's wrong) */
613       if (SUBMODE(ltp_quant))
614       {
615          int pit_min, pit_max;
616          /* Long-term prediction */
617          if (SUBMODE(lbr_pitch) != -1)
618          {
619             /* Low bit-rate pitch handling */
620             int margin;
621             margin = SUBMODE(lbr_pitch);
622             if (margin)
623             {
624                if (ol_pitch < st->min_pitch+margin-1)
625                   ol_pitch=st->min_pitch+margin-1;
626                if (ol_pitch > st->max_pitch-margin)
627                   ol_pitch=st->max_pitch-margin;
628                pit_min = ol_pitch-margin+1;
629                pit_max = ol_pitch+margin;
630             } else {
631                pit_min=pit_max=ol_pitch;
632             }
633          } else {
634             pit_min = st->min_pitch;
635             pit_max = st->max_pitch;
636          }
637          
638          /* Force pitch to use only the current frame if needed */
639          if (st->bounded_pitch && pit_max>offset)
640             pit_max=offset;
641
642          /* Perform pitch search */
643          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
644                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
645                                     st->lpcSize, st->subframeSize, bits, stack, 
646                                     exc2, syn_resp, st->complexity);
647
648          st->pitch[sub]=pitch;
649       } else {
650          speex_error ("No pitch prediction, what's wrong");
651       }
652
653       /* Update target for adaptive codebook contribution */
654       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, stack);
655       for (i=0;i<st->subframeSize;i++)
656          target[i]-=res[i];
657
658
659       /* Quantization of innovation */
660       {
661          float *innov;
662          float ener=0, ener_1;
663
664          innov = st->innov+sub*st->subframeSize;
665          for (i=0;i<st->subframeSize;i++)
666             innov[i]=0;
667          
668          residue_percep_zero(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize, stack);
669          for (i=0;i<st->subframeSize;i++)
670             ener+=st->buf2[i]*st->buf2[i];
671          ener=sqrt(.1+ener/st->subframeSize);
672          /*for (i=0;i<st->subframeSize;i++)
673             printf ("%f\n", st->buf2[i]/ener);
674          */
675          
676          ener /= ol_gain;
677
678          /* Calculate gain correction for the sub-frame (if any) */
679          if (SUBMODE(have_subframe_gain)) 
680          {
681             int qe;
682             ener=log(ener);
683             if (SUBMODE(have_subframe_gain)==3)
684             {
685                qe = vq_index(&ener, exc_gain_quant_scal3, 1, 8);
686                speex_bits_pack(bits, qe, 3);
687                ener=exc_gain_quant_scal3[qe];
688             } else {
689                qe = vq_index(&ener, exc_gain_quant_scal1, 1, 2);
690                speex_bits_pack(bits, qe, 1);
691                ener=exc_gain_quant_scal1[qe];               
692             }
693             ener=exp(ener);
694          } else {
695             ener=1;
696          }
697
698          ener*=ol_gain;
699
700          /*printf ("%f %f\n", ener, ol_gain);*/
701
702          ener_1 = 1/ener;
703
704          /* Normalize innovation */
705          for (i=0;i<st->subframeSize;i++)
706             target[i]*=ener_1;
707          
708          /* Quantize innovation */
709          if (SUBMODE(innovation_quant))
710          {
711             /* Codebook search */
712             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
713                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
714                                       innov, syn_resp, bits, stack, st->complexity);
715             
716             /* De-normalize innovation and update excitation */
717             for (i=0;i<st->subframeSize;i++)
718                innov[i]*=ener;
719             for (i=0;i<st->subframeSize;i++)
720                exc[i] += innov[i];
721          } else {
722             speex_error("No fixed codebook");
723          }
724
725          /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
726          if (SUBMODE(double_codebook)) {
727             char *tmp_stack=stack;
728             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
729             for (i=0;i<st->subframeSize;i++)
730                innov2[i]=0;
731             for (i=0;i<st->subframeSize;i++)
732                target[i]*=2.2;
733             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
734                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
735                                       innov2, syn_resp, bits, tmp_stack, st->complexity);
736             for (i=0;i<st->subframeSize;i++)
737                innov2[i]*=ener*(1/2.2);
738             for (i=0;i<st->subframeSize;i++)
739                exc[i] += innov2[i];
740          }
741
742          for (i=0;i<st->subframeSize;i++)
743             target[i]*=ener;
744
745       }
746
747       /*Keep the previous memory*/
748       for (i=0;i<st->lpcSize;i++)
749          mem[i]=st->mem_sp[i];
750       /* Final signal synthesis from excitation */
751       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
752
753       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
754       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
755       for (i=0;i<st->subframeSize;i++)
756          exc2[i]=exc[i];
757    }
758
759    /* Store the LSPs for interpolation in the next frame */
760    if (st->submodeID>=1)
761    {
762       for (i=0;i<st->lpcSize;i++)
763          st->old_lsp[i] = st->lsp[i];
764       for (i=0;i<st->lpcSize;i++)
765          st->old_qlsp[i] = st->qlsp[i];
766    }
767
768    if (st->submodeID==1)
769    {
770       if (st->dtx_count)
771          speex_bits_pack(bits, 15, 4);
772       else
773          speex_bits_pack(bits, 0, 4);
774    }
775
776    /* The next frame will not be the first (Duh!) */
777    st->first = 0;
778
779    {
780       float ener=0, err=0;
781       float snr;
782       for (i=0;i<st->frameSize;i++)
783       {
784          ener+=st->frame[i]*st->frame[i];
785          err += (st->frame[i]-orig[i])*(st->frame[i]-orig[i]);
786       }
787       snr = 10*log10((ener+1)/(err+1));
788       /*printf ("%f %f %f\n", snr, ener, err);*/
789    }
790
791    /* Replace input by synthesized speech */
792    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
793    for (i=1;i<st->frameSize;i++)
794      in[i]=st->frame[i] + st->preemph*in[i-1];
795    st->pre_mem2=in[st->frameSize-1];
796
797    if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
798       st->bounded_pitch = 1;
799    else
800       st->bounded_pitch = 0;
801
802    return 1;
803 }
804
805
806 void *nb_decoder_init(SpeexMode *m)
807 {
808    DecState *st;
809    SpeexNBMode *mode;
810    int i;
811
812    mode=(SpeexNBMode*)m->mode;
813    st = (DecState *)speex_alloc(sizeof(DecState)+4000*sizeof(float));
814    st->mode=m;
815
816    st->stack = ((char*)st) + sizeof(DecState);
817
818    st->first=1;
819    /* Codec parameters, should eventually have several "modes"*/
820    st->frameSize = mode->frameSize;
821    st->windowSize = st->frameSize*3/2;
822    st->nbSubframes=mode->frameSize/mode->subframeSize;
823    st->subframeSize=mode->subframeSize;
824    st->lpcSize = mode->lpcSize;
825    st->bufSize = mode->bufSize;
826    st->gamma1=mode->gamma1;
827    st->gamma2=mode->gamma2;
828    st->min_pitch=mode->pitchStart;
829    st->max_pitch=mode->pitchEnd;
830    st->preemph = mode->preemph;
831
832    st->submodes=mode->submodes;
833    st->submodeID=mode->defaultSubmode;
834
835    st->pre_mem=0;
836    st->lpc_enh_enabled=0;
837
838
839    st->inBuf = PUSH(st->stack, st->bufSize, float);
840    st->frame = st->inBuf + st->bufSize - st->windowSize;
841    st->excBuf = PUSH(st->stack, st->bufSize, float);
842    st->exc = st->excBuf + st->bufSize - st->windowSize;
843    for (i=0;i<st->bufSize;i++)
844       st->inBuf[i]=0;
845    for (i=0;i<st->bufSize;i++)
846       st->excBuf[i]=0;
847    st->innov = PUSH(st->stack, st->frameSize, float);
848
849    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, float);
850    st->qlsp = PUSH(st->stack, st->lpcSize, float);
851    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
852    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
853    st->mem_sp = PUSH(st->stack, 5*st->lpcSize, float);
854    st->comb_mem = PUSHS(st->stack, CombFilterMem);
855    comp_filter_mem_init (st->comb_mem);
856
857    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
858    st->last_pitch = 40;
859    st->count_lost=0;
860    st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
861    st->pitch_gain_buf_idx = 0;
862
863    st->sampling_rate=8000;
864    st->last_ol_gain = 0;
865
866    st->user_callback.func = &speex_default_user_handler;
867    st->user_callback.data = NULL;
868    for (i=0;i<16;i++)
869       st->speex_callbacks[i].func = NULL;
870
871    st->voc_m1=st->voc_m2=st->voc_mean=0;
872    st->voc_offset=0;
873    st->dtx_enabled=0;
874    return st;
875 }
876
877 void nb_decoder_destroy(void *state)
878 {
879    DecState *st;
880    st=(DecState*)state;
881    
882    speex_free(state);
883 }
884
885 #define median3(a, b, c)        ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
886
887 static void nb_decode_lost(DecState *st, float *out, char *stack)
888 {
889    int i, sub;
890    float *awk1, *awk2, *awk3;
891    float pitch_gain, fact, gain_med;
892
893    fact = exp(-.04*st->count_lost*st->count_lost);
894    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
895    if (gain_med < st->last_pitch_gain)
896       st->last_pitch_gain = gain_med;
897    
898    pitch_gain = st->last_pitch_gain;
899    if (pitch_gain>.95)
900       pitch_gain=.95;
901
902    pitch_gain *= fact;
903
904    /* Shift all buffers by one frame */
905    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
906    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
907
908    awk1=PUSH(stack, (st->lpcSize+1), float);
909    awk2=PUSH(stack, (st->lpcSize+1), float);
910    awk3=PUSH(stack, (st->lpcSize+1), float);
911
912    for (sub=0;sub<st->nbSubframes;sub++)
913    {
914       int offset;
915       float *sp, *exc;
916       /* Offset relative to start of frame */
917       offset = st->subframeSize*sub;
918       /* Original signal */
919       sp=st->frame+offset;
920       /* Excitation */
921       exc=st->exc+offset;
922       /* Excitation after post-filter*/
923
924       /* Calculate perceptually enhanced LPC filter */
925       if (st->lpc_enh_enabled)
926       {
927          float r=.9;
928          
929          float k1,k2,k3;
930          if (st->submodes[st->submodeID] != NULL)
931          {
932             k1=SUBMODE(lpc_enh_k1);
933             k2=SUBMODE(lpc_enh_k2);
934          } else {
935             k1=k2=.7;
936          }
937          k3=(1-(1-r*k1)/(1-r*k2))/r;
938          if (!st->lpc_enh_enabled)
939          {
940             k1=k2;
941             k3=0;
942          }
943          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
944          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
945          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
946       }
947         
948       /* Make up a plausible excitation */
949       /* THIS CAN BE IMPROVED */
950       /*if (pitch_gain>.95)
951         pitch_gain=.95;*/
952       {
953          float innov_gain=0;
954          for (i=0;i<st->frameSize;i++)
955             innov_gain += st->innov[i]*st->innov[i];
956          innov_gain=sqrt(innov_gain/st->frameSize);
957       for (i=0;i<st->subframeSize;i++)
958       {
959 #if 0
960          exc[i] = pitch_gain * exc[i - st->last_pitch] + fact*sqrt(1-pitch_gain)*st->innov[i+offset];
961          /*Just so it give the same lost packets as with if 0*/
962          /*rand();*/
963 #else
964          /*exc[i]=pitch_gain*exc[i-st->last_pitch] +  fact*st->innov[i+offset];*/
965          exc[i]=pitch_gain*exc[i-st->last_pitch] + 
966          fact*sqrt(1-pitch_gain)*speex_rand(innov_gain);
967 #endif
968       }
969       }
970       for (i=0;i<st->subframeSize;i++)
971          sp[i]=exc[i];
972       
973       /* Signal synthesis */
974       if (st->lpc_enh_enabled)
975       {
976          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
977                      st->mem_sp+st->lpcSize);
978          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
979                      st->mem_sp);
980       } else {
981          for (i=0;i<st->lpcSize;i++)
982             st->mem_sp[st->lpcSize+i] = 0;
983          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
984                      st->mem_sp);
985       }      
986    }
987
988    out[0] = st->frame[0] + st->preemph*st->pre_mem;
989    for (i=1;i<st->frameSize;i++)
990       out[i]=st->frame[i] + st->preemph*out[i-1];
991    st->pre_mem=out[st->frameSize-1];
992    
993    st->first = 0;
994    st->count_lost++;
995    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = pitch_gain;
996    if (st->pitch_gain_buf_idx > 2) /* rollover */
997       st->pitch_gain_buf_idx = 0;
998 }
999
1000 int nb_decode(void *state, SpeexBits *bits, float *out)
1001 {
1002    DecState *st;
1003    int i, sub;
1004    int pitch;
1005    float pitch_gain[3];
1006    float ol_gain=0;
1007    int ol_pitch=0;
1008    float ol_pitch_coef=0;
1009    int best_pitch=40;
1010    float best_pitch_gain=0;
1011    int wideband;
1012    int m;
1013    char *stack;
1014    float *awk1, *awk2, *awk3;
1015    float pitch_average=0;
1016
1017    st=(DecState*)state;
1018    stack=st->stack;
1019
1020    /* Check if we're in DTX mode*/
1021    if (!bits && st->dtx_enabled)
1022    {
1023       st->submodeID=0;
1024    } else 
1025    {
1026       /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1027       if (!bits)
1028       {
1029          nb_decode_lost(st, out, stack);
1030          return 0;
1031       }
1032
1033       /* Search for next narrwoband block (handle requests, skip wideband blocks) */
1034       do {
1035          wideband = speex_bits_unpack_unsigned(bits, 1);
1036          if (wideband) /* Skip wideband block (for compatibility) */
1037          {
1038             int submode;
1039             int advance;
1040             advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1041             speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1042             if (advance < 0)
1043             {
1044                speex_warning ("Invalid wideband mode encountered. Corrupted stream?");
1045                return -2;
1046             } 
1047             advance -= (SB_SUBMODE_BITS+1);
1048             speex_bits_advance(bits, advance);
1049             wideband = speex_bits_unpack_unsigned(bits, 1);
1050             if (wideband)
1051             {
1052                int submode;
1053                int advance;
1054                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1055                speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1056                if (advance < 0)
1057                {
1058                   speex_warning ("Invalid wideband mode encountered: corrupted stream?");
1059                   return -2;
1060                } 
1061                advance -= (SB_SUBMODE_BITS+1);
1062                speex_bits_advance(bits, advance);
1063                wideband = speex_bits_unpack_unsigned(bits, 1);
1064                if (wideband)
1065                {
1066                   speex_warning ("More than to wideband layers found: corrupted stream?");
1067                   return -2;
1068                }
1069
1070             }
1071          }
1072
1073          m = speex_bits_unpack_unsigned(bits, 4);
1074          if (m==15) /* We found a terminator */
1075          {
1076             return -1;
1077          } else if (m==14) /* Speex in-band request */
1078          {
1079             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1080             if (ret)
1081                return ret;
1082          } else if (m==13) /* User in-band request */
1083          {
1084             int ret = st->user_callback.func(bits, state, st->user_callback.data);
1085             if (ret)
1086                return ret;
1087          } else if (m>7) /* Invalid mode */
1088          {
1089             return -2;
1090          }
1091       
1092       } while (m>7);
1093
1094       /* Get the sub-mode that was used */
1095       st->submodeID = m;
1096
1097    }
1098
1099    /* Shift all buffers by one frame */
1100    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1101    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1102
1103    /* If null mode (no transmission), just set a couple things to zero*/
1104    if (st->submodes[st->submodeID] == NULL)
1105    {
1106       float *lpc;
1107       lpc = PUSH(stack,11, float);
1108       bw_lpc(.93, st->interp_qlpc, lpc, 10);
1109       /*for (i=0;i<st->frameSize;i++)
1110         st->exc[i]=0;*/
1111       {
1112          float innov_gain=0;
1113          float pitch_gain=st->last_pitch_gain;
1114          if (pitch_gain>.6)
1115             pitch_gain=.6;
1116          for (i=0;i<st->frameSize;i++)
1117             innov_gain += st->innov[i]*st->innov[i];
1118          innov_gain=sqrt(innov_gain/st->frameSize);
1119          for (i=0;i<st->frameSize;i++)
1120             st->exc[i]=0;
1121          speex_rand_vec(innov_gain, st->exc, st->frameSize);
1122       }
1123
1124
1125       st->first=1;
1126
1127       /* Final signal synthesis from excitation */
1128       iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
1129
1130       out[0] = st->frame[0] + st->preemph*st->pre_mem;
1131       for (i=1;i<st->frameSize;i++)
1132          out[i]=st->frame[i] + st->preemph*out[i-1];
1133       st->pre_mem=out[st->frameSize-1];
1134       st->count_lost=0;
1135       return 0;
1136    }
1137
1138    /* Unquantize LSPs */
1139    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1140
1141    /*Damp memory if a frame was lost and the LSP changed too much*/
1142    if (st->count_lost)
1143    {
1144       float lsp_dist=0, fact;
1145       for (i=0;i<st->lpcSize;i++)
1146          lsp_dist += fabs(st->old_qlsp[i] - st->qlsp[i]);
1147       fact = .6*exp(-.2*lsp_dist);
1148       for (i=0;i<2*st->lpcSize;i++)
1149          st->mem_sp[i] *= fact;
1150    }
1151
1152
1153    /* Handle first frame and lost-packet case */
1154    if (st->first || st->count_lost)
1155    {
1156       for (i=0;i<st->lpcSize;i++)
1157          st->old_qlsp[i] = st->qlsp[i];
1158    }
1159
1160    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1161    if (SUBMODE(lbr_pitch)!=-1)
1162    {
1163       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1164    } 
1165    
1166    if (SUBMODE(forced_pitch_gain))
1167    {
1168       int quant;
1169       quant = speex_bits_unpack_unsigned(bits, 4);
1170       ol_pitch_coef=0.066667*quant;
1171    }
1172    
1173    /* Get global excitation gain */
1174    {
1175       int qe;
1176       qe = speex_bits_unpack_unsigned(bits, 5);
1177       ol_gain = exp(qe/3.5);
1178    }
1179
1180    awk1=PUSH(stack, st->lpcSize+1, float);
1181    awk2=PUSH(stack, st->lpcSize+1, float);
1182    awk3=PUSH(stack, st->lpcSize+1, float);
1183
1184    if (st->submodeID==1)
1185    {
1186       int extra;
1187       extra = speex_bits_unpack_unsigned(bits, 4);
1188
1189       if (extra==15)
1190          st->dtx_enabled=1;
1191       else
1192          st->dtx_enabled=0;
1193    }
1194    if (st->submodeID>1)
1195       st->dtx_enabled=0;
1196
1197    /*Loop on subframes */
1198    for (sub=0;sub<st->nbSubframes;sub++)
1199    {
1200       int offset;
1201       float *sp, *exc, tmp;
1202
1203       /* Offset relative to start of frame */
1204       offset = st->subframeSize*sub;
1205       /* Original signal */
1206       sp=st->frame+offset;
1207       /* Excitation */
1208       exc=st->exc+offset;
1209       /* Excitation after post-filter*/
1210
1211       /* LSP interpolation (quantized and unquantized) */
1212       tmp = (1.0 + sub)/st->nbSubframes;
1213       for (i=0;i<st->lpcSize;i++)
1214          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
1215
1216       /* Make sure the LSP's are stable */
1217       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
1218
1219
1220       /* Compute interpolated LPCs (unquantized) */
1221       for (i=0;i<st->lpcSize;i++)
1222          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
1223       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
1224
1225       /* Compute enhanced synthesis filter */
1226       if (st->lpc_enh_enabled)
1227       {
1228          float r=.9;
1229          
1230          float k1,k2,k3;
1231          k1=SUBMODE(lpc_enh_k1);
1232          k2=SUBMODE(lpc_enh_k2);
1233          k3=(1-(1-r*k1)/(1-r*k2))/r;
1234          if (!st->lpc_enh_enabled)
1235          {
1236             k1=k2;
1237             k3=0;
1238          }
1239          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1240          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1241          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1242          
1243       }
1244
1245       /* Compute analysis filter at w=pi */
1246       tmp=1;
1247       st->pi_gain[sub]=0;
1248       for (i=0;i<=st->lpcSize;i++)
1249       {
1250          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
1251          tmp = -tmp;
1252       }
1253
1254       /* Reset excitation */
1255       for (i=0;i<st->subframeSize;i++)
1256          exc[i]=0;
1257
1258       /*Adaptive codebook contribution*/
1259       if (SUBMODE(ltp_unquant))
1260       {
1261          int pit_min, pit_max;
1262          /* Handle pitch constraints if any */
1263          if (SUBMODE(lbr_pitch) != -1)
1264          {
1265             int margin;
1266             margin = SUBMODE(lbr_pitch);
1267             if (margin)
1268             {
1269 /* GT - need optimization?
1270                if (ol_pitch < st->min_pitch+margin-1)
1271                   ol_pitch=st->min_pitch+margin-1;
1272                if (ol_pitch > st->max_pitch-margin)
1273                   ol_pitch=st->max_pitch-margin;
1274                pit_min = ol_pitch-margin+1;
1275                pit_max = ol_pitch+margin;
1276 */
1277                pit_min = ol_pitch-margin+1;
1278                if (pit_min < st->min_pitch)
1279                   pit_min = st->min_pitch;
1280                pit_max = ol_pitch+margin;
1281                if (pit_max > st->max_pitch)
1282                   pit_max = st->max_pitch;
1283             } else {
1284                pit_min = pit_max = ol_pitch;
1285             }
1286          } else {
1287             pit_min = st->min_pitch;
1288             pit_max = st->max_pitch;
1289          }
1290
1291          /* Pitch synthesis */
1292          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1293                               st->subframeSize, &pitch, &pitch_gain[0], bits, stack, st->count_lost, offset, st->last_pitch_gain);
1294          
1295          /* If we had lost frames, check energy of last received frame */
1296          if (st->count_lost && ol_gain < st->last_ol_gain)
1297          {
1298             float fact = ol_gain/(st->last_ol_gain+1);
1299             for (i=0;i<st->subframeSize;i++)
1300                exc[i]*=fact;
1301          }
1302
1303          tmp = fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1304          tmp = fabs(pitch_gain[1]);
1305          if (pitch_gain[0]>0)
1306             tmp += pitch_gain[0];
1307          else
1308             tmp -= .5*pitch_gain[0];
1309          if (pitch_gain[2]>0)
1310             tmp += pitch_gain[2];
1311          else
1312             tmp -= .5*pitch_gain[0];
1313
1314
1315          pitch_average += tmp;
1316          if (tmp>best_pitch_gain)
1317          {
1318             best_pitch = pitch;
1319             best_pitch_gain = tmp;
1320             /*            best_pitch_gain = tmp*.9;
1321                         if (best_pitch_gain>.85)
1322                         best_pitch_gain=.85;*/
1323          }
1324       } else {
1325          speex_error("No pitch prediction, what's wrong");
1326       }
1327       
1328       /* Unquantize the innovation */
1329       {
1330          int q_energy;
1331          float ener;
1332          float *innov;
1333          
1334          innov = st->innov+sub*st->subframeSize;
1335          for (i=0;i<st->subframeSize;i++)
1336             innov[i]=0;
1337
1338          /* Decode sub-frame gain correction */
1339          if (SUBMODE(have_subframe_gain)==3)
1340          {
1341             q_energy = speex_bits_unpack_unsigned(bits, 3);
1342             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1343          } else if (SUBMODE(have_subframe_gain)==1)
1344          {
1345             q_energy = speex_bits_unpack_unsigned(bits, 1);
1346             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1347          } else {
1348             ener = ol_gain;
1349          }
1350                   
1351          if (SUBMODE(innovation_unquant))
1352          {
1353             /*Fixed codebook contribution*/
1354             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1355          } else {
1356             speex_error("No fixed codebook");
1357          }
1358
1359          /* De-normalize innovation and update excitation */
1360          for (i=0;i<st->subframeSize;i++)
1361             innov[i]*=ener;
1362
1363          /*Vocoder mode*/
1364          if (st->submodeID==1) 
1365          {
1366             float g=ol_pitch_coef;
1367
1368             
1369             for (i=0;i<st->subframeSize;i++)
1370                exc[i]=0;
1371             while (st->voc_offset<st->subframeSize)
1372             {
1373                if (st->voc_offset>=0)
1374                   exc[st->voc_offset]=sqrt(1.0*ol_pitch);
1375                st->voc_offset+=ol_pitch;
1376             }
1377             st->voc_offset -= st->subframeSize;
1378
1379             g=.5+2*(g-.6);
1380             if (g<0)
1381                g=0;
1382             if (g>1)
1383                g=1;
1384             for (i=0;i<st->subframeSize;i++)
1385             {
1386                float tmp=exc[i];
1387                exc[i]=.8*g*exc[i]*ol_gain + .6*g*st->voc_m1*ol_gain + .5*g*innov[i] - .5*g*st->voc_m2 + (1-g)*innov[i];
1388                st->voc_m1 = tmp;
1389                st->voc_m2=innov[i];
1390                st->voc_mean = .95*st->voc_mean + .05*exc[i];
1391                exc[i]-=st->voc_mean;
1392             }
1393          } else {
1394             for (i=0;i<st->subframeSize;i++)
1395                exc[i]+=innov[i];
1396          }
1397          /* Decode second codebook (only for some modes) */
1398          if (SUBMODE(double_codebook))
1399          {
1400             char *tmp_stack=stack;
1401             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1402             for (i=0;i<st->subframeSize;i++)
1403                innov2[i]=0;
1404             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, tmp_stack);
1405             for (i=0;i<st->subframeSize;i++)
1406                innov2[i]*=ener*(1/2.2);
1407             for (i=0;i<st->subframeSize;i++)
1408                exc[i] += innov2[i];
1409          }
1410
1411       }
1412
1413       for (i=0;i<st->subframeSize;i++)
1414          sp[i]=exc[i];
1415
1416       /* Signal synthesis */
1417       if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0)
1418          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1419                               pitch, pitch_gain, SUBMODE(comb_gain), st->comb_mem);
1420       if (st->lpc_enh_enabled)
1421       {
1422          /* Use enhanced LPC filter */
1423          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1424                      st->mem_sp+st->lpcSize);
1425          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1426                      st->mem_sp);
1427       } else {
1428          /* Use regular filter */
1429          for (i=0;i<st->lpcSize;i++)
1430             st->mem_sp[st->lpcSize+i] = 0;
1431          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1432                      st->mem_sp);
1433       }
1434    }
1435    
1436    /*Copy output signal*/
1437    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1438    for (i=1;i<st->frameSize;i++)
1439      out[i]=st->frame[i] + st->preemph*out[i-1];
1440    st->pre_mem=out[st->frameSize-1];
1441
1442
1443    /* Store the LSPs for interpolation in the next frame */
1444    for (i=0;i<st->lpcSize;i++)
1445       st->old_qlsp[i] = st->qlsp[i];
1446
1447    /* The next frame will not be the first (Duh!) */
1448    st->first = 0;
1449    st->count_lost=0;
1450    st->last_pitch = best_pitch;
1451    st->last_pitch_gain = .25*pitch_average;
1452    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1453    if (st->pitch_gain_buf_idx > 2) /* rollover */
1454       st->pitch_gain_buf_idx = 0;
1455
1456    st->last_ol_gain = ol_gain;
1457
1458    return 0;
1459 }
1460
1461 void nb_encoder_ctl(void *state, int request, void *ptr)
1462 {
1463    EncState *st;
1464    st=(EncState*)state;     
1465    switch(request)
1466    {
1467    case SPEEX_GET_FRAME_SIZE:
1468       (*(int*)ptr) = st->frameSize;
1469       break;
1470    case SPEEX_SET_LOW_MODE:
1471    case SPEEX_SET_MODE:
1472       st->submodeSelect = st->submodeID = (*(int*)ptr);
1473       break;
1474    case SPEEX_GET_LOW_MODE:
1475    case SPEEX_GET_MODE:
1476       (*(int*)ptr) = st->submodeID;
1477       break;
1478    case SPEEX_SET_VBR:
1479       st->vbr_enabled = (*(int*)ptr);
1480       break;
1481    case SPEEX_GET_VBR:
1482       (*(int*)ptr) = st->vbr_enabled;
1483       break;
1484    case SPEEX_SET_VAD:
1485       st->vad_enabled = (*(int*)ptr);
1486       break;
1487    case SPEEX_GET_VAD:
1488       (*(int*)ptr) = st->vad_enabled;
1489       break;
1490    case SPEEX_SET_DTX:
1491       st->dtx_enabled = (*(int*)ptr);
1492       break;
1493    case SPEEX_GET_DTX:
1494       (*(int*)ptr) = st->dtx_enabled;
1495       break;
1496    case SPEEX_SET_ABR:
1497       st->abr_enabled = (*(int*)ptr);
1498       st->vbr_enabled = 1;
1499       {
1500          int i=10, rate, target;
1501          float vbr_qual;
1502          target = (*(int*)ptr);
1503          while (i>=0)
1504          {
1505             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1506             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1507             if (rate <= target)
1508                break;
1509             i--;
1510          }
1511          vbr_qual=i;
1512          if (vbr_qual<0)
1513             vbr_qual=0;
1514          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1515          st->abr_count=0;
1516          st->abr_drift=0;
1517          st->abr_drift2=0;
1518       }
1519       
1520       break;
1521    case SPEEX_GET_ABR:
1522       (*(int*)ptr) = st->abr_enabled;
1523       break;
1524    case SPEEX_SET_VBR_QUALITY:
1525       st->vbr_quality = (*(float*)ptr);
1526       break;
1527    case SPEEX_GET_VBR_QUALITY:
1528       (*(float*)ptr) = st->vbr_quality;
1529       break;
1530    case SPEEX_SET_QUALITY:
1531       {
1532          int quality = (*(int*)ptr);
1533          if (quality < 0)
1534             quality = 0;
1535          if (quality > 10)
1536             quality = 10;
1537          st->submodeSelect = st->submodeID = ((SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1538       }
1539       break;
1540    case SPEEX_SET_COMPLEXITY:
1541       st->complexity = (*(int*)ptr);
1542       if (st->complexity<1)
1543          st->complexity=1;
1544       break;
1545    case SPEEX_GET_COMPLEXITY:
1546       (*(int*)ptr) = st->complexity;
1547       break;
1548    case SPEEX_SET_BITRATE:
1549       {
1550          int i=10, rate, target;
1551          target = (*(int*)ptr);
1552          while (i>=0)
1553          {
1554             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1555             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1556             if (rate <= target)
1557                break;
1558             i--;
1559          }
1560       }
1561       break;
1562    case SPEEX_GET_BITRATE:
1563       if (st->submodes[st->submodeID])
1564          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1565       else
1566          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1567       break;
1568    case SPEEX_SET_SAMPLING_RATE:
1569       st->sampling_rate = (*(int*)ptr);
1570       break;
1571    case SPEEX_GET_SAMPLING_RATE:
1572       (*(int*)ptr)=st->sampling_rate;
1573       break;
1574    case SPEEX_RESET_STATE:
1575       {
1576          int i;
1577          st->bounded_pitch = 1;
1578          st->first = 1;
1579          for (i=0;i<st->lpcSize;i++)
1580             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1581          for (i=0;i<st->lpcSize;i++)
1582             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1583          for (i=0;i<st->bufSize;i++)
1584             st->excBuf[i]=st->swBuf[i]=st->inBuf[i]=st->exc2Buf[i]=0;
1585       }
1586       break;
1587    case SPEEX_GET_PI_GAIN:
1588       {
1589          int i;
1590          float *g = (float*)ptr;
1591          for (i=0;i<st->nbSubframes;i++)
1592             g[i]=st->pi_gain[i];
1593       }
1594       break;
1595    case SPEEX_GET_EXC:
1596       {
1597          int i;
1598          float *e = (float*)ptr;
1599          for (i=0;i<st->frameSize;i++)
1600             e[i]=st->exc[i];
1601       }
1602       break;
1603    case SPEEX_GET_INNOV:
1604       {
1605          int i;
1606          float *e = (float*)ptr;
1607          for (i=0;i<st->frameSize;i++)
1608             e[i]=st->innov[i];
1609       }
1610       break;
1611    case SPEEX_GET_RELATIVE_QUALITY:
1612       (*(float*)ptr)=st->relative_quality;
1613       break;
1614    default:
1615       speex_warning_int("Unknown nb_ctl request: ", request);
1616    }
1617 }
1618
1619 void nb_decoder_ctl(void *state, int request, void *ptr)
1620 {
1621    DecState *st;
1622    st=(DecState*)state;
1623    switch(request)
1624    {
1625    case SPEEX_GET_LOW_MODE:
1626    case SPEEX_GET_MODE:
1627       (*(int*)ptr) = st->submodeID;
1628       break;
1629    case SPEEX_SET_ENH:
1630       st->lpc_enh_enabled = *((int*)ptr);
1631       break;
1632    case SPEEX_GET_ENH:
1633       *((int*)ptr) = st->lpc_enh_enabled;
1634       break;
1635    case SPEEX_GET_FRAME_SIZE:
1636       (*(int*)ptr) = st->frameSize;
1637       break;
1638    case SPEEX_GET_BITRATE:
1639       if (st->submodes[st->submodeID])
1640          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1641       else
1642          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1643       break;
1644    case SPEEX_SET_SAMPLING_RATE:
1645       st->sampling_rate = (*(int*)ptr);
1646       break;
1647    case SPEEX_GET_SAMPLING_RATE:
1648       (*(int*)ptr)=st->sampling_rate;
1649       break;
1650    case SPEEX_SET_HANDLER:
1651       {
1652          SpeexCallback *c = (SpeexCallback*)ptr;
1653          st->speex_callbacks[c->callback_id].func=c->func;
1654          st->speex_callbacks[c->callback_id].data=c->data;
1655          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1656       }
1657       break;
1658    case SPEEX_SET_USER_HANDLER:
1659       {
1660          SpeexCallback *c = (SpeexCallback*)ptr;
1661          st->user_callback.func=c->func;
1662          st->user_callback.data=c->data;
1663          st->user_callback.callback_id=c->callback_id;
1664       }
1665       break;
1666    case SPEEX_RESET_STATE:
1667       {
1668          int i;
1669          for (i=0;i<2*st->lpcSize;i++)
1670             st->mem_sp[i]=0;
1671          for (i=0;i<st->bufSize;i++)
1672             st->excBuf[i]=st->inBuf[i]=0;
1673       }
1674       break;
1675    case SPEEX_GET_PI_GAIN:
1676       {
1677          int i;
1678          float *g = (float*)ptr;
1679          for (i=0;i<st->nbSubframes;i++)
1680             g[i]=st->pi_gain[i];
1681       }
1682       break;
1683    case SPEEX_GET_EXC:
1684       {
1685          int i;
1686          float *e = (float*)ptr;
1687          for (i=0;i<st->frameSize;i++)
1688             e[i]=st->exc[i];
1689       }
1690       break;
1691    case SPEEX_GET_INNOV:
1692       {
1693          int i;
1694          float *e = (float*)ptr;
1695          for (i=0;i<st->frameSize;i++)
1696             e[i]=st->innov[i];
1697       }
1698       break;
1699    case SPEEX_GET_DTX_STATUS:
1700       *((int*)ptr) = st->dtx_enabled;
1701       break;
1702    default:
1703       speex_warning_int("Unknown nb_ctl request: ", request);
1704    }
1705 }