Wideband quantization improved by adding a sub-frame gain, codebook
[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 "nb_celp.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(SpeexMode *m)
188 {
189    int i;
190    SBEncState *st;
191    SpeexSBMode *mode;
192
193    st = malloc(sizeof(SBEncState));
194    st->mode = m;
195    mode = m->mode;
196
197    st->st_low = nb_encoder_init(mode->nb_mode);
198    st->full_frame_size = 2*mode->frameSize;
199    st->frame_size = mode->frameSize;
200    st->subframeSize = mode->subframeSize;
201    st->nbSubframes = mode->frameSize/mode->subframeSize;
202    st->windowSize = mode->windowSize;
203    st->lpcSize=mode->lpcSize;
204    st->bufSize=mode->bufSize;
205
206    st->lag_factor = mode->lag_factor;
207    st->lpc_floor = mode->lpc_floor;
208    st->gamma1=mode->gamma1;
209    st->gamma2=mode->gamma2;
210    st->first=1;
211    st->stack = calloc(10000, sizeof(float));
212
213    st->x0=calloc(st->full_frame_size, sizeof(float));
214    st->x1=calloc(st->full_frame_size, sizeof(float));
215    st->x0d=calloc(st->frame_size, sizeof(float));
216    st->x1d=calloc(st->frame_size, sizeof(float));
217    st->high=calloc(st->full_frame_size, sizeof(float));
218    st->y0=calloc(st->full_frame_size, sizeof(float));
219    st->y1=calloc(st->full_frame_size, sizeof(float));
220
221    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
222    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
223    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
224    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
225
226    st->buf=calloc(st->windowSize, sizeof(float));
227    st->excBuf=calloc(st->bufSize, sizeof(float));
228    st->exc = st->excBuf + st->bufSize - st->windowSize;
229    /*st->exc=st->excBuf+st->frame_size;*/
230
231    st->res=calloc(st->frame_size, sizeof(float));
232    st->sw=calloc(st->frame_size, sizeof(float));
233    st->target=calloc(st->frame_size, sizeof(float));
234    st->window=calloc(st->windowSize, sizeof(float));
235    for (i=0;i<st->windowSize;i++)
236       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
237
238    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
239    for (i=0;i<st->lpcSize+1;i++)
240       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
241
242    st->rc = malloc(st->lpcSize*sizeof(float));
243    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
244    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
245    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
246    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
247    st->lsp = malloc(st->lpcSize*sizeof(float));
248    st->qlsp = malloc(st->lpcSize*sizeof(float));
249    st->old_lsp = malloc(st->lpcSize*sizeof(float));
250    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
251    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
252    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
253    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
254    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
255
256    st->mem_sp = calloc(st->lpcSize, sizeof(float));
257    st->mem_sp2 = calloc(st->lpcSize, sizeof(float));
258    st->mem_sw = calloc(st->lpcSize, sizeof(float));
259
260    st->lsp_quant = mode->lsp_quant;
261    st->innovation_quant = mode->innovation_quant;
262    st->innovation_params = mode->innovation_params;
263    return st;
264 }
265
266 void sb_encoder_destroy(void *state)
267 {
268    SBEncState *st=state;
269
270    nb_encoder_destroy(st->st_low);
271    free(st->x0);
272    free(st->x0d);
273    free(st->x1);
274    free(st->x1d);
275    free(st->high);
276    free(st->y0);
277    free(st->y1);
278    free(st->h0_mem);
279    free(st->h1_mem);
280    free(st->g0_mem);
281    free(st->g1_mem);
282    
283    free(st->buf);
284    free(st->window);
285    free(st->excBuf);
286    free(st->sw);
287    free(st->res);
288    free(st->target);
289    free(st->lagWindow);
290    free(st->rc);
291    free(st->autocorr);
292    free(st->lpc);
293    free(st->bw_lpc1);
294    free(st->bw_lpc2);
295    free(st->lsp);
296    free(st->qlsp);
297    free(st->old_lsp);
298    free(st->old_qlsp);
299    free(st->interp_lsp);
300    free(st->interp_qlsp);
301    free(st->interp_lpc);
302    free(st->interp_qlpc);
303
304    free(st->mem_sp);
305    free(st->mem_sp2);
306    free(st->mem_sw);
307
308    free(st->stack);
309
310    free(st);
311 }
312
313
314 void sb_encode(void *state, float *in, FrameBits *bits)
315 {
316    SBEncState *st;
317    int i, roots, sub;
318
319    st = state;
320
321    /* Compute the two sub-bands by filtering with h0 and h1*/
322    fir_mem(in, h0, st->x0, st->full_frame_size, QMF_ORDER, st->h0_mem);
323    fir_mem(in, h1, st->x1, st->full_frame_size, QMF_ORDER, st->h1_mem);
324    /* Down-sample x0 and x1 */
325    for (i=0;i<st->frame_size;i++)
326    {
327       st->x0d[i]=st->x0[i<<1];
328       st->x1d[i]=st->x1[i<<1];
329    }
330    /* Encode the narrowband part*/
331    nb_encode(st->st_low, st->x0d, bits);
332
333    /* High-band buffering / sync with low band */
334    for (i=0;i<st->frame_size;i++)
335    {
336       /*st->excBuf[i]=st->exc[i];*/
337       st->high[i]=st->high[st->frame_size+i];
338       st->high[st->frame_size+i]=st->x1d[i];
339    }
340
341    memmove(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
342
343    /* Start encoding the high-band */
344
345    for (i=0;i<st->windowSize;i++)
346       st->buf[i] = st->high[i] * st->window[i];
347
348    /* Compute auto-correlation */
349    autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
350
351    st->autocorr[0] += 1;        /* prevents NANs */
352    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
353    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
354    for (i=0;i<st->lpcSize+1;i++)
355       st->autocorr[i] *= st->lagWindow[i];
356
357    /* Levinson-Durbin */
358    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
359    st->lpc[0]=1;
360
361    /* LPC to LSPs (x-domain) transform */
362    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
363    if (roots!=st->lpcSize)
364    {
365       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
366       exit(1);
367    }
368
369    /* x-domain to angle domain*/
370    for (i=0;i<st->lpcSize;i++)
371       st->lsp[i] = acos(st->lsp[i]);
372
373    /* LSP quantization */
374    st->lsp_quant(st->lsp, st->qlsp, st->lpcSize, bits);
375    
376    /*printf ("high_lsp:");
377    for (i=0;i<st->lpcSize;i++)
378       printf (" %f", st->lsp[i]);
379       printf ("\n");*/
380    /*for (i=0;i<st->lpcSize;i++)
381      st->qlsp[i]=st->lsp[i];*/
382    
383
384    if (st->first)
385    {
386       for (i=0;i<st->lpcSize;i++)
387          st->old_lsp[i] = st->lsp[i];
388       for (i=0;i<st->lpcSize;i++)
389          st->old_qlsp[i] = st->qlsp[i];
390    }
391    
392    for (sub=0;sub<st->nbSubframes;sub++)
393    {
394       float *exc, *sp, *mem, *res, *target, *sw, tmp, filter_ratio;
395       int offset;
396       float rl, rh, eh=0, el=0;
397       int fold;
398
399       offset = st->subframeSize*sub;
400       sp=st->high+offset;
401       exc=st->excBuf+offset;
402       res=st->res+offset;
403       target=st->target+offset;
404       sw=st->sw+offset;
405
406       mem=PUSH(st->stack, st->lpcSize);
407       
408       /* LSP interpolation (quantized and unquantized) */
409       tmp = (.5 + sub)/st->nbSubframes;
410       for (i=0;i<st->lpcSize;i++)
411          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
412       for (i=0;i<st->lpcSize;i++)
413          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
414
415       /* Compute interpolated LPCs (quantized and unquantized) */
416       for (i=0;i<st->lpcSize;i++)
417          st->interp_lsp[i] = cos(st->interp_lsp[i]);
418       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
419
420       for (i=0;i<st->lpcSize;i++)
421          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
422       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
423
424       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
425       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
426
427       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
428          filters */
429       rl=rh=0;
430       tmp=1;
431       for (i=0;i<=st->lpcSize;i++)
432       {
433          rh += tmp*st->interp_qlpc[i];
434          tmp = -tmp;
435       }
436       rl = ((EncState*)st->st_low)->pi_gain[sub];
437       rl=1/(fabs(rl)+.01);
438       rh=1/(fabs(rh)+.01);
439       /* Compute ratio, will help predict the gain */
440       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
441
442       fold = filter_ratio<5;
443       /*printf ("filter_ratio %f\n", filter_ratio);*/
444       fold=0;
445
446       /* Compute "real excitation" */
447       residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
448       /* Compute energy of low-band and high-band excitation */
449       for (i=0;i<st->subframeSize;i++)
450          eh+=sqr(exc[i]);
451       for (i=0;i<st->subframeSize;i++)
452          el+=sqr(((EncState*)st->st_low)->exc[offset+i]);
453
454       if (fold) {/* 1 for spectral folding excitation, 0 for stochastic */
455          float g;
456          /*speex_bits_pack(bits, 1, 1);*/
457
458 #if 1
459          /* Gain to use if we want to use the low-band excitation for high-band */
460          g=eh/(.01+el);
461          g=sqrt(g);
462
463          g *= filter_ratio;
464          /* Gain quantization */
465          {
466             int quant = (int) floor(.5 + 9.4 * log(10*(g+.0001)));
467             if (quant<0)
468                quant=0;
469             if (quant>31)
470                quant=31;
471             speex_bits_pack(bits, quant, 5);
472             g= .1*exp(quant/9.4);
473          }
474          /*printf ("folding gain: %f\n", g);*/
475          g /= filter_ratio;
476
477          /* High-band excitation using the low-band excitation and a gain */
478          for (i=0;i<st->subframeSize;i++)
479             exc[i]=g*((EncState*)st->st_low)->exc[offset+i];
480 #endif
481          /* Update the input signal using the non-coded memory */
482          /* FIXME: is that right? */
483          /*syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);*/
484
485          /* FIXME: Update perceptually weighted signal in case we switch to the
486             other mode */
487       } else {
488          float gc, scale, scale_1;
489          float *innov;
490
491          /*speex_bits_pack(bits, 0, 1);*/
492          innov = PUSH(st->stack, st->subframeSize);
493
494
495          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
496
497          {
498             int qgc = (int)floor(.5+3.7*(log(gc)+2));
499             if (qgc<0)
500                qgc=0;
501             if (qgc>15)
502                qgc=15;
503             speex_bits_pack(bits, qgc, 4);
504             gc = exp((1/3.7)*qgc-2);
505          }
506
507          scale = gc*sqrt(1+el)/filter_ratio;
508          scale_1 = 1/scale;
509          if (0 && rand()%5==0)
510          {
511             float sc = 1/sqrt(.1+eh/st->subframeSize);
512             if (rand()&1)
513                sc=-sc;
514             for (i=0;i<st->subframeSize;i++)
515             {
516                float tmp=exc[i]*sc;
517                if (i%8==0)
518                   printf ("\nhexc");
519                printf (" %f", tmp);
520             }
521          }
522
523          /* Reset excitation */
524          for (i=0;i<st->subframeSize;i++)
525             exc[i]=0;
526          
527          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
528          for (i=0;i<st->lpcSize;i++)
529             mem[i]=st->mem_sp[i];
530          syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
531          for (i=0;i<st->lpcSize;i++)
532             mem[i]=st->mem_sp[i];
533          residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
534          for (i=0;i<st->lpcSize;i++)
535             mem[i]=st->mem_sw[i];
536          syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
537          
538          /* Compute weighted signal */
539          for (i=0;i<st->lpcSize;i++)
540             mem[i]=st->mem_sp[i];
541          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
542          for (i=0;i<st->lpcSize;i++)
543             mem[i]=st->mem_sw[i];
544          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
545          
546          /* Compute target signal */
547          for (i=0;i<st->subframeSize;i++)
548             target[i]=sw[i]-res[i];
549
550          for (i=0;i<st->subframeSize;i++)
551            exc[i]=0;
552
553
554          for (i=0;i<st->subframeSize;i++)
555             target[i]*=scale_1;
556          
557          /* Reset excitation */
558          for (i=0;i<st->subframeSize;i++)
559             innov[i]=0;
560
561          /*print_vec(target, st->subframeSize, "\ntarget");*/
562          st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
563                                 st->innovation_params, st->lpcSize, st->subframeSize, 
564                                 innov, bits, st->stack);
565          /*print_vec(target, st->subframeSize, "after");*/
566
567          for (i=0;i<st->subframeSize;i++)
568             exc[i] += innov[i]*scale;
569
570          POP(st->stack);
571       }
572 #if 1
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 #endif
583       
584       
585       POP(st->stack);
586    }
587
588
589 #ifndef RELEASE
590    /* Up-sample coded low-band and high-band*/
591    for (i=0;i<st->frame_size;i++)
592    {
593       st->x0[(i<<1)]=st->x0d[i];
594       st->x1[(i<<1)]=st->high[i];
595       st->x0[(i<<1)+1]=0;
596       st->x1[(i<<1)+1]=0;
597    }
598    /* Reconstruct the original */
599    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
600    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
601    for (i=0;i<st->full_frame_size;i++)
602       in[i]=2*(st->y0[i]-st->y1[i]);
603 #endif
604    for (i=0;i<st->lpcSize;i++)
605       st->old_lsp[i] = st->lsp[i];
606    for (i=0;i<st->lpcSize;i++)
607       st->old_qlsp[i] = st->qlsp[i];
608
609    st->first=0;
610 }
611
612
613
614
615
616 void *sb_decoder_init(SpeexMode *m)
617 {
618    int i;
619    SBDecState *st;
620    SpeexSBMode *mode;
621    st = malloc(sizeof(SBDecState));
622    st->mode = m;
623    mode=m->mode;
624
625    st->st_low = nb_decoder_init(mode->nb_mode);
626    st->full_frame_size = 2*mode->frameSize;
627    st->frame_size = mode->frameSize;
628    st->subframeSize = 40;
629    st->nbSubframes = 4;
630    st->lpcSize=8;
631    st->pf_order=15;
632    st->pf_gamma=.05;
633
634    st->first=1;
635    st->stack = calloc(10000, sizeof(float));
636
637    st->x0=calloc(st->full_frame_size, sizeof(float));
638    st->x1=calloc(st->full_frame_size, sizeof(float));
639    st->x0d=calloc(st->frame_size, sizeof(float));
640    st->x1d=calloc(st->frame_size, sizeof(float));
641    st->high=calloc(st->full_frame_size, sizeof(float));
642    st->y0=calloc(st->full_frame_size, sizeof(float));
643    st->y1=calloc(st->full_frame_size, sizeof(float));
644
645    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
646    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
647    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
648    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
649
650    st->pf_exc=calloc(st->full_frame_size, sizeof(float));
651    st->exc=calloc(st->frame_size, sizeof(float));
652    st->pf_window=calloc(st->full_frame_size, sizeof(float));
653    st->pf_autocorr=calloc(st->pf_order+1, sizeof(float));
654    st->pf_lpc=calloc(st->pf_order+1, sizeof(float));
655    st->pf_bwlpc=calloc(st->pf_order+1, sizeof(float));
656    for (i=0;i<st->full_frame_size;i++)
657       st->pf_window[i]=.5*(1-cos(2*M_PI*i/st->full_frame_size));
658
659    st->qlsp = malloc(st->lpcSize*sizeof(float));
660    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
661    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
662    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
663
664    st->mem_sp = calloc(st->lpcSize, sizeof(float));
665    st->mem_pf_exc1 = calloc(st->pf_order, sizeof(float));
666    st->mem_pf_exc2 = calloc(st->pf_order, sizeof(float));
667    st->mem_pf_sp = calloc(st->pf_order, sizeof(float));
668
669    st->lsp_unquant = mode->lsp_unquant;
670    st->innovation_unquant = mode->innovation_unquant;
671    st->innovation_params = mode->innovation_params;
672    return st;
673 }
674
675 void sb_decoder_destroy(void *state)
676 {
677    SBDecState *st;
678    st = state;
679    nb_decoder_destroy(st->st_low);
680    free(st->x0);
681    free(st->x0d);
682    free(st->x1);
683    free(st->x1d);
684    free(st->high);
685    free(st->y0);
686    free(st->y1);
687    free(st->h0_mem);
688    free(st->h1_mem);
689    free(st->g0_mem);
690    free(st->g1_mem);
691    
692    free(st->exc);
693    free(st->pf_exc);
694    free(st->pf_window);
695    free(st->pf_autocorr);
696    free(st->pf_lpc);
697    free(st->pf_bwlpc);
698    free(st->qlsp);
699    free(st->old_qlsp);
700    free(st->interp_qlsp);
701    free(st->interp_qlpc);
702
703    free(st->mem_sp);
704    free(st->mem_pf_exc1);
705    free(st->mem_pf_exc2);
706    free(st->mem_pf_sp);
707
708    free(st->stack);
709
710    free(state);
711 }
712
713
714 void sb_decode(void *state, FrameBits *bits, float *out)
715 {
716    int i, sub;
717    SBDecState *st;
718    
719    st = state;
720    /* Decode the low-band */
721    nb_decode(st->st_low, bits, st->x0d);
722
723    for (i=0;i<st->frame_size;i++)
724       st->exc[i]=0;
725
726    st->lsp_unquant(st->qlsp, st->lpcSize, bits);
727    
728    if (st->first)
729    {
730       for (i=0;i<st->lpcSize;i++)
731          st->old_qlsp[i] = st->qlsp[i];
732    }
733    
734    for (sub=0;sub<st->nbSubframes;sub++)
735    {
736       float *exc, *sp, tmp, filter_ratio, gain, el=0;
737       int offset;
738       
739       offset = st->subframeSize*sub;
740       sp=st->high+offset;
741       exc=st->exc+offset;
742       
743       /* LSP interpolation */
744       tmp = (.5 + sub)/st->nbSubframes;
745       for (i=0;i<st->lpcSize;i++)
746          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
747
748       /* LSPs to x-domain */
749       for (i=0;i<st->lpcSize;i++)
750          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
751
752       /* LSP to LPC */
753       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
754
755       {
756          float rl=0, rh=0;
757          tmp=1;
758          for (i=0;i<=st->lpcSize;i++)
759          {
760             rh += tmp*st->interp_qlpc[i];
761             tmp = -tmp;
762          }
763          rl = ((DecState*)st->st_low)->pi_gain[sub];
764          rl=1/(fabs(rl)+.01);
765          rh=1/(fabs(rh)+.01);
766          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
767       }
768
769       for (i=0;i<st->subframeSize;i++)
770          el+=sqr(((DecState*)st->st_low)->exc[offset+i]);
771       gain=(1+sqrt(el/st->subframeSize))/filter_ratio;
772       
773       for (i=0;i<st->subframeSize;i++)
774          exc[i]=0;
775       if (0/*speex_bits_unpack_unsigned(bits, 1)*/)
776       {
777          float g;
778          int quant;
779          quant = speex_bits_unpack_unsigned(bits, 5);
780          g= .1*exp(quant/9.4);
781          
782          /*printf ("unquant folding gain: %f\n", g);*/
783          g /= filter_ratio;
784          
785          g *= .8;
786          /* High-band excitation using the low-band excitation and a gain */
787          for (i=0;i<st->subframeSize;i++)
788             exc[i]=g*((DecState*)st->st_low)->exc[offset+i];
789       } else {
790          float gc, scale;
791          int qgc = speex_bits_unpack_unsigned(bits, 4);
792
793          gc = exp((1/3.7)*qgc-2);
794
795          scale = gc*sqrt(1+el)/filter_ratio;
796
797
798          st->innovation_unquant(exc, st->innovation_params, st->subframeSize, 
799                                 bits, st->stack);
800          for (i=0;i<st->subframeSize;i++)
801             exc[i]*=scale;
802       }
803       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
804
805    }
806
807    /* Up-sample coded low-band and high-band*/
808    for (i=0;i<st->frame_size;i++)
809    {
810       st->x0[(i<<1)]=st->x0d[i];
811       st->x1[(i<<1)]=st->high[i];
812       st->x0[(i<<1)+1]=0;
813       st->x1[(i<<1)+1]=0;
814    }
815    /* Reconstruct the original */
816    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
817    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
818    for (i=0;i<st->full_frame_size;i++)
819       out[i]=2*(st->y0[i]-st->y1[i]);
820
821
822    if (0)
823    {
824       float tmp=1, e1=0, e2=0, g;
825       for (i=0;i<st->full_frame_size;i++)
826          st->pf_exc[i] = out[i] * st->pf_window[i];
827       
828       /* Compute auto-correlation */
829       autocorr(st->pf_exc, st->pf_autocorr, st->pf_order+1, st->full_frame_size);
830       
831       st->pf_autocorr[0] += 1;        /* prevents NANs */
832       st->pf_autocorr[0] *= 1.0001;    /* Noise floor in auto-correlation domain */
833       
834       /* Levinson-Durbin */
835       wld(st->pf_lpc+1, st->pf_autocorr, st->pf_exc, st->pf_order);
836       st->pf_lpc[0]=1;
837       
838       for (i=1;i<st->pf_order;i++)
839       {
840          tmp*=st->pf_gamma;
841          st->pf_bwlpc[i] = st->pf_lpc[i]*tmp;
842       }
843       st->pf_bwlpc[0]=1;
844       
845       /*print_vec(st->pf_lpc, st->pf_order, "post-filter LPC");*/
846       residue_mem(out, st->pf_lpc, st->pf_exc, st->full_frame_size, 
847                   st->pf_order, st->mem_pf_exc1);
848       for (i=0;i<st->full_frame_size;i++)
849          e1 += st->pf_exc[i]*st->pf_exc[i];
850       syn_filt_mem(st->pf_exc, st->pf_bwlpc, st->pf_exc, st->full_frame_size, 
851                   st->pf_order, st->mem_pf_exc2);
852       for (i=0;i<st->full_frame_size;i++)
853          e2 += st->pf_exc[i]*st->pf_exc[i];
854       g=sqrt(e1/(e2+.1));
855       /*printf ("post-filter gain: %f\n", g);*/
856       for (i=0;i<st->full_frame_size;i++)
857          st->pf_exc[i]=g*st->pf_exc[i];
858
859       syn_filt_mem(st->pf_exc, st->pf_lpc, out, st->full_frame_size, 
860                   st->pf_order, st->mem_pf_sp);
861    }
862
863
864    for (i=0;i<st->lpcSize;i++)
865       st->old_qlsp[i] = st->qlsp[i];
866
867    st->first=0;
868
869 }