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