Big changes in both narrowband and wideband. Retrained LSP codebook,
[speexdsp.git] / libspeex / nb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: speex.c
3
4    This library is free software; you can redistribute it and/or
5    modify it under the terms of the GNU Lesser General Public
6    License as published by the Free Software Foundation; either
7    version 2.1 of the License, or (at your option) any later version.
8    
9    This library is distributed in the hope that it will be useful,
10    but WITHOUT ANY WARRANTY; without even the implied warranty of
11    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12    Lesser General Public License for more details.
13    
14    You should have received a copy of the GNU Lesser General Public
15    License along with this library; if not, write to the Free Software
16    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17 */
18
19 #include <stdlib.h>
20 #include <stdio.h>
21 #include <string.h>
22 #include <math.h>
23 #include "nb_celp.h"
24 #include "lpc.h"
25 #include "lsp.h"
26 #include "ltp.h"
27 #include "quant_lsp.h"
28 #include "cb_search.h"
29 #include "filters.h"
30 #include "stack_alloc.h"
31 #include "vq.h"
32 #include "speex_bits.h"
33
34 #ifndef M_PI
35 #define M_PI           3.14159265358979323846  /* pi */
36 #endif
37
38 #define sqr(x) ((x)*(x))
39 #define min(a,b) ((a) < (b) ? (a) : (b))
40
41 void *nb_encoder_init(SpeexMode *m)
42 {
43    EncState *st;
44    SpeexNBMode *mode;
45    int i;
46    float tmp;
47
48    mode=m->mode;
49    st = malloc(sizeof(EncState));
50    st->mode=m;
51    /* Codec parameters, should eventually have several "modes"*/
52    st->frameSize = mode->frameSize;
53    st->windowSize = mode->windowSize;
54    st->nbSubframes=mode->frameSize/mode->subframeSize;
55    st->subframeSize=mode->subframeSize;
56    st->lpcSize = mode->lpcSize;
57    st->bufSize = mode->bufSize;
58    st->gamma1=mode->gamma1;
59    st->gamma2=mode->gamma2;
60    st->min_pitch=mode->pitchStart;
61    st->max_pitch=mode->pitchEnd;
62    st->lag_factor=mode->lag_factor;
63    st->lpc_floor = mode->lpc_floor;
64    st->preemph = mode->preemph;
65   
66
67    st->lsp_quant = mode->lsp_quant;
68    st->ltp_quant = mode->ltp_quant;
69    st->ltp_params = mode->ltp_params;
70    st->innovation_quant = mode->innovation_quant;
71    st->innovation_params = mode->innovation_params;
72
73    st->pre_mem=0;
74    st->pre_mem2=0;
75
76    /* Over-sampling filter (fractional pitch)*/
77    st->os_fact=4;
78    st->os_filt_ord2=4*st->os_fact;
79    st->os_filt = malloc((1+2*st->os_filt_ord2)*sizeof(float));
80    st->os_filt[st->os_filt_ord2] = 1;
81    for (i=1;i<=st->os_filt_ord2;i++)
82    {
83       float x=M_PI*i/st->os_fact;
84       st->os_filt[st->os_filt_ord2-i] = st->os_filt[st->os_filt_ord2+i]=sin(x)/x*(.5+.5*cos(M_PI*i/st->os_filt_ord2));
85    }
86    /* Normalizing the over-sampling filter */
87    tmp=0;
88    for (i=0;i<2*st->os_filt_ord2+1;i++)
89       tmp += st->os_filt[i];
90    tmp=1/tmp;
91    for (i=0;i<2*st->os_filt_ord2+1;i++)
92       st->os_filt[i] *= tmp;
93
94    /*for (i=0;i<2*st->os_filt_ord2+1;i++)
95       printf ("%f ", st->os_filt[i]);
96       printf ("\n");*/
97
98    /* Allocating input buffer */
99    st->inBuf = calloc(st->bufSize,sizeof(float));
100    st->frame = st->inBuf + st->bufSize - st->windowSize;
101    /* Allocating excitation buffer */
102    st->excBuf = calloc(st->bufSize,sizeof(float));
103    st->exc = st->excBuf + st->bufSize - st->windowSize;
104    st->swBuf = calloc(st->bufSize,sizeof(float));
105    st->sw = st->swBuf + st->bufSize - st->windowSize;
106
107    st->exc2Buf = calloc(st->bufSize,sizeof(float));
108    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
109
110    /* Hanning window */
111    st->window = malloc(st->windowSize*sizeof(float));
112    for (i=0;i<st->windowSize;i++)
113       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
114
115    /* Create the window for autocorrelation (lag-windowing) */
116    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
117    for (i=0;i<st->lpcSize+1;i++)
118       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
119
120    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
121
122    st->stack = calloc(20000, sizeof(float));
123
124    st->buf2 = malloc(st->windowSize*sizeof(float));
125
126    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
127    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
128    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
129    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
130    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
131
132    st->lsp = malloc(st->lpcSize*sizeof(float));
133    st->qlsp = malloc(st->lpcSize*sizeof(float));
134    st->old_lsp = malloc(st->lpcSize*sizeof(float));
135    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
136    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
137    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
138    st->rc = malloc(st->lpcSize*sizeof(float));
139    st->first = 1;
140    
141    st->mem_sp = calloc(st->lpcSize, sizeof(float));
142    st->mem_sw = calloc(st->lpcSize, sizeof(float));
143
144    st->pi_gain = calloc(st->nbSubframes, sizeof(float));
145
146    st->pitch = calloc(st->nbSubframes, sizeof(int));
147    return st;
148 }
149
150 void nb_encoder_destroy(void *state)
151 {
152    EncState *st=state;
153    /* Free all allocated memory */
154    free(st->inBuf);
155    free(st->excBuf);
156    free(st->swBuf);
157    free(st->os_filt);
158    free(st->exc2Buf);
159    free(st->stack);
160
161    free(st->window);
162    free(st->buf2);
163    free(st->lpc);
164    free(st->interp_lpc);
165    free(st->interp_qlpc);
166    
167    free(st->bw_lpc1);
168    free(st->bw_lpc2);
169    free(st->autocorr);
170    free(st->lagWindow);
171    free(st->lsp);
172    free(st->qlsp);
173    free(st->old_lsp);
174    free(st->interp_lsp);
175    free(st->old_qlsp);
176    free(st->interp_qlsp);
177    free(st->rc);
178
179    free(st->mem_sp);
180    free(st->mem_sw);
181    free(st->pi_gain);
182    free(st->pitch);
183    
184    free(st);
185 }
186
187 void nb_encode(void *state, float *in, FrameBits *bits)
188 {
189    EncState *st;
190    int i, sub, roots;
191    float error;
192    
193    st=state;
194    /* Copy new data in input buffer */
195    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
196    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
197    for (i=1;i<st->frameSize;i++)
198       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
199    st->pre_mem = in[st->frameSize-1];
200
201    memmove(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
202    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
203    memmove(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
204
205    /* Window for analysis */
206    for (i=0;i<st->windowSize;i++)
207       st->buf2[i] = st->frame[i] * st->window[i];
208
209    /* Compute auto-correlation */
210    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
211
212    st->autocorr[0] += 1;        /* prevents NANs */
213    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
214    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
215    for (i=0;i<st->lpcSize+1;i++)
216       st->autocorr[i] *= st->lagWindow[i];
217
218    /* Levinson-Durbin */
219    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
220    st->lpc[0]=1;
221
222    /* LPC to LSPs (x-domain) transform */
223    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
224    if (roots!=st->lpcSize)
225    {
226       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
227       exit(1);
228    }
229
230    /* x-domain to angle domain*/
231    for (i=0;i<st->lpcSize;i++)
232       st->lsp[i] = acos(st->lsp[i]);
233    /*print_vec(st->lsp, 10, "LSP:");*/
234    /* LSP Quantization */
235    st->lsp_quant(st->lsp, st->qlsp, st->lpcSize, bits);
236
237    /*for (i=0;i<st->lpcSize;i++)
238      st->qlsp[i]=st->lsp[i];*/
239    /*printf ("LSP ");
240    for (i=0;i<st->lpcSize;i++)
241       printf ("%f ", st->lsp[i]);
242    printf ("\n");
243    printf ("QLSP ");
244    for (i=0;i<st->lpcSize;i++)
245       printf ("%f ", st->qlsp[i]);
246    printf ("\n");*/
247    /* Special case for first frame */
248    if (st->first)
249    {
250       for (i=0;i<st->lpcSize;i++)
251          st->old_lsp[i] = st->lsp[i];
252       for (i=0;i<st->lpcSize;i++)
253          st->old_qlsp[i] = st->qlsp[i];
254    }
255
256    /* Loop on sub-frames */
257    for (sub=0;sub<st->nbSubframes;sub++)
258    {
259       float esig, enoise, snr, tmp;
260       int   offset;
261       float *sp, *sw, *res, *exc, *target, *mem, *exc2;
262       int pitch;
263
264       /* Offset relative to start of frame */
265       offset = st->subframeSize*sub;
266       /* Original signal */
267       sp=st->frame+offset;
268       /* Excitation */
269       exc=st->exc+offset;
270       /* Weighted signal */
271       sw=st->sw+offset;
272
273       exc2=st->exc2+offset;
274
275       /* Filter response */
276       res = PUSH(st->stack, st->subframeSize);
277       /* Target signal */
278       target = PUSH(st->stack, st->subframeSize);
279       mem = PUSH(st->stack, st->lpcSize);
280
281       /* LSP interpolation (quantized and unquantized) */
282       tmp = (.5 + sub)/st->nbSubframes;
283       for (i=0;i<st->lpcSize;i++)
284          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
285       for (i=0;i<st->lpcSize;i++)
286          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
287
288       if (0) {
289          float *h=PUSH(st->stack, 8);
290          for (i=0;i<8;i++)
291             h[i]=0;
292          h[0]=1;
293          
294          residue_zero(h, st->bw_lpc1, h, 8, st->lpcSize);
295          syn_filt_zero(h, st->interp_qlpc, h, 8, st->lpcSize);
296          syn_filt_zero(h, st->bw_lpc2, h, 8, st->lpcSize);
297          print_vec(h, 8, "lpc_resp");
298          POP(st->stack);
299       }
300       
301       /* Compute interpolated LPCs (quantized and unquantized) */
302       for (i=0;i<st->lpcSize;i++)
303          st->interp_lsp[i] = cos(st->interp_lsp[i]);
304       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
305
306       for (i=0;i<st->lpcSize;i++)
307          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
308       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
309
310       tmp=1;
311       st->pi_gain[sub]=0;
312       for (i=0;i<=st->lpcSize;i++)
313       {
314          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
315          tmp = -tmp;
316       }
317      
318
319       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
320       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
321       if (st->gamma2>=0)
322          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
323       else
324       {
325          st->bw_lpc2[0]=1;
326          st->bw_lpc2[1]=-st->preemph;
327          for (i=2;i<=st->lpcSize;i++)
328             st->bw_lpc2[i]=0;
329       }
330 #ifdef DEBUG
331       printf ("\nlpc0 ");
332       for (i=0;i<=st->lpcSize;i++)
333          printf ("%f ", st->interp_lpc[i]);
334       printf ("\nlpc1 ");
335       for (i=0;i<=st->lpcSize;i++)
336          printf ("%f ", st->bw_lpc1[i]);
337       printf ("\nlpc2 ");
338       for (i=0;i<=st->lpcSize;i++)
339          printf ("%f ", st->bw_lpc2[i]);
340       printf ("\n\n");
341 #endif
342       /* Reset excitation */
343       for (i=0;i<st->subframeSize;i++)
344          exc[i]=0;
345       for (i=0;i<st->subframeSize;i++)
346          exc2[i]=0;
347
348       /* Compute zero response of A(z/g1) / ( A(z/g2) * Aq(z) ) */
349       for (i=0;i<st->lpcSize;i++)
350          mem[i]=st->mem_sp[i];
351       syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
352       for (i=0;i<st->lpcSize;i++)
353          mem[i]=st->mem_sp[i];
354       residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
355       for (i=0;i<st->lpcSize;i++)
356          mem[i]=st->mem_sw[i];
357       syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
358
359       /* Compute weighted signal */
360       for (i=0;i<st->lpcSize;i++)
361          mem[i]=st->mem_sp[i];
362       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
363       for (i=0;i<st->lpcSize;i++)
364          mem[i]=st->mem_sw[i];
365       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
366       
367       esig=0;
368       for (i=0;i<st->subframeSize;i++)
369          esig+=sw[i]*sw[i];
370       
371       /* Compute target signal */
372       for (i=0;i<st->subframeSize;i++)
373          target[i]=sw[i]-res[i];
374
375       for (i=0;i<st->subframeSize;i++)
376          exc[i]=0;
377
378       /* Long-term prediction */
379       pitch = st->ltp_quant(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
380                     exc, st->ltp_params, st->min_pitch, st->max_pitch, 
381                     st->lpcSize, st->subframeSize, bits, st->stack, exc2);
382       st->pitch[sub]=pitch;
383
384       /* Update target for adaptive codebook contribution */
385       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
386       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
387       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
388       for (i=0;i<st->subframeSize;i++)
389         target[i]-=res[i];
390
391       /* Compute noise energy and SNR */
392       enoise=0;
393       for (i=0;i<st->subframeSize;i++)
394          enoise += target[i]*target[i];
395       snr = 10*log10((esig+1)/(enoise+1));
396       /*st->pitch[sub]=(int)snr;*/
397 #ifdef DEBUG
398       printf ("pitch SNR = %f\n", snr);
399 #endif
400
401
402 #if 0 /*If set to 1, compute "real innovation" i.e. cheat to get perfect reconstruction*/
403       syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
404       residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
405       residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
406       /*if (1||(snr>9 && (rand()%6==0)))
407       {
408          float ener=0;
409          printf ("exc ");
410          for (i=0;i<st->subframeSize;i++)
411          {
412             ener+=st->buf2[i]*st->buf2[i];
413             if (i && i%5==0)
414                printf ("\nexc ");
415             printf ("%f ", st->buf2[i]);
416          }
417          printf ("\n");
418       printf ("innovation_energy = %f\n", ener);
419       }*/
420       if (rand()%5==0 && snr>5)
421       {
422          float ener=0, sign=1;
423          if (rand()%2)
424             sign=-1;
425          for (i=0;i<st->subframeSize;i++)
426          {
427             ener+=st->buf2[i]*st->buf2[i];
428          }
429          ener=sign/sqrt(.01+ener/st->subframeSize);
430          for (i=0;i<st->subframeSize;i++)
431          {
432             if (i%8==0)
433                printf ("\nexc ");
434             printf ("%f ", ener*st->buf2[i]);
435          }
436          printf ("\n");
437       }
438
439       for (i=0;i<st->subframeSize;i++)
440          exc[i]+=st->buf2[i];
441 #else
442       if (0)
443       {
444       /* Perform a split-codebook search */
445       st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
446                            st->innovation_params, st->lpcSize,
447                            st->subframeSize, exc, bits, st->stack);
448       }
449       else
450       {
451          float *innov;
452          float ener=0, ener_1;
453          innov=PUSH(st->stack, st->subframeSize);
454          for (i=0;i<st->subframeSize;i++)
455             innov[i]=0;
456          syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
457          residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
458          residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
459          for (i=0;i<st->subframeSize;i++)
460             ener+=st->buf2[i]*st->buf2[i];
461          ener=sqrt(.1+ener/st->subframeSize);
462
463          {
464             int qe = (int)(floor(7*log(ener)));
465             if (qe<0)
466                qe=0;
467             if (qe>63)
468                qe=63;
469             ener = exp(qe/7.0);
470             speex_bits_pack(bits, qe, 6);
471             printf ("quant_energy: %d %f\n", qe, ener);
472          }
473          ener_1 = 1/ener;
474          
475          for (i=0;i<st->subframeSize;i++)
476             target[i]*=ener_1;
477 #if 1
478          st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
479                                 st->innovation_params, st->lpcSize, st->subframeSize, 
480                                 innov, bits, st->stack);
481          
482          for (i=0;i<st->subframeSize;i++)
483             exc[i] += innov[i]*ener;
484 #else
485          for (i=0;i<st->subframeSize;i++)
486             exc[i] += st->buf2[i];
487 #endif
488          POP(st->stack);
489
490       }
491 #endif
492       /* Compute weighted noise energy and SNR */
493       enoise=0;
494       for (i=0;i<st->subframeSize;i++)
495          enoise += target[i]*target[i];
496       snr = 10*log10((esig+1)/(enoise+1));
497 #ifdef DEBUG
498       printf ("seg SNR = %f\n", snr);
499 #endif
500       /*Keep the previous memory*/
501       for (i=0;i<st->lpcSize;i++)
502          mem[i]=st->mem_sp[i];
503       /* Final signal synthesis from excitation */
504       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
505
506       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
507       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
508       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
509
510 #if 0
511       /*for (i=0;i<st->subframeSize;i++)
512         exc2[i]=.75*exc[i]+.2*exc[i-pitch]+.05*exc[i-2*pitch];*/
513       {
514          float max_exc=0;
515          for (i=0;i<st->subframeSize;i++)
516             if (fabs(exc[i])>max_exc)
517                max_exc=fabs(exc[i]);
518          max_exc=1/(max_exc+.01);
519          for (i=0;i<st->subframeSize;i++)
520          {
521             float xx=max_exc*exc[i];
522             exc2[i]=exc[i]*(1-exp(-100*xx*xx));
523          }
524       }
525 #else
526       for (i=0;i<st->subframeSize;i++)
527          exc2[i]=exc[i];
528 #endif
529       POP(st->stack);
530       POP(st->stack);
531       POP(st->stack);
532    }
533
534    /* Store the LSPs for interpolation in the next frame */
535    for (i=0;i<st->lpcSize;i++)
536       st->old_lsp[i] = st->lsp[i];
537    for (i=0;i<st->lpcSize;i++)
538       st->old_qlsp[i] = st->qlsp[i];
539
540    /* The next frame will not be the first (Duh!) */
541    st->first = 0;
542
543    /* Replace input by synthesized speech */
544    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
545    for (i=1;i<st->frameSize;i++)
546      in[i]=st->frame[i] + st->preemph*in[i-1];
547    st->pre_mem2=in[st->frameSize-1];
548 }
549
550
551 void *nb_decoder_init(SpeexMode *m)
552 {
553    DecState *st;
554    SpeexNBMode *mode;
555    int i;
556
557    mode=m->mode;
558    st = malloc(sizeof(DecState));
559    st->mode=m;
560
561    st->first=1;
562    /* Codec parameters, should eventually have several "modes"*/
563    st->frameSize = mode->frameSize;
564    st->windowSize = mode->windowSize;
565    st->nbSubframes=mode->frameSize/mode->subframeSize;
566    st->subframeSize=mode->subframeSize;
567    st->lpcSize = mode->lpcSize;
568    st->bufSize = mode->bufSize;
569    st->gamma1=mode->gamma1;
570    st->gamma2=mode->gamma2;
571    st->min_pitch=mode->pitchStart;
572    st->max_pitch=mode->pitchEnd;
573    st->preemph = mode->preemph;
574
575    st->pre_mem=0;
576    st->lsp_unquant = mode->lsp_unquant;
577    st->ltp_unquant = mode->ltp_unquant;
578    st->ltp_params = mode->ltp_params;
579
580    st->innovation_unquant = mode->innovation_unquant;
581    st->innovation_params = mode->innovation_params;
582
583    st->stack = calloc(10000, sizeof(float));
584
585    st->inBuf = malloc(st->bufSize*sizeof(float));
586    st->frame = st->inBuf + st->bufSize - st->windowSize;
587    st->excBuf = malloc(st->bufSize*sizeof(float));
588    st->exc = st->excBuf + st->bufSize - st->windowSize;
589    for (i=0;i<st->bufSize;i++)
590       st->inBuf[i]=0;
591    for (i=0;i<st->bufSize;i++)
592       st->excBuf[i]=0;
593
594    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
595    st->qlsp = malloc(st->lpcSize*sizeof(float));
596    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
597    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
598    st->mem_sp = calloc(st->lpcSize, sizeof(float));
599
600    st->pi_gain = calloc(st->nbSubframes, sizeof(float));
601    
602    return st;
603 }
604
605 void nb_decoder_destroy(void *state)
606 {
607    DecState *st;
608    st=state;
609    free(st->inBuf);
610    free(st->excBuf);
611    free(st->interp_qlpc);
612    free(st->qlsp);
613    free(st->old_qlsp);
614    free(st->interp_qlsp);
615    free(st->stack);
616    free(st->mem_sp);
617    free(st->pi_gain);
618    
619    free(state);
620 }
621
622 void nb_decode(void *state, FrameBits *bits, float *out)
623 {
624    DecState *st;
625    int i, sub;
626
627    st=state;
628
629    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
630    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
631
632
633    st->lsp_unquant(st->qlsp, st->lpcSize, bits);
634    if (st->first)
635    {
636       for (i=0;i<st->lpcSize;i++)
637          st->old_qlsp[i] = st->qlsp[i];
638    }
639
640    /*Loop on subframes */
641    for (sub=0;sub<st->nbSubframes;sub++)
642    {
643       int offset;
644       float *sp, *exc, tmp;
645       
646       /* Offset relative to start of frame */
647       offset = st->subframeSize*sub;
648       /* Original signal */
649       sp=st->frame+offset;
650       /* Excitation */
651       exc=st->exc+offset;
652
653       /* LSP interpolation (quantized and unquantized) */
654       tmp = (.5 + sub)/st->nbSubframes;
655       for (i=0;i<st->lpcSize;i++)
656          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
657
658       /* Compute interpolated LPCs (unquantized) */
659       for (i=0;i<st->lpcSize;i++)
660          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
661       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
662
663       tmp=1;
664       st->pi_gain[sub]=0;
665       for (i=0;i<=st->lpcSize;i++)
666       {
667          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
668          tmp = -tmp;
669       }
670
671       /* Reset excitation */
672       for (i=0;i<st->subframeSize;i++)
673          exc[i]=0;
674
675       /*Adaptive codebook contribution*/
676       st->ltp_unquant(exc, st->min_pitch, st->max_pitch, st->ltp_params, st->subframeSize, bits, st->stack);
677       
678
679       {
680          int q_energy;
681          float ener;
682          float *innov;
683          
684          innov = PUSH(st->stack, st->subframeSize);
685          for (i=0;i<st->subframeSize;i++)
686             innov[i]=0;
687
688          q_energy = speex_bits_unpack_unsigned(bits, 6);
689          ener = exp(q_energy/7.0);
690          printf ("unquant_energy: %d %f\n", q_energy, ener);
691          
692          /*Fixed codebook contribution*/
693          st->innovation_unquant(innov, st->innovation_params, st->subframeSize, bits, st->stack);
694
695          for (i=0;i<st->subframeSize;i++)
696             exc[i]+=ener*innov[i];
697
698          POP(st->stack);
699       }
700
701       /*Compute decoded signal*/
702       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
703
704    }
705    
706    /*Copy output signal*/
707    for (i=0;i<st->frameSize;i++)
708       out[i]=st->frame[i];
709
710    out[0] = st->frame[0] + st->preemph*st->pre_mem;
711    for (i=1;i<st->frameSize;i++)
712      out[i]=st->frame[i] + st->preemph*out[i-1];
713    st->pre_mem=out[st->frameSize-1];
714
715
716    /* Store the LSPs for interpolation in the next frame */
717    for (i=0;i<st->lpcSize;i++)
718       st->old_qlsp[i] = st->qlsp[i];
719
720    /* The next frame will not be the first (Duh!) */
721    st->first = 0;
722
723 }