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