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