Fixed a bunch of typos pointed to by: larry@doolittle.boa.org
[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    /* Asymmetric "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          int choice=0;
362          float min_diff=100;
363          mode = 8;
364          while (mode)
365          {
366             int v1;
367             float thresh;
368             v1=(int)floor(st->vbr_quality);
369             if (v1==10)
370                thresh = vbr_nb_thresh[mode][v1];
371             else
372                thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
373             if (st->relative_quality > thresh && 
374                 st->relative_quality-thresh<min_diff)
375             {
376                choice = mode;
377                min_diff = st->relative_quality-thresh;
378             }
379             mode--;
380          }
381          mode=choice;
382          if (mode==0)
383          {
384             if (st->dtx_count==0 || lsp_dist>.05 || st->dtx_count>20)
385             {
386                mode=1;
387                st->dtx_count=1;
388             } else {
389                mode=0;
390                st->dtx_count++;
391             }
392          } else {
393             st->dtx_count=0;
394          }
395
396          speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
397
398          if (st->abr_enabled)
399          {
400             int bitrate;
401             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
402             st->abr_drift+=(bitrate-st->abr_enabled);
403             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
404             st->abr_count += 1.0;
405          }
406
407       } else {
408          /*VAD only case*/
409          int mode;
410          if (st->relative_quality<2)
411          {
412             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
413             {
414                st->dtx_count=1;
415                mode=1;
416             } else {
417                mode=0;
418                st->dtx_count++;
419             }
420          } else {
421             st->dtx_count = 0;
422             mode=st->submodeSelect;
423          }
424          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
425          st->submodeID=mode;
426       } 
427    } else {
428       st->relative_quality = -1;
429    }
430
431    /* First, transmit a zero for narrowband */
432    speex_bits_pack(bits, 0, 1);
433
434    /* Transmit the sub-mode we use for this frame */
435    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
436
437
438    /* If null mode (no transmission), just set a couple things to zero*/
439    if (st->submodes[st->submodeID] == NULL)
440    {
441       for (i=0;i<st->frameSize;i++)
442          st->exc[i]=st->exc2[i]=st->sw[i]=0;
443
444       for (i=0;i<st->lpcSize;i++)
445          st->mem_sw[i]=0;
446       st->first=1;
447       st->bounded_pitch = 1;
448
449       /* Final signal synthesis from excitation */
450       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
451
452       in[0] = st->frame[0] + st->preemph*st->pre_mem2;
453       for (i=1;i<st->frameSize;i++)
454          in[i]=st->frame[i] + st->preemph*in[i-1];
455       st->pre_mem2=in[st->frameSize-1];
456
457       return 0;
458
459    }
460
461    /* LSP Quantization */
462    if (st->first)
463    {
464       for (i=0;i<st->lpcSize;i++)
465          st->old_lsp[i] = st->lsp[i];
466    }
467
468
469    /*Quantize LSPs*/
470 #if 1 /*0 for unquantized*/
471    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
472 #else
473    for (i=0;i<st->lpcSize;i++)
474      st->qlsp[i]=st->lsp[i];
475 #endif
476
477    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
478    if (SUBMODE(lbr_pitch)!=-1)
479    {
480       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
481    } 
482    
483    if (SUBMODE(forced_pitch_gain))
484    {
485       int quant;
486       quant = (int)floor(.5+15*ol_pitch_coef);
487       if (quant>15)
488          quant=0;
489       if (quant<0)
490          quant=0;
491       speex_bits_pack(bits, quant, 4);
492       ol_pitch_coef=0.066667*quant;
493    }
494    
495    
496    /*Quantize and transmit open-loop excitation gain*/
497    {
498       int qe = (int)(floor(3.5*log(ol_gain)));
499       if (qe<0)
500          qe=0;
501       if (qe>31)
502          qe=31;
503       ol_gain = exp(qe/3.5);
504       speex_bits_pack(bits, qe, 5);
505    }
506
507    /* Special case for first frame */
508    if (st->first)
509    {
510       for (i=0;i<st->lpcSize;i++)
511          st->old_qlsp[i] = st->qlsp[i];
512    }
513
514    /* Filter response */
515    res = PUSH(stack, st->subframeSize, float);
516    /* Target signal */
517    target = PUSH(stack, st->subframeSize, float);
518    syn_resp = PUSH(stack, st->subframeSize, float);
519    mem = PUSH(stack, st->lpcSize, float);
520    orig = PUSH(stack, st->frameSize, float);
521    for (i=0;i<st->frameSize;i++)
522       orig[i]=st->frame[i];
523
524    /* Loop on sub-frames */
525    for (sub=0;sub<st->nbSubframes;sub++)
526    {
527       float tmp;
528       int   offset;
529       float *sp, *sw, *exc, *exc2;
530       int pitch;
531
532       /* Offset relative to start of frame */
533       offset = st->subframeSize*sub;
534       /* Original signal */
535       sp=st->frame+offset;
536       /* Excitation */
537       exc=st->exc+offset;
538       /* Weighted signal */
539       sw=st->sw+offset;
540
541       exc2=st->exc2+offset;
542
543
544       /* LSP interpolation (quantized and unquantized) */
545       tmp = (1.0 + sub)/st->nbSubframes;
546       for (i=0;i<st->lpcSize;i++)
547          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
548       for (i=0;i<st->lpcSize;i++)
549          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
550
551       /* Make sure the filters are stable */
552       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
553       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
554
555       /* Compute interpolated LPCs (quantized and unquantized) */
556       for (i=0;i<st->lpcSize;i++)
557          st->interp_lsp[i] = cos(st->interp_lsp[i]);
558       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
559
560       for (i=0;i<st->lpcSize;i++)
561          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
562       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
563
564       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
565       tmp=1;
566       st->pi_gain[sub]=0;
567       for (i=0;i<=st->lpcSize;i++)
568       {
569          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
570          tmp = -tmp;
571       }
572
573       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
574       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
575       if (st->gamma2>=0)
576          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
577       else
578       {
579          st->bw_lpc2[0]=1;
580          st->bw_lpc2[1]=-st->preemph;
581          for (i=2;i<=st->lpcSize;i++)
582             st->bw_lpc2[i]=0;
583       }
584
585       /* Compute impulse response of A(z/g1) / ( A(z)*A(z/g2) )*/
586       for (i=0;i<st->subframeSize;i++)
587          exc[i]=0;
588       exc[0]=1;
589       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
590
591       /* Reset excitation */
592       for (i=0;i<st->subframeSize;i++)
593          exc[i]=0;
594       for (i=0;i<st->subframeSize;i++)
595          exc2[i]=0;
596
597       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
598       for (i=0;i<st->lpcSize;i++)
599          mem[i]=st->mem_sp[i];
600       iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
601       
602       for (i=0;i<st->lpcSize;i++)
603          mem[i]=st->mem_sw[i];
604       filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
605       
606       /* Compute weighted signal */
607       for (i=0;i<st->lpcSize;i++)
608          mem[i]=st->mem_sw[i];
609       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
610       
611       /* Compute target signal */
612       for (i=0;i<st->subframeSize;i++)
613          target[i]=sw[i]-res[i];
614
615       for (i=0;i<st->subframeSize;i++)
616          exc[i]=exc2[i]=0;
617
618       /* If we have a long-term predictor (otherwise, something's wrong) */
619       if (SUBMODE(ltp_quant))
620       {
621          int pit_min, pit_max;
622          /* Long-term prediction */
623          if (SUBMODE(lbr_pitch) != -1)
624          {
625             /* Low bit-rate pitch handling */
626             int margin;
627             margin = SUBMODE(lbr_pitch);
628             if (margin)
629             {
630                if (ol_pitch < st->min_pitch+margin-1)
631                   ol_pitch=st->min_pitch+margin-1;
632                if (ol_pitch > st->max_pitch-margin)
633                   ol_pitch=st->max_pitch-margin;
634                pit_min = ol_pitch-margin+1;
635                pit_max = ol_pitch+margin;
636             } else {
637                pit_min=pit_max=ol_pitch;
638             }
639          } else {
640             pit_min = st->min_pitch;
641             pit_max = st->max_pitch;
642          }
643          
644          /* Force pitch to use only the current frame if needed */
645          if (st->bounded_pitch && pit_max>offset)
646             pit_max=offset;
647
648          /* Perform pitch search */
649          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
650                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
651                                     st->lpcSize, st->subframeSize, bits, stack, 
652                                     exc2, syn_resp, st->complexity);
653
654          st->pitch[sub]=pitch;
655       } else {
656          speex_error ("No pitch prediction, what's wrong");
657       }
658
659       /* Update target for adaptive codebook contribution */
660       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, stack);
661       for (i=0;i<st->subframeSize;i++)
662          target[i]-=res[i];
663
664
665       /* Quantization of innovation */
666       {
667          float *innov;
668          float ener=0, ener_1;
669
670          innov = st->innov+sub*st->subframeSize;
671          for (i=0;i<st->subframeSize;i++)
672             innov[i]=0;
673          
674          residue_percep_zero(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize, stack);
675          for (i=0;i<st->subframeSize;i++)
676             ener+=st->buf2[i]*st->buf2[i];
677          ener=sqrt(.1+ener/st->subframeSize);
678          /*for (i=0;i<st->subframeSize;i++)
679             printf ("%f\n", st->buf2[i]/ener);
680          */
681          
682          ener /= ol_gain;
683
684          /* Calculate gain correction for the sub-frame (if any) */
685          if (SUBMODE(have_subframe_gain)) 
686          {
687             int qe;
688             ener=log(ener);
689             if (SUBMODE(have_subframe_gain)==3)
690             {
691                qe = vq_index(&ener, exc_gain_quant_scal3, 1, 8);
692                speex_bits_pack(bits, qe, 3);
693                ener=exc_gain_quant_scal3[qe];
694             } else {
695                qe = vq_index(&ener, exc_gain_quant_scal1, 1, 2);
696                speex_bits_pack(bits, qe, 1);
697                ener=exc_gain_quant_scal1[qe];               
698             }
699             ener=exp(ener);
700          } else {
701             ener=1;
702          }
703
704          ener*=ol_gain;
705
706          /*printf ("%f %f\n", ener, ol_gain);*/
707
708          ener_1 = 1/ener;
709
710          /* Normalize innovation */
711          for (i=0;i<st->subframeSize;i++)
712             target[i]*=ener_1;
713          
714          /* Quantize innovation */
715          if (SUBMODE(innovation_quant))
716          {
717             /* Codebook search */
718             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
719                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
720                                       innov, syn_resp, bits, stack, st->complexity);
721             
722             /* De-normalize innovation and update excitation */
723             for (i=0;i<st->subframeSize;i++)
724                innov[i]*=ener;
725             for (i=0;i<st->subframeSize;i++)
726                exc[i] += innov[i];
727          } else {
728             speex_error("No fixed codebook");
729          }
730
731          /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
732          if (SUBMODE(double_codebook)) {
733             char *tmp_stack=stack;
734             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
735             for (i=0;i<st->subframeSize;i++)
736                innov2[i]=0;
737             for (i=0;i<st->subframeSize;i++)
738                target[i]*=2.2;
739             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
740                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
741                                       innov2, syn_resp, bits, tmp_stack, st->complexity);
742             for (i=0;i<st->subframeSize;i++)
743                innov2[i]*=ener*(1/2.2);
744             for (i=0;i<st->subframeSize;i++)
745                exc[i] += innov2[i];
746          }
747
748          for (i=0;i<st->subframeSize;i++)
749             target[i]*=ener;
750
751       }
752
753       /*Keep the previous memory*/
754       for (i=0;i<st->lpcSize;i++)
755          mem[i]=st->mem_sp[i];
756       /* Final signal synthesis from excitation */
757       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
758
759       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
760       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
761       for (i=0;i<st->subframeSize;i++)
762          exc2[i]=exc[i];
763    }
764
765    /* Store the LSPs for interpolation in the next frame */
766    if (st->submodeID>=1)
767    {
768       for (i=0;i<st->lpcSize;i++)
769          st->old_lsp[i] = st->lsp[i];
770       for (i=0;i<st->lpcSize;i++)
771          st->old_qlsp[i] = st->qlsp[i];
772    }
773
774    if (st->submodeID==1)
775    {
776       if (st->dtx_count)
777          speex_bits_pack(bits, 15, 4);
778       else
779          speex_bits_pack(bits, 0, 4);
780    }
781
782    /* The next frame will not be the first (Duh!) */
783    st->first = 0;
784
785    {
786       float ener=0, err=0;
787       float snr;
788       for (i=0;i<st->frameSize;i++)
789       {
790          ener+=st->frame[i]*st->frame[i];
791          err += (st->frame[i]-orig[i])*(st->frame[i]-orig[i]);
792       }
793       snr = 10*log10((ener+1)/(err+1));
794       /*printf ("%f %f %f\n", snr, ener, err);*/
795    }
796
797    /* Replace input by synthesized speech */
798    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
799    for (i=1;i<st->frameSize;i++)
800      in[i]=st->frame[i] + st->preemph*in[i-1];
801    st->pre_mem2=in[st->frameSize-1];
802
803    if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
804       st->bounded_pitch = 1;
805    else
806       st->bounded_pitch = 0;
807
808    return 1;
809 }
810
811
812 void *nb_decoder_init(SpeexMode *m)
813 {
814    DecState *st;
815    SpeexNBMode *mode;
816    int i;
817
818    mode=(SpeexNBMode*)m->mode;
819    st = (DecState *)speex_alloc(sizeof(DecState)+4000*sizeof(float));
820    st->mode=m;
821
822    st->stack = ((char*)st) + sizeof(DecState);
823
824    st->first=1;
825    /* Codec parameters, should eventually have several "modes"*/
826    st->frameSize = mode->frameSize;
827    st->windowSize = st->frameSize*3/2;
828    st->nbSubframes=mode->frameSize/mode->subframeSize;
829    st->subframeSize=mode->subframeSize;
830    st->lpcSize = mode->lpcSize;
831    st->bufSize = mode->bufSize;
832    st->gamma1=mode->gamma1;
833    st->gamma2=mode->gamma2;
834    st->min_pitch=mode->pitchStart;
835    st->max_pitch=mode->pitchEnd;
836    st->preemph = mode->preemph;
837
838    st->submodes=mode->submodes;
839    st->submodeID=mode->defaultSubmode;
840
841    st->pre_mem=0;
842    st->lpc_enh_enabled=0;
843
844
845    st->inBuf = PUSH(st->stack, st->bufSize, float);
846    st->frame = st->inBuf + st->bufSize - st->windowSize;
847    st->excBuf = PUSH(st->stack, st->bufSize, float);
848    st->exc = st->excBuf + st->bufSize - st->windowSize;
849    for (i=0;i<st->bufSize;i++)
850       st->inBuf[i]=0;
851    for (i=0;i<st->bufSize;i++)
852       st->excBuf[i]=0;
853    st->innov = PUSH(st->stack, st->frameSize, float);
854
855    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, float);
856    st->qlsp = PUSH(st->stack, st->lpcSize, float);
857    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
858    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
859    st->mem_sp = PUSH(st->stack, 5*st->lpcSize, float);
860    st->comb_mem = PUSHS(st->stack, CombFilterMem);
861    comp_filter_mem_init (st->comb_mem);
862
863    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
864    st->last_pitch = 40;
865    st->count_lost=0;
866    st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
867    st->pitch_gain_buf_idx = 0;
868
869    st->sampling_rate=8000;
870    st->last_ol_gain = 0;
871
872    st->user_callback.func = &speex_default_user_handler;
873    st->user_callback.data = NULL;
874    for (i=0;i<16;i++)
875       st->speex_callbacks[i].func = NULL;
876
877    st->voc_m1=st->voc_m2=st->voc_mean=0;
878    st->voc_offset=0;
879    st->dtx_enabled=0;
880    return st;
881 }
882
883 void nb_decoder_destroy(void *state)
884 {
885    DecState *st;
886    st=(DecState*)state;
887    
888    speex_free(state);
889 }
890
891 #define median3(a, b, c)        ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
892
893 static void nb_decode_lost(DecState *st, float *out, char *stack)
894 {
895    int i, sub;
896    float *awk1, *awk2, *awk3;
897    float pitch_gain, fact, gain_med;
898
899    fact = exp(-.04*st->count_lost*st->count_lost);
900    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
901    if (gain_med < st->last_pitch_gain)
902       st->last_pitch_gain = gain_med;
903    
904    pitch_gain = st->last_pitch_gain;
905    if (pitch_gain>.95)
906       pitch_gain=.95;
907
908    pitch_gain *= fact;
909
910    /* Shift all buffers by one frame */
911    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
912    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
913
914    awk1=PUSH(stack, (st->lpcSize+1), float);
915    awk2=PUSH(stack, (st->lpcSize+1), float);
916    awk3=PUSH(stack, (st->lpcSize+1), float);
917
918    for (sub=0;sub<st->nbSubframes;sub++)
919    {
920       int offset;
921       float *sp, *exc;
922       /* Offset relative to start of frame */
923       offset = st->subframeSize*sub;
924       /* Original signal */
925       sp=st->frame+offset;
926       /* Excitation */
927       exc=st->exc+offset;
928       /* Excitation after post-filter*/
929
930       /* Calculate perceptually enhanced LPC filter */
931       if (st->lpc_enh_enabled)
932       {
933          float r=.9;
934          
935          float k1,k2,k3;
936          if (st->submodes[st->submodeID] != NULL)
937          {
938             k1=SUBMODE(lpc_enh_k1);
939             k2=SUBMODE(lpc_enh_k2);
940          } else {
941             k1=k2=.7;
942          }
943          k3=(1-(1-r*k1)/(1-r*k2))/r;
944          if (!st->lpc_enh_enabled)
945          {
946             k1=k2;
947             k3=0;
948          }
949          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
950          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
951          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
952       }
953         
954       /* Make up a plausible excitation */
955       /* THIS CAN BE IMPROVED */
956       /*if (pitch_gain>.95)
957         pitch_gain=.95;*/
958       {
959          float innov_gain=0;
960          for (i=0;i<st->frameSize;i++)
961             innov_gain += st->innov[i]*st->innov[i];
962          innov_gain=sqrt(innov_gain/st->frameSize);
963       for (i=0;i<st->subframeSize;i++)
964       {
965 #if 0
966          exc[i] = pitch_gain * exc[i - st->last_pitch] + fact*sqrt(1-pitch_gain)*st->innov[i+offset];
967          /*Just so it give the same lost packets as with if 0*/
968          /*rand();*/
969 #else
970          /*exc[i]=pitch_gain*exc[i-st->last_pitch] +  fact*st->innov[i+offset];*/
971          exc[i]=pitch_gain*exc[i-st->last_pitch] + 
972          fact*sqrt(1-pitch_gain)*speex_rand(innov_gain);
973 #endif
974       }
975       }
976       for (i=0;i<st->subframeSize;i++)
977          sp[i]=exc[i];
978       
979       /* Signal synthesis */
980       if (st->lpc_enh_enabled)
981       {
982          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
983                      st->mem_sp+st->lpcSize);
984          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
985                      st->mem_sp);
986       } else {
987          for (i=0;i<st->lpcSize;i++)
988             st->mem_sp[st->lpcSize+i] = 0;
989          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
990                      st->mem_sp);
991       }      
992    }
993
994    out[0] = st->frame[0] + st->preemph*st->pre_mem;
995    for (i=1;i<st->frameSize;i++)
996       out[i]=st->frame[i] + st->preemph*out[i-1];
997    st->pre_mem=out[st->frameSize-1];
998    
999    st->first = 0;
1000    st->count_lost++;
1001    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = pitch_gain;
1002    if (st->pitch_gain_buf_idx > 2) /* rollover */
1003       st->pitch_gain_buf_idx = 0;
1004 }
1005
1006 int nb_decode(void *state, SpeexBits *bits, float *out)
1007 {
1008    DecState *st;
1009    int i, sub;
1010    int pitch;
1011    float pitch_gain[3];
1012    float ol_gain=0;
1013    int ol_pitch=0;
1014    float ol_pitch_coef=0;
1015    int best_pitch=40;
1016    float best_pitch_gain=0;
1017    int wideband;
1018    int m;
1019    char *stack;
1020    float *awk1, *awk2, *awk3;
1021    float pitch_average=0;
1022
1023    st=(DecState*)state;
1024    stack=st->stack;
1025
1026    /* Check if we're in DTX mode*/
1027    if (!bits && st->dtx_enabled)
1028    {
1029       st->submodeID=0;
1030    } else 
1031    {
1032       /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1033       if (!bits)
1034       {
1035          nb_decode_lost(st, out, stack);
1036          return 0;
1037       }
1038
1039       /* Search for next narrowband block (handle requests, skip wideband blocks) */
1040       do {
1041          wideband = speex_bits_unpack_unsigned(bits, 1);
1042          if (wideband) /* Skip wideband block (for compatibility) */
1043          {
1044             int submode;
1045             int advance;
1046             advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1047             speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1048             if (advance < 0)
1049             {
1050                speex_warning ("Invalid wideband mode encountered. Corrupted stream?");
1051                return -2;
1052             } 
1053             advance -= (SB_SUBMODE_BITS+1);
1054             speex_bits_advance(bits, advance);
1055             wideband = speex_bits_unpack_unsigned(bits, 1);
1056             if (wideband)
1057             {
1058                int submode;
1059                int advance;
1060                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1061                speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1062                if (advance < 0)
1063                {
1064                   speex_warning ("Invalid wideband mode encountered: corrupted stream?");
1065                   return -2;
1066                } 
1067                advance -= (SB_SUBMODE_BITS+1);
1068                speex_bits_advance(bits, advance);
1069                wideband = speex_bits_unpack_unsigned(bits, 1);
1070                if (wideband)
1071                {
1072                   speex_warning ("More than to wideband layers found: corrupted stream?");
1073                   return -2;
1074                }
1075
1076             }
1077          }
1078
1079          /* FIXME: Check for overflow */
1080          m = speex_bits_unpack_unsigned(bits, 4);
1081          if (m==15) /* We found a terminator */
1082          {
1083             return -1;
1084          } else if (m==14) /* Speex in-band request */
1085          {
1086             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1087             if (ret)
1088                return ret;
1089          } else if (m==13) /* User in-band request */
1090          {
1091             int ret = st->user_callback.func(bits, state, st->user_callback.data);
1092             if (ret)
1093                return ret;
1094          } else if (m>8) /* Invalid mode */
1095          {
1096             speex_warning("Invalid mode encountered: corrupted stream?");
1097             return -2;
1098          }
1099       
1100       } while (m>8);
1101
1102       /* Get the sub-mode that was used */
1103       st->submodeID = m;
1104
1105    }
1106
1107    /* Shift all buffers by one frame */
1108    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1109    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1110
1111    /* If null mode (no transmission), just set a couple things to zero*/
1112    if (st->submodes[st->submodeID] == NULL)
1113    {
1114       float *lpc;
1115       lpc = PUSH(stack,11, float);
1116       bw_lpc(.93, st->interp_qlpc, lpc, 10);
1117       /*for (i=0;i<st->frameSize;i++)
1118         st->exc[i]=0;*/
1119       {
1120          float innov_gain=0;
1121          float pitch_gain=st->last_pitch_gain;
1122          if (pitch_gain>.6)
1123             pitch_gain=.6;
1124          for (i=0;i<st->frameSize;i++)
1125             innov_gain += st->innov[i]*st->innov[i];
1126          innov_gain=sqrt(innov_gain/st->frameSize);
1127          for (i=0;i<st->frameSize;i++)
1128             st->exc[i]=0;
1129          speex_rand_vec(innov_gain, st->exc, st->frameSize);
1130       }
1131
1132
1133       st->first=1;
1134
1135       /* Final signal synthesis from excitation */
1136       iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
1137
1138       out[0] = st->frame[0] + st->preemph*st->pre_mem;
1139       for (i=1;i<st->frameSize;i++)
1140          out[i]=st->frame[i] + st->preemph*out[i-1];
1141       st->pre_mem=out[st->frameSize-1];
1142       st->count_lost=0;
1143       return 0;
1144    }
1145
1146    /* Unquantize LSPs */
1147    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1148
1149    /*Damp memory if a frame was lost and the LSP changed too much*/
1150    if (st->count_lost)
1151    {
1152       float lsp_dist=0, fact;
1153       for (i=0;i<st->lpcSize;i++)
1154          lsp_dist += fabs(st->old_qlsp[i] - st->qlsp[i]);
1155       fact = .6*exp(-.2*lsp_dist);
1156       for (i=0;i<2*st->lpcSize;i++)
1157          st->mem_sp[i] *= fact;
1158    }
1159
1160
1161    /* Handle first frame and lost-packet case */
1162    if (st->first || st->count_lost)
1163    {
1164       for (i=0;i<st->lpcSize;i++)
1165          st->old_qlsp[i] = st->qlsp[i];
1166    }
1167
1168    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1169    if (SUBMODE(lbr_pitch)!=-1)
1170    {
1171       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1172    } 
1173    
1174    if (SUBMODE(forced_pitch_gain))
1175    {
1176       int quant;
1177       quant = speex_bits_unpack_unsigned(bits, 4);
1178       ol_pitch_coef=0.066667*quant;
1179    }
1180    
1181    /* Get global excitation gain */
1182    {
1183       int qe;
1184       qe = speex_bits_unpack_unsigned(bits, 5);
1185       ol_gain = exp(qe/3.5);
1186    }
1187
1188    awk1=PUSH(stack, st->lpcSize+1, float);
1189    awk2=PUSH(stack, st->lpcSize+1, float);
1190    awk3=PUSH(stack, st->lpcSize+1, float);
1191
1192    if (st->submodeID==1)
1193    {
1194       int extra;
1195       extra = speex_bits_unpack_unsigned(bits, 4);
1196
1197       if (extra==15)
1198          st->dtx_enabled=1;
1199       else
1200          st->dtx_enabled=0;
1201    }
1202    if (st->submodeID>1)
1203       st->dtx_enabled=0;
1204
1205    /*Loop on subframes */
1206    for (sub=0;sub<st->nbSubframes;sub++)
1207    {
1208       int offset;
1209       float *sp, *exc, tmp;
1210
1211       /* Offset relative to start of frame */
1212       offset = st->subframeSize*sub;
1213       /* Original signal */
1214       sp=st->frame+offset;
1215       /* Excitation */
1216       exc=st->exc+offset;
1217       /* Excitation after post-filter*/
1218
1219       /* LSP interpolation (quantized and unquantized) */
1220       tmp = (1.0 + sub)/st->nbSubframes;
1221       for (i=0;i<st->lpcSize;i++)
1222          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
1223
1224       /* Make sure the LSP's are stable */
1225       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
1226
1227
1228       /* Compute interpolated LPCs (unquantized) */
1229       for (i=0;i<st->lpcSize;i++)
1230          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
1231       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
1232
1233       /* Compute enhanced synthesis filter */
1234       if (st->lpc_enh_enabled)
1235       {
1236          float r=.9;
1237          
1238          float k1,k2,k3;
1239          k1=SUBMODE(lpc_enh_k1);
1240          k2=SUBMODE(lpc_enh_k2);
1241          k3=(1-(1-r*k1)/(1-r*k2))/r;
1242          if (!st->lpc_enh_enabled)
1243          {
1244             k1=k2;
1245             k3=0;
1246          }
1247          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1248          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1249          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1250          
1251       }
1252
1253       /* Compute analysis filter at w=pi */
1254       tmp=1;
1255       st->pi_gain[sub]=0;
1256       for (i=0;i<=st->lpcSize;i++)
1257       {
1258          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
1259          tmp = -tmp;
1260       }
1261
1262       /* Reset excitation */
1263       for (i=0;i<st->subframeSize;i++)
1264          exc[i]=0;
1265
1266       /*Adaptive codebook contribution*/
1267       if (SUBMODE(ltp_unquant))
1268       {
1269          int pit_min, pit_max;
1270          /* Handle pitch constraints if any */
1271          if (SUBMODE(lbr_pitch) != -1)
1272          {
1273             int margin;
1274             margin = SUBMODE(lbr_pitch);
1275             if (margin)
1276             {
1277 /* GT - need optimization?
1278                if (ol_pitch < st->min_pitch+margin-1)
1279                   ol_pitch=st->min_pitch+margin-1;
1280                if (ol_pitch > st->max_pitch-margin)
1281                   ol_pitch=st->max_pitch-margin;
1282                pit_min = ol_pitch-margin+1;
1283                pit_max = ol_pitch+margin;
1284 */
1285                pit_min = ol_pitch-margin+1;
1286                if (pit_min < st->min_pitch)
1287                   pit_min = st->min_pitch;
1288                pit_max = ol_pitch+margin;
1289                if (pit_max > st->max_pitch)
1290                   pit_max = st->max_pitch;
1291             } else {
1292                pit_min = pit_max = ol_pitch;
1293             }
1294          } else {
1295             pit_min = st->min_pitch;
1296             pit_max = st->max_pitch;
1297          }
1298
1299          /* Pitch synthesis */
1300          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1301                               st->subframeSize, &pitch, &pitch_gain[0], bits, stack, st->count_lost, offset, st->last_pitch_gain);
1302          
1303          /* If we had lost frames, check energy of last received frame */
1304          if (st->count_lost && ol_gain < st->last_ol_gain)
1305          {
1306             float fact = ol_gain/(st->last_ol_gain+1);
1307             for (i=0;i<st->subframeSize;i++)
1308                exc[i]*=fact;
1309          }
1310
1311          tmp = fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1312          tmp = fabs(pitch_gain[1]);
1313          if (pitch_gain[0]>0)
1314             tmp += pitch_gain[0];
1315          else
1316             tmp -= .5*pitch_gain[0];
1317          if (pitch_gain[2]>0)
1318             tmp += pitch_gain[2];
1319          else
1320             tmp -= .5*pitch_gain[0];
1321
1322
1323          pitch_average += tmp;
1324          if (tmp>best_pitch_gain)
1325          {
1326             best_pitch = pitch;
1327             best_pitch_gain = tmp;
1328             /*            best_pitch_gain = tmp*.9;
1329                         if (best_pitch_gain>.85)
1330                         best_pitch_gain=.85;*/
1331          }
1332       } else {
1333          speex_error("No pitch prediction, what's wrong");
1334       }
1335       
1336       /* Unquantize the innovation */
1337       {
1338          int q_energy;
1339          float ener;
1340          float *innov;
1341          
1342          innov = st->innov+sub*st->subframeSize;
1343          for (i=0;i<st->subframeSize;i++)
1344             innov[i]=0;
1345
1346          /* Decode sub-frame gain correction */
1347          if (SUBMODE(have_subframe_gain)==3)
1348          {
1349             q_energy = speex_bits_unpack_unsigned(bits, 3);
1350             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1351          } else if (SUBMODE(have_subframe_gain)==1)
1352          {
1353             q_energy = speex_bits_unpack_unsigned(bits, 1);
1354             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1355          } else {
1356             ener = ol_gain;
1357          }
1358                   
1359          if (SUBMODE(innovation_unquant))
1360          {
1361             /*Fixed codebook contribution*/
1362             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1363          } else {
1364             speex_error("No fixed codebook");
1365          }
1366
1367          /* De-normalize innovation and update excitation */
1368          for (i=0;i<st->subframeSize;i++)
1369             innov[i]*=ener;
1370
1371          /*Vocoder mode*/
1372          if (st->submodeID==1) 
1373          {
1374             float g=ol_pitch_coef;
1375
1376             
1377             for (i=0;i<st->subframeSize;i++)
1378                exc[i]=0;
1379             while (st->voc_offset<st->subframeSize)
1380             {
1381                if (st->voc_offset>=0)
1382                   exc[st->voc_offset]=sqrt(1.0*ol_pitch);
1383                st->voc_offset+=ol_pitch;
1384             }
1385             st->voc_offset -= st->subframeSize;
1386
1387             g=.5+2*(g-.6);
1388             if (g<0)
1389                g=0;
1390             if (g>1)
1391                g=1;
1392             for (i=0;i<st->subframeSize;i++)
1393             {
1394                float tmp=exc[i];
1395                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];
1396                st->voc_m1 = tmp;
1397                st->voc_m2=innov[i];
1398                st->voc_mean = .95*st->voc_mean + .05*exc[i];
1399                exc[i]-=st->voc_mean;
1400             }
1401          } else {
1402             for (i=0;i<st->subframeSize;i++)
1403                exc[i]+=innov[i];
1404          }
1405          /* Decode second codebook (only for some modes) */
1406          if (SUBMODE(double_codebook))
1407          {
1408             char *tmp_stack=stack;
1409             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1410             for (i=0;i<st->subframeSize;i++)
1411                innov2[i]=0;
1412             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, tmp_stack);
1413             for (i=0;i<st->subframeSize;i++)
1414                innov2[i]*=ener*(1/2.2);
1415             for (i=0;i<st->subframeSize;i++)
1416                exc[i] += innov2[i];
1417          }
1418
1419       }
1420
1421       for (i=0;i<st->subframeSize;i++)
1422          sp[i]=exc[i];
1423
1424       /* Signal synthesis */
1425       if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0)
1426          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1427                               pitch, pitch_gain, SUBMODE(comb_gain), st->comb_mem);
1428       if (st->lpc_enh_enabled)
1429       {
1430          /* Use enhanced LPC filter */
1431          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1432                      st->mem_sp+st->lpcSize);
1433          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1434                      st->mem_sp);
1435       } else {
1436          /* Use regular filter */
1437          for (i=0;i<st->lpcSize;i++)
1438             st->mem_sp[st->lpcSize+i] = 0;
1439          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1440                      st->mem_sp);
1441       }
1442    }
1443    
1444    /*Copy output signal*/
1445    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1446    for (i=1;i<st->frameSize;i++)
1447      out[i]=st->frame[i] + st->preemph*out[i-1];
1448    st->pre_mem=out[st->frameSize-1];
1449
1450
1451    /* Store the LSPs for interpolation in the next frame */
1452    for (i=0;i<st->lpcSize;i++)
1453       st->old_qlsp[i] = st->qlsp[i];
1454
1455    /* The next frame will not be the first (Duh!) */
1456    st->first = 0;
1457    st->count_lost=0;
1458    st->last_pitch = best_pitch;
1459    st->last_pitch_gain = .25*pitch_average;
1460    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1461    if (st->pitch_gain_buf_idx > 2) /* rollover */
1462       st->pitch_gain_buf_idx = 0;
1463
1464    st->last_ol_gain = ol_gain;
1465
1466    return 0;
1467 }
1468
1469 int nb_encoder_ctl(void *state, int request, void *ptr)
1470 {
1471    EncState *st;
1472    st=(EncState*)state;     
1473    switch(request)
1474    {
1475    case SPEEX_GET_FRAME_SIZE:
1476       (*(int*)ptr) = st->frameSize;
1477       break;
1478    case SPEEX_SET_LOW_MODE:
1479    case SPEEX_SET_MODE:
1480       st->submodeSelect = st->submodeID = (*(int*)ptr);
1481       break;
1482    case SPEEX_GET_LOW_MODE:
1483    case SPEEX_GET_MODE:
1484       (*(int*)ptr) = st->submodeID;
1485       break;
1486    case SPEEX_SET_VBR:
1487       st->vbr_enabled = (*(int*)ptr);
1488       break;
1489    case SPEEX_GET_VBR:
1490       (*(int*)ptr) = st->vbr_enabled;
1491       break;
1492    case SPEEX_SET_VAD:
1493       st->vad_enabled = (*(int*)ptr);
1494       break;
1495    case SPEEX_GET_VAD:
1496       (*(int*)ptr) = st->vad_enabled;
1497       break;
1498    case SPEEX_SET_DTX:
1499       st->dtx_enabled = (*(int*)ptr);
1500       break;
1501    case SPEEX_GET_DTX:
1502       (*(int*)ptr) = st->dtx_enabled;
1503       break;
1504    case SPEEX_SET_ABR:
1505       st->abr_enabled = (*(int*)ptr);
1506       st->vbr_enabled = 1;
1507       {
1508          int i=10, rate, target;
1509          float vbr_qual;
1510          target = (*(int*)ptr);
1511          while (i>=0)
1512          {
1513             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1514             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1515             if (rate <= target)
1516                break;
1517             i--;
1518          }
1519          vbr_qual=i;
1520          if (vbr_qual<0)
1521             vbr_qual=0;
1522          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1523          st->abr_count=0;
1524          st->abr_drift=0;
1525          st->abr_drift2=0;
1526       }
1527       
1528       break;
1529    case SPEEX_GET_ABR:
1530       (*(int*)ptr) = st->abr_enabled;
1531       break;
1532    case SPEEX_SET_VBR_QUALITY:
1533       st->vbr_quality = (*(float*)ptr);
1534       break;
1535    case SPEEX_GET_VBR_QUALITY:
1536       (*(float*)ptr) = st->vbr_quality;
1537       break;
1538    case SPEEX_SET_QUALITY:
1539       {
1540          int quality = (*(int*)ptr);
1541          if (quality < 0)
1542             quality = 0;
1543          if (quality > 10)
1544             quality = 10;
1545          st->submodeSelect = st->submodeID = ((SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1546       }
1547       break;
1548    case SPEEX_SET_COMPLEXITY:
1549       st->complexity = (*(int*)ptr);
1550       if (st->complexity<1)
1551          st->complexity=1;
1552       break;
1553    case SPEEX_GET_COMPLEXITY:
1554       (*(int*)ptr) = st->complexity;
1555       break;
1556    case SPEEX_SET_BITRATE:
1557       {
1558          int i=10, rate, target;
1559          target = (*(int*)ptr);
1560          while (i>=0)
1561          {
1562             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1563             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1564             if (rate <= target)
1565                break;
1566             i--;
1567          }
1568       }
1569       break;
1570    case SPEEX_GET_BITRATE:
1571       if (st->submodes[st->submodeID])
1572          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1573       else
1574          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1575       break;
1576    case SPEEX_SET_SAMPLING_RATE:
1577       st->sampling_rate = (*(int*)ptr);
1578       break;
1579    case SPEEX_GET_SAMPLING_RATE:
1580       (*(int*)ptr)=st->sampling_rate;
1581       break;
1582    case SPEEX_RESET_STATE:
1583       {
1584          int i;
1585          st->bounded_pitch = 1;
1586          st->first = 1;
1587          for (i=0;i<st->lpcSize;i++)
1588             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1589          for (i=0;i<st->lpcSize;i++)
1590             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1591          for (i=0;i<st->bufSize;i++)
1592             st->excBuf[i]=st->swBuf[i]=st->inBuf[i]=st->exc2Buf[i]=0;
1593       }
1594       break;
1595    case SPEEX_GET_PI_GAIN:
1596       {
1597          int i;
1598          float *g = (float*)ptr;
1599          for (i=0;i<st->nbSubframes;i++)
1600             g[i]=st->pi_gain[i];
1601       }
1602       break;
1603    case SPEEX_GET_EXC:
1604       {
1605          int i;
1606          float *e = (float*)ptr;
1607          for (i=0;i<st->frameSize;i++)
1608             e[i]=st->exc[i];
1609       }
1610       break;
1611    case SPEEX_GET_INNOV:
1612       {
1613          int i;
1614          float *e = (float*)ptr;
1615          for (i=0;i<st->frameSize;i++)
1616             e[i]=st->innov[i];
1617       }
1618       break;
1619    case SPEEX_GET_RELATIVE_QUALITY:
1620       (*(float*)ptr)=st->relative_quality;
1621       break;
1622    default:
1623       speex_warning_int("Unknown nb_ctl request: ", request);
1624       return -1;
1625    }
1626    return 0;
1627 }
1628
1629 int nb_decoder_ctl(void *state, int request, void *ptr)
1630 {
1631    DecState *st;
1632    st=(DecState*)state;
1633    switch(request)
1634    {
1635    case SPEEX_GET_LOW_MODE:
1636    case SPEEX_GET_MODE:
1637       (*(int*)ptr) = st->submodeID;
1638       break;
1639    case SPEEX_SET_ENH:
1640       st->lpc_enh_enabled = *((int*)ptr);
1641       break;
1642    case SPEEX_GET_ENH:
1643       *((int*)ptr) = st->lpc_enh_enabled;
1644       break;
1645    case SPEEX_GET_FRAME_SIZE:
1646       (*(int*)ptr) = st->frameSize;
1647       break;
1648    case SPEEX_GET_BITRATE:
1649       if (st->submodes[st->submodeID])
1650          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1651       else
1652          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1653       break;
1654    case SPEEX_SET_SAMPLING_RATE:
1655       st->sampling_rate = (*(int*)ptr);
1656       break;
1657    case SPEEX_GET_SAMPLING_RATE:
1658       (*(int*)ptr)=st->sampling_rate;
1659       break;
1660    case SPEEX_SET_HANDLER:
1661       {
1662          SpeexCallback *c = (SpeexCallback*)ptr;
1663          st->speex_callbacks[c->callback_id].func=c->func;
1664          st->speex_callbacks[c->callback_id].data=c->data;
1665          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1666       }
1667       break;
1668    case SPEEX_SET_USER_HANDLER:
1669       {
1670          SpeexCallback *c = (SpeexCallback*)ptr;
1671          st->user_callback.func=c->func;
1672          st->user_callback.data=c->data;
1673          st->user_callback.callback_id=c->callback_id;
1674       }
1675       break;
1676    case SPEEX_RESET_STATE:
1677       {
1678          int i;
1679          for (i=0;i<2*st->lpcSize;i++)
1680             st->mem_sp[i]=0;
1681          for (i=0;i<st->bufSize;i++)
1682             st->excBuf[i]=st->inBuf[i]=0;
1683       }
1684       break;
1685    case SPEEX_GET_PI_GAIN:
1686       {
1687          int i;
1688          float *g = (float*)ptr;
1689          for (i=0;i<st->nbSubframes;i++)
1690             g[i]=st->pi_gain[i];
1691       }
1692       break;
1693    case SPEEX_GET_EXC:
1694       {
1695          int i;
1696          float *e = (float*)ptr;
1697          for (i=0;i<st->frameSize;i++)
1698             e[i]=st->exc[i];
1699       }
1700       break;
1701    case SPEEX_GET_INNOV:
1702       {
1703          int i;
1704          float *e = (float*)ptr;
1705          for (i=0;i<st->frameSize;i++)
1706             e[i]=st->innov[i];
1707       }
1708       break;
1709    case SPEEX_GET_DTX_STATUS:
1710       *((int*)ptr) = st->dtx_enabled;
1711       break;
1712    default:
1713       speex_warning_int("Unknown nb_ctl request: ", request);
1714       return -1;
1715    }
1716    return 0;
1717 }