Fixed a couple stupid bugs
[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 = (SBEncState*)speex_alloc(sizeof(SBEncState));
135    st->mode = m;
136    mode = (SpeexSBMode*)m->mode;
137
138    st->st_low = speex_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    {
151       /* FIXME: Should do this without explicit reference to a mode */
152       int mod=6;
153       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &mod);
154    }
155
156    st->lag_factor = mode->lag_factor;
157    st->lpc_floor = mode->lpc_floor;
158    st->gamma1=mode->gamma1;
159    st->gamma2=mode->gamma2;
160    st->first=1;
161    st->stack = speex_alloc(20000*sizeof(float));
162
163    st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
164    st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
165    st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
166    st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
167    st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
168
169    st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
170    st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
171    st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
172    st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
173
174    st->buf=(float*)speex_alloc(st->windowSize*sizeof(float));
175    st->excBuf=(float*)speex_alloc(st->bufSize*sizeof(float));
176    st->exc = st->excBuf + st->bufSize - st->windowSize;
177
178    st->res=(float*)speex_alloc(st->frame_size*sizeof(float));
179    st->sw=(float*)speex_alloc(st->frame_size*sizeof(float));
180    st->target=(float*)speex_alloc(st->frame_size*sizeof(float));
181    /*Asymetric "pseudo-Hamming" window*/
182    {
183       int part1, part2;
184       part1 = st->subframeSize*7/2;
185       part2 = st->subframeSize*5/2;
186       st->window = (float*)speex_alloc(st->windowSize*sizeof(float));
187       for (i=0;i<part1;i++)
188          st->window[i]=.54-.46*cos(M_PI*i/part1);
189       for (i=0;i<part2;i++)
190          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
191    }
192
193    st->lagWindow = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
194    for (i=0;i<st->lpcSize+1;i++)
195       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
196
197    st->rc = (float*)speex_alloc(st->lpcSize*sizeof(float));
198    st->autocorr = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
199    st->lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
200    st->bw_lpc1 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
201    st->bw_lpc2 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
202    st->lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
203    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
204    st->old_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
205    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
206    st->interp_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
207    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
208    st->interp_lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
209    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
210    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
211
212    st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
213    st->mem_sp2 = (float*)speex_alloc(st->lpcSize*sizeof(float));
214    st->mem_sw = (float*)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=(SBEncState*)state;
223
224    speex_encoder_destroy(st->st_low);
225    speex_free(st->x0d);
226    speex_free(st->x1d);
227    speex_free(st->high);
228    speex_free(st->y0);
229    speex_free(st->y1);
230    speex_free(st->h0_mem);
231    speex_free(st->h1_mem);
232    speex_free(st->g0_mem);
233    speex_free(st->g1_mem);
234    
235    speex_free(st->buf);
236    speex_free(st->window);
237    speex_free(st->excBuf);
238    speex_free(st->sw);
239    speex_free(st->res);
240    speex_free(st->target);
241    speex_free(st->lagWindow);
242    speex_free(st->rc);
243    speex_free(st->autocorr);
244    speex_free(st->lpc);
245    speex_free(st->bw_lpc1);
246    speex_free(st->bw_lpc2);
247    speex_free(st->lsp);
248    speex_free(st->qlsp);
249    speex_free(st->old_lsp);
250    speex_free(st->old_qlsp);
251    speex_free(st->interp_lsp);
252    speex_free(st->interp_qlsp);
253    speex_free(st->interp_lpc);
254    speex_free(st->interp_qlpc);
255
256    speex_free(st->mem_sp);
257    speex_free(st->mem_sp2);
258    speex_free(st->mem_sw);
259    speex_free(st->pi_gain);
260
261    speex_free(st->stack);
262
263    speex_free(st);
264 }
265
266
267 void sb_encode(void *state, float *in, SpeexBits *bits)
268 {
269    SBEncState *st;
270    int i, roots, sub;
271    void *stack;
272    float *mem, *innov, *syn_resp;
273    float *low_pi_gain, *low_exc, *low_innov;
274
275    st = (SBEncState*)state;
276    stack=st->stack;
277
278    /* Compute the two sub-bands by filtering with h0 and h1*/
279    qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
280     
281    /* Encode the narrowband part*/
282    speex_encode(st->st_low, st->x0d, bits);
283
284    speex_bits_pack(bits, 1, 1);
285    speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
286
287    /* High-band buffering / sync with low band */
288    for (i=0;i<st->windowSize-st->frame_size;i++)
289       st->high[i] = st->high[st->frame_size+i];
290    for (i=0;i<st->frame_size;i++)
291       st->high[st->windowSize-st->frame_size+i]=st->x1d[i];
292
293    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
294
295
296    low_pi_gain = PUSH(stack, st->nbSubframes, float);
297    low_exc = PUSH(stack, st->frame_size, float);
298    low_innov = PUSH(stack, st->frame_size, float);
299    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
300    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
301    speex_encoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
302    
303    /* Start encoding the high-band */
304    for (i=0;i<st->windowSize;i++)
305       st->buf[i] = st->high[i] * st->window[i];
306
307    /* Compute auto-correlation */
308    _spx_autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
309
310    st->autocorr[0] += 1;        /* prevents NANs */
311    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
312    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
313    for (i=0;i<st->lpcSize+1;i++)
314       st->autocorr[i] *= st->lagWindow[i];
315
316    /* Levinson-Durbin */
317    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
318    st->lpc[0]=1;
319
320    /* LPC to LSPs (x-domain) transform */
321    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
322    if (roots!=st->lpcSize)
323    {
324       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.02, stack);
325       if (roots!=st->lpcSize) {
326          /*fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);*/
327
328          /*If we can't find all LSP's, do some damage control and use a flat filter*/
329          for (i=0;i<st->lpcSize;i++)
330          {
331             st->lsp[i]=cos(M_PI*((float)(i+1))/(st->lpcSize+1));
332          }
333       }
334    }
335
336    /* x-domain to angle domain*/
337    for (i=0;i<st->lpcSize;i++)
338       st->lsp[i] = acos(st->lsp[i]);
339
340    /* If null mode (no transmission), just set a couple things to zero*/
341    if (st->submodes[st->submodeID] == NULL)
342    {
343       for (i=0;i<st->frame_size;i++)
344          st->exc[i]=st->sw[i]=0;
345
346       for (i=0;i<st->lpcSize;i++)
347          st->mem_sw[i]=0;
348       st->first=1;
349
350       /* Final signal synthesis from excitation */
351       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
352
353 #ifndef RELEASE
354
355       /* Reconstruct the original */
356       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
357       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
358
359       for (i=0;i<st->full_frame_size;i++)
360          in[i]=2*(st->y0[i]-st->y1[i]);
361 #endif
362
363       return;
364
365    }
366
367
368    /* LSP quantization */
369    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
370    
371    /*printf ("high_lsp:");
372    for (i=0;i<st->lpcSize;i++)
373       printf (" %f", st->lsp[i]);
374       printf ("\n");*/
375    /*for (i=0;i<st->lpcSize;i++)
376      st->qlsp[i]=st->lsp[i];*/
377    
378
379    if (st->first)
380    {
381       for (i=0;i<st->lpcSize;i++)
382          st->old_lsp[i] = st->lsp[i];
383       for (i=0;i<st->lpcSize;i++)
384          st->old_qlsp[i] = st->qlsp[i];
385    }
386    
387    mem=PUSH(stack, st->lpcSize, float);
388    syn_resp=PUSH(stack, st->subframeSize, float);
389    innov = PUSH(stack, st->subframeSize, float);
390
391    for (sub=0;sub<st->nbSubframes;sub++)
392    {
393       float *exc, *sp, *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->exc+offset;
401       res=st->res+offset;
402       target=st->target+offset;
403       sw=st->sw+offset;
404       
405       /* LSP interpolation (quantized and unquantized) */
406       tmp = (1.0 + sub)/st->nbSubframes;
407       for (i=0;i<st->lpcSize;i++)
408          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
409       for (i=0;i<st->lpcSize;i++)
410          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
411       
412       /* Compute interpolated LPCs (quantized and unquantized) */
413       for (i=0;i<st->lpcSize;i++)
414          st->interp_lsp[i] = cos(st->interp_lsp[i]);
415       for (i=0;i<st->lpcSize;i++)
416          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
417
418       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
419       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
420
421       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
422       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
423
424       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
425       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
426
427       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
428          filters */
429       rl=rh=0;
430       tmp=1;
431       st->pi_gain[sub]=0;
432       for (i=0;i<=st->lpcSize;i++)
433       {
434          rh += tmp*st->interp_qlpc[i];
435          tmp = -tmp;
436          st->pi_gain[sub]+=st->interp_qlpc[i];
437       }
438       rl = 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(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
466          /* Gain quantization */
467          {
468             int quant = (int) floor(.5 + 27 + 8.0 * log((g+.0001)));
469             if (quant<0)
470                quant=0;
471             if (quant>31)
472                quant=31;
473             speex_bits_pack(bits, quant, 5);
474             g= .1*exp(quant/9.4);
475          }
476          /*printf ("folding gain: %f\n", g);*/
477          g /= filter_ratio;
478
479       } else {
480          float gc, scale, scale_1;
481
482          for (i=0;i<st->subframeSize;i++)
483             el+=sqr(low_exc[offset+i]);
484          /*speex_bits_pack(bits, 0, 1);*/
485
486          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
487          {
488             int qgc = (int)floor(.5+3.7*(log(gc)+2));
489             if (qgc<0)
490                qgc=0;
491             if (qgc>15)
492                qgc=15;
493             speex_bits_pack(bits, qgc, 4);
494             gc = exp((1/3.7)*qgc-2);
495          }
496
497          scale = gc*sqrt(1+el)/filter_ratio;
498          scale_1 = 1/scale;
499          if (0 && rand()%5==0)
500          {
501             float sc = 1/sqrt(.1+eh/st->subframeSize);
502             if (rand()&1)
503                sc=-sc;
504             for (i=0;i<st->subframeSize;i++)
505             {
506                float tmp=exc[i]*sc;
507                if (i%8==0)
508                   printf ("\nhexc");
509                printf (" %f", tmp);
510             }
511          }
512
513          for (i=0;i<st->subframeSize;i++)
514             exc[i]=0;
515          exc[0]=1;
516          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
517          
518          /* Reset excitation */
519          for (i=0;i<st->subframeSize;i++)
520             exc[i]=0;
521          
522          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
523          for (i=0;i<st->lpcSize;i++)
524             mem[i]=st->mem_sp[i];
525          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
526
527          for (i=0;i<st->lpcSize;i++)
528             mem[i]=st->mem_sw[i];
529          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
530
531          /* Compute weighted signal */
532          for (i=0;i<st->lpcSize;i++)
533             mem[i]=st->mem_sw[i];
534          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
535
536          /* Compute target signal */
537          for (i=0;i<st->subframeSize;i++)
538             target[i]=sw[i]-res[i];
539
540          for (i=0;i<st->subframeSize;i++)
541            exc[i]=0;
542
543
544          for (i=0;i<st->subframeSize;i++)
545             target[i]*=scale_1;
546          
547          /* Reset excitation */
548          for (i=0;i<st->subframeSize;i++)
549             innov[i]=0;
550
551          /*print_vec(target, st->subframeSize, "\ntarget");*/
552          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
553                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
554                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
555          /*print_vec(target, st->subframeSize, "after");*/
556
557          for (i=0;i<st->subframeSize;i++)
558             exc[i] += innov[i]*scale;
559
560          if (SUBMODE(double_codebook)) {
561             void *tmp_stack=stack;
562             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
563             for (i=0;i<st->subframeSize;i++)
564                innov2[i]=0;
565             for (i=0;i<st->subframeSize;i++)
566                target[i]*=2.5;
567             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
568                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
569                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
570             for (i=0;i<st->subframeSize;i++)
571                innov2[i]*=scale*(1/2.5);
572             for (i=0;i<st->subframeSize;i++)
573                exc[i] += innov2[i];
574          }
575
576
577          if (0) {
578             float en=0;
579             for (i=0;i<st->subframeSize;i++)
580                en+=exc[i]*exc[i];
581             en=sqrt(eh/(1+en));
582             for (i=0;i<st->subframeSize;i++)
583                exc[i]*=en;
584             printf ("correction high: %f\n", en);
585          }
586
587       }
588
589          /*Keep the previous memory*/
590          for (i=0;i<st->lpcSize;i++)
591             mem[i]=st->mem_sp[i];
592          /* Final signal synthesis from excitation */
593          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
594          
595          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
596          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
597    }
598
599
600 #ifndef RELEASE
601
602    /* Reconstruct the original */
603    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
604    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
605
606    for (i=0;i<st->full_frame_size;i++)
607       in[i]=2*(st->y0[i]-st->y1[i]);
608 #endif
609    for (i=0;i<st->lpcSize;i++)
610       st->old_lsp[i] = st->lsp[i];
611    for (i=0;i<st->lpcSize;i++)
612       st->old_qlsp[i] = st->qlsp[i];
613
614    st->first=0;
615 }
616
617
618
619
620
621 void *sb_decoder_init(SpeexMode *m)
622 {
623    SBDecState *st;
624    SpeexSBMode *mode;
625    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
626    st->mode = m;
627    mode=(SpeexSBMode*)m->mode;
628
629    st->st_low = speex_decoder_init(mode->nb_mode);
630    st->full_frame_size = 2*mode->frameSize;
631    st->frame_size = mode->frameSize;
632    st->subframeSize = mode->subframeSize;
633    st->nbSubframes = mode->frameSize/mode->subframeSize;
634    st->lpcSize=8;
635
636    st->submodes=mode->submodes;
637    st->submodeID=mode->defaultSubmode;
638
639    st->first=1;
640    st->stack = speex_alloc(20000*sizeof(float));
641
642    st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
643    st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
644    st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
645    st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
646    st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
647
648    st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
649    st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
650    st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
651    st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
652
653    st->exc=(float*)speex_alloc(st->frame_size*sizeof(float));
654
655    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
656    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
657    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
658    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
659
660    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
661    st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
662    return st;
663 }
664
665 void sb_decoder_destroy(void *state)
666 {
667    SBDecState *st;
668    st = (SBDecState*)state;
669    speex_decoder_destroy(st->st_low);
670    speex_free(st->x0d);
671    speex_free(st->x1d);
672    speex_free(st->high);
673    speex_free(st->y0);
674    speex_free(st->y1);
675    speex_free(st->h0_mem);
676    speex_free(st->h1_mem);
677    speex_free(st->g0_mem);
678    speex_free(st->g1_mem);
679    
680    speex_free(st->exc);
681    speex_free(st->qlsp);
682    speex_free(st->old_qlsp);
683    speex_free(st->interp_qlsp);
684    speex_free(st->interp_qlpc);
685    speex_free(st->pi_gain);
686
687    speex_free(st->mem_sp);
688
689    speex_free(st->stack);
690
691    speex_free(state);
692 }
693
694 static void sb_decode_lost(SBDecState *st, float *out, void *stack)
695 {
696    int i;
697    for (i=0;i<st->frame_size;i++)
698       st->exc[i]*=0.8;
699    
700    st->first=1;
701    
702    /* Final signal synthesis from excitation */
703    iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
704    
705    /* Reconstruct the original */
706    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
707    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
708
709    for (i=0;i<st->full_frame_size;i++)
710       out[i]=2*(st->y0[i]-st->y1[i]);
711    
712    return;
713 }
714
715 int sb_decode(void *state, SpeexBits *bits, float *out)
716 {
717    int i, sub;
718    SBDecState *st;
719    int wideband;
720    int ret;
721    void *stack;
722    float *low_pi_gain, *low_exc, *low_innov;
723
724    st = (SBDecState*)state;
725    stack=st->stack;
726
727    /* Decode the low-band */
728    ret = speex_decode(st->st_low, bits, st->x0d);
729
730    /* If error decoding the narrowband part, propagate error */
731    if (ret!=0)
732    {
733       return ret;
734    }
735
736    if (!bits)
737    {
738       sb_decode_lost(st, out, stack);
739       return 0;
740    }
741
742    /*Check "wideband bit"*/
743    wideband = speex_bits_peek(bits);
744    if (wideband)
745    {
746       /*Regular wideband frame, read the submode*/
747       wideband = speex_bits_unpack_unsigned(bits, 1);
748       st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
749    } else
750    {
751       /*Was a narrowband frame, set "null submode"*/
752       st->submodeID = 0;
753    }
754
755    for (i=0;i<st->frame_size;i++)
756       st->exc[i]=0;
757
758    /* If null mode (no transmission), just set a couple things to zero*/
759    if (st->submodes[st->submodeID] == NULL)
760    {
761       for (i=0;i<st->frame_size;i++)
762          st->exc[i]=0;
763
764       st->first=1;
765
766       /* Final signal synthesis from excitation */
767       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
768
769       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
770       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
771
772       for (i=0;i<st->full_frame_size;i++)
773          out[i]=2*(st->y0[i]-st->y1[i]);
774
775       return 0;
776
777    }
778
779    low_pi_gain = PUSH(stack, st->nbSubframes, float);
780    low_exc = PUSH(stack, st->frame_size, float);
781    low_innov = PUSH(stack, st->frame_size, float);
782    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
783    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
784    speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
785
786    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
787    
788    if (st->first)
789    {
790       for (i=0;i<st->lpcSize;i++)
791          st->old_qlsp[i] = st->qlsp[i];
792    }
793    
794    for (sub=0;sub<st->nbSubframes;sub++)
795    {
796       float *exc, *sp, tmp, filter_ratio, el=0;
797       int offset;
798       float rl=0,rh=0;
799       
800       offset = st->subframeSize*sub;
801       sp=st->high+offset;
802       exc=st->exc+offset;
803       
804       /* LSP interpolation */
805       tmp = (1.0 + sub)/st->nbSubframes;
806       for (i=0;i<st->lpcSize;i++)
807          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
808
809       /* LSPs to x-domain */
810       for (i=0;i<st->lpcSize;i++)
811          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
812
813       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
814
815       /* LSP to LPC */
816       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
817
818       /* Calculate reponse ratio between the low and high filter in the middle
819          of the band (4000 Hz) */
820       
821          tmp=1;
822          st->pi_gain[sub]=0;
823          for (i=0;i<=st->lpcSize;i++)
824          {
825             rh += tmp*st->interp_qlpc[i];
826             tmp = -tmp;
827             st->pi_gain[sub]+=st->interp_qlpc[i];
828          }
829          rl = low_pi_gain[sub];
830          rl=1/(fabs(rl)+.01);
831          rh=1/(fabs(rh)+.01);
832          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
833       
834       
835       for (i=0;i<st->subframeSize;i++)
836          exc[i]=0;
837       if (!SUBMODE(innovation_unquant))
838       {
839          float g;
840          int quant;
841
842          for (i=0;i<st->subframeSize;i++)
843             el+=sqr(low_innov[offset+i]);
844          quant = speex_bits_unpack_unsigned(bits, 5);
845          g= exp(((float)quant-27)/8.0);
846          
847          /*printf ("unquant folding gain: %f\n", g);*/
848          g /= filter_ratio;
849          
850          /* High-band excitation using the low-band excitation and a gain */
851          for (i=0;i<st->subframeSize;i++)
852             exc[i]=.8*g*low_innov[offset+i];
853       } else {
854          float gc, scale;
855          int qgc = speex_bits_unpack_unsigned(bits, 4);
856          for (i=0;i<st->subframeSize;i++)
857             el+=sqr(low_exc[offset+i]);
858
859
860          gc = exp((1/3.7)*qgc-2);
861
862          scale = gc*sqrt(1+el)/filter_ratio;
863
864
865          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
866                                 bits, stack);
867          for (i=0;i<st->subframeSize;i++)
868             exc[i]*=scale;
869
870          if (SUBMODE(double_codebook)) {
871             void *tmp_stack=stack;
872             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
873             for (i=0;i<st->subframeSize;i++)
874                innov2[i]=0;
875             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
876                                 bits, tmp_stack);
877             for (i=0;i<st->subframeSize;i++)
878                innov2[i]*=scale*(1/2.5);
879             for (i=0;i<st->subframeSize;i++)
880                exc[i] += innov2[i];
881          }
882
883       }
884       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
885
886    }
887
888    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
889    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
890
891    for (i=0;i<st->full_frame_size;i++)
892       out[i]=2*(st->y0[i]-st->y1[i]);
893
894    for (i=0;i<st->lpcSize;i++)
895       st->old_qlsp[i] = st->qlsp[i];
896
897    st->first=0;
898
899    return 0;
900 }
901
902
903 void sb_encoder_ctl(void *state, int request, void *ptr)
904 {
905    SBEncState *st;
906    st=(SBEncState*)state;
907    switch(request)
908    {
909    case SPEEX_GET_FRAME_SIZE:
910       (*(int*)ptr) = st->full_frame_size;
911       break;
912    case SPEEX_SET_HIGH_MODE:
913       st->submodeID = (*(int*)ptr);
914       break;
915    case SPEEX_SET_LOW_MODE:
916       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, ptr);
917       break;
918    case SPEEX_SET_VBR:
919       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
920       break;
921    case SPEEX_SET_VBR_QUALITY:
922       {
923          int q;
924          float qual = (*(float*)ptr)+1;
925          if (qual>10)
926             qual=10;
927          q=(int)floor(.5+*(float*)ptr);
928          if (q>10)
929             q=10;
930          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
931          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
932          break;
933       }
934    case SPEEX_SET_QUALITY:
935       {
936          int nb_mode;
937          int quality = (*(int*)ptr);
938          switch (quality)
939          {
940          case 0:
941             nb_mode=0;
942             st->submodeID = 0;
943             break;
944          case 1:
945             nb_mode=1;
946             st->submodeID = 1;
947             break;
948          case 2:
949             nb_mode=2;
950             st->submodeID = 1;
951             break;
952          case 3:
953             nb_mode=3;
954             st->submodeID = 1;
955             break;
956          case 4:
957             nb_mode=4;
958             st->submodeID = 1;
959             break;
960          case 5:
961             nb_mode=5;
962             st->submodeID = 1;
963             break;
964          case 6:
965             nb_mode=5;
966             st->submodeID = 2;
967             break;
968          case 7:
969             nb_mode=6;
970             st->submodeID = 2;
971             break;
972          case 8:
973             nb_mode=6;
974             st->submodeID = 3;
975             break;
976          case 9:
977             nb_mode=7;
978             st->submodeID = 3;
979             break;
980          case 10:
981             nb_mode=7;
982             st->submodeID = 4;
983             break;
984          default:
985             fprintf(stderr, "Unknown sb_ctl quality: %d\n", quality);
986          }
987          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_mode);
988       }
989       break;
990    case SPEEX_SET_COMPLEXITY:
991       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
992       st->complexity = (*(int*)ptr);
993       if (st->complexity<1)
994          st->complexity=1;
995       break;
996    case SPEEX_GET_COMPLEXITY:
997       (*(int*)ptr) = st->complexity;
998       break;
999    case SPEEX_GET_BITRATE:
1000       speex_encoder_ctl(st->st_low, request, ptr);
1001       if (st->submodes[st->submodeID])
1002          (*(int*)ptr) += 50*SUBMODE(bits_per_frame);
1003       else
1004          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
1005       break;
1006    case SPEEX_GET_PI_GAIN:
1007       {
1008          int i;
1009          float *g = (float*)ptr;
1010          for (i=0;i<st->nbSubframes;i++)
1011             g[i]=st->pi_gain[i];
1012       }
1013       break;
1014    case SPEEX_GET_EXC:
1015       {
1016          int i;
1017          float *e = (float*)ptr;
1018          for (i=0;i<st->full_frame_size;i++)
1019             e[i]=0;
1020          for (i=0;i<st->frame_size;i++)
1021             e[2*i]=2*st->exc[i];
1022       }
1023       break;
1024    case SPEEX_GET_INNOV:
1025       {
1026          int i;
1027          float *e = (float*)ptr;
1028          for (i=0;i<st->full_frame_size;i++)
1029             e[i]=0;
1030          for (i=0;i<st->frame_size;i++)
1031             e[2*i]=2*st->exc[i];
1032       }
1033       break;
1034    default:
1035       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1036    }
1037
1038 }
1039
1040 void sb_decoder_ctl(void *state, int request, void *ptr)
1041 {
1042    SBDecState *st;
1043    st=(SBDecState*)state;
1044    switch(request)
1045    {
1046    case SPEEX_GET_FRAME_SIZE:
1047       (*(int*)ptr) = st->full_frame_size;
1048       break;
1049    case SPEEX_SET_ENH:
1050       speex_decoder_ctl(st->st_low, request, ptr);
1051       break;
1052    case SPEEX_GET_BITRATE:
1053       speex_decoder_ctl(st->st_low, request, ptr);
1054       if (st->submodes[st->submodeID])
1055          (*(int*)ptr) += 50*SUBMODE(bits_per_frame);
1056       else
1057          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
1058       break;
1059    case SPEEX_SET_HANDLER:
1060       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1061       break;
1062    case SPEEX_SET_USER_HANDLER:
1063       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1064       break;
1065    case SPEEX_GET_PI_GAIN:
1066       {
1067          int i;
1068          float *g = (float*)ptr;
1069          for (i=0;i<st->nbSubframes;i++)
1070             g[i]=st->pi_gain[i];
1071       }
1072       break;
1073    case SPEEX_GET_EXC:
1074       {
1075          int i;
1076          float *e = (float*)ptr;
1077          for (i=0;i<st->full_frame_size;i++)
1078             e[i]=0;
1079          for (i=0;i<st->frame_size;i++)
1080             e[2*i]=2*st->exc[i];
1081       }
1082       break;
1083    case SPEEX_GET_INNOV:
1084       {
1085          int i;
1086          float *e = (float*)ptr;
1087          for (i=0;i<st->full_frame_size;i++)
1088             e[i]=0;
1089          for (i=0;i<st->frame_size;i++)
1090             e[2*i]=2*st->exc[i];
1091       }
1092       break;
1093    default:
1094       fprintf(stderr, "Unknown sb_ctl request: %d\n", request);
1095    }
1096
1097 }