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