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