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