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