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