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