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