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