Changed the LPC analysis window to an asymetric (pseudo-)Hanning window
[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 #include <stdlib.h>
35
36 #ifndef M_PI
37 #define M_PI           3.14159265358979323846  /* pi */
38 #endif
39
40 #define sqr(x) ((x)*(x))
41
42
43 #if 0
44 #define QMF_ORDER 32
45 static float h0[32] = {
46    0.0006910579, -0.001403793,
47    -0.001268303, 0.004234195,
48    0.001414246, -0.009458318,
49    -0.0001303859, 0.01798145,
50    -0.004187483, -0.03123862,
51    0.01456844, 0.05294745,
52    -0.03934878, -0.09980243,
53    0.1285579, 0.4664053,
54    0.4664053, 0.1285579,
55    -0.09980243, -0.03934878,
56    0.05294745, 0.01456844,
57    -0.03123862, -0.004187483,
58    0.01798145, -0.0001303859,
59    -0.009458318, 0.001414246,
60    0.004234195, -0.001268303,
61    -0.001403793, 0.0006910579
62 };
63
64 static float h1[32] = {
65    0.0006910579, 0.001403793,
66    -0.001268303, -0.004234195,
67    0.001414246, 0.009458318,
68    -0.0001303859, -0.01798145,
69    -0.004187483, 0.03123862,
70    0.01456844, -0.05294745,
71    -0.03934878, 0.09980243,
72    0.1285579, -0.4664053,
73    0.4664053, -0.1285579,
74    -0.09980243, 0.03934878,
75    0.05294745, -0.01456844,
76    -0.03123862, 0.004187483,
77    0.01798145, 0.0001303859,
78    -0.009458318, -0.001414246,
79    0.004234195, 0.001268303,
80    -0.001403793, -0.0006910579
81 };
82 #else 
83 #define QMF_ORDER 64
84 static float h0[64] = {
85    3.596189e-05, -0.0001123515,
86    -0.0001104587, 0.0002790277,
87    0.0002298438, -0.0005953563,
88    -0.0003823631, 0.00113826,
89    0.0005308539, -0.001986177,
90    -0.0006243724, 0.003235877,
91    0.0005743159, -0.004989147,
92    -0.0002584767, 0.007367171,
93    -0.0004857935, -0.01050689,
94    0.001894714, 0.01459396,
95    -0.004313674, -0.01994365,
96    0.00828756, 0.02716055,
97    -0.01485397, -0.03764973,
98    0.026447, 0.05543245,
99    -0.05095487, -0.09779096,
100    0.1382363, 0.4600981,
101    0.4600981, 0.1382363,
102    -0.09779096, -0.05095487,
103    0.05543245, 0.026447,
104    -0.03764973, -0.01485397,
105    0.02716055, 0.00828756,
106    -0.01994365, -0.004313674,
107    0.01459396, 0.001894714,
108    -0.01050689, -0.0004857935,
109    0.007367171, -0.0002584767,
110    -0.004989147, 0.0005743159,
111    0.003235877, -0.0006243724,
112    -0.001986177, 0.0005308539,
113    0.00113826, -0.0003823631,
114    -0.0005953563, 0.0002298438,
115    0.0002790277, -0.0001104587,
116    -0.0001123515, 3.596189e-05
117 };
118
119 static float h1[64] = {
120    3.596189e-05, 0.0001123515,
121    -0.0001104587, -0.0002790277,
122    0.0002298438, 0.0005953563,
123    -0.0003823631, -0.00113826,
124    0.0005308539, 0.001986177,
125    -0.0006243724, -0.003235877,
126    0.0005743159, 0.004989147,
127    -0.0002584767, -0.007367171,
128    -0.0004857935, 0.01050689,
129    0.001894714, -0.01459396,
130    -0.004313674, 0.01994365,
131    0.00828756, -0.02716055,
132    -0.01485397, 0.03764973,
133    0.026447, -0.05543245,
134    -0.05095487, 0.09779096,
135    0.1382363, -0.4600981,
136    0.4600981, -0.1382363,
137    -0.09779096, 0.05095487,
138    0.05543245, -0.026447,
139    -0.03764973, 0.01485397,
140    0.02716055, -0.00828756,
141    -0.01994365, 0.004313674,
142    0.01459396, -0.001894714,
143    -0.01050689, 0.0004857935,
144    0.007367171, 0.0002584767,
145    -0.004989147, -0.0005743159,
146    0.003235877, 0.0006243724,
147    -0.001986177, -0.0005308539,
148    0.00113826, 0.0003823631,
149    -0.0005953563, -0.0002298438,
150    0.0002790277, 0.0001104587,
151    -0.0001123515, -3.596189e-05
152 };
153 #endif
154
155 void *sb_encoder_init(SpeexMode *m)
156 {
157    int i;
158    SBEncState *st;
159    SpeexSBMode *mode;
160
161    st = malloc(sizeof(SBEncState));
162    st->mode = m;
163    mode = m->mode;
164
165    st->st_low = nb_encoder_init(mode->nb_mode);
166    st->full_frame_size = 2*mode->frameSize;
167    st->frame_size = mode->frameSize;
168    st->subframeSize = mode->subframeSize;
169    st->nbSubframes = mode->frameSize/mode->subframeSize;
170    st->windowSize = st->frame_size*3/2;
171    st->lpcSize=mode->lpcSize;
172    st->bufSize=mode->bufSize;
173
174    st->lag_factor = mode->lag_factor;
175    st->lpc_floor = mode->lpc_floor;
176    st->gamma1=mode->gamma1;
177    st->gamma2=mode->gamma2;
178    st->first=1;
179    st->stack = calloc(10000, sizeof(float));
180
181    st->x0=calloc(st->full_frame_size, sizeof(float));
182    st->x1=calloc(st->full_frame_size, sizeof(float));
183    st->x0d=calloc(st->frame_size, sizeof(float));
184    st->x1d=calloc(st->frame_size, sizeof(float));
185    st->high=calloc(st->full_frame_size, sizeof(float));
186    st->y0=calloc(st->full_frame_size, sizeof(float));
187    st->y1=calloc(st->full_frame_size, sizeof(float));
188
189    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
190    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
191    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
192    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
193
194    st->buf=calloc(st->windowSize, sizeof(float));
195    st->excBuf=calloc(st->bufSize, sizeof(float));
196    st->exc = st->excBuf + st->bufSize - st->windowSize;
197    /*st->exc=st->excBuf+st->frame_size;*/
198
199    st->res=calloc(st->frame_size, sizeof(float));
200    st->sw=calloc(st->frame_size, sizeof(float));
201    st->target=calloc(st->frame_size, sizeof(float));
202    /*Asymetric "pseudo-Hanning" window*/
203    {
204       int part1, part2;
205       part1 = st->subframeSize*7/2;
206       part2 = st->subframeSize*5/2;
207       st->window = malloc(st->windowSize*sizeof(float));
208       for (i=0;i<part1;i++)
209          st->window[i]=.5*(1-cos(M_PI*i/part1));
210       for (i=0;i<part2;i++)
211          st->window[part1+i]=.5*(1+cos(M_PI*i/part2));
212    }
213
214    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
215    for (i=0;i<st->lpcSize+1;i++)
216       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
217
218    st->rc = malloc(st->lpcSize*sizeof(float));
219    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
220    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
221    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
222    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
223    st->lsp = malloc(st->lpcSize*sizeof(float));
224    st->qlsp = malloc(st->lpcSize*sizeof(float));
225    st->old_lsp = malloc(st->lpcSize*sizeof(float));
226    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
227    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
228    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
229    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
230    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
231
232    st->mem_sp = calloc(st->lpcSize, sizeof(float));
233    st->mem_sp2 = calloc(st->lpcSize, sizeof(float));
234    st->mem_sw = calloc(st->lpcSize, sizeof(float));
235
236    st->lsp_quant = mode->lsp_quant;
237    st->innovation_quant = mode->innovation_quant;
238    st->innovation_params = mode->innovation_params;
239    return st;
240 }
241
242 void sb_encoder_destroy(void *state)
243 {
244    SBEncState *st=state;
245
246    nb_encoder_destroy(st->st_low);
247    free(st->x0);
248    free(st->x0d);
249    free(st->x1);
250    free(st->x1d);
251    free(st->high);
252    free(st->y0);
253    free(st->y1);
254    free(st->h0_mem);
255    free(st->h1_mem);
256    free(st->g0_mem);
257    free(st->g1_mem);
258    
259    free(st->buf);
260    free(st->window);
261    free(st->excBuf);
262    free(st->sw);
263    free(st->res);
264    free(st->target);
265    free(st->lagWindow);
266    free(st->rc);
267    free(st->autocorr);
268    free(st->lpc);
269    free(st->bw_lpc1);
270    free(st->bw_lpc2);
271    free(st->lsp);
272    free(st->qlsp);
273    free(st->old_lsp);
274    free(st->old_qlsp);
275    free(st->interp_lsp);
276    free(st->interp_qlsp);
277    free(st->interp_lpc);
278    free(st->interp_qlpc);
279
280    free(st->mem_sp);
281    free(st->mem_sp2);
282    free(st->mem_sw);
283
284    free(st->stack);
285
286    free(st);
287 }
288
289
290 void sb_encode(void *state, float *in, SpeexBits *bits)
291 {
292    SBEncState *st;
293    int i, roots, sub;
294
295    st = state;
296
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    nb_encode(st->st_low, st->x0d, bits);
308
309    /* High-band buffering / sync with low band */
310 #if 0
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 #else
318    for (i=0;i<st->windowSize-st->frame_size;i++)
319       st->high[i] = st->high[st->frame_size+i];
320    for (i=0;i<st->frame_size;i++)
321       st->high[st->windowSize-st->frame_size+i]=st->x1d[i];
322 #endif
323
324    memmove(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
325
326    /* Start encoding the high-band */
327
328    for (i=0;i<st->windowSize;i++)
329       st->buf[i] = st->high[i] * st->window[i];
330
331    /* Compute auto-correlation */
332    autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
333
334    st->autocorr[0] += 1;        /* prevents NANs */
335    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
336    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
337    for (i=0;i<st->lpcSize+1;i++)
338       st->autocorr[i] *= st->lagWindow[i];
339
340    /* Levinson-Durbin */
341    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
342    st->lpc[0]=1;
343
344    /* LPC to LSPs (x-domain) transform */
345    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
346    if (roots!=st->lpcSize)
347    {
348       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
349       exit(1);
350    }
351
352    /* x-domain to angle domain*/
353    for (i=0;i<st->lpcSize;i++)
354       st->lsp[i] = acos(st->lsp[i]);
355
356    /* LSP quantization */
357    st->lsp_quant(st->lsp, st->qlsp, st->lpcSize, bits);
358    
359    /*printf ("high_lsp:");
360    for (i=0;i<st->lpcSize;i++)
361       printf (" %f", st->lsp[i]);
362       printf ("\n");*/
363    /*for (i=0;i<st->lpcSize;i++)
364      st->qlsp[i]=st->lsp[i];*/
365    
366
367    if (st->first)
368    {
369       for (i=0;i<st->lpcSize;i++)
370          st->old_lsp[i] = st->lsp[i];
371       for (i=0;i<st->lpcSize;i++)
372          st->old_qlsp[i] = st->qlsp[i];
373    }
374    
375    for (sub=0;sub<st->nbSubframes;sub++)
376    {
377       float *exc, *sp, *mem, *res, *target, *sw, tmp, filter_ratio;
378       int offset;
379       float rl, rh, eh=0, el=0;
380       int fold;
381
382       offset = st->subframeSize*sub;
383       sp=st->high+offset;
384       exc=st->excBuf+offset;
385       res=st->res+offset;
386       target=st->target+offset;
387       sw=st->sw+offset;
388
389       mem=PUSH(st->stack, st->lpcSize);
390       
391       /* LSP interpolation (quantized and unquantized) */
392       tmp = (1.0 + sub)/st->nbSubframes;
393       for (i=0;i<st->lpcSize;i++)
394          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
395       for (i=0;i<st->lpcSize;i++)
396          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
397       
398       /* Compute interpolated LPCs (quantized and unquantized) */
399       for (i=0;i<st->lpcSize;i++)
400          st->interp_lsp[i] = cos(st->interp_lsp[i]);
401       for (i=0;i<st->lpcSize;i++)
402          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
403
404       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
405       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
406
407       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
408       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
409
410       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
411       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
412
413       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
414          filters */
415       rl=rh=0;
416       tmp=1;
417       for (i=0;i<=st->lpcSize;i++)
418       {
419          rh += tmp*st->interp_qlpc[i];
420          tmp = -tmp;
421       }
422       rl = ((EncState*)st->st_low)->pi_gain[sub];
423       rl=1/(fabs(rl)+.01);
424       rh=1/(fabs(rh)+.01);
425       /* Compute ratio, will help predict the gain */
426       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
427
428       fold = filter_ratio<5;
429       /*printf ("filter_ratio %f\n", filter_ratio);*/
430       fold=0;
431
432       /* Compute "real excitation" */
433       residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
434       /* Compute energy of low-band and high-band excitation */
435       for (i=0;i<st->subframeSize;i++)
436          eh+=sqr(exc[i]);
437       for (i=0;i<st->subframeSize;i++)
438          el+=sqr(((EncState*)st->st_low)->exc[offset+i]);
439
440       if (fold) {/* 1 for spectral folding excitation, 0 for stochastic */
441          float g;
442          /*speex_bits_pack(bits, 1, 1);*/
443
444 #if 1
445          /* Gain to use if we want to use the low-band excitation for high-band */
446          g=eh/(.01+el);
447          g=sqrt(g);
448
449          g *= filter_ratio;
450          /* Gain quantization */
451          {
452             int quant = (int) floor(.5 + 9.4 * log(10*(g+.0001)));
453             if (quant<0)
454                quant=0;
455             if (quant>31)
456                quant=31;
457             speex_bits_pack(bits, quant, 5);
458             g= .1*exp(quant/9.4);
459          }
460          /*printf ("folding gain: %f\n", g);*/
461          g /= filter_ratio;
462
463 #if 0
464          {
465             float noise_gain;
466             float *noise;
467             float *alias;
468             static int init=0;
469             if (!init)
470             {
471                srand48(0);
472                init=1;
473             }
474             noise_gain = 3*sqrt(el/st->subframeSize);
475             noise = PUSH(st->stack, st->subframeSize);
476
477             for (i=0;i<st->subframeSize;i++)
478                noise[i]=noise_gain*(drand48()-.5);
479
480             alias = ((EncState*)st->st_low)->exc+offset;
481             /* Too lazy to do the memory properly */
482             exc[0]=.5*.35*g*(alias[0]+noise[0]);
483             for (i=1;i<st->subframeSize;i++)
484             {
485                exc[i]=.7*.5*g* (.8*(noise[i]+.8*noise[i-1]) + alias[i]-.8*alias[i-1]);
486             }
487             POP(st->stack);
488          }
489 #else
490          /* High-band excitation using the low-band excitation and a gain */
491          for (i=0;i<st->subframeSize;i++)
492             exc[i]=g*((EncState*)st->st_low)->exc[offset+i];
493 #endif
494
495 #endif
496          /* Update the input signal using the non-coded memory */
497          /* FIXME: is that right? */
498          /*syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);*/
499
500          /* FIXME: Update perceptually weighted signal in case we switch to the
501             other mode */
502       } else {
503          float gc, scale, scale_1;
504          float *innov;
505
506          /*speex_bits_pack(bits, 0, 1);*/
507          innov = PUSH(st->stack, st->subframeSize);
508
509
510          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
511
512          {
513             int qgc = (int)floor(.5+3.7*(log(gc)+2));
514             if (qgc<0)
515                qgc=0;
516             if (qgc>15)
517                qgc=15;
518             speex_bits_pack(bits, qgc, 4);
519             gc = exp((1/3.7)*qgc-2);
520          }
521
522          scale = gc*sqrt(1+el)/filter_ratio;
523          scale_1 = 1/scale;
524          if (0 && rand()%5==0)
525          {
526             float sc = 1/sqrt(.1+eh/st->subframeSize);
527             if (rand()&1)
528                sc=-sc;
529             for (i=0;i<st->subframeSize;i++)
530             {
531                float tmp=exc[i]*sc;
532                if (i%8==0)
533                   printf ("\nhexc");
534                printf (" %f", tmp);
535             }
536          }
537
538          /* Reset excitation */
539          for (i=0;i<st->subframeSize;i++)
540             exc[i]=0;
541          
542          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
543          for (i=0;i<st->lpcSize;i++)
544             mem[i]=st->mem_sp[i];
545          syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
546          for (i=0;i<st->lpcSize;i++)
547             mem[i]=st->mem_sp[i];
548          residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
549          for (i=0;i<st->lpcSize;i++)
550             mem[i]=st->mem_sw[i];
551          syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
552          
553          /* Compute weighted signal */
554          for (i=0;i<st->lpcSize;i++)
555             mem[i]=st->mem_sp[i];
556          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
557          for (i=0;i<st->lpcSize;i++)
558             mem[i]=st->mem_sw[i];
559          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
560          
561          /* Compute target signal */
562          for (i=0;i<st->subframeSize;i++)
563             target[i]=sw[i]-res[i];
564
565          for (i=0;i<st->subframeSize;i++)
566            exc[i]=0;
567
568
569          for (i=0;i<st->subframeSize;i++)
570             target[i]*=scale_1;
571          
572          /* Reset excitation */
573          for (i=0;i<st->subframeSize;i++)
574             innov[i]=0;
575
576          /*print_vec(target, st->subframeSize, "\ntarget");*/
577          st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
578                                 st->innovation_params, st->lpcSize, st->subframeSize, 
579                                 innov, bits, st->stack);
580          /*print_vec(target, st->subframeSize, "after");*/
581
582          for (i=0;i<st->subframeSize;i++)
583             exc[i] += innov[i]*scale;
584
585          if (0) {
586             float en=0;
587             for (i=0;i<st->subframeSize;i++)
588                en+=exc[i]*exc[i];
589             en=sqrt(eh/(1+en));
590             for (i=0;i<st->subframeSize;i++)
591                exc[i]*=en;
592             printf ("correction high: %f\n", en);
593          }
594
595          POP(st->stack);
596       }
597 #if 1
598          /*Keep the previous memory*/
599          for (i=0;i<st->lpcSize;i++)
600             mem[i]=st->mem_sp[i];
601          /* Final signal synthesis from excitation */
602          syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
603          
604          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
605          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
606          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
607 #endif
608       
609       
610       POP(st->stack);
611    }
612
613
614 #ifndef RELEASE
615    /* Up-sample coded low-band and high-band*/
616    for (i=0;i<st->frame_size;i++)
617    {
618       st->x0[(i<<1)]=st->x0d[i];
619       st->x1[(i<<1)]=st->high[i];
620       st->x0[(i<<1)+1]=0;
621       st->x1[(i<<1)+1]=0;
622    }
623    /* Reconstruct the original */
624    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
625    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
626    for (i=0;i<st->full_frame_size;i++)
627       in[i]=2*(st->y0[i]-st->y1[i]);
628 #endif
629    for (i=0;i<st->lpcSize;i++)
630       st->old_lsp[i] = st->lsp[i];
631    for (i=0;i<st->lpcSize;i++)
632       st->old_qlsp[i] = st->qlsp[i];
633
634    st->first=0;
635 }
636
637
638
639
640
641 void *sb_decoder_init(SpeexMode *m)
642 {
643    int i;
644    SBDecState *st;
645    SpeexSBMode *mode;
646    st = malloc(sizeof(SBDecState));
647    st->mode = m;
648    mode=m->mode;
649
650    st->st_low = nb_decoder_init(mode->nb_mode);
651    st->full_frame_size = 2*mode->frameSize;
652    st->frame_size = mode->frameSize;
653    st->subframeSize = 40;
654    st->nbSubframes = 4;
655    st->lpcSize=8;
656    st->pf_order=15;
657    st->pf_gamma=.05;
658
659    st->first=1;
660    st->stack = calloc(10000, sizeof(float));
661
662    st->x0=calloc(st->full_frame_size, sizeof(float));
663    st->x1=calloc(st->full_frame_size, sizeof(float));
664    st->x0d=calloc(st->frame_size, sizeof(float));
665    st->x1d=calloc(st->frame_size, sizeof(float));
666    st->high=calloc(st->full_frame_size, sizeof(float));
667    st->y0=calloc(st->full_frame_size, sizeof(float));
668    st->y1=calloc(st->full_frame_size, sizeof(float));
669
670    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
671    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
672    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
673    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
674
675    st->pf_exc=calloc(st->full_frame_size, sizeof(float));
676    st->exc=calloc(st->frame_size, sizeof(float));
677    st->pf_window=calloc(st->full_frame_size, sizeof(float));
678    st->pf_autocorr=calloc(st->pf_order+1, sizeof(float));
679    st->pf_lpc=calloc(st->pf_order+1, sizeof(float));
680    st->pf_bwlpc=calloc(st->pf_order+1, sizeof(float));
681    for (i=0;i<st->full_frame_size;i++)
682       st->pf_window[i]=.5*(1-cos(2*M_PI*i/st->full_frame_size));
683
684    st->qlsp = malloc(st->lpcSize*sizeof(float));
685    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
686    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
687    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
688
689    st->mem_sp = calloc(st->lpcSize, sizeof(float));
690    st->mem_pf_exc1 = calloc(st->pf_order, sizeof(float));
691    st->mem_pf_exc2 = calloc(st->pf_order, sizeof(float));
692    st->mem_pf_sp = calloc(st->pf_order, sizeof(float));
693
694    st->lsp_unquant = mode->lsp_unquant;
695    st->innovation_unquant = mode->innovation_unquant;
696    st->innovation_params = mode->innovation_params;
697    return st;
698 }
699
700 void sb_decoder_destroy(void *state)
701 {
702    SBDecState *st;
703    st = state;
704    nb_decoder_destroy(st->st_low);
705    free(st->x0);
706    free(st->x0d);
707    free(st->x1);
708    free(st->x1d);
709    free(st->high);
710    free(st->y0);
711    free(st->y1);
712    free(st->h0_mem);
713    free(st->h1_mem);
714    free(st->g0_mem);
715    free(st->g1_mem);
716    
717    free(st->exc);
718    free(st->pf_exc);
719    free(st->pf_window);
720    free(st->pf_autocorr);
721    free(st->pf_lpc);
722    free(st->pf_bwlpc);
723    free(st->qlsp);
724    free(st->old_qlsp);
725    free(st->interp_qlsp);
726    free(st->interp_qlpc);
727
728    free(st->mem_sp);
729    free(st->mem_pf_exc1);
730    free(st->mem_pf_exc2);
731    free(st->mem_pf_sp);
732
733    free(st->stack);
734
735    free(state);
736 }
737
738
739 void sb_decode(void *state, SpeexBits *bits, float *out, int lost)
740 {
741    int i, sub;
742    SBDecState *st;
743    
744    st = state;
745    /* Decode the low-band */
746    nb_decode(st->st_low, bits, st->x0d, lost);
747
748    for (i=0;i<st->frame_size;i++)
749       st->exc[i]=0;
750
751    st->lsp_unquant(st->qlsp, st->lpcSize, bits);
752    
753    if (st->first)
754    {
755       for (i=0;i<st->lpcSize;i++)
756          st->old_qlsp[i] = st->qlsp[i];
757    }
758    
759    for (sub=0;sub<st->nbSubframes;sub++)
760    {
761       float *exc, *sp, tmp, filter_ratio, gain, el=0;
762       int offset;
763       
764       offset = st->subframeSize*sub;
765       sp=st->high+offset;
766       exc=st->exc+offset;
767       
768       /* LSP interpolation */
769       tmp = (1.0 + sub)/st->nbSubframes;
770       for (i=0;i<st->lpcSize;i++)
771          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
772
773       /* LSPs to x-domain */
774       for (i=0;i<st->lpcSize;i++)
775          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
776
777       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
778
779       /* LSP to LPC */
780       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
781
782       {
783          float rl=0, rh=0;
784          tmp=1;
785          for (i=0;i<=st->lpcSize;i++)
786          {
787             rh += tmp*st->interp_qlpc[i];
788             tmp = -tmp;
789          }
790          rl = ((DecState*)st->st_low)->pi_gain[sub];
791          rl=1/(fabs(rl)+.01);
792          rh=1/(fabs(rh)+.01);
793          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
794       }
795
796       for (i=0;i<st->subframeSize;i++)
797          el+=sqr(((DecState*)st->st_low)->exc[offset+i]);
798       gain=(1+sqrt(el/st->subframeSize))/filter_ratio;
799       
800       for (i=0;i<st->subframeSize;i++)
801          exc[i]=0;
802       if (0/*speex_bits_unpack_unsigned(bits, 1)*/)
803       {
804          float g;
805          int quant;
806          quant = speex_bits_unpack_unsigned(bits, 5);
807          g= .1*exp(quant/9.4);
808          
809          /*printf ("unquant folding gain: %f\n", g);*/
810          g /= filter_ratio;
811          
812          /* High-band excitation using the low-band excitation and a gain */
813          for (i=0;i<st->subframeSize;i++)
814             exc[i]=g*((DecState*)st->st_low)->exc[offset+i];
815       } else {
816          float gc, scale;
817          int qgc = speex_bits_unpack_unsigned(bits, 4);
818
819          gc = exp((1/3.7)*qgc-2);
820
821          scale = gc*sqrt(1+el)/filter_ratio;
822
823
824          st->innovation_unquant(exc, st->innovation_params, st->subframeSize, 
825                                 bits, st->stack);
826          for (i=0;i<st->subframeSize;i++)
827             exc[i]*=scale;
828       }
829       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
830
831    }
832
833    /* Up-sample coded low-band and high-band*/
834    for (i=0;i<st->frame_size;i++)
835    {
836       st->x0[(i<<1)]=st->x0d[i];
837       st->x1[(i<<1)]=st->high[i];
838       st->x0[(i<<1)+1]=0;
839       st->x1[(i<<1)+1]=0;
840    }
841    /* Reconstruct the original */
842    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
843    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
844    for (i=0;i<st->full_frame_size;i++)
845       out[i]=2*(st->y0[i]-st->y1[i]);
846
847
848    if (0)
849    {
850       float tmp=1, e1=0, e2=0, g;
851       for (i=0;i<st->full_frame_size;i++)
852          st->pf_exc[i] = out[i] * st->pf_window[i];
853       
854       /* Compute auto-correlation */
855       autocorr(st->pf_exc, st->pf_autocorr, st->pf_order+1, st->full_frame_size);
856       
857       st->pf_autocorr[0] += 1;        /* prevents NANs */
858       st->pf_autocorr[0] *= 1.0001;    /* Noise floor in auto-correlation domain */
859       
860       /* Levinson-Durbin */
861       wld(st->pf_lpc+1, st->pf_autocorr, st->pf_exc, st->pf_order);
862       st->pf_lpc[0]=1;
863       
864       for (i=1;i<st->pf_order;i++)
865       {
866          tmp*=st->pf_gamma;
867          st->pf_bwlpc[i] = st->pf_lpc[i]*tmp;
868       }
869       st->pf_bwlpc[0]=1;
870       
871       /*print_vec(st->pf_lpc, st->pf_order, "post-filter LPC");*/
872       residue_mem(out, st->pf_lpc, st->pf_exc, st->full_frame_size, 
873                   st->pf_order, st->mem_pf_exc1);
874       for (i=0;i<st->full_frame_size;i++)
875          e1 += st->pf_exc[i]*st->pf_exc[i];
876       syn_filt_mem(st->pf_exc, st->pf_bwlpc, st->pf_exc, st->full_frame_size, 
877                   st->pf_order, st->mem_pf_exc2);
878       for (i=0;i<st->full_frame_size;i++)
879          e2 += st->pf_exc[i]*st->pf_exc[i];
880       g=sqrt(e1/(e2+.1));
881       /*printf ("post-filter gain: %f\n", g);*/
882       for (i=0;i<st->full_frame_size;i++)
883          st->pf_exc[i]=g*st->pf_exc[i];
884
885       syn_filt_mem(st->pf_exc, st->pf_lpc, out, st->full_frame_size, 
886                   st->pf_order, st->mem_pf_sp);
887    }
888
889
890    for (i=0;i<st->lpcSize;i++)
891       st->old_qlsp[i] = st->qlsp[i];
892
893    st->first=0;
894
895 }
896
897
898 void sb_encoder_ctl(void *state, int request, void *ptr)
899 {
900    SBEncState *st;
901    st=state;
902    switch(request)
903    {
904    case SPEEX_GET_FRAME_SIZE:
905       (*(int*)ptr) = st->full_frame_size;
906       break;
907    default:
908       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
909    }
910
911 }
912
913 void sb_decoder_ctl(void *state, int request, void *ptr)
914 {
915    SBDecState *st;
916    st=state;
917    switch(request)
918    {
919    case SPEEX_GET_FRAME_SIZE:
920       (*(int*)ptr) = st->full_frame_size;
921       break;
922    case SPEEX_SET_PF:
923       speex_decoder_ctl(st->st_low, request, ptr);
924       break;
925    default:
926       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
927    }
928
929 }