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