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