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