Decoder back in sync with the encoder
[speexdsp.git] / libspeex / sb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: sb_celp.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
20 #include "speex.h"
21 #include "sb_celp.h"
22 #include "stdlib.h"
23 #include "filters.h"
24 #include <math.h>
25 #include "lpc.h"
26 #include "lsp.h"
27 #include <stdio.h>
28 #include "stack_alloc.h"
29 #include "cb_search.h"
30 #include "quant_lsp.h"
31 #include "vq.h"
32 #include <string.h>
33 #include "ltp.h"
34
35 #ifndef M_PI
36 #define M_PI           3.14159265358979323846  /* pi */
37 #endif
38
39 extern float stoc[];
40
41 #define sqr(x) ((x)*(x))
42
43 float quant_high_gain[16]={
44    -2.387860,
45    -1.504710,
46    -0.988013,
47    -0.610249,
48    -0.310298,
49    -0.050495,
50    0.188963,
51    0.413744,
52    0.628971,
53    0.840555,
54    1.055630,
55    1.283410,
56    1.544990,
57    1.855790,
58    2.281910,
59    3.002660
60 };
61
62
63 float quant_high_gain2[8] = {
64    -1.51541,
65    -0.70324,
66    -0.17024,
67    0.26748,
68    0.67232,
69    1.08402,
70    1.56110,
71    2.25160,
72 };
73
74
75 #if 0
76 #define QMF_ORDER 32
77 static float h0[32] = {
78    0.0006910579, -0.001403793,
79    -0.001268303, 0.004234195,
80    0.001414246, -0.009458318,
81    -0.0001303859, 0.01798145,
82    -0.004187483, -0.03123862,
83    0.01456844, 0.05294745,
84    -0.03934878, -0.09980243,
85    0.1285579, 0.4664053,
86    0.4664053, 0.1285579,
87    -0.09980243, -0.03934878,
88    0.05294745, 0.01456844,
89    -0.03123862, -0.004187483,
90    0.01798145, -0.0001303859,
91    -0.009458318, 0.001414246,
92    0.004234195, -0.001268303,
93    -0.001403793, 0.0006910579
94 };
95
96 static float h1[32] = {
97    0.0006910579, 0.001403793,
98    -0.001268303, -0.004234195,
99    0.001414246, 0.009458318,
100    -0.0001303859, -0.01798145,
101    -0.004187483, 0.03123862,
102    0.01456844, -0.05294745,
103    -0.03934878, 0.09980243,
104    0.1285579, -0.4664053,
105    0.4664053, -0.1285579,
106    -0.09980243, 0.03934878,
107    0.05294745, -0.01456844,
108    -0.03123862, 0.004187483,
109    0.01798145, 0.0001303859,
110    -0.009458318, -0.001414246,
111    0.004234195, 0.001268303,
112    -0.001403793, -0.0006910579
113 };
114 #else 
115 #define QMF_ORDER 64
116 static float h0[64] = {
117    3.596189e-05, -0.0001123515,
118    -0.0001104587, 0.0002790277,
119    0.0002298438, -0.0005953563,
120    -0.0003823631, 0.00113826,
121    0.0005308539, -0.001986177,
122    -0.0006243724, 0.003235877,
123    0.0005743159, -0.004989147,
124    -0.0002584767, 0.007367171,
125    -0.0004857935, -0.01050689,
126    0.001894714, 0.01459396,
127    -0.004313674, -0.01994365,
128    0.00828756, 0.02716055,
129    -0.01485397, -0.03764973,
130    0.026447, 0.05543245,
131    -0.05095487, -0.09779096,
132    0.1382363, 0.4600981,
133    0.4600981, 0.1382363,
134    -0.09779096, -0.05095487,
135    0.05543245, 0.026447,
136    -0.03764973, -0.01485397,
137    0.02716055, 0.00828756,
138    -0.01994365, -0.004313674,
139    0.01459396, 0.001894714,
140    -0.01050689, -0.0004857935,
141    0.007367171, -0.0002584767,
142    -0.004989147, 0.0005743159,
143    0.003235877, -0.0006243724,
144    -0.001986177, 0.0005308539,
145    0.00113826, -0.0003823631,
146    -0.0005953563, 0.0002298438,
147    0.0002790277, -0.0001104587,
148    -0.0001123515, 3.596189e-05
149 };
150
151 static float h1[64] = {
152    3.596189e-05, 0.0001123515,
153    -0.0001104587, -0.0002790277,
154    0.0002298438, 0.0005953563,
155    -0.0003823631, -0.00113826,
156    0.0005308539, 0.001986177,
157    -0.0006243724, -0.003235877,
158    0.0005743159, 0.004989147,
159    -0.0002584767, -0.007367171,
160    -0.0004857935, 0.01050689,
161    0.001894714, -0.01459396,
162    -0.004313674, 0.01994365,
163    0.00828756, -0.02716055,
164    -0.01485397, 0.03764973,
165    0.026447, -0.05543245,
166    -0.05095487, 0.09779096,
167    0.1382363, -0.4600981,
168    0.4600981, -0.1382363,
169    -0.09779096, 0.05095487,
170    0.05543245, -0.026447,
171    -0.03764973, 0.01485397,
172    0.02716055, -0.00828756,
173    -0.01994365, 0.004313674,
174    0.01459396, -0.001894714,
175    -0.01050689, 0.0004857935,
176    0.007367171, 0.0002584767,
177    -0.004989147, -0.0005743159,
178    0.003235877, 0.0006243724,
179    -0.001986177, -0.0005308539,
180    0.00113826, 0.0003823631,
181    -0.0005953563, -0.0002298438,
182    0.0002790277, 0.0001104587,
183    -0.0001123515, -3.596189e-05
184 };
185 #endif
186
187 void sb_encoder_init(SBEncState *st, SpeexMode *mode)
188 {
189    int i;
190    encoder_init(&st->st_low, mode);
191    st->full_frame_size = 2*st->st_low.frameSize;
192    st->frame_size = st->st_low.frameSize;
193    st->subframeSize = st->st_low.subframeSize;
194    st->nbSubframes = st->st_low.nbSubframes;
195    st->windowSize = mode->windowSize;
196    st->lpcSize=8;
197    st->bufSize=st->st_low.bufSize;
198
199    st->lag_factor = .002;
200    st->lpc_floor = 1.0001;
201    st->gamma1=.9;
202    st->gamma2=.6;
203    st->first=1;
204    st->stack = calloc(10000, sizeof(float));
205
206    st->x0=calloc(st->full_frame_size, sizeof(float));
207    st->x1=calloc(st->full_frame_size, sizeof(float));
208    st->x0d=calloc(st->frame_size, sizeof(float));
209    st->x1d=calloc(st->frame_size, sizeof(float));
210    st->high=calloc(st->full_frame_size, sizeof(float));
211    st->y0=calloc(st->full_frame_size, sizeof(float));
212    st->y1=calloc(st->full_frame_size, sizeof(float));
213
214    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
215    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
216    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
217    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
218
219    st->buf=calloc(st->windowSize, sizeof(float));
220    st->excBuf=calloc(st->bufSize, sizeof(float));
221    st->exc = st->excBuf + st->bufSize - st->windowSize;
222    /*st->exc=st->excBuf+st->frame_size;*/
223
224    st->res=calloc(st->frame_size, sizeof(float));
225    st->sw=calloc(st->frame_size, sizeof(float));
226    st->target=calloc(st->frame_size, sizeof(float));
227    st->window=calloc(st->windowSize, sizeof(float));
228    for (i=0;i<st->windowSize;i++)
229       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
230
231    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
232    for (i=0;i<st->lpcSize+1;i++)
233       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
234
235    st->rc = malloc(st->lpcSize*sizeof(float));
236    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
237    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
238    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
239    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
240    st->lsp = malloc(st->lpcSize*sizeof(float));
241    st->qlsp = malloc(st->lpcSize*sizeof(float));
242    st->old_lsp = malloc(st->lpcSize*sizeof(float));
243    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
244    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
245    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
246    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
247    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
248
249    st->mem_sp = calloc(st->lpcSize, sizeof(float));
250    st->mem_sp2 = calloc(st->lpcSize, sizeof(float));
251    st->mem_sw = calloc(st->lpcSize, sizeof(float));
252
253 }
254
255 void sb_encoder_destroy(SBEncState *st)
256 {
257    encoder_destroy(&st->st_low);
258    free(st->x0);
259    free(st->x0d);
260    free(st->x1);
261    free(st->x1d);
262    free(st->high);
263    free(st->y0);
264    free(st->y1);
265    free(st->h0_mem);
266    free(st->h1_mem);
267    free(st->g0_mem);
268    free(st->g1_mem);
269    
270    free(st->buf);
271    free(st->window);
272    free(st->excBuf);
273    free(st->sw);
274    free(st->res);
275    free(st->target);
276    free(st->lagWindow);
277    free(st->rc);
278    free(st->autocorr);
279    free(st->lpc);
280    free(st->bw_lpc1);
281    free(st->bw_lpc2);
282    free(st->lsp);
283    free(st->qlsp);
284    free(st->old_lsp);
285    free(st->old_qlsp);
286    free(st->interp_lsp);
287    free(st->interp_qlsp);
288    free(st->interp_lpc);
289    free(st->interp_qlpc);
290
291    free(st->mem_sp);
292    free(st->mem_sp2);
293    free(st->mem_sw);
294
295    free(st->stack);
296    
297 }
298
299 extern float hexc_table[];
300 split_cb_params split_cb_high = {
301    8,               /*subvect_size*/
302    5,               /*nb_subvect*/
303    hexc_table,       /*shape_cb*/
304    8,               /*shape_bits*/
305 };
306
307 void sb_encode(SBEncState *st, float *in, FrameBits *bits)
308 {
309    int i, roots, sub;
310    /* Compute the two sub-bands by filtering with h0 and h1*/
311    fir_mem(in, h0, st->x0, st->full_frame_size, QMF_ORDER, st->h0_mem);
312    fir_mem(in, h1, st->x1, st->full_frame_size, QMF_ORDER, st->h1_mem);
313    /* Down-sample x0 and x1 */
314    for (i=0;i<st->frame_size;i++)
315    {
316       st->x0d[i]=st->x0[i<<1];
317       st->x1d[i]=st->x1[i<<1];
318    }
319    /* Encode the narrowband part*/
320    encode(&st->st_low, st->x0d, bits);
321
322    /* High-band buffering / sync with low band */
323    for (i=0;i<st->frame_size;i++)
324    {
325       /*st->excBuf[i]=st->exc[i];*/
326       st->high[i]=st->high[st->frame_size+i];
327       st->high[st->frame_size+i]=st->x1d[i];
328    }
329
330    memmove(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
331
332    /* Start encoding the high-band */
333
334    for (i=0;i<st->windowSize;i++)
335       st->buf[i] = st->high[i] * st->window[i];
336
337    /* Compute auto-correlation */
338    autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
339
340    st->autocorr[0] += 1;        /* prevents NANs */
341    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
342    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
343    for (i=0;i<st->lpcSize+1;i++)
344       st->autocorr[i] *= st->lagWindow[i];
345
346    /* Levinson-Durbin */
347    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
348    st->lpc[0]=1;
349
350    /* LPC to LSPs (x-domain) transform */
351    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
352    if (roots!=st->lpcSize)
353    {
354       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
355       exit(1);
356    }
357
358    /* x-domain to angle domain*/
359    for (i=0;i<st->lpcSize;i++)
360       st->lsp[i] = acos(st->lsp[i]);
361
362    /* LSP quantization */
363    lsp_quant_high(st->lsp, st->qlsp, st->lpcSize, bits);
364    
365    /*printf ("high_lsp:");
366    for (i=0;i<st->lpcSize;i++)
367       printf (" %f", st->lsp[i]);
368       printf ("\n");*/
369    /*for (i=0;i<st->lpcSize;i++)
370      st->qlsp[i]=st->lsp[i];*/
371    
372
373    if (st->first)
374    {
375       for (i=0;i<st->lpcSize;i++)
376          st->old_lsp[i] = st->lsp[i];
377       for (i=0;i<st->lpcSize;i++)
378          st->old_qlsp[i] = st->qlsp[i];
379    }
380    
381    for (sub=0;sub<st->nbSubframes;sub++)
382    {
383       float *exc, *sp, *mem, *res, *target, *sw, tmp, filter_ratio;
384       int offset;
385       float rl, rh;
386
387       offset = st->subframeSize*sub;
388       sp=st->high+offset;
389       exc=st->excBuf+offset;
390       res=st->res+offset;
391       target=st->target+offset;
392       sw=st->sw+offset;
393
394       mem=PUSH(st->stack, st->lpcSize);
395       
396       /* LSP interpolation (quantized and unquantized) */
397       tmp = (.5 + sub)/st->nbSubframes;
398       for (i=0;i<st->lpcSize;i++)
399          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
400       for (i=0;i<st->lpcSize;i++)
401          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
402
403       /* Compute interpolated LPCs (quantized and unquantized) */
404       for (i=0;i<st->lpcSize;i++)
405          st->interp_lsp[i] = cos(st->interp_lsp[i]);
406       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
407
408       for (i=0;i<st->lpcSize;i++)
409          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
410       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
411
412       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
413       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
414
415       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
416          filters */
417       rl=rh=0;
418       tmp=1;
419       for (i=0;i<=st->lpcSize;i++)
420       {
421          rh += tmp*st->interp_qlpc[i];
422          tmp = -tmp;
423       }
424       rl = st->st_low.pi_gain[sub];
425       rl=1/(fabs(rl)+.01);
426       rh=1/(fabs(rh)+.01);
427       /* Compute ratio, will help predict the gain */
428       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
429
430       
431       if (0) {/* 1 for spectral folding excitation, 0 for stochastic */
432          float el=0,eh=0,g;
433          /* Backup memory */
434          for (i=0;i<st->lpcSize;i++)
435             mem[i]=st->mem_sp[i];
436          /* Compute "real excitation" */
437          residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp);
438          printf ("high_exc:");
439          for (i=0;i<st->subframeSize;i++)
440             printf (" %f", exc[i]);
441          printf ("\nlow_exc:");
442          for (i=0;i<st->subframeSize;i++)
443             printf (" %f", st->st_low.exc[offset+i]);
444          printf ("\n");
445 #if 1
446          /* Compute energy of low-band and high-band excitation */
447          for (i=0;i<st->subframeSize;i++)
448             eh+=sqr(exc[i]);
449          for (i=0;i<st->subframeSize;i++)
450             el+=sqr(st->st_low.exc[offset+i]);
451          if (st->st_low.pitch[sub]>8 || (rand()%5)==0)
452          {
453          for (i=0;i<st->subframeSize;i++)
454          {
455             float p=(.1+exc[i])*filter_ratio/(1+sqrt(el/st->subframeSize));
456             if (i%8==0)
457                printf ("\nhexc: ");
458             printf ("%f ", p);
459          }
460          printf ("\n");
461          }
462          /* Gain to use if we want to use the low-band excitation for high-band */
463          g=eh/(.01+el);
464          g=sqrt(g);
465
466          /* High-band excitation using the low-band excitation and a gain */
467          for (i=0;i<st->subframeSize;i++)
468             exc[i]=g*st->st_low.exc[offset+i];
469
470          /* FIXME: Should encode the gain here */
471 #endif
472          /* Update the input signal using the non-coded memory */
473          syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, mem);
474       } else {
475          float el=0;
476          float gc;
477          float *innov;
478
479          innov = PUSH(st->stack, st->subframeSize);
480
481          for (i=0;i<st->subframeSize;i++)
482             el+=sqr(st->st_low.exc[offset+i]);
483
484          gc = (.01+filter_ratio)/(1+sqrt(el/st->subframeSize));
485
486          /* Reset excitation */
487          for (i=0;i<st->subframeSize;i++)
488             exc[i]=0;
489          
490          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
491          for (i=0;i<st->lpcSize;i++)
492             mem[i]=st->mem_sp[i];
493          syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
494          for (i=0;i<st->lpcSize;i++)
495             mem[i]=st->mem_sp[i];
496          residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
497          for (i=0;i<st->lpcSize;i++)
498             mem[i]=st->mem_sw[i];
499          syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
500          
501          /* Compute weighted signal */
502          for (i=0;i<st->lpcSize;i++)
503             mem[i]=st->mem_sp[i];
504          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
505          for (i=0;i<st->lpcSize;i++)
506             mem[i]=st->mem_sw[i];
507          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
508          
509          /* Compute target signal */
510          for (i=0;i<st->subframeSize;i++)
511             target[i]=sw[i]-res[i];
512
513          for (i=0;i<st->subframeSize;i++)
514            exc[i]=0;
515
516
517          for (i=0;i<st->subframeSize;i++)
518             target[i]*=gc;
519          
520          /* Reset excitation */
521          for (i=0;i<st->subframeSize;i++)
522             innov[i]=0;
523
524          print_vec(target, st->subframeSize, "\ntarget");
525          split_cb_search_nogain(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
526                                 &split_cb_high, st->lpcSize, st->subframeSize, 
527                                 innov, bits, st->stack);
528          print_vec(target, st->subframeSize, "after");
529
530          for (i=0;i<st->subframeSize;i++)
531             exc[i] += innov[i]/gc;
532 #if 1
533          /*Keep the previous memory*/
534          for (i=0;i<st->lpcSize;i++)
535             mem[i]=st->mem_sp[i];
536          /* Final signal synthesis from excitation */
537          syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
538          
539          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
540          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
541          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
542 #endif
543          POP(st->stack);
544       }
545       
546       POP(st->stack);
547    }
548
549
550 #ifndef RELEASE
551    /* Up-sample coded low-band and high-band*/
552    for (i=0;i<st->frame_size;i++)
553    {
554       st->x0[(i<<1)]=st->x0d[i];
555       st->x1[(i<<1)]=st->high[i];
556       st->x0[(i<<1)+1]=0;
557       st->x1[(i<<1)+1]=0;
558    }
559    /* Reconstruct the original */
560    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
561    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
562    for (i=0;i<st->full_frame_size;i++)
563       in[i]=2*(st->y0[i]-st->y1[i]);
564 #endif
565    for (i=0;i<st->lpcSize;i++)
566       st->old_lsp[i] = st->lsp[i];
567    for (i=0;i<st->lpcSize;i++)
568       st->old_qlsp[i] = st->qlsp[i];
569
570    st->first=0;
571 }
572
573
574
575
576
577 void sb_decoder_init(SBDecState *st, SpeexMode *mode)
578 {
579    decoder_init(&st->st_low, mode);
580    st->full_frame_size = 2*st->st_low.frameSize;
581    st->frame_size = st->st_low.frameSize;
582    st->subframeSize = 40;
583    st->nbSubframes = 4;
584    st->lpcSize=8;
585
586    st->first=1;
587    st->stack = calloc(10000, sizeof(float));
588
589    st->x0=calloc(st->full_frame_size, sizeof(float));
590    st->x1=calloc(st->full_frame_size, sizeof(float));
591    st->x0d=calloc(st->frame_size, sizeof(float));
592    st->x1d=calloc(st->frame_size, sizeof(float));
593    st->high=calloc(st->full_frame_size, sizeof(float));
594    st->y0=calloc(st->full_frame_size, sizeof(float));
595    st->y1=calloc(st->full_frame_size, sizeof(float));
596
597    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
598    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
599    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
600    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
601
602    st->exc=calloc(st->frame_size, sizeof(float));
603
604    st->qlsp = malloc(st->lpcSize*sizeof(float));
605    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
606    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
607    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
608
609    st->mem_sp = calloc(st->lpcSize, sizeof(float));
610    st->mem_sw = calloc(st->lpcSize, sizeof(float));
611
612 }
613
614 void sb_decoder_destroy(SBDecState *st)
615 {
616    decoder_destroy(&st->st_low);
617    free(st->x0);
618    free(st->x0d);
619    free(st->x1);
620    free(st->x1d);
621    free(st->high);
622    free(st->y0);
623    free(st->y1);
624    free(st->h0_mem);
625    free(st->h1_mem);
626    free(st->g0_mem);
627    free(st->g1_mem);
628    
629    free(st->exc);
630    free(st->qlsp);
631    free(st->old_qlsp);
632    free(st->interp_qlsp);
633    free(st->interp_qlpc);
634
635    free(st->mem_sp);
636    free(st->mem_sw);
637
638    free(st->stack);
639    
640 }
641
642
643 void sb_decode(SBDecState *st, FrameBits *bits, float *out)
644 {
645    int i, sub;
646
647    /* Decode the low-band */
648    decode(&st->st_low, bits, st->x0d);
649
650    for (i=0;i<st->frame_size;i++)
651       st->exc[i]=0;
652
653    lsp_unquant_high(st->qlsp, st->lpcSize, bits);
654    
655    if (st->first)
656    {
657       for (i=0;i<st->lpcSize;i++)
658          st->old_qlsp[i] = st->qlsp[i];
659    }
660    
661    for (sub=0;sub<st->nbSubframes;sub++)
662    {
663       float *exc, *sp, tmp, filter_ratio, gain, el=0;
664       int offset;
665       
666       offset = st->subframeSize*sub;
667       sp=st->high+offset;
668       exc=st->exc+offset;
669       
670       /* LSP interpolation */
671       tmp = (.5 + sub)/st->nbSubframes;
672       for (i=0;i<st->lpcSize;i++)
673          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
674
675       /* LSPs to x-domain */
676       for (i=0;i<st->lpcSize;i++)
677          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
678
679       /* LSP to LPC */
680       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
681
682       {
683          float rl=0, rh=0;
684          tmp=1;
685          for (i=0;i<=st->lpcSize;i++)
686          {
687             rh += tmp*st->interp_qlpc[i];
688             tmp = -tmp;
689          }
690          rl = st->st_low.pi_gain[sub];
691          rl=1/(fabs(rl)+.01);
692          rh=1/(fabs(rh)+.01);
693          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
694       }
695
696       for (i=0;i<st->subframeSize;i++)
697          el+=sqr(st->st_low.exc[offset+i]);
698       gain=(1+sqrt(el/st->subframeSize))/filter_ratio;
699       
700       for (i=0;i<st->subframeSize;i++)
701          exc[i]=0;
702       split_cb_nogain_unquant(exc, &split_cb_high, st->subframeSize, gain, 
703                               bits, st->stack);
704
705       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
706
707    }
708
709    /* Up-sample coded low-band and high-band*/
710    for (i=0;i<st->frame_size;i++)
711    {
712       st->x0[(i<<1)]=st->x0d[i];
713       st->x1[(i<<1)]=st->high[i];
714       st->x0[(i<<1)+1]=0;
715       st->x1[(i<<1)+1]=0;
716    }
717    /* Reconstruct the original */
718    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
719    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
720    for (i=0;i<st->full_frame_size;i++)
721       out[i]=2*(st->y0[i]-st->y1[i]);
722
723    for (i=0;i<st->lpcSize;i++)
724       st->old_qlsp[i] = st->qlsp[i];
725
726    st->first=0;
727
728 }