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