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