oops... another stupid bug
[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_enabled || 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=15;
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                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1059                speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1060                if (advance < 0)
1061                {
1062                   speex_warning ("Invalid wideband mode encountered: corrupted stream?");
1063                   return -2;
1064                } 
1065                advance -= (SB_SUBMODE_BITS+1);
1066                speex_bits_advance(bits, advance);
1067                wideband = speex_bits_unpack_unsigned(bits, 1);
1068                if (wideband)
1069                {
1070                   speex_warning ("More than to wideband layers found: corrupted stream?");
1071                   return -2;
1072                }
1073
1074             }
1075          }
1076
1077          /* FIXME: Check for overflow */
1078          m = speex_bits_unpack_unsigned(bits, 4);
1079          if (m==15) /* We found a terminator */
1080          {
1081             return -1;
1082          } else if (m==14) /* Speex in-band request */
1083          {
1084             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1085             if (ret)
1086                return ret;
1087          } else if (m==13) /* User in-band request */
1088          {
1089             int ret = st->user_callback.func(bits, state, st->user_callback.data);
1090             if (ret)
1091                return ret;
1092          } else if (m>8) /* Invalid mode */
1093          {
1094             speex_warning("Invalid mode encountered: corrupted stream?");
1095             return -2;
1096          }
1097       
1098       } while (m>8);
1099
1100       /* Get the sub-mode that was used */
1101       st->submodeID = m;
1102
1103    }
1104
1105    /* Shift all buffers by one frame */
1106    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1107    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1108
1109    /* If null mode (no transmission), just set a couple things to zero*/
1110    if (st->submodes[st->submodeID] == NULL)
1111    {
1112       float *lpc;
1113       lpc = PUSH(stack,11, float);
1114       bw_lpc(.93, st->interp_qlpc, lpc, 10);
1115       /*for (i=0;i<st->frameSize;i++)
1116         st->exc[i]=0;*/
1117       {
1118          float innov_gain=0;
1119          float pgain=st->last_pitch_gain;
1120          if (pgain>.6)
1121             pgain=.6;
1122          for (i=0;i<st->frameSize;i++)
1123             innov_gain += st->innov[i]*st->innov[i];
1124          innov_gain=sqrt(innov_gain/st->frameSize);
1125          for (i=0;i<st->frameSize;i++)
1126             st->exc[i]=0;
1127          speex_rand_vec(innov_gain, st->exc, st->frameSize);
1128       }
1129
1130
1131       st->first=1;
1132
1133       /* Final signal synthesis from excitation */
1134       iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
1135
1136       out[0] = st->frame[0] + st->preemph*st->pre_mem;
1137       for (i=1;i<st->frameSize;i++)
1138          out[i]=st->frame[i] + st->preemph*out[i-1];
1139       st->pre_mem=out[st->frameSize-1];
1140       st->count_lost=0;
1141       return 0;
1142    }
1143
1144    /* Unquantize LSPs */
1145    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1146
1147    /*Damp memory if a frame was lost and the LSP changed too much*/
1148    if (st->count_lost)
1149    {
1150       float lsp_dist=0, fact;
1151       for (i=0;i<st->lpcSize;i++)
1152          lsp_dist += fabs(st->old_qlsp[i] - st->qlsp[i]);
1153       fact = .6*exp(-.2*lsp_dist);
1154       for (i=0;i<2*st->lpcSize;i++)
1155          st->mem_sp[i] *= fact;
1156    }
1157
1158
1159    /* Handle first frame and lost-packet case */
1160    if (st->first || st->count_lost)
1161    {
1162       for (i=0;i<st->lpcSize;i++)
1163          st->old_qlsp[i] = st->qlsp[i];
1164    }
1165
1166    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1167    if (SUBMODE(lbr_pitch)!=-1)
1168    {
1169       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1170    } 
1171    
1172    if (SUBMODE(forced_pitch_gain))
1173    {
1174       int quant;
1175       quant = speex_bits_unpack_unsigned(bits, 4);
1176       ol_pitch_coef=0.066667*quant;
1177    }
1178    
1179    /* Get global excitation gain */
1180    {
1181       int qe;
1182       qe = speex_bits_unpack_unsigned(bits, 5);
1183       ol_gain = exp(qe/3.5);
1184    }
1185
1186    awk1=PUSH(stack, st->lpcSize+1, float);
1187    awk2=PUSH(stack, st->lpcSize+1, float);
1188    awk3=PUSH(stack, st->lpcSize+1, float);
1189
1190    if (st->submodeID==1)
1191    {
1192       int extra;
1193       extra = speex_bits_unpack_unsigned(bits, 4);
1194
1195       if (extra==15)
1196          st->dtx_enabled=1;
1197       else
1198          st->dtx_enabled=0;
1199    }
1200    if (st->submodeID>1)
1201       st->dtx_enabled=0;
1202
1203    /*Loop on subframes */
1204    for (sub=0;sub<st->nbSubframes;sub++)
1205    {
1206       int offset;
1207       float *sp, *exc, tmp;
1208
1209       /* Offset relative to start of frame */
1210       offset = st->subframeSize*sub;
1211       /* Original signal */
1212       sp=st->frame+offset;
1213       /* Excitation */
1214       exc=st->exc+offset;
1215       /* Excitation after post-filter*/
1216
1217       /* LSP interpolation (quantized and unquantized) */
1218       tmp = (1.0 + sub)/st->nbSubframes;
1219       for (i=0;i<st->lpcSize;i++)
1220          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
1221
1222       /* Make sure the LSP's are stable */
1223       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
1224
1225
1226       /* Compute interpolated LPCs (unquantized) */
1227       for (i=0;i<st->lpcSize;i++)
1228          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
1229       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
1230
1231       /* Compute enhanced synthesis filter */
1232       if (st->lpc_enh_enabled)
1233       {
1234          float r=.9;
1235          
1236          float k1,k2,k3;
1237          k1=SUBMODE(lpc_enh_k1);
1238          k2=SUBMODE(lpc_enh_k2);
1239          k3=(1-(1-r*k1)/(1-r*k2))/r;
1240          if (!st->lpc_enh_enabled)
1241          {
1242             k1=k2;
1243             k3=0;
1244          }
1245          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1246          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1247          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1248          
1249       }
1250
1251       /* Compute analysis filter at w=pi */
1252       tmp=1;
1253       st->pi_gain[sub]=0;
1254       for (i=0;i<=st->lpcSize;i++)
1255       {
1256          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
1257          tmp = -tmp;
1258       }
1259
1260       /* Reset excitation */
1261       for (i=0;i<st->subframeSize;i++)
1262          exc[i]=0;
1263
1264       /*Adaptive codebook contribution*/
1265       if (SUBMODE(ltp_unquant))
1266       {
1267          int pit_min, pit_max;
1268          /* Handle pitch constraints if any */
1269          if (SUBMODE(lbr_pitch) != -1)
1270          {
1271             int margin;
1272             margin = SUBMODE(lbr_pitch);
1273             if (margin)
1274             {
1275 /* GT - need optimization?
1276                if (ol_pitch < st->min_pitch+margin-1)
1277                   ol_pitch=st->min_pitch+margin-1;
1278                if (ol_pitch > st->max_pitch-margin)
1279                   ol_pitch=st->max_pitch-margin;
1280                pit_min = ol_pitch-margin+1;
1281                pit_max = ol_pitch+margin;
1282 */
1283                pit_min = ol_pitch-margin+1;
1284                if (pit_min < st->min_pitch)
1285                   pit_min = st->min_pitch;
1286                pit_max = ol_pitch+margin;
1287                if (pit_max > st->max_pitch)
1288                   pit_max = st->max_pitch;
1289             } else {
1290                pit_min = pit_max = ol_pitch;
1291             }
1292          } else {
1293             pit_min = st->min_pitch;
1294             pit_max = st->max_pitch;
1295          }
1296
1297          /* Pitch synthesis */
1298          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1299                               st->subframeSize, &pitch, &pitch_gain[0], bits, stack, st->count_lost, offset, st->last_pitch_gain);
1300          
1301          /* If we had lost frames, check energy of last received frame */
1302          if (st->count_lost && ol_gain < st->last_ol_gain)
1303          {
1304             float fact = ol_gain/(st->last_ol_gain+1);
1305             for (i=0;i<st->subframeSize;i++)
1306                exc[i]*=fact;
1307          }
1308
1309          tmp = fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1310          tmp = fabs(pitch_gain[1]);
1311          if (pitch_gain[0]>0)
1312             tmp += pitch_gain[0];
1313          else
1314             tmp -= .5*pitch_gain[0];
1315          if (pitch_gain[2]>0)
1316             tmp += pitch_gain[2];
1317          else
1318             tmp -= .5*pitch_gain[0];
1319
1320
1321          pitch_average += tmp;
1322          if (tmp>best_pitch_gain)
1323          {
1324             best_pitch = pitch;
1325             best_pitch_gain = tmp;
1326             /*            best_pitch_gain = tmp*.9;
1327                         if (best_pitch_gain>.85)
1328                         best_pitch_gain=.85;*/
1329          }
1330       } else {
1331          speex_error("No pitch prediction, what's wrong");
1332       }
1333       
1334       /* Unquantize the innovation */
1335       {
1336          int q_energy;
1337          float ener;
1338          float *innov;
1339          
1340          innov = st->innov+sub*st->subframeSize;
1341          for (i=0;i<st->subframeSize;i++)
1342             innov[i]=0;
1343
1344          /* Decode sub-frame gain correction */
1345          if (SUBMODE(have_subframe_gain)==3)
1346          {
1347             q_energy = speex_bits_unpack_unsigned(bits, 3);
1348             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1349          } else if (SUBMODE(have_subframe_gain)==1)
1350          {
1351             q_energy = speex_bits_unpack_unsigned(bits, 1);
1352             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1353          } else {
1354             ener = ol_gain;
1355          }
1356                   
1357          if (SUBMODE(innovation_unquant))
1358          {
1359             /*Fixed codebook contribution*/
1360             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1361          } else {
1362             speex_error("No fixed codebook");
1363          }
1364
1365          /* De-normalize innovation and update excitation */
1366          for (i=0;i<st->subframeSize;i++)
1367             innov[i]*=ener;
1368
1369          /*Vocoder mode*/
1370          if (st->submodeID==1) 
1371          {
1372             float g=ol_pitch_coef;
1373
1374             
1375             for (i=0;i<st->subframeSize;i++)
1376                exc[i]=0;
1377             while (st->voc_offset<st->subframeSize)
1378             {
1379                if (st->voc_offset>=0)
1380                   exc[st->voc_offset]=sqrt(1.0*ol_pitch);
1381                st->voc_offset+=ol_pitch;
1382             }
1383             st->voc_offset -= st->subframeSize;
1384
1385             g=.5+2*(g-.6);
1386             if (g<0)
1387                g=0;
1388             if (g>1)
1389                g=1;
1390             for (i=0;i<st->subframeSize;i++)
1391             {
1392                float exci=exc[i];
1393                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];
1394                st->voc_m1 = exci;
1395                st->voc_m2=innov[i];
1396                st->voc_mean = .95*st->voc_mean + .05*exc[i];
1397                exc[i]-=st->voc_mean;
1398             }
1399          } else {
1400             for (i=0;i<st->subframeSize;i++)
1401                exc[i]+=innov[i];
1402          }
1403          /* Decode second codebook (only for some modes) */
1404          if (SUBMODE(double_codebook))
1405          {
1406             char *tmp_stack=stack;
1407             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1408             for (i=0;i<st->subframeSize;i++)
1409                innov2[i]=0;
1410             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, tmp_stack);
1411             for (i=0;i<st->subframeSize;i++)
1412                innov2[i]*=ener*(1/2.2);
1413             for (i=0;i<st->subframeSize;i++)
1414                exc[i] += innov2[i];
1415          }
1416
1417       }
1418
1419       for (i=0;i<st->subframeSize;i++)
1420          sp[i]=exc[i];
1421
1422       /* Signal synthesis */
1423       if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0)
1424          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1425                               pitch, pitch_gain, SUBMODE(comb_gain), st->comb_mem);
1426       if (st->lpc_enh_enabled)
1427       {
1428          /* Use enhanced LPC filter */
1429          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1430                      st->mem_sp+st->lpcSize);
1431          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1432                      st->mem_sp);
1433       } else {
1434          /* Use regular filter */
1435          for (i=0;i<st->lpcSize;i++)
1436             st->mem_sp[st->lpcSize+i] = 0;
1437          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1438                      st->mem_sp);
1439       }
1440    }
1441    
1442    /*Copy output signal*/
1443    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1444    for (i=1;i<st->frameSize;i++)
1445      out[i]=st->frame[i] + st->preemph*out[i-1];
1446    st->pre_mem=out[st->frameSize-1];
1447
1448
1449    /* Store the LSPs for interpolation in the next frame */
1450    for (i=0;i<st->lpcSize;i++)
1451       st->old_qlsp[i] = st->qlsp[i];
1452
1453    /* The next frame will not be the first (Duh!) */
1454    st->first = 0;
1455    st->count_lost=0;
1456    st->last_pitch = best_pitch;
1457    st->last_pitch_gain = .25*pitch_average;
1458    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1459    if (st->pitch_gain_buf_idx > 2) /* rollover */
1460       st->pitch_gain_buf_idx = 0;
1461
1462    st->last_ol_gain = ol_gain;
1463
1464    return 0;
1465 }
1466
1467 int nb_encoder_ctl(void *state, int request, void *ptr)
1468 {
1469    EncState *st;
1470    st=(EncState*)state;     
1471    switch(request)
1472    {
1473    case SPEEX_GET_FRAME_SIZE:
1474       (*(int*)ptr) = st->frameSize;
1475       break;
1476    case SPEEX_SET_LOW_MODE:
1477    case SPEEX_SET_MODE:
1478       st->submodeSelect = st->submodeID = (*(int*)ptr);
1479       break;
1480    case SPEEX_GET_LOW_MODE:
1481    case SPEEX_GET_MODE:
1482       (*(int*)ptr) = st->submodeID;
1483       break;
1484    case SPEEX_SET_VBR:
1485       st->vbr_enabled = (*(int*)ptr);
1486       break;
1487    case SPEEX_GET_VBR:
1488       (*(int*)ptr) = st->vbr_enabled;
1489       break;
1490    case SPEEX_SET_VAD:
1491       st->vad_enabled = (*(int*)ptr);
1492       break;
1493    case SPEEX_GET_VAD:
1494       (*(int*)ptr) = st->vad_enabled;
1495       break;
1496    case SPEEX_SET_DTX:
1497       st->dtx_enabled = (*(int*)ptr);
1498       break;
1499    case SPEEX_GET_DTX:
1500       (*(int*)ptr) = st->dtx_enabled;
1501       break;
1502    case SPEEX_SET_ABR:
1503       st->abr_enabled = (*(int*)ptr);
1504       st->vbr_enabled = 1;
1505       {
1506          int i=10, rate, target;
1507          float vbr_qual;
1508          target = (*(int*)ptr);
1509          while (i>=0)
1510          {
1511             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1512             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1513             if (rate <= target)
1514                break;
1515             i--;
1516          }
1517          vbr_qual=i;
1518          if (vbr_qual<0)
1519             vbr_qual=0;
1520          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1521          st->abr_count=0;
1522          st->abr_drift=0;
1523          st->abr_drift2=0;
1524       }
1525       
1526       break;
1527    case SPEEX_GET_ABR:
1528       (*(int*)ptr) = st->abr_enabled;
1529       break;
1530    case SPEEX_SET_VBR_QUALITY:
1531       st->vbr_quality = (*(float*)ptr);
1532       break;
1533    case SPEEX_GET_VBR_QUALITY:
1534       (*(float*)ptr) = st->vbr_quality;
1535       break;
1536    case SPEEX_SET_QUALITY:
1537       {
1538          int quality = (*(int*)ptr);
1539          if (quality < 0)
1540             quality = 0;
1541          if (quality > 10)
1542             quality = 10;
1543          st->submodeSelect = st->submodeID = ((SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1544       }
1545       break;
1546    case SPEEX_SET_COMPLEXITY:
1547       st->complexity = (*(int*)ptr);
1548       if (st->complexity<1)
1549          st->complexity=1;
1550       break;
1551    case SPEEX_GET_COMPLEXITY:
1552       (*(int*)ptr) = st->complexity;
1553       break;
1554    case SPEEX_SET_BITRATE:
1555       {
1556          int i=10, rate, target;
1557          target = (*(int*)ptr);
1558          while (i>=0)
1559          {
1560             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1561             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1562             if (rate <= target)
1563                break;
1564             i--;
1565          }
1566       }
1567       break;
1568    case SPEEX_GET_BITRATE:
1569       if (st->submodes[st->submodeID])
1570          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1571       else
1572          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1573       break;
1574    case SPEEX_SET_SAMPLING_RATE:
1575       st->sampling_rate = (*(int*)ptr);
1576       break;
1577    case SPEEX_GET_SAMPLING_RATE:
1578       (*(int*)ptr)=st->sampling_rate;
1579       break;
1580    case SPEEX_RESET_STATE:
1581       {
1582          int i;
1583          st->bounded_pitch = 1;
1584          st->first = 1;
1585          for (i=0;i<st->lpcSize;i++)
1586             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1587          for (i=0;i<st->lpcSize;i++)
1588             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1589          for (i=0;i<st->bufSize;i++)
1590             st->excBuf[i]=st->swBuf[i]=st->inBuf[i]=st->exc2Buf[i]=0;
1591       }
1592       break;
1593    case SPEEX_GET_PI_GAIN:
1594       {
1595          int i;
1596          float *g = (float*)ptr;
1597          for (i=0;i<st->nbSubframes;i++)
1598             g[i]=st->pi_gain[i];
1599       }
1600       break;
1601    case SPEEX_GET_EXC:
1602       {
1603          int i;
1604          float *e = (float*)ptr;
1605          for (i=0;i<st->frameSize;i++)
1606             e[i]=st->exc[i];
1607       }
1608       break;
1609    case SPEEX_GET_INNOV:
1610       {
1611          int i;
1612          float *e = (float*)ptr;
1613          for (i=0;i<st->frameSize;i++)
1614             e[i]=st->innov[i];
1615       }
1616       break;
1617    case SPEEX_GET_RELATIVE_QUALITY:
1618       (*(float*)ptr)=st->relative_quality;
1619       break;
1620    default:
1621       speex_warning_int("Unknown nb_ctl request: ", request);
1622       return -1;
1623    }
1624    return 0;
1625 }
1626
1627 int nb_decoder_ctl(void *state, int request, void *ptr)
1628 {
1629    DecState *st;
1630    st=(DecState*)state;
1631    switch(request)
1632    {
1633    case SPEEX_GET_LOW_MODE:
1634    case SPEEX_GET_MODE:
1635       (*(int*)ptr) = st->submodeID;
1636       break;
1637    case SPEEX_SET_ENH:
1638       st->lpc_enh_enabled = *((int*)ptr);
1639       break;
1640    case SPEEX_GET_ENH:
1641       *((int*)ptr) = st->lpc_enh_enabled;
1642       break;
1643    case SPEEX_GET_FRAME_SIZE:
1644       (*(int*)ptr) = st->frameSize;
1645       break;
1646    case SPEEX_GET_BITRATE:
1647       if (st->submodes[st->submodeID])
1648          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1649       else
1650          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1651       break;
1652    case SPEEX_SET_SAMPLING_RATE:
1653       st->sampling_rate = (*(int*)ptr);
1654       break;
1655    case SPEEX_GET_SAMPLING_RATE:
1656       (*(int*)ptr)=st->sampling_rate;
1657       break;
1658    case SPEEX_SET_HANDLER:
1659       {
1660          SpeexCallback *c = (SpeexCallback*)ptr;
1661          st->speex_callbacks[c->callback_id].func=c->func;
1662          st->speex_callbacks[c->callback_id].data=c->data;
1663          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1664       }
1665       break;
1666    case SPEEX_SET_USER_HANDLER:
1667       {
1668          SpeexCallback *c = (SpeexCallback*)ptr;
1669          st->user_callback.func=c->func;
1670          st->user_callback.data=c->data;
1671          st->user_callback.callback_id=c->callback_id;
1672       }
1673       break;
1674    case SPEEX_RESET_STATE:
1675       {
1676          int i;
1677          for (i=0;i<2*st->lpcSize;i++)
1678             st->mem_sp[i]=0;
1679          for (i=0;i<st->bufSize;i++)
1680             st->excBuf[i]=st->inBuf[i]=0;
1681       }
1682       break;
1683    case SPEEX_GET_PI_GAIN:
1684       {
1685          int i;
1686          float *g = (float*)ptr;
1687          for (i=0;i<st->nbSubframes;i++)
1688             g[i]=st->pi_gain[i];
1689       }
1690       break;
1691    case SPEEX_GET_EXC:
1692       {
1693          int i;
1694          float *e = (float*)ptr;
1695          for (i=0;i<st->frameSize;i++)
1696             e[i]=st->exc[i];
1697       }
1698       break;
1699    case SPEEX_GET_INNOV:
1700       {
1701          int i;
1702          float *e = (float*)ptr;
1703          for (i=0;i<st->frameSize;i++)
1704             e[i]=st->innov[i];
1705       }
1706       break;
1707    case SPEEX_GET_DTX_STATUS:
1708       *((int*)ptr) = st->dtx_enabled;
1709       break;
1710    default:
1711       speex_warning_int("Unknown nb_ctl request: ", request);
1712       return -1;
1713    }
1714    return 0;
1715 }