Preparing for beta 3, cleaned up the mode/bit-rate code in speexdec,
[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 <stdlib.h>
33 #include <stdio.h>
34 #include <math.h>
35 #include "nb_celp.h"
36 #include "lpc.h"
37 #include "lsp.h"
38 #include "ltp.h"
39 #include "quant_lsp.h"
40 #include "cb_search.h"
41 #include "filters.h"
42 #include "stack_alloc.h"
43 #include "vq.h"
44 #include "speex_bits.h"
45 #include "vbr.h"
46 #include "misc.h"
47 #include "speex_callbacks.h"
48
49 #ifdef SLOW_TRIG
50 #include "math_approx.h"
51 #define cos speex_cos
52 #endif
53
54 extern int training_weight;
55 #ifndef M_PI
56 #define M_PI           3.14159265358979323846  /* pi */
57 #endif
58
59 #define SUBMODE(x) st->submodes[st->submodeID]->x
60
61 float exc_gain_quant_scal3[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
62
63 float exc_gain_quant_scal1[2]={-0.35, 0.05};
64
65 #define sqr(x) ((x)*(x))
66
67 #ifndef min
68 #define min(a,b) ((a) < (b) ? (a) : (b))
69 #endif
70
71 void *nb_encoder_init(SpeexMode *m)
72 {
73    EncState *st;
74    SpeexNBMode *mode;
75    int i;
76
77    mode=(SpeexNBMode *)m->mode;
78    st = (EncState*)speex_alloc(sizeof(EncState));
79    st->mode=m;
80    /* Codec parameters, should eventually have several "modes"*/
81    st->frameSize = mode->frameSize;
82    st->windowSize = st->frameSize*3/2;
83    st->nbSubframes=mode->frameSize/mode->subframeSize;
84    st->subframeSize=mode->subframeSize;
85    st->lpcSize = mode->lpcSize;
86    st->bufSize = mode->bufSize;
87    st->gamma1=mode->gamma1;
88    st->gamma2=mode->gamma2;
89    st->min_pitch=mode->pitchStart;
90    st->max_pitch=mode->pitchEnd;
91    st->lag_factor=mode->lag_factor;
92    st->lpc_floor = mode->lpc_floor;
93    st->preemph = mode->preemph;
94   
95    st->submodes=mode->submodes;
96    st->submodeID=mode->defaultSubmode;
97    st->pre_mem=0;
98    st->pre_mem2=0;
99    st->bounded_pitch = 0;
100
101    /* Allocating input buffer */
102    st->inBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
103    st->frame = st->inBuf + st->bufSize - st->windowSize;
104    /* Allocating excitation buffer */
105    st->excBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
106    st->exc = st->excBuf + st->bufSize - st->windowSize;
107    st->swBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
108    st->sw = st->swBuf + st->bufSize - st->windowSize;
109
110    st->exc2Buf = (float*)speex_alloc(st->bufSize*sizeof(float));
111    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
112
113    st->innov = (float*)speex_alloc(st->frameSize*sizeof(float));
114
115    /* Asymetric "pseudo-Hamming" window */
116    {
117       int part1, part2;
118       part1 = st->subframeSize*7/2;
119       part2 = st->subframeSize*5/2;
120       st->window = (float*)speex_alloc(st->windowSize*sizeof(float));
121       for (i=0;i<part1;i++)
122          st->window[i]=.54-.46*cos(M_PI*i/part1);
123       for (i=0;i<part2;i++)
124          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
125    }
126    /* Create the window for autocorrelation (lag-windowing) */
127    st->lagWindow = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
128    for (i=0;i<st->lpcSize+1;i++)
129       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
130
131    st->autocorr = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
132
133    st->stack = (float*)speex_alloc(20000*sizeof(float));
134
135    st->buf2 = (float*)speex_alloc(st->windowSize*sizeof(float));
136
137    st->lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
138    st->interp_lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
139    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
140    st->bw_lpc1 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
141    st->bw_lpc2 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
142
143    st->lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
144    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
145    st->old_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
146    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
147    st->interp_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
148    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
149    st->rc = (float*)speex_alloc(st->lpcSize*sizeof(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 = (float*)speex_alloc(st->lpcSize*sizeof(float));
157    st->mem_sw = (float*)speex_alloc(st->lpcSize*sizeof(float));
158    st->mem_sw_whole = (float*)speex_alloc(st->lpcSize*sizeof(float));
159    st->mem_exc = (float*)speex_alloc(st->lpcSize*sizeof(float));
160
161    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
162
163    st->pitch = (int*)speex_alloc(st->nbSubframes*sizeof(int));
164
165    if (1) {
166       st->vbr = (VBRState*)speex_alloc(sizeof(VBRState));
167       vbr_init(st->vbr);
168       st->vbr_quality = 8;
169       st->vbr_enabled = 0;
170    } else {
171       st->vbr = 0;
172    }
173    st->complexity=2;
174
175    return st;
176 }
177
178 void nb_encoder_destroy(void *state)
179 {
180    EncState *st=(EncState *)state;
181    /* Free all allocated memory */
182    speex_free(st->inBuf);
183    speex_free(st->excBuf);
184    speex_free(st->swBuf);
185    speex_free(st->exc2Buf);
186    speex_free(st->innov);
187    speex_free((float*)st->stack);
188
189    speex_free(st->window);
190    speex_free(st->buf2);
191    speex_free(st->lpc);
192    speex_free(st->interp_lpc);
193    speex_free(st->interp_qlpc);
194    
195    speex_free(st->bw_lpc1);
196    speex_free(st->bw_lpc2);
197    speex_free(st->autocorr);
198    speex_free(st->lagWindow);
199    speex_free(st->lsp);
200    speex_free(st->qlsp);
201    speex_free(st->old_lsp);
202    speex_free(st->interp_lsp);
203    speex_free(st->old_qlsp);
204    speex_free(st->interp_qlsp);
205    speex_free(st->rc);
206
207    speex_free(st->mem_sp);
208    speex_free(st->mem_sw);
209    speex_free(st->mem_sw_whole);
210    speex_free(st->mem_exc);
211    speex_free(st->pi_gain);
212    speex_free(st->pitch);
213
214    vbr_destroy(st->vbr);
215    speex_free(st->vbr);
216
217    /*Free state memory... should be last*/
218    speex_free((float*)st);
219 }
220
221 void nb_encode(void *state, float *in, SpeexBits *bits)
222 {
223    EncState *st;
224    int i, sub, roots;
225    int ol_pitch;
226    float ol_pitch_coef;
227    float ol_gain;
228    float delta_qual=0;
229    float *res, *target, *mem;
230    void *stack;
231    float *syn_resp;
232
233    st=(EncState *)state;
234    stack=st->stack;
235
236    /* Copy new data in input buffer */
237    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
238    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
239    for (i=1;i<st->frameSize;i++)
240       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
241    st->pre_mem = in[st->frameSize-1];
242
243    /* Move signals 1 frame towards the past */
244    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
245    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
246    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
247
248
249    /* Window for analysis */
250    for (i=0;i<st->windowSize;i++)
251       st->buf2[i] = st->frame[i] * st->window[i];
252
253    /* Compute auto-correlation */
254    _spx_autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
255
256    st->autocorr[0] += 10;        /* prevents NANs */
257    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
258
259    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
260    for (i=0;i<st->lpcSize+1;i++)
261       st->autocorr[i] *= st->lagWindow[i];
262
263    /* Levinson-Durbin */
264    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
265    st->lpc[0]=1;
266
267    /* LPC to LSPs (x-domain) transform */
268    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
269    /* Check if we found all the roots */
270    if (roots==st->lpcSize)
271    {
272       /* LSP x-domain to angle domain*/
273       for (i=0;i<st->lpcSize;i++)
274          st->lsp[i] = acos(st->lsp[i]);
275    } else {
276       /* Search again if we can afford it */
277       if (st->complexity>1)
278          roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.05, stack);
279       if (roots==st->lpcSize) 
280       {
281          /* LSP x-domain to angle domain*/
282          for (i=0;i<st->lpcSize;i++)
283             st->lsp[i] = acos(st->lsp[i]);
284       } else {
285          /*If we can't find all LSP's, do some damage control and use previous filter*/
286          for (i=0;i<st->lpcSize;i++)
287          {
288             st->lsp[i]=st->old_lsp[i];
289          }
290       }
291    }
292
293    /* LSP Quantization */
294    if (st->first)
295    {
296       for (i=0;i<st->lpcSize;i++)
297          st->old_lsp[i] = st->lsp[i];
298    }
299
300    if (0) {
301       float dd=0;
302       for (i=0;i<st->lpcSize;i++)
303          dd += fabs(st->old_lsp[i] - st->lsp[i]);
304       printf ("lspdist = %f\n", dd);
305    }
306
307    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
308    {
309       for (i=0;i<st->lpcSize;i++)
310          st->interp_lsp[i] = .5*st->old_lsp[i] + .5*st->lsp[i];
311
312       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
313
314       /* Compute interpolated LPCs (unquantized) for whole frame*/
315       for (i=0;i<st->lpcSize;i++)
316          st->interp_lsp[i] = cos(st->interp_lsp[i]);
317       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
318
319
320       /*Open-loop pitch*/
321       if (!st->submodes[st->submodeID] || st->vbr_enabled || SUBMODE(forced_pitch_gain) ||
322           SUBMODE(lbr_pitch) != -1)
323       {
324          int nol_pitch[4];
325          float nol_pitch_coef[4];
326          
327          bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
328          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
329          
330          filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);
331
332          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
333                                nol_pitch, nol_pitch_coef, 4, stack);
334          ol_pitch=nol_pitch[0];
335          ol_pitch_coef = nol_pitch_coef[0];
336          /*Try to remove pitch multiples*/
337          for (i=1;i<4;i++)
338          {
339             if ((nol_pitch_coef[i] > .85*ol_pitch_coef) && 
340                 (fabs(2*nol_pitch[i]-ol_pitch)<=2 || fabs(3*nol_pitch[i]-ol_pitch)<=4 || 
341                  fabs(4*nol_pitch[i]-ol_pitch)<=6 || fabs(5*nol_pitch[i]-ol_pitch)<=8))
342             {
343                /*ol_pitch_coef=nol_pitch_coef[i];*/
344                ol_pitch = nol_pitch[i];
345             }
346          }
347          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
348          /*printf ("ol_pitch: %d %f\n", ol_pitch, ol_pitch_coef);*/
349       } else {
350          ol_pitch=0;
351          ol_pitch_coef=0;
352       }
353       /*Compute "real" excitation*/
354       fir_mem2(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc);
355
356       /* Compute open-loop excitation gain */
357       ol_gain=0;
358       for (i=0;i<st->frameSize;i++)
359          ol_gain += st->exc[i]*st->exc[i];
360       
361       ol_gain=sqrt(1+ol_gain/st->frameSize);
362    }
363
364    /*Experimental VBR stuff*/
365    if (st->vbr)
366    {
367       delta_qual = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
368       /*if (delta_qual<0)*/
369          delta_qual*=.1*(3+st->vbr_quality);
370       if (st->vbr_enabled) 
371       {
372          int qual = (int)floor(st->vbr_quality+delta_qual+.5);
373          if (qual<1 && delta_qual>-3.5)
374             qual=1;
375          if (qual<0)
376             qual=0;
377          if (qual>10)
378             qual=10;
379          if (qual==10 && st->vbr_quality<10)
380             qual=9;
381          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &qual);
382       }
383    }
384    /*printf ("VBR quality = %f\n", vbr_qual);*/
385
386    /* First, transmit a zero for narrowband */
387    speex_bits_pack(bits, 0, 1);
388
389    /* Transmit the sub-mode we use for this frame */
390    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
391
392
393    /* If null mode (no transmission), just set a couple things to zero*/
394    if (st->submodes[st->submodeID] == NULL)
395    {
396       for (i=0;i<st->frameSize;i++)
397          st->exc[i]=st->exc2[i]=st->sw[i]=0;
398
399       for (i=0;i<st->lpcSize;i++)
400          st->mem_sw[i]=0;
401       st->first=1;
402
403       /* Final signal synthesis from excitation */
404       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
405
406       in[0] = st->frame[0] + st->preemph*st->pre_mem2;
407       for (i=1;i<st->frameSize;i++)
408          in[i]=st->frame[i] + st->preemph*in[i-1];
409       st->pre_mem2=in[st->frameSize-1];
410
411       return;
412
413    }
414
415    /*Quantize LSPs*/
416 #if 1 /*0 for unquantized*/
417    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
418 #else
419    for (i=0;i<st->lpcSize;i++)
420      st->qlsp[i]=st->lsp[i];
421 #endif
422
423    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
424    if (SUBMODE(lbr_pitch)!=-1)
425    {
426       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
427    } 
428    
429    if (SUBMODE(forced_pitch_gain))
430    {
431       int quant;
432       quant = (int)floor(.5+15*ol_pitch_coef);
433       if (quant>15)
434          quant=0;
435       if (quant<0)
436          quant=0;
437       speex_bits_pack(bits, quant, 4);
438       ol_pitch_coef=0.066667*quant;
439    }
440    
441    
442    /*Quantize and transmit open-loop excitation gain*/
443    {
444       int qe = (int)(floor(3.5*log(ol_gain)));
445       if (qe<0)
446          qe=0;
447       if (qe>31)
448          qe=31;
449       ol_gain = exp(qe/3.5);
450       speex_bits_pack(bits, qe, 5);
451    }
452
453    /* Special case for first frame */
454    if (st->first)
455    {
456       for (i=0;i<st->lpcSize;i++)
457          st->old_qlsp[i] = st->qlsp[i];
458    }
459
460    /* Filter response */
461    res = PUSH(stack, st->subframeSize, float);
462    /* Target signal */
463    target = PUSH(stack, st->subframeSize, float);
464    syn_resp = PUSH(stack, st->subframeSize, float);
465    mem = PUSH(stack, st->lpcSize, float);
466
467    /* Loop on sub-frames */
468    for (sub=0;sub<st->nbSubframes;sub++)
469    {
470       float tmp;
471       int   offset;
472       float *sp, *sw, *exc, *exc2;
473       int pitch;
474
475       /* Offset relative to start of frame */
476       offset = st->subframeSize*sub;
477       /* Original signal */
478       sp=st->frame+offset;
479       /* Excitation */
480       exc=st->exc+offset;
481       /* Weighted signal */
482       sw=st->sw+offset;
483
484       exc2=st->exc2+offset;
485
486
487       /* LSP interpolation (quantized and unquantized) */
488       tmp = (1.0 + sub)/st->nbSubframes;
489       for (i=0;i<st->lpcSize;i++)
490          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
491       for (i=0;i<st->lpcSize;i++)
492          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
493
494       /* Make sure the filters are stable */
495       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
496       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
497
498       /* Compute interpolated LPCs (quantized and unquantized) */
499       for (i=0;i<st->lpcSize;i++)
500          st->interp_lsp[i] = cos(st->interp_lsp[i]);
501       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
502
503       for (i=0;i<st->lpcSize;i++)
504          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
505       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
506
507       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
508       tmp=1;
509       st->pi_gain[sub]=0;
510       for (i=0;i<=st->lpcSize;i++)
511       {
512          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
513          tmp = -tmp;
514       }
515      
516
517       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
518       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
519       if (st->gamma2>=0)
520          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
521       else
522       {
523          st->bw_lpc2[0]=1;
524          st->bw_lpc2[1]=-st->preemph;
525          for (i=2;i<=st->lpcSize;i++)
526             st->bw_lpc2[i]=0;
527       }
528
529       /* Compute impulse response of A(z/g1) / ( A(z)*A(z/g2) )*/
530       for (i=0;i<st->subframeSize;i++)
531          exc[i]=0;
532       exc[0]=1;
533       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
534
535       /* Reset excitation */
536       for (i=0;i<st->subframeSize;i++)
537          exc[i]=0;
538       for (i=0;i<st->subframeSize;i++)
539          exc2[i]=0;
540
541       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
542       for (i=0;i<st->lpcSize;i++)
543          mem[i]=st->mem_sp[i];
544       iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
545       
546       for (i=0;i<st->lpcSize;i++)
547          mem[i]=st->mem_sw[i];
548       filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
549       
550       /* Compute weighted signal */
551       for (i=0;i<st->lpcSize;i++)
552          mem[i]=st->mem_sw[i];
553       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
554       
555       /* Compute target signal */
556       for (i=0;i<st->subframeSize;i++)
557          target[i]=sw[i]-res[i];
558
559       for (i=0;i<st->subframeSize;i++)
560          exc[i]=exc2[i]=0;
561
562       /* If we have a long-term predictor (otherwise, something's wrong) */
563       if (SUBMODE(ltp_quant))
564       {
565          int pit_min, pit_max;
566          /* Long-term prediction */
567          if (SUBMODE(lbr_pitch) != -1)
568          {
569             /* Low bit-rate pitch handling */
570             int margin;
571             margin = SUBMODE(lbr_pitch);
572             if (margin)
573             {
574                if (ol_pitch < st->min_pitch+margin-1)
575                   ol_pitch=st->min_pitch+margin-1;
576                if (ol_pitch > st->max_pitch-margin)
577                   ol_pitch=st->max_pitch-margin;
578                pit_min = ol_pitch-margin+1;
579                pit_max = ol_pitch+margin;
580             } else {
581                pit_min=pit_max=ol_pitch;
582             }
583          } else {
584             pit_min = st->min_pitch;
585             pit_max = st->max_pitch;
586          }
587          
588          /* Force pitch to use only the current frame if needed */
589          if (st->bounded_pitch && pit_max>offset)
590             pit_max=offset;
591
592          /* Perform pitch search */
593          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
594                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
595                                     st->lpcSize, st->subframeSize, bits, stack, 
596                                     exc2, syn_resp, st->complexity);
597
598          st->pitch[sub]=pitch;
599       } else {
600          fprintf (stderr, "No pitch prediction, what's wrong\n");
601       }
602
603       /* Update target for adaptive codebook contribution */
604       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, stack);
605       for (i=0;i<st->subframeSize;i++)
606          target[i]-=res[i];
607
608
609       /* Quantization of innovation */
610       {
611          float *innov;
612          float ener=0, ener_1;
613
614          innov = st->innov+sub*st->subframeSize;
615          for (i=0;i<st->subframeSize;i++)
616             innov[i]=0;
617          
618          residue_percep_zero(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize, stack);
619          for (i=0;i<st->subframeSize;i++)
620             ener+=st->buf2[i]*st->buf2[i];
621          ener=sqrt(.1+ener/st->subframeSize);
622
623          
624          ener /= ol_gain;
625
626          /* Calculate gain correction for the sub-frame (if any) */
627          if (SUBMODE(have_subframe_gain)) 
628          {
629             int qe;
630             ener=log(ener);
631             if (SUBMODE(have_subframe_gain)==3)
632             {
633                qe = vq_index(&ener, exc_gain_quant_scal3, 1, 8);
634                speex_bits_pack(bits, qe, 3);
635                ener=exc_gain_quant_scal3[qe];
636             } else {
637                qe = vq_index(&ener, exc_gain_quant_scal1, 1, 2);
638                speex_bits_pack(bits, qe, 1);
639                ener=exc_gain_quant_scal1[qe];               
640             }
641             ener=exp(ener);
642          } else {
643             ener=1;
644          }
645
646          ener*=ol_gain;
647
648          ener_1 = 1/ener;
649
650          if (0) {
651             int start=rand()%35;
652             printf ("norm_exc: ");
653             for (i=start;i<start+5;i++)
654                printf ("%f ", ener_1*st->buf2[i]);
655             printf ("\n");
656          }
657          
658          /* Normalize innovation */
659          for (i=0;i<st->subframeSize;i++)
660             target[i]*=ener_1;
661          
662          /* Quantize innovation */
663          if (SUBMODE(innovation_quant))
664          {
665             /* Codebook search */
666             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
667                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
668                                       innov, syn_resp, bits, stack, st->complexity);
669             
670             /* De-normalize innovation and update excitation */
671             for (i=0;i<st->subframeSize;i++)
672                innov[i]*=ener;
673             for (i=0;i<st->subframeSize;i++)
674                exc[i] += innov[i];
675          } else {
676             fprintf(stderr, "No fixed codebook\n");
677          }
678
679          /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
680          if (SUBMODE(double_codebook)) {
681             void *tmp_stack=stack;
682             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
683             for (i=0;i<st->subframeSize;i++)
684                innov2[i]=0;
685             for (i=0;i<st->subframeSize;i++)
686                target[i]*=2.2;
687             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
688                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
689                                       innov2, syn_resp, bits, tmp_stack, st->complexity);
690             for (i=0;i<st->subframeSize;i++)
691                innov2[i]*=ener*(1/2.2);
692             for (i=0;i<st->subframeSize;i++)
693                exc[i] += innov2[i];
694          }
695
696          for (i=0;i<st->subframeSize;i++)
697             target[i]*=ener;
698
699       }
700
701       /*Keep the previous memory*/
702       for (i=0;i<st->lpcSize;i++)
703          mem[i]=st->mem_sp[i];
704       /* Final signal synthesis from excitation */
705       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
706
707       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
708       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
709       for (i=0;i<st->subframeSize;i++)
710          exc2[i]=exc[i];
711    }
712
713    /* Store the LSPs for interpolation in the next frame */
714    for (i=0;i<st->lpcSize;i++)
715       st->old_lsp[i] = st->lsp[i];
716    for (i=0;i<st->lpcSize;i++)
717       st->old_qlsp[i] = st->qlsp[i];
718
719    /* The next frame will not be the first (Duh!) */
720    st->first = 0;
721
722    /* Replace input by synthesized speech */
723    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
724    for (i=1;i<st->frameSize;i++)
725      in[i]=st->frame[i] + st->preemph*in[i-1];
726    st->pre_mem2=in[st->frameSize-1];
727
728    if (SUBMODE(innovation_quant) == noise_codebook_quant)
729       st->bounded_pitch = 1;
730    else
731       st->bounded_pitch = 0;
732 }
733
734
735 void *nb_decoder_init(SpeexMode *m)
736 {
737    DecState *st;
738    SpeexNBMode *mode;
739    int i;
740
741    mode=(SpeexNBMode*)m->mode;
742    st = (DecState *)speex_alloc(sizeof(DecState));
743    st->mode=m;
744
745    st->first=1;
746    /* Codec parameters, should eventually have several "modes"*/
747    st->frameSize = mode->frameSize;
748    st->windowSize = st->frameSize*3/2;
749    st->nbSubframes=mode->frameSize/mode->subframeSize;
750    st->subframeSize=mode->subframeSize;
751    st->lpcSize = mode->lpcSize;
752    st->bufSize = mode->bufSize;
753    st->gamma1=mode->gamma1;
754    st->gamma2=mode->gamma2;
755    st->min_pitch=mode->pitchStart;
756    st->max_pitch=mode->pitchEnd;
757    st->preemph = mode->preemph;
758
759    st->submodes=mode->submodes;
760    st->submodeID=mode->defaultSubmode;
761
762    st->pre_mem=0;
763    st->lpc_enh_enabled=0;
764
765    st->stack = speex_alloc(20000*sizeof(float));
766
767    st->inBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
768    st->frame = st->inBuf + st->bufSize - st->windowSize;
769    st->excBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
770    st->exc = st->excBuf + st->bufSize - st->windowSize;
771    for (i=0;i<st->bufSize;i++)
772       st->inBuf[i]=0;
773    for (i=0;i<st->bufSize;i++)
774       st->excBuf[i]=0;
775    st->innov = (float*)speex_alloc(st->frameSize*sizeof(float));
776
777    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
778    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
779    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
780    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
781    st->mem_sp = (float*)speex_alloc(5*st->lpcSize*sizeof(float));
782
783    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
784    st->last_pitch = 40;
785    st->count_lost=0;
786
787
788    st->user_callback.func = &speex_default_user_handler;
789    st->user_callback.data = NULL;
790    for (i=0;i<16;i++)
791       st->speex_callbacks[i].func = NULL;
792
793    return st;
794 }
795
796 void nb_decoder_destroy(void *state)
797 {
798    DecState *st;
799    st=(DecState*)state;
800    speex_free(st->inBuf);
801    speex_free(st->excBuf);
802    speex_free(st->innov);
803    speex_free(st->interp_qlpc);
804    speex_free(st->qlsp);
805    speex_free(st->old_qlsp);
806    speex_free(st->interp_qlsp);
807    speex_free(st->stack);
808    speex_free(st->mem_sp);
809    speex_free(st->pi_gain);
810    
811    speex_free(state);
812 }
813
814 static void nb_decode_lost(DecState *st, float *out, void *stack)
815 {
816    int i, sub;
817    float *awk1, *awk2, *awk3;
818    /*float exc_ener=0,g;*/
819    /* Shift all buffers by one frame */
820    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
821    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
822
823    awk1=PUSH(stack, (st->lpcSize+1), float);
824    awk2=PUSH(stack, (st->lpcSize+1), float);
825    awk3=PUSH(stack, (st->lpcSize+1), float);
826
827    for (sub=0;sub<st->nbSubframes;sub++)
828    {
829       int offset;
830       float *sp, *exc;
831       /* Offset relative to start of frame */
832       offset = st->subframeSize*sub;
833       /* Original signal */
834       sp=st->frame+offset;
835       /* Excitation */
836       exc=st->exc+offset;
837       /* Excitation after post-filter*/
838
839       /* Calculate perceptually enhanced LPC filter */
840       if (st->lpc_enh_enabled)
841       {
842          float r=.9;
843          
844          float k1,k2,k3;
845          k1=SUBMODE(lpc_enh_k1);
846          k2=SUBMODE(lpc_enh_k2);
847          k3=(1-(1-r*k1)/(1-r*k2))/r;
848          if (!st->lpc_enh_enabled)
849          {
850             k1=k2;
851             k3=0;
852          }
853          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
854          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
855          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
856          
857       }
858         
859       /* Make up a plausible excitation */
860       /* THIS CAN BE IMPROVED */
861       for (i=0;i<st->subframeSize;i++)
862       {
863          exc[i]=st->last_pitch_gain*exc[i-st->last_pitch] + 
864          .8*st->innov[i+offset];
865       }
866
867       for (i=0;i<st->subframeSize;i++)
868          sp[i]=exc[i];
869       
870       /* Signal synthesis */
871       if (st->lpc_enh_enabled)
872       {
873          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
874                      st->mem_sp+st->lpcSize);
875          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
876                      st->mem_sp);
877       } else {
878          for (i=0;i<st->lpcSize;i++)
879             st->mem_sp[st->lpcSize+i] = 0;
880          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
881                      st->mem_sp);
882       }
883   
884    }
885
886    out[0] = st->frame[0] + st->preemph*st->pre_mem;
887    for (i=1;i<st->frameSize;i++)
888       out[i]=st->frame[i] + st->preemph*out[i-1];
889    st->pre_mem=out[st->frameSize-1];
890    
891    st->first = 0;
892    st->count_lost++;
893 }
894
895
896 int nb_decode(void *state, SpeexBits *bits, float *out)
897 {
898    DecState *st;
899    int i, sub;
900    int pitch;
901    float pitch_gain[3];
902    float ol_gain;
903    int ol_pitch=0;
904    float ol_pitch_coef=0;
905    int best_pitch=40;
906    float best_pitch_gain=-1;
907    int wideband;
908    int m;
909    void *stack;
910    float *awk1, *awk2, *awk3;
911    st=(DecState*)state;
912    stack=st->stack;
913
914    /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
915    if (!bits)
916    {
917       nb_decode_lost(st, out, stack);
918       return 0;
919    }
920
921    /* Search for next narrwoband block (handle requests, skip wideband blocks) */
922    do {
923       wideband = speex_bits_unpack_unsigned(bits, 1);
924       if (wideband) /* Skip wideband block (for compatibility) */
925       {
926          int submode;
927          int advance;
928          submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
929          advance = submode;
930          speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
931          advance -= (SB_SUBMODE_BITS+1);
932          speex_bits_advance(bits, advance);
933          wideband = speex_bits_unpack_unsigned(bits, 1);
934          if (wideband)
935          {
936             fprintf (stderr, "Corrupted stream?\n");
937          }
938       }
939
940       m = speex_bits_unpack_unsigned(bits, 4);
941       if (m==15) /* We found a terminator */
942       {
943          return -1;
944       } else if (m==14) /* Speex in-band request */
945       {
946          int ret = speex_inband_handler(bits, st->speex_callbacks, state);
947          if (ret)
948             return ret;
949       } else if (m==13) /* User in-band request */
950       {
951          int ret = st->user_callback.func(bits, state, st->user_callback.data);
952          if (ret)
953             return ret;
954       } else if (m>7) /* Invalid mode */
955       {
956          return -2;
957       }
958       
959    } while (m>7);
960
961    /* Get the sub-mode that was used */
962    st->submodeID = m;
963
964    /* Shift all buffers by one frame */
965    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
966    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
967
968    /* If null mode (no transmission), just set a couple things to zero*/
969    if (st->submodes[st->submodeID] == NULL)
970    {
971       for (i=0;i<st->frameSize;i++)
972          st->exc[i]=0;
973       st->first=1;
974       
975       /* Final signal synthesis from excitation */
976       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
977
978       out[0] = st->frame[0] + st->preemph*st->pre_mem;
979       for (i=1;i<st->frameSize;i++)
980          out[i]=st->frame[i] + st->preemph*out[i-1];
981       st->pre_mem=out[st->frameSize-1];
982       st->count_lost=0;
983       return 0;
984    }
985
986    /* Unquantize LSPs */
987    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
988
989    /* Handle first frame and lost-packet case */
990    if (st->first || st->count_lost)
991    {
992       for (i=0;i<st->lpcSize;i++)
993          st->old_qlsp[i] = st->qlsp[i];
994    }
995
996    /* Get open-loop pitch estimation for low bit-rate pitch coding */
997    if (SUBMODE(lbr_pitch)!=-1)
998    {
999       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1000    } 
1001    
1002    if (SUBMODE(forced_pitch_gain))
1003    {
1004       int quant;
1005       quant = speex_bits_unpack_unsigned(bits, 4);
1006       ol_pitch_coef=0.066667*quant;
1007       /*fprintf (stderr, "unquant pitch coef: %f\n", ol_pitch_coef);*/
1008    }
1009    
1010    /* Get global excitation gain */
1011    {
1012       int qe;
1013       qe = speex_bits_unpack_unsigned(bits, 5);
1014       ol_gain = exp(qe/3.5);
1015       /*printf ("decode_ol_gain: %f\n", ol_gain);*/
1016    }
1017
1018    awk1=PUSH(stack, st->lpcSize+1, float);
1019    awk2=PUSH(stack, st->lpcSize+1, float);
1020    awk3=PUSH(stack, st->lpcSize+1, float);
1021
1022    /*Loop on subframes */
1023    for (sub=0;sub<st->nbSubframes;sub++)
1024    {
1025       int offset;
1026       float *sp, *exc, tmp;
1027
1028       /* Offset relative to start of frame */
1029       offset = st->subframeSize*sub;
1030       /* Original signal */
1031       sp=st->frame+offset;
1032       /* Excitation */
1033       exc=st->exc+offset;
1034       /* Excitation after post-filter*/
1035
1036       /* LSP interpolation (quantized and unquantized) */
1037       tmp = (1.0 + sub)/st->nbSubframes;
1038       for (i=0;i<st->lpcSize;i++)
1039          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
1040
1041       /* Make sure the LSP's are stable */
1042       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
1043
1044
1045       /* Compute interpolated LPCs (unquantized) */
1046       for (i=0;i<st->lpcSize;i++)
1047          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
1048       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
1049
1050       /* Compute enhanced synthesis filter */
1051       if (st->lpc_enh_enabled)
1052       {
1053          float r=.9;
1054          
1055          float k1,k2,k3;
1056          k1=SUBMODE(lpc_enh_k1);
1057          k2=SUBMODE(lpc_enh_k2);
1058          k3=(1-(1-r*k1)/(1-r*k2))/r;
1059          if (!st->lpc_enh_enabled)
1060          {
1061             k1=k2;
1062             k3=0;
1063          }
1064          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1065          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1066          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1067          
1068       }
1069
1070       /* Compute analysis filter at w=pi */
1071       tmp=1;
1072       st->pi_gain[sub]=0;
1073       for (i=0;i<=st->lpcSize;i++)
1074       {
1075          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
1076          tmp = -tmp;
1077       }
1078
1079       /* Reset excitation */
1080       for (i=0;i<st->subframeSize;i++)
1081          exc[i]=0;
1082
1083       /*Adaptive codebook contribution*/
1084       if (SUBMODE(ltp_unquant))
1085       {
1086          int pit_min, pit_max;
1087          /* Handle pitch constraints if any */
1088          if (SUBMODE(lbr_pitch) != -1)
1089          {
1090             int margin;
1091             margin = SUBMODE(lbr_pitch);
1092             if (margin)
1093             {
1094                if (ol_pitch < st->min_pitch+margin-1)
1095                   ol_pitch=st->min_pitch+margin-1;
1096                if (ol_pitch > st->max_pitch-margin)
1097                   ol_pitch=st->max_pitch-margin;
1098                pit_min = ol_pitch-margin+1;
1099                pit_max = ol_pitch+margin;
1100             } else {
1101                pit_min=pit_max=ol_pitch;
1102             }
1103          } else {
1104             pit_min = st->min_pitch;
1105             pit_max = st->max_pitch;
1106          }
1107
1108          /* Pitch synthesis */
1109          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1110                               st->subframeSize, &pitch, &pitch_gain[0], bits, stack, st->count_lost);
1111          
1112          tmp = (pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1113          if (tmp>best_pitch_gain)
1114          {
1115             best_pitch = pitch;
1116             best_pitch_gain = tmp*.9;
1117             if (best_pitch_gain>.85)
1118                best_pitch_gain=.85;
1119          }
1120       } else {
1121          fprintf (stderr, "No pitch prediction, what's wrong\n");
1122       }
1123       
1124       /* Unquantize the innovation */
1125       {
1126          int q_energy;
1127          float ener;
1128          float *innov;
1129          
1130          innov = st->innov+sub*st->subframeSize;
1131          for (i=0;i<st->subframeSize;i++)
1132             innov[i]=0;
1133
1134          /* Decode sub-frame gain correction */
1135          if (SUBMODE(have_subframe_gain)==3)
1136          {
1137             q_energy = speex_bits_unpack_unsigned(bits, 3);
1138             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1139          } else if (SUBMODE(have_subframe_gain)==1)
1140          {
1141             q_energy = speex_bits_unpack_unsigned(bits, 1);
1142             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1143          } else {
1144             ener = ol_gain;
1145          }
1146          
1147          /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
1148          
1149          if (SUBMODE(innovation_unquant))
1150          {
1151             /*Fixed codebook contribution*/
1152             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1153          } else {
1154             fprintf(stderr, "No fixed codebook\n");
1155          }
1156
1157          /* De-normalize innovation and update excitation */
1158          for (i=0;i<st->subframeSize;i++)
1159             innov[i]*=ener;
1160          for (i=0;i<st->subframeSize;i++)
1161             exc[i]+=innov[i];
1162
1163          /* Decode second codebook (only for some modes) */
1164          if (SUBMODE(double_codebook))
1165          {
1166             void *tmp_stack=stack;
1167             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1168             for (i=0;i<st->subframeSize;i++)
1169                innov2[i]=0;
1170             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, tmp_stack);
1171             for (i=0;i<st->subframeSize;i++)
1172                innov2[i]*=ener*(1/2.2);
1173             for (i=0;i<st->subframeSize;i++)
1174                exc[i] += innov2[i];
1175          }
1176
1177       }
1178
1179       for (i=0;i<st->subframeSize;i++)
1180          sp[i]=exc[i];
1181
1182       /* Signal synthesis */
1183       if (st->lpc_enh_enabled && SUBMODE(comb_gain>0))
1184          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1185                               pitch, pitch_gain, .5);
1186       if (st->lpc_enh_enabled)
1187       {
1188          /* Use enhanced LPC filter */
1189          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1190                      st->mem_sp+st->lpcSize);
1191          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1192                      st->mem_sp);
1193       } else {
1194          /* Use regular filter */
1195          for (i=0;i<st->lpcSize;i++)
1196             st->mem_sp[st->lpcSize+i] = 0;
1197          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1198                      st->mem_sp);
1199       }
1200    }
1201    
1202    /*Copy output signal*/
1203    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1204    for (i=1;i<st->frameSize;i++)
1205      out[i]=st->frame[i] + st->preemph*out[i-1];
1206    st->pre_mem=out[st->frameSize-1];
1207
1208
1209    /* Store the LSPs for interpolation in the next frame */
1210    for (i=0;i<st->lpcSize;i++)
1211       st->old_qlsp[i] = st->qlsp[i];
1212
1213    /* The next frame will not be the first (Duh!) */
1214    st->first = 0;
1215    st->count_lost=0;
1216    st->last_pitch = best_pitch;
1217    st->last_pitch_gain = best_pitch_gain;
1218
1219    return 0;
1220 }
1221
1222 void nb_encoder_ctl(void *state, int request, void *ptr)
1223 {
1224    EncState *st;
1225    st=(EncState*)state;     
1226    switch(request)
1227    {
1228    case SPEEX_GET_FRAME_SIZE:
1229       (*(int*)ptr) = st->frameSize;
1230       break;
1231    case SPEEX_SET_LOW_MODE:
1232    case SPEEX_SET_MODE:
1233       st->submodeID = (*(int*)ptr);
1234       break;
1235    case SPEEX_GET_LOW_MODE:
1236    case SPEEX_GET_MODE:
1237       (*(int*)ptr) = st->submodeID;
1238       break;
1239    case SPEEX_SET_VBR:
1240       st->vbr_enabled = (*(int*)ptr);
1241       break;
1242    case SPEEX_GET_VBR:
1243       (*(int*)ptr) = st->vbr_enabled;
1244       break;
1245    case SPEEX_SET_VBR_QUALITY:
1246       st->vbr_quality = (*(float*)ptr);
1247       break;
1248    case SPEEX_GET_VBR_QUALITY:
1249       (*(float*)ptr) = st->vbr_quality;
1250       break;
1251    case SPEEX_SET_QUALITY:
1252       {
1253          int quality = (*(int*)ptr);
1254          if (quality<=0)
1255             st->submodeID = 0;
1256          else if (quality<=1)
1257             st->submodeID = 1;
1258          else if (quality<=2)
1259             st->submodeID = 2;
1260          else if (quality<=4)
1261             st->submodeID = 3;
1262          else if (quality<=6)
1263             st->submodeID = 4;
1264          else if (quality<=8)
1265             st->submodeID = 5;
1266          else if (quality<=9)
1267             st->submodeID = 6;
1268          else if (quality<=10)
1269             st->submodeID = 7;
1270          else
1271             fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);
1272       }
1273       break;
1274    case SPEEX_SET_COMPLEXITY:
1275       st->complexity = (*(int*)ptr);
1276       if (st->complexity<1)
1277          st->complexity=1;
1278       break;
1279    case SPEEX_GET_COMPLEXITY:
1280       (*(int*)ptr) = st->complexity;
1281       break;
1282    case SPEEX_GET_BITRATE:
1283       if (st->submodes[st->submodeID])
1284          (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
1285       else
1286          (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
1287       break;
1288    case SPEEX_GET_PI_GAIN:
1289       {
1290          int i;
1291          float *g = (float*)ptr;
1292          for (i=0;i<st->nbSubframes;i++)
1293             g[i]=st->pi_gain[i];
1294       }
1295       break;
1296    case SPEEX_GET_EXC:
1297       {
1298          int i;
1299          float *e = (float*)ptr;
1300          for (i=0;i<st->frameSize;i++)
1301             e[i]=st->exc[i];
1302       }
1303       break;
1304    case SPEEX_GET_INNOV:
1305       {
1306          int i;
1307          float *e = (float*)ptr;
1308          for (i=0;i<st->frameSize;i++)
1309             e[i]=st->innov[i];
1310       }
1311       break;
1312    default:
1313       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1314    }
1315 }
1316
1317 void nb_decoder_ctl(void *state, int request, void *ptr)
1318 {
1319    DecState *st;
1320    st=(DecState*)state;
1321    switch(request)
1322    {
1323    case SPEEX_SET_ENH:
1324       st->lpc_enh_enabled = *((int*)ptr);
1325       break;
1326    case SPEEX_GET_ENH:
1327       *((int*)ptr) = st->lpc_enh_enabled;
1328       break;
1329    case SPEEX_GET_FRAME_SIZE:
1330       (*(int*)ptr) = st->frameSize;
1331       break;
1332    case SPEEX_GET_BITRATE:
1333       if (st->submodes[st->submodeID])
1334          (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
1335       else
1336          (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
1337       break;
1338    case SPEEX_SET_HANDLER:
1339       {
1340          SpeexCallback *c = (SpeexCallback*)ptr;
1341          st->speex_callbacks[c->callback_id].func=c->func;
1342          st->speex_callbacks[c->callback_id].data=c->data;
1343          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1344       }
1345       break;
1346    case SPEEX_SET_USER_HANDLER:
1347       {
1348          SpeexCallback *c = (SpeexCallback*)ptr;
1349          st->user_callback.func=c->func;
1350          st->user_callback.data=c->data;
1351          st->user_callback.callback_id=c->callback_id;
1352       }
1353       break;
1354    case SPEEX_GET_PI_GAIN:
1355       {
1356          int i;
1357          float *g = (float*)ptr;
1358          for (i=0;i<st->nbSubframes;i++)
1359             g[i]=st->pi_gain[i];
1360       }
1361       break;
1362    case SPEEX_GET_EXC:
1363       {
1364          int i;
1365          float *e = (float*)ptr;
1366          for (i=0;i<st->frameSize;i++)
1367             e[i]=st->exc[i];
1368       }
1369       break;
1370    case SPEEX_GET_INNOV:
1371       {
1372          int i;
1373          float *e = (float*)ptr;
1374          for (i=0;i<st->frameSize;i++)
1375             e[i]=st->innov[i];
1376       }
1377       break;
1378    default:
1379       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1380    }
1381 }