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