Many improvements (hopefully) to packet loss concealing, part of it from a
[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->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
799    st->pitch_gain_buf_idx = 0;
800
801    st->sampling_rate=8000;
802    st->last_ol_gain = 0;
803
804    st->user_callback.func = &speex_default_user_handler;
805    st->user_callback.data = NULL;
806    for (i=0;i<16;i++)
807       st->speex_callbacks[i].func = NULL;
808
809    return st;
810 }
811
812 void nb_decoder_destroy(void *state)
813 {
814    DecState *st;
815    st=(DecState*)state;
816    speex_free(st->inBuf);
817    speex_free(st->excBuf);
818    speex_free(st->innov);
819    speex_free(st->interp_qlpc);
820    speex_free(st->qlsp);
821    speex_free(st->old_qlsp);
822    speex_free(st->interp_qlsp);
823    speex_free(st->stack);
824    speex_free(st->mem_sp);
825    speex_free(st->pi_gain);
826    
827    speex_free(state);
828 }
829
830 /* 2.5 branches*/
831 #define median3(a, b, c)        ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
832
833 /*static float pitch_gain_pattern[4] = { 0.95, 0.9, 0.8, 0.4 };
834  */
835 static void nb_decode_lost(DecState *st, float *out, void *stack)
836 {
837    int i, sub;
838    float *awk1, *awk2, *awk3;
839    float pitch_gain, fact, gain_med;
840
841    /*if (st->count_lost < 4)
842       fact = pitch_gain_pattern[st->count_lost];
843    else
844       fact = 0.2;
845    */
846    fact = exp(-.04*st->count_lost*st->count_lost);
847    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
848    if (gain_med < st->last_pitch_gain)
849       st->last_pitch_gain = gain_med;
850    
851    pitch_gain = st->last_pitch_gain;
852    if (pitch_gain>.95)
853       pitch_gain=.95;
854
855    pitch_gain *= fact;
856    
857 #ifdef DEBUG
858    printf ("recovered unquantized pitch: %d, gain: %f (fact=%f, buf=[%f %f %f], med=%f)\n", st->last_pitch, pitch_gain, fact, st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2], gain_med);
859 #endif
860
861    /* Shift all buffers by one frame */
862    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
863    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
864
865    awk1=PUSH(stack, (st->lpcSize+1), float);
866    awk2=PUSH(stack, (st->lpcSize+1), float);
867    awk3=PUSH(stack, (st->lpcSize+1), float);
868
869    for (sub=0;sub<st->nbSubframes;sub++)
870    {
871       int offset;
872       float *sp, *exc;
873       /* Offset relative to start of frame */
874       offset = st->subframeSize*sub;
875       /* Original signal */
876       sp=st->frame+offset;
877       /* Excitation */
878       exc=st->exc+offset;
879       /* Excitation after post-filter*/
880
881       /* Calculate perceptually enhanced LPC filter */
882       if (st->lpc_enh_enabled)
883       {
884          float r=.9;
885          
886          float k1,k2,k3;
887          k1=SUBMODE(lpc_enh_k1);
888          k2=SUBMODE(lpc_enh_k2);
889          k3=(1-(1-r*k1)/(1-r*k2))/r;
890          if (!st->lpc_enh_enabled)
891          {
892             k1=k2;
893             k3=0;
894          }
895          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
896          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
897          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
898       }
899         
900       /* Make up a plausible excitation */
901       /* THIS CAN BE IMPROVED */
902       /*if (pitch_gain>.95)
903         pitch_gain=.95;*/
904       /*printf ("%f\n", pitch_gain);*/
905       {
906          float innov_gain=0;
907          for (i=0;i<st->frameSize;i++)
908             innov_gain += st->innov[i]*st->innov[i];
909          innov_gain=sqrt(innov_gain/160);
910       for (i=0;i<st->subframeSize;i++)
911       {
912 #if 0
913          exc[i] = pitch_gain * exc[i - st->last_pitch] + fact*sqrt(1-pitch_gain)*st->innov[i+offset];
914          /*Just so it give the same lost packets as with if 0*/
915          rand();
916 #else
917          /*exc[i]=pitch_gain*exc[i-st->last_pitch] +  fact*st->innov[i+offset];*/
918          exc[i]=pitch_gain*exc[i-st->last_pitch] + 
919          fact*sqrt(1-pitch_gain)*3*innov_gain*((((float)rand())/RAND_MAX)-.5);
920 #endif
921       }
922       }
923       for (i=0;i<st->subframeSize;i++)
924          sp[i]=exc[i];
925       
926       /* Signal synthesis */
927       if (st->lpc_enh_enabled)
928       {
929          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
930                      st->mem_sp+st->lpcSize);
931          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
932                      st->mem_sp);
933       } else {
934          for (i=0;i<st->lpcSize;i++)
935             st->mem_sp[st->lpcSize+i] = 0;
936          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
937                      st->mem_sp);
938       }      
939    }
940
941    out[0] = st->frame[0] + st->preemph*st->pre_mem;
942    for (i=1;i<st->frameSize;i++)
943       out[i]=st->frame[i] + st->preemph*out[i-1];
944    st->pre_mem=out[st->frameSize-1];
945    
946    st->first = 0;
947    st->count_lost++;
948    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = pitch_gain;
949    if (st->pitch_gain_buf_idx > 2) /* rollover */
950       st->pitch_gain_buf_idx = 0;
951 }
952
953 int nb_decode(void *state, SpeexBits *bits, float *out)
954 {
955    DecState *st;
956    int i, sub;
957    int pitch;
958    float pitch_gain[3];
959    float ol_gain;
960    int ol_pitch=0;
961    float ol_pitch_coef=0;
962    int best_pitch=40;
963    float best_pitch_gain=0;
964    int wideband;
965    int m;
966    void *stack;
967    float *awk1, *awk2, *awk3;
968    float pitch_average=0;
969
970    st=(DecState*)state;
971    stack=st->stack;
972
973    /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
974    if (!bits)
975    {
976       nb_decode_lost(st, out, stack);
977       return 0;
978    }
979
980    /* Search for next narrwoband block (handle requests, skip wideband blocks) */
981    do {
982       wideband = speex_bits_unpack_unsigned(bits, 1);
983       if (wideband) /* Skip wideband block (for compatibility) */
984       {
985          int submode;
986          int advance;
987          advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
988          speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
989          advance -= (SB_SUBMODE_BITS+1);
990          speex_bits_advance(bits, advance);
991          wideband = speex_bits_unpack_unsigned(bits, 1);
992          if (wideband)
993          {
994             fprintf (stderr, "Corrupted stream?\n");
995          }
996       }
997
998       m = speex_bits_unpack_unsigned(bits, 4);
999       if (m==15) /* We found a terminator */
1000       {
1001          return -1;
1002       } else if (m==14) /* Speex in-band request */
1003       {
1004          int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1005          if (ret)
1006             return ret;
1007       } else if (m==13) /* User in-band request */
1008       {
1009          int ret = st->user_callback.func(bits, state, st->user_callback.data);
1010          if (ret)
1011             return ret;
1012       } else if (m>7) /* Invalid mode */
1013       {
1014          return -2;
1015       }
1016       
1017    } while (m>7);
1018
1019    /* Get the sub-mode that was used */
1020    st->submodeID = m;
1021
1022    /* Shift all buffers by one frame */
1023    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1024    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1025
1026    /* If null mode (no transmission), just set a couple things to zero*/
1027    if (st->submodes[st->submodeID] == NULL)
1028    {
1029       for (i=0;i<st->frameSize;i++)
1030          st->exc[i]=0;
1031       st->first=1;
1032
1033       /* Final signal synthesis from excitation */
1034       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
1035
1036       out[0] = st->frame[0] + st->preemph*st->pre_mem;
1037       for (i=1;i<st->frameSize;i++)
1038          out[i]=st->frame[i] + st->preemph*out[i-1];
1039       st->pre_mem=out[st->frameSize-1];
1040       st->count_lost=0;
1041       return 0;
1042    }
1043
1044    /* Unquantize LSPs */
1045    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1046
1047    /*Damp memory if a frame was lost and the LSP changed too much*/
1048    if (st->count_lost)
1049    {
1050       float lsp_dist=0, fact;
1051       for (i=0;i<st->lpcSize;i++)
1052          lsp_dist += fabs(st->old_qlsp[i] - st->qlsp[i]);
1053       fact = .6*exp(-.2*lsp_dist);
1054       for (i=0;i<2*st->lpcSize;i++)
1055          st->mem_sp[i] *= fact;
1056    }
1057
1058
1059    /* Handle first frame and lost-packet case */
1060    if (st->first || st->count_lost)
1061    {
1062       for (i=0;i<st->lpcSize;i++)
1063          st->old_qlsp[i] = st->qlsp[i];
1064    }
1065
1066    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1067    if (SUBMODE(lbr_pitch)!=-1)
1068    {
1069       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1070    } 
1071    
1072    if (SUBMODE(forced_pitch_gain))
1073    {
1074       int quant;
1075       quant = speex_bits_unpack_unsigned(bits, 4);
1076       ol_pitch_coef=0.066667*quant;
1077       /*fprintf (stderr, "unquant pitch coef: %f\n", ol_pitch_coef);*/
1078    }
1079    
1080    /* Get global excitation gain */
1081    {
1082       int qe;
1083       qe = speex_bits_unpack_unsigned(bits, 5);
1084       ol_gain = exp(qe/3.5);
1085       /*printf ("decode_ol_gain: %f\n", ol_gain);*/
1086    }
1087
1088    awk1=PUSH(stack, st->lpcSize+1, float);
1089    awk2=PUSH(stack, st->lpcSize+1, float);
1090    awk3=PUSH(stack, st->lpcSize+1, float);
1091
1092    /*Loop on subframes */
1093    for (sub=0;sub<st->nbSubframes;sub++)
1094    {
1095       int offset;
1096       float *sp, *exc, tmp;
1097
1098       /* Offset relative to start of frame */
1099       offset = st->subframeSize*sub;
1100       /* Original signal */
1101       sp=st->frame+offset;
1102       /* Excitation */
1103       exc=st->exc+offset;
1104       /* Excitation after post-filter*/
1105
1106       /* LSP interpolation (quantized and unquantized) */
1107       tmp = (1.0 + sub)/st->nbSubframes;
1108       for (i=0;i<st->lpcSize;i++)
1109          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
1110
1111       /* Make sure the LSP's are stable */
1112       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
1113
1114
1115       /* Compute interpolated LPCs (unquantized) */
1116       for (i=0;i<st->lpcSize;i++)
1117          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
1118       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
1119
1120       /* Compute enhanced synthesis filter */
1121       if (st->lpc_enh_enabled)
1122       {
1123          float r=.9;
1124          
1125          float k1,k2,k3;
1126          k1=SUBMODE(lpc_enh_k1);
1127          k2=SUBMODE(lpc_enh_k2);
1128          k3=(1-(1-r*k1)/(1-r*k2))/r;
1129          if (!st->lpc_enh_enabled)
1130          {
1131             k1=k2;
1132             k3=0;
1133          }
1134          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1135          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1136          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1137          
1138       }
1139
1140       /* Compute analysis filter at w=pi */
1141       tmp=1;
1142       st->pi_gain[sub]=0;
1143       for (i=0;i<=st->lpcSize;i++)
1144       {
1145          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
1146          tmp = -tmp;
1147       }
1148
1149       /* Reset excitation */
1150       for (i=0;i<st->subframeSize;i++)
1151          exc[i]=0;
1152
1153       /*Adaptive codebook contribution*/
1154       if (SUBMODE(ltp_unquant))
1155       {
1156          int pit_min, pit_max;
1157          /* Handle pitch constraints if any */
1158          if (SUBMODE(lbr_pitch) != -1)
1159          {
1160             int margin;
1161             margin = SUBMODE(lbr_pitch);
1162             if (margin)
1163             {
1164 /* GT - need optimization?
1165                if (ol_pitch < st->min_pitch+margin-1)
1166                   ol_pitch=st->min_pitch+margin-1;
1167                if (ol_pitch > st->max_pitch-margin)
1168                   ol_pitch=st->max_pitch-margin;
1169                pit_min = ol_pitch-margin+1;
1170                pit_max = ol_pitch+margin;
1171 */
1172                pit_min = ol_pitch-margin+1;
1173                if (pit_min < st->min_pitch)
1174                   pit_min = st->min_pitch;
1175                pit_max = ol_pitch+margin;
1176                if (pit_max > st->max_pitch)
1177                   pit_max = st->max_pitch;
1178             } else {
1179                pit_min = pit_max = ol_pitch;
1180             }
1181          } else {
1182             pit_min = st->min_pitch;
1183             pit_max = st->max_pitch;
1184          }
1185
1186          /* Pitch synthesis */
1187          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1188                               st->subframeSize, &pitch, &pitch_gain[0], bits, stack, st->count_lost, offset, st->last_pitch_gain);
1189          
1190          /* If we had lost frames, check energy of last received frame */
1191          if (st->count_lost && ol_gain < st->last_ol_gain)
1192          {
1193             float fact = ol_gain/(st->last_ol_gain+1);
1194             for (i=0;i<st->subframeSize;i++)
1195                exc[i]*=fact;
1196          }
1197
1198          tmp = fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1199          tmp = fabs(pitch_gain[1]);
1200          if (pitch_gain[0]>0)
1201             tmp += pitch_gain[0];
1202          else
1203             tmp -= .5*pitch_gain[0];
1204          if (pitch_gain[2]>0)
1205             tmp += pitch_gain[2];
1206          else
1207             tmp -= .5*pitch_gain[0];
1208
1209
1210          pitch_average += tmp;
1211          if (tmp>best_pitch_gain)
1212          {
1213             best_pitch = pitch;
1214             best_pitch_gain = tmp;
1215             /*            best_pitch_gain = tmp*.9;
1216                         if (best_pitch_gain>.85)
1217                         best_pitch_gain=.85;*/
1218          }
1219       } else {
1220          fprintf (stderr, "No pitch prediction, what's wrong\n");
1221       }
1222       
1223       /* Unquantize the innovation */
1224       {
1225          int q_energy;
1226          float ener;
1227          float *innov;
1228          
1229          innov = st->innov+sub*st->subframeSize;
1230          for (i=0;i<st->subframeSize;i++)
1231             innov[i]=0;
1232
1233          /* Decode sub-frame gain correction */
1234          if (SUBMODE(have_subframe_gain)==3)
1235          {
1236             q_energy = speex_bits_unpack_unsigned(bits, 3);
1237             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1238          } else if (SUBMODE(have_subframe_gain)==1)
1239          {
1240             q_energy = speex_bits_unpack_unsigned(bits, 1);
1241             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1242          } else {
1243             ener = ol_gain;
1244          }
1245          
1246          /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
1247          
1248          if (SUBMODE(innovation_unquant))
1249          {
1250             /*Fixed codebook contribution*/
1251             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1252          } else {
1253             fprintf(stderr, "No fixed codebook\n");
1254          }
1255
1256          /* De-normalize innovation and update excitation */
1257          for (i=0;i<st->subframeSize;i++)
1258             innov[i]*=ener;
1259          for (i=0;i<st->subframeSize;i++)
1260             exc[i]+=innov[i];
1261
1262          /* Decode second codebook (only for some modes) */
1263          if (SUBMODE(double_codebook))
1264          {
1265             void *tmp_stack=stack;
1266             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1267             for (i=0;i<st->subframeSize;i++)
1268                innov2[i]=0;
1269             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, tmp_stack);
1270             for (i=0;i<st->subframeSize;i++)
1271                innov2[i]*=ener*(1/2.2);
1272             for (i=0;i<st->subframeSize;i++)
1273                exc[i] += innov2[i];
1274          }
1275
1276       }
1277
1278       for (i=0;i<st->subframeSize;i++)
1279          sp[i]=exc[i];
1280
1281       /* Signal synthesis */
1282       if (st->lpc_enh_enabled && SUBMODE(comb_gain>0))
1283          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1284                               pitch, pitch_gain, .5);
1285       if (st->lpc_enh_enabled)
1286       {
1287          /* Use enhanced LPC filter */
1288          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1289                      st->mem_sp+st->lpcSize);
1290          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1291                      st->mem_sp);
1292       } else {
1293          /* Use regular filter */
1294          for (i=0;i<st->lpcSize;i++)
1295             st->mem_sp[st->lpcSize+i] = 0;
1296          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1297                      st->mem_sp);
1298       }
1299    }
1300    
1301    /*Copy output signal*/
1302    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1303    for (i=1;i<st->frameSize;i++)
1304      out[i]=st->frame[i] + st->preemph*out[i-1];
1305    st->pre_mem=out[st->frameSize-1];
1306
1307
1308    /* Store the LSPs for interpolation in the next frame */
1309    for (i=0;i<st->lpcSize;i++)
1310       st->old_qlsp[i] = st->qlsp[i];
1311
1312    /* The next frame will not be the first (Duh!) */
1313    st->first = 0;
1314    st->count_lost=0;
1315    st->last_pitch = best_pitch;
1316    st->last_pitch_gain = .25*pitch_average;
1317    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1318    if (st->pitch_gain_buf_idx > 2) /* rollover */
1319       st->pitch_gain_buf_idx = 0;
1320
1321    st->last_ol_gain = ol_gain;
1322
1323
1324 #ifdef DEBUG
1325 printf ("last pitch: {%d, %f} buf=[%f %f %f]\n", st->last_pitch, st->last_pitch_gain, st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
1326 #endif
1327
1328    return 0;
1329 }
1330
1331 void nb_encoder_ctl(void *state, int request, void *ptr)
1332 {
1333    EncState *st;
1334    st=(EncState*)state;     
1335    switch(request)
1336    {
1337    case SPEEX_GET_FRAME_SIZE:
1338       (*(int*)ptr) = st->frameSize;
1339       break;
1340    case SPEEX_SET_LOW_MODE:
1341    case SPEEX_SET_MODE:
1342       st->submodeID = (*(int*)ptr);
1343       break;
1344    case SPEEX_GET_LOW_MODE:
1345    case SPEEX_GET_MODE:
1346       (*(int*)ptr) = st->submodeID;
1347       break;
1348    case SPEEX_SET_VBR:
1349       st->vbr_enabled = (*(int*)ptr);
1350       break;
1351    case SPEEX_GET_VBR:
1352       (*(int*)ptr) = st->vbr_enabled;
1353       break;
1354    case SPEEX_SET_VBR_QUALITY:
1355       st->vbr_quality = (*(float*)ptr);
1356       break;
1357    case SPEEX_GET_VBR_QUALITY:
1358       (*(float*)ptr) = st->vbr_quality;
1359       break;
1360    case SPEEX_SET_QUALITY:
1361       {
1362          int quality = (*(int*)ptr);
1363          /*
1364          if (quality<=0)
1365             st->submodeID = 0;
1366          else if (quality<=1)
1367             st->submodeID = 1;
1368          else if (quality<=2)
1369             st->submodeID = 2;
1370          else if (quality<=4)
1371             st->submodeID = 3;
1372          else if (quality<=6)
1373             st->submodeID = 4;
1374          else if (quality<=8)
1375             st->submodeID = 5;
1376          else if (quality<=9)
1377             st->submodeID = 6;
1378          else if (quality<=10)
1379             st->submodeID = 7;
1380          else
1381          fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);*/
1382          if (quality < 0)
1383             quality = 0;
1384          if (quality > 10)
1385             quality = 10;
1386          st->submodeID = ((SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1387       }
1388       break;
1389    case SPEEX_SET_COMPLEXITY:
1390       st->complexity = (*(int*)ptr);
1391       if (st->complexity<1)
1392          st->complexity=1;
1393       break;
1394    case SPEEX_GET_COMPLEXITY:
1395       (*(int*)ptr) = st->complexity;
1396       break;
1397    case SPEEX_SET_BITRATE:
1398       {
1399          int i=10, rate, target;
1400          target = (*(int*)ptr);
1401          while (i>=1)
1402          {
1403             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1404             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1405             if (rate <= target)
1406                break;
1407             i--;
1408          }
1409       }
1410       break;
1411    case SPEEX_GET_BITRATE:
1412       if (st->submodes[st->submodeID])
1413          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1414       else
1415          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1416       break;
1417    case SPEEX_SET_SAMPLING_RATE:
1418       st->sampling_rate = (*(int*)ptr);
1419       break;
1420    case SPEEX_GET_SAMPLING_RATE:
1421       (*(int*)ptr)=st->sampling_rate;
1422       break;
1423    case SPEEX_GET_PI_GAIN:
1424       {
1425          int i;
1426          float *g = (float*)ptr;
1427          for (i=0;i<st->nbSubframes;i++)
1428             g[i]=st->pi_gain[i];
1429       }
1430       break;
1431    case SPEEX_GET_EXC:
1432       {
1433          int i;
1434          float *e = (float*)ptr;
1435          for (i=0;i<st->frameSize;i++)
1436             e[i]=st->exc[i];
1437       }
1438       break;
1439    case SPEEX_GET_INNOV:
1440       {
1441          int i;
1442          float *e = (float*)ptr;
1443          for (i=0;i<st->frameSize;i++)
1444             e[i]=st->innov[i];
1445       }
1446       break;
1447    case SPEEX_GET_RELATIVE_QUALITY:
1448       (*(float*)ptr)=st->relative_quality;
1449       break;
1450    default:
1451       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1452    }
1453 }
1454
1455 void nb_decoder_ctl(void *state, int request, void *ptr)
1456 {
1457    DecState *st;
1458    st=(DecState*)state;
1459    switch(request)
1460    {
1461    case SPEEX_SET_ENH:
1462       st->lpc_enh_enabled = *((int*)ptr);
1463       break;
1464    case SPEEX_GET_ENH:
1465       *((int*)ptr) = st->lpc_enh_enabled;
1466       break;
1467    case SPEEX_GET_FRAME_SIZE:
1468       (*(int*)ptr) = st->frameSize;
1469       break;
1470    case SPEEX_GET_BITRATE:
1471       if (st->submodes[st->submodeID])
1472          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1473       else
1474          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1475       break;
1476    case SPEEX_SET_SAMPLING_RATE:
1477       st->sampling_rate = (*(int*)ptr);
1478       break;
1479    case SPEEX_GET_SAMPLING_RATE:
1480       (*(int*)ptr)=st->sampling_rate;
1481       break;
1482    case SPEEX_SET_HANDLER:
1483       {
1484          SpeexCallback *c = (SpeexCallback*)ptr;
1485          st->speex_callbacks[c->callback_id].func=c->func;
1486          st->speex_callbacks[c->callback_id].data=c->data;
1487          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1488       }
1489       break;
1490    case SPEEX_SET_USER_HANDLER:
1491       {
1492          SpeexCallback *c = (SpeexCallback*)ptr;
1493          st->user_callback.func=c->func;
1494          st->user_callback.data=c->data;
1495          st->user_callback.callback_id=c->callback_id;
1496       }
1497       break;
1498    case SPEEX_GET_PI_GAIN:
1499       {
1500          int i;
1501          float *g = (float*)ptr;
1502          for (i=0;i<st->nbSubframes;i++)
1503             g[i]=st->pi_gain[i];
1504       }
1505       break;
1506    case SPEEX_GET_EXC:
1507       {
1508          int i;
1509          float *e = (float*)ptr;
1510          for (i=0;i<st->frameSize;i++)
1511             e[i]=st->exc[i];
1512       }
1513       break;
1514    case SPEEX_GET_INNOV:
1515       {
1516          int i;
1517          float *e = (float*)ptr;
1518          for (i=0;i<st->frameSize;i++)
1519             e[i]=st->innov[i];
1520       }
1521       break;
1522    default:
1523       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1524    }
1525 }