Think I fixed the "ultra-wideband" gain problem
[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    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          if (st->full_frame_size==640)
467          {
468             printf ("encode: %f %f %f %f %f\n", g, rl, rh, el, eh);
469          }
470          /* Gain quantization */
471          {
472             int quant = (int) floor(.5 + 27 + 8.0 * log((g+.0001)));
473             if (quant<0)
474                quant=0;
475             if (quant>31)
476                quant=31;
477             speex_bits_pack(bits, quant, 5);
478             g= .1*exp(quant/9.4);
479          }
480          /*printf ("folding gain: %f\n", g);*/
481          g /= filter_ratio;
482
483       } else {
484          float gc, scale, scale_1;
485
486          for (i=0;i<st->subframeSize;i++)
487             el+=sqr(low_exc[offset+i]);
488          /*speex_bits_pack(bits, 0, 1);*/
489
490          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
491          {
492             int qgc = (int)floor(.5+3.7*(log(gc)+2));
493             if (qgc<0)
494                qgc=0;
495             if (qgc>15)
496                qgc=15;
497             speex_bits_pack(bits, qgc, 4);
498             gc = exp((1/3.7)*qgc-2);
499          }
500
501          scale = gc*sqrt(1+el)/filter_ratio;
502          scale_1 = 1/scale;
503          if (0 && rand()%5==0)
504          {
505             float sc = 1/sqrt(.1+eh/st->subframeSize);
506             if (rand()&1)
507                sc=-sc;
508             for (i=0;i<st->subframeSize;i++)
509             {
510                float tmp=exc[i]*sc;
511                if (i%8==0)
512                   printf ("\nhexc");
513                printf (" %f", tmp);
514             }
515          }
516
517          for (i=0;i<st->subframeSize;i++)
518             exc[i]=0;
519          exc[0]=1;
520          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
521          
522          /* Reset excitation */
523          for (i=0;i<st->subframeSize;i++)
524             exc[i]=0;
525          
526          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
527          for (i=0;i<st->lpcSize;i++)
528             mem[i]=st->mem_sp[i];
529          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
530
531          for (i=0;i<st->lpcSize;i++)
532             mem[i]=st->mem_sw[i];
533          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
534
535          /* Compute weighted signal */
536          for (i=0;i<st->lpcSize;i++)
537             mem[i]=st->mem_sw[i];
538          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
539
540          /* Compute target signal */
541          for (i=0;i<st->subframeSize;i++)
542             target[i]=sw[i]-res[i];
543
544          for (i=0;i<st->subframeSize;i++)
545            exc[i]=0;
546
547
548          for (i=0;i<st->subframeSize;i++)
549             target[i]*=scale_1;
550          
551          /* Reset excitation */
552          for (i=0;i<st->subframeSize;i++)
553             innov[i]=0;
554
555          /*print_vec(target, st->subframeSize, "\ntarget");*/
556          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
557                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
558                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
559          /*print_vec(target, st->subframeSize, "after");*/
560
561          for (i=0;i<st->subframeSize;i++)
562             exc[i] += innov[i]*scale;
563
564          if (SUBMODE(double_codebook)) {
565             void *tmp_stack=stack;
566             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
567             for (i=0;i<st->subframeSize;i++)
568                innov2[i]=0;
569             for (i=0;i<st->subframeSize;i++)
570                target[i]*=2.5;
571             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
572                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
573                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
574             for (i=0;i<st->subframeSize;i++)
575                innov2[i]*=scale*(1/2.5);
576             for (i=0;i<st->subframeSize;i++)
577                exc[i] += innov2[i];
578          }
579
580
581          if (0) {
582             float en=0;
583             for (i=0;i<st->subframeSize;i++)
584                en+=exc[i]*exc[i];
585             en=sqrt(eh/(1+en));
586             for (i=0;i<st->subframeSize;i++)
587                exc[i]*=en;
588             printf ("correction high: %f\n", en);
589          }
590
591       }
592
593          /*Keep the previous memory*/
594          for (i=0;i<st->lpcSize;i++)
595             mem[i]=st->mem_sp[i];
596          /* Final signal synthesis from excitation */
597          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
598          
599          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
600          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
601    }
602
603
604 #ifndef RELEASE
605
606    /* Reconstruct the original */
607    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
608    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
609
610    for (i=0;i<st->full_frame_size;i++)
611       in[i]=2*(st->y0[i]-st->y1[i]);
612 #endif
613    for (i=0;i<st->lpcSize;i++)
614       st->old_lsp[i] = st->lsp[i];
615    for (i=0;i<st->lpcSize;i++)
616       st->old_qlsp[i] = st->qlsp[i];
617
618    st->first=0;
619 }
620
621
622
623
624
625 void *sb_decoder_init(SpeexMode *m)
626 {
627    SBDecState *st;
628    SpeexSBMode *mode;
629    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
630    st->mode = m;
631    mode=(SpeexSBMode*)m->mode;
632
633    st->st_low = speex_decoder_init(mode->nb_mode);
634    st->full_frame_size = 2*mode->frameSize;
635    st->frame_size = mode->frameSize;
636    st->subframeSize = mode->subframeSize;
637    st->nbSubframes = mode->frameSize/mode->subframeSize;
638    st->lpcSize=8;
639
640    st->submodes=mode->submodes;
641    st->submodeID=mode->defaultSubmode;
642
643    st->first=1;
644    st->stack = speex_alloc(20000*sizeof(float));
645
646    st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
647    st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
648    st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
649    st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
650    st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
651
652    st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
653    st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
654    st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
655    st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
656
657    st->exc=(float*)speex_alloc(st->frame_size*sizeof(float));
658
659    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
660    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
661    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
662    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
663
664    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
665    st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
666    return st;
667 }
668
669 void sb_decoder_destroy(void *state)
670 {
671    SBDecState *st;
672    st = (SBDecState*)state;
673    speex_decoder_destroy(st->st_low);
674    speex_free(st->x0d);
675    speex_free(st->x1d);
676    speex_free(st->high);
677    speex_free(st->y0);
678    speex_free(st->y1);
679    speex_free(st->h0_mem);
680    speex_free(st->h1_mem);
681    speex_free(st->g0_mem);
682    speex_free(st->g1_mem);
683    
684    speex_free(st->exc);
685    speex_free(st->qlsp);
686    speex_free(st->old_qlsp);
687    speex_free(st->interp_qlsp);
688    speex_free(st->interp_qlpc);
689    speex_free(st->pi_gain);
690
691    speex_free(st->mem_sp);
692
693    speex_free(st->stack);
694
695    speex_free(state);
696 }
697
698 static void sb_decode_lost(SBDecState *st, float *out, void *stack)
699 {
700    int i;
701    for (i=0;i<st->frame_size;i++)
702       st->exc[i]*=0.8;
703    
704    st->first=1;
705    
706    /* Final signal synthesis from excitation */
707    iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
708    
709    /* Reconstruct the original */
710    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
711    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
712
713    for (i=0;i<st->full_frame_size;i++)
714       out[i]=2*(st->y0[i]-st->y1[i]);
715    
716    return;
717 }
718
719 int sb_decode(void *state, SpeexBits *bits, float *out)
720 {
721    int i, sub;
722    SBDecState *st;
723    int wideband;
724    int ret;
725    void *stack;
726    float *low_pi_gain, *low_exc, *low_innov;
727
728    st = (SBDecState*)state;
729    stack=st->stack;
730
731    /* Decode the low-band */
732    ret = speex_decode(st->st_low, bits, st->x0d);
733
734    /* If error decoding the narrowband part, propagate error */
735    if (ret!=0)
736    {
737       return ret;
738    }
739
740    if (!bits)
741    {
742       sb_decode_lost(st, out, stack);
743       return 0;
744    }
745
746    /*Check "wideband bit"*/
747    wideband = speex_bits_peek(bits);
748    if (wideband)
749    {
750       /*Regular wideband frame, read the submode*/
751       wideband = speex_bits_unpack_unsigned(bits, 1);
752       st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
753    } else
754    {
755       /*Was a narrowband frame, set "null submode"*/
756       st->submodeID = 0;
757    }
758
759    for (i=0;i<st->frame_size;i++)
760       st->exc[i]=0;
761
762    /* If null mode (no transmission), just set a couple things to zero*/
763    if (st->submodes[st->submodeID] == NULL)
764    {
765       for (i=0;i<st->frame_size;i++)
766          st->exc[i]=0;
767
768       st->first=1;
769
770       /* Final signal synthesis from excitation */
771       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
772
773       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
774       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
775
776       for (i=0;i<st->full_frame_size;i++)
777          out[i]=2*(st->y0[i]-st->y1[i]);
778
779       return 0;
780
781    }
782
783    low_pi_gain = PUSH(stack, st->nbSubframes, float);
784    low_exc = PUSH(stack, st->frame_size, float);
785    low_innov = PUSH(stack, st->frame_size, float);
786    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
787    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
788    speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
789
790    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
791    
792    if (st->first)
793    {
794       for (i=0;i<st->lpcSize;i++)
795          st->old_qlsp[i] = st->qlsp[i];
796    }
797    
798    for (sub=0;sub<st->nbSubframes;sub++)
799    {
800       float *exc, *sp, tmp, filter_ratio, el=0;
801       int offset;
802       float rl=0,rh=0;
803       
804       offset = st->subframeSize*sub;
805       sp=st->high+offset;
806       exc=st->exc+offset;
807       
808       /* LSP interpolation */
809       tmp = (1.0 + sub)/st->nbSubframes;
810       for (i=0;i<st->lpcSize;i++)
811          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
812
813       /* LSPs to x-domain */
814       for (i=0;i<st->lpcSize;i++)
815          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
816
817       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
818
819       /* LSP to LPC */
820       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
821
822       /* Calculate reponse ratio between the low and high filter in the middle
823          of the band (4000 Hz) */
824       
825          tmp=1;
826          st->pi_gain[sub]=0;
827          for (i=0;i<=st->lpcSize;i++)
828          {
829             rh += tmp*st->interp_qlpc[i];
830             tmp = -tmp;
831             st->pi_gain[sub]+=st->interp_qlpc[i];
832          }
833          rl = low_pi_gain[sub];
834          rl=1/(fabs(rl)+.01);
835          rh=1/(fabs(rh)+.01);
836          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
837       
838       
839       for (i=0;i<st->subframeSize;i++)
840          exc[i]=0;
841       if (!SUBMODE(innovation_unquant))
842       {
843          float g;
844          int quant;
845          printf ("subframes: %d\n", st->subframeSize);
846          for (i=0;i<st->subframeSize;i++)
847             el+=sqr(low_innov[offset+i]);
848          quant = speex_bits_unpack_unsigned(bits, 5);
849          g= exp(((float)quant-27)/8.0);
850          if (st->full_frame_size==640)
851          {
852             printf ("decode: %f %f %f %f\n", g, rl, rh, el);
853          }
854          
855          /*printf ("unquant folding gain: %f\n", g);*/
856          g /= filter_ratio;
857          
858          /* High-band excitation using the low-band excitation and a gain */
859          for (i=0;i<st->subframeSize;i++)
860             exc[i]=.8*g*low_innov[offset+i];
861       } else {
862          float gc, scale;
863          int qgc = speex_bits_unpack_unsigned(bits, 4);
864          for (i=0;i<st->subframeSize;i++)
865             el+=sqr(low_exc[offset+i]);
866
867
868          gc = exp((1/3.7)*qgc-2);
869
870          scale = gc*sqrt(1+el)/filter_ratio;
871
872
873          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
874                                 bits, stack);
875          for (i=0;i<st->subframeSize;i++)
876             exc[i]*=scale;
877
878          if (SUBMODE(double_codebook)) {
879             void *tmp_stack=stack;
880             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
881             for (i=0;i<st->subframeSize;i++)
882                innov2[i]=0;
883             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
884                                 bits, tmp_stack);
885             for (i=0;i<st->subframeSize;i++)
886                innov2[i]*=scale*(1/2.5);
887             for (i=0;i<st->subframeSize;i++)
888                exc[i] += innov2[i];
889          }
890
891       }
892       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
893
894    }
895
896    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
897    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
898
899    for (i=0;i<st->full_frame_size;i++)
900       out[i]=2*(st->y0[i]-st->y1[i]);
901
902    for (i=0;i<st->lpcSize;i++)
903       st->old_qlsp[i] = st->qlsp[i];
904
905    st->first=0;
906
907    return 0;
908 }
909
910
911 void sb_encoder_ctl(void *state, int request, void *ptr)
912 {
913    SBEncState *st;
914    st=(SBEncState*)state;
915    switch(request)
916    {
917    case SPEEX_GET_FRAME_SIZE:
918       (*(int*)ptr) = st->full_frame_size;
919       break;
920    case SPEEX_SET_HIGH_MODE:
921       st->submodeID = (*(int*)ptr);
922       break;
923    case SPEEX_SET_LOW_MODE:
924       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, ptr);
925       break;
926    case SPEEX_SET_VBR:
927       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
928       break;
929    case SPEEX_SET_VBR_QUALITY:
930       {
931          int q;
932          float qual = (*(float*)ptr)+1;
933          if (qual>10)
934             qual=10;
935          q=(int)floor(.5+*(float*)ptr);
936          if (q>10)
937             q=10;
938          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
939          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
940          break;
941       }
942    case SPEEX_SET_QUALITY:
943       {
944          int nb_mode;
945          int quality = (*(int*)ptr);
946          switch (quality)
947          {
948          case 0:
949             nb_mode=0;
950             st->submodeID = 0;
951             break;
952          case 1:
953             nb_mode=1;
954             st->submodeID = 1;
955             break;
956          case 2:
957             nb_mode=2;
958             st->submodeID = 1;
959             break;
960          case 3:
961             nb_mode=3;
962             st->submodeID = 1;
963             break;
964          case 4:
965             nb_mode=4;
966             st->submodeID = 1;
967             break;
968          case 5:
969             nb_mode=5;
970             st->submodeID = 1;
971             break;
972          case 6:
973             nb_mode=5;
974             st->submodeID = 2;
975             break;
976          case 7:
977             nb_mode=6;
978             st->submodeID = 2;
979             break;
980          case 8:
981             nb_mode=6;
982             st->submodeID = 3;
983             break;
984          case 9:
985             nb_mode=7;
986             st->submodeID = 3;
987             break;
988          case 10:
989             nb_mode=7;
990             st->submodeID = 4;
991             break;
992          default:
993             fprintf(stderr, "Unknown sb_ctl quality: %d\n", quality);
994          }
995          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_mode);
996       }
997       break;
998    case SPEEX_SET_COMPLEXITY:
999       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1000       st->complexity = (*(int*)ptr);
1001       if (st->complexity<1)
1002          st->complexity=1;
1003       break;
1004    case SPEEX_GET_COMPLEXITY:
1005       (*(int*)ptr) = st->complexity;
1006       break;
1007    case SPEEX_GET_BITRATE:
1008       speex_encoder_ctl(st->st_low, request, ptr);
1009       if (st->submodes[st->submodeID])
1010          (*(int*)ptr) += 50*SUBMODE(bits_per_frame);
1011       else
1012          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
1013       break;
1014    case SPEEX_GET_PI_GAIN:
1015       {
1016          int i;
1017          float *g = (float*)ptr;
1018          for (i=0;i<st->nbSubframes;i++)
1019             g[i]=st->pi_gain[i];
1020       }
1021       break;
1022    case SPEEX_GET_EXC:
1023       {
1024          int i;
1025          float *e = (float*)ptr;
1026          for (i=0;i<st->full_frame_size;i++)
1027             e[i]=0;
1028          for (i=0;i<st->frame_size;i++)
1029             e[2*i]=2*st->exc[i];
1030       }
1031       break;
1032    case SPEEX_GET_INNOV:
1033       {
1034          int i;
1035          float *e = (float*)ptr;
1036          for (i=0;i<st->full_frame_size;i++)
1037             e[i]=0;
1038          for (i=0;i<st->frame_size;i++)
1039             e[2*i]=2*st->exc[i];
1040       }
1041       break;
1042    default:
1043       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1044    }
1045
1046 }
1047
1048 void sb_decoder_ctl(void *state, int request, void *ptr)
1049 {
1050    SBDecState *st;
1051    st=(SBDecState*)state;
1052    switch(request)
1053    {
1054    case SPEEX_GET_FRAME_SIZE:
1055       (*(int*)ptr) = st->full_frame_size;
1056       break;
1057    case SPEEX_SET_ENH:
1058       speex_decoder_ctl(st->st_low, request, ptr);
1059       break;
1060    case SPEEX_GET_BITRATE:
1061       speex_decoder_ctl(st->st_low, request, ptr);
1062       if (st->submodes[st->submodeID])
1063          (*(int*)ptr) += 50*SUBMODE(bits_per_frame);
1064       else
1065          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
1066       break;
1067    case SPEEX_GET_PI_GAIN:
1068       {
1069          int i;
1070          float *g = (float*)ptr;
1071          for (i=0;i<st->nbSubframes;i++)
1072             g[i]=st->pi_gain[i];
1073       }
1074       break;
1075    case SPEEX_GET_EXC:
1076       {
1077          int i;
1078          float *e = (float*)ptr;
1079          for (i=0;i<st->full_frame_size;i++)
1080             e[i]=0;
1081          for (i=0;i<st->frame_size;i++)
1082             e[2*i]=2*st->exc[i];
1083       }
1084       break;
1085    case SPEEX_GET_INNOV:
1086       {
1087          int i;
1088          float *e = (float*)ptr;
1089          for (i=0;i<st->full_frame_size;i++)
1090             e[i]=0;
1091          for (i=0;i<st->frame_size;i++)
1092             e[2*i]=2*st->exc[i];
1093       }
1094       break;
1095    default:
1096       fprintf(stderr, "Unknown sb_ctl request: %d\n", request);
1097    }
1098
1099 }