Fixed a couple stupid bugs
[speexdsp.git] / libspeex / sb_celp.c
index bdb2145..e13aa11 100644 (file)
@@ -131,11 +131,11 @@ void *sb_encoder_init(SpeexMode *m)
    SBEncState *st;
    SpeexSBMode *mode;
 
-   st = speex_alloc(sizeof(SBEncState));
+   st = (SBEncState*)speex_alloc(sizeof(SBEncState));
    st->mode = m;
-   mode = m->mode;
+   mode = (SpeexSBMode*)m->mode;
 
-   st->st_low = nb_encoder_init(mode->nb_mode);
+   st->st_low = speex_encoder_init(mode->nb_mode);
    st->full_frame_size = 2*mode->frameSize;
    st->frame_size = mode->frameSize;
    st->subframeSize = mode->subframeSize;
@@ -146,7 +146,9 @@ void *sb_encoder_init(SpeexMode *m)
 
    st->submodes=mode->submodes;
    st->submodeID=mode->defaultSubmode;
+   
    {
+      /* FIXME: Should do this without explicit reference to a mode */
       int mod=6;
       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &mod);
    }
@@ -156,62 +158,60 @@ void *sb_encoder_init(SpeexMode *m)
    st->gamma1=mode->gamma1;
    st->gamma2=mode->gamma2;
    st->first=1;
-   st->stack = speex_alloc(10000*sizeof(float));
-
-   st->x0=speex_alloc(st->full_frame_size*sizeof(float));
-   st->x1=speex_alloc(st->full_frame_size*sizeof(float));
-   st->x0d=speex_alloc(st->frame_size*sizeof(float));
-   st->x1d=speex_alloc(st->frame_size*sizeof(float));
-   st->high=speex_alloc(st->full_frame_size*sizeof(float));
-   st->y0=speex_alloc(st->full_frame_size*sizeof(float));
-   st->y1=speex_alloc(st->full_frame_size*sizeof(float));
-
-   st->h0_mem=speex_alloc(QMF_ORDER*sizeof(float));
-   st->h1_mem=speex_alloc(QMF_ORDER*sizeof(float));
-   st->g0_mem=speex_alloc(QMF_ORDER*sizeof(float));
-   st->g1_mem=speex_alloc(QMF_ORDER*sizeof(float));
-
-   st->buf=speex_alloc(st->windowSize*sizeof(float));
-   st->excBuf=speex_alloc(st->bufSize*sizeof(float));
+   st->stack = speex_alloc(20000*sizeof(float));
+
+   st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
+   st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
+   st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
+   st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
+   st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
+
+   st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
+   st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
+   st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
+   st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
+
+   st->buf=(float*)speex_alloc(st->windowSize*sizeof(float));
+   st->excBuf=(float*)speex_alloc(st->bufSize*sizeof(float));
    st->exc = st->excBuf + st->bufSize - st->windowSize;
-   /*st->exc=st->excBuf+st->frame_size;*/
 
-   st->res=speex_alloc(st->frame_size*sizeof(float));
-   st->sw=speex_alloc(st->frame_size*sizeof(float));
-   st->target=speex_alloc(st->frame_size*sizeof(float));
+   st->res=(float*)speex_alloc(st->frame_size*sizeof(float));
+   st->sw=(float*)speex_alloc(st->frame_size*sizeof(float));
+   st->target=(float*)speex_alloc(st->frame_size*sizeof(float));
    /*Asymetric "pseudo-Hamming" window*/
    {
       int part1, part2;
       part1 = st->subframeSize*7/2;
       part2 = st->subframeSize*5/2;
-      st->window = speex_alloc(st->windowSize*sizeof(float));
+      st->window = (float*)speex_alloc(st->windowSize*sizeof(float));
       for (i=0;i<part1;i++)
          st->window[i]=.54-.46*cos(M_PI*i/part1);
       for (i=0;i<part2;i++)
          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
    }
 
-   st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->lagWindow = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
    for (i=0;i<st->lpcSize+1;i++)
       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
 
-   st->rc = speex_alloc(st->lpcSize*sizeof(float));
-   st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(float));
-   st->lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
-   st->bw_lpc1 = speex_alloc((st->lpcSize+1)*sizeof(float));
-   st->bw_lpc2 = speex_alloc((st->lpcSize+1)*sizeof(float));
-   st->lsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->old_lsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->interp_lsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->interp_lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
-   st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
-
-   st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
-   st->mem_sp2 = speex_alloc(st->lpcSize*sizeof(float));
-   st->mem_sw = speex_alloc(st->lpcSize*sizeof(float));
+   st->rc = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->autocorr = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->bw_lpc1 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->bw_lpc2 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->old_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->interp_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->interp_lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
+
+   st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->mem_sp2 = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->mem_sw = (float*)speex_alloc(st->lpcSize*sizeof(float));
    st->complexity=2;
 
    return st;
@@ -219,12 +219,10 @@ void *sb_encoder_init(SpeexMode *m)
 
 void sb_encoder_destroy(void *state)
 {
-   SBEncState *st=state;
+   SBEncState *st=(SBEncState*)state;
 
-   nb_encoder_destroy(st->st_low);
-   speex_free(st->x0);
+   speex_encoder_destroy(st->st_low);
    speex_free(st->x0d);
-   speex_free(st->x1);
    speex_free(st->x1d);
    speex_free(st->high);
    speex_free(st->y0);
@@ -258,6 +256,7 @@ void sb_encoder_destroy(void *state)
    speex_free(st->mem_sp);
    speex_free(st->mem_sp2);
    speex_free(st->mem_sw);
+   speex_free(st->pi_gain);
 
    speex_free(st->stack);
 
@@ -269,25 +268,18 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
 {
    SBEncState *st;
    int i, roots, sub;
-   float *stack;
-   float *mem, *innov;
+   void *stack;
+   float *mem, *innov, *syn_resp;
+   float *low_pi_gain, *low_exc, *low_innov;
 
-   st = state;
+   st = (SBEncState*)state;
    stack=st->stack;
 
    /* Compute the two sub-bands by filtering with h0 and h1*/
-   fir_mem(in, h0, st->x0, st->full_frame_size, QMF_ORDER, st->h0_mem);
-   fir_mem(in, h1, st->x1, st->full_frame_size, QMF_ORDER, st->h1_mem);
-
-   /* Down-sample x0 and x1 */
-   for (i=0;i<st->frame_size;i++)
-      st->x1d[i]=st->x1[i<<1];
-
-   for (i=0;i<st->frame_size;i++)
-      st->x0d[i]=st->x0[i<<1];
-
+   qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
+    
    /* Encode the narrowband part*/
-   nb_encode(st->st_low, st->x0d, bits);
+   speex_encode(st->st_low, st->x0d, bits);
 
    speex_bits_pack(bits, 1, 1);
    speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
@@ -300,13 +292,20 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
 
    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
 
-   /* Start encoding the high-band */
 
+   low_pi_gain = PUSH(stack, st->nbSubframes, float);
+   low_exc = PUSH(stack, st->frame_size, float);
+   low_innov = PUSH(stack, st->frame_size, float);
+   speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
+   speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
+   speex_encoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
+   
+   /* Start encoding the high-band */
    for (i=0;i<st->windowSize;i++)
       st->buf[i] = st->high[i] * st->window[i];
 
    /* Compute auto-correlation */
-   autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
+   _spx_autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
 
    st->autocorr[0] += 1;        /* prevents NANs */
    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
@@ -352,17 +351,11 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
 
 #ifndef RELEASE
-      /* Up-sample coded low-band and high-band*/
-      for (i=0;i<st->frame_size;i++)
-      {
-         st->x0[(i<<1)]=st->x0d[i];
-         st->x1[(i<<1)]=st->high[i];
-         st->x0[(i<<1)+1]=0;
-         st->x1[(i<<1)+1]=0;
-      }
+
       /* Reconstruct the original */
-      fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
-      fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
+      fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
+      fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
+
       for (i=0;i<st->full_frame_size;i++)
          in[i]=2*(st->y0[i]-st->y1[i]);
 #endif
@@ -391,8 +384,9 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
          st->old_qlsp[i] = st->qlsp[i];
    }
    
-   mem=PUSH(stack, st->lpcSize);
-   innov = PUSH(stack, st->subframeSize);
+   mem=PUSH(stack, st->lpcSize, float);
+   syn_resp=PUSH(stack, st->subframeSize, float);
+   innov = PUSH(stack, st->subframeSize, float);
 
    for (sub=0;sub<st->nbSubframes;sub++)
    {
@@ -403,7 +397,7 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
 
       offset = st->subframeSize*sub;
       sp=st->high+offset;
-      exc=st->excBuf+offset;
+      exc=st->exc+offset;
       res=st->res+offset;
       target=st->target+offset;
       sw=st->sw+offset;
@@ -434,12 +428,14 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
          filters */
       rl=rh=0;
       tmp=1;
+      st->pi_gain[sub]=0;
       for (i=0;i<=st->lpcSize;i++)
       {
          rh += tmp*st->interp_qlpc[i];
          tmp = -tmp;
+         st->pi_gain[sub]+=st->interp_qlpc[i];
       }
-      rl = ((EncState*)st->st_low)->pi_gain[sub];
+      rl = low_pi_gain[sub];
       rl=1/(fabs(rl)+.01);
       rh=1/(fabs(rh)+.01);
       /* Compute ratio, will help predict the gain */
@@ -459,13 +455,14 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
          float g;
          /*speex_bits_pack(bits, 1, 1);*/
          for (i=0;i<st->subframeSize;i++)
-            el+=sqr(((EncState*)st->st_low)->innov[offset+i]);
+            el+=sqr(low_innov[offset+i]);
 
          /* Gain to use if we want to use the low-band excitation for high-band */
          g=eh/(.01+el);
          g=sqrt(g);
 
          g *= filter_ratio;
+
          /* Gain quantization */
          {
             int quant = (int) floor(.5 + 27 + 8.0 * log((g+.0001)));
@@ -483,7 +480,7 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
          float gc, scale, scale_1;
 
          for (i=0;i<st->subframeSize;i++)
-            el+=sqr(((EncState*)st->st_low)->exc[offset+i]);
+            el+=sqr(low_exc[offset+i]);
          /*speex_bits_pack(bits, 0, 1);*/
 
          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
@@ -513,6 +510,11 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
             }
          }
 
+         for (i=0;i<st->subframeSize;i++)
+            exc[i]=0;
+         exc[0]=1;
+         syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
+         
          /* Reset excitation */
          for (i=0;i<st->subframeSize;i++)
             exc[i]=0;
@@ -531,23 +533,6 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
             mem[i]=st->mem_sw[i];
          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
 
-#if 0
-         /*for (i=0;i<st->lpcSize;i++)
-            mem[i]=st->mem_sp[i];
-         residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
-         for (i=0;i<st->lpcSize;i++)
-            mem[i]=st->mem_sw[i];
-            syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);*/
-         
-         /* Compute weighted signal */
-         for (i=0;i<st->lpcSize;i++)
-            mem[i]=st->mem_sp[i];
-         residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
-         for (i=0;i<st->lpcSize;i++)
-            mem[i]=st->mem_sw[i];
-         syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
-#endif
-
          /* Compute target signal */
          for (i=0;i<st->subframeSize;i++)
             target[i]=sw[i]-res[i];
@@ -565,23 +550,23 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
 
          /*print_vec(target, st->subframeSize, "\ntarget");*/
          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
-                                SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
-                                innov, bits, stack, st->complexity);
+                                   SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
+                                   innov, syn_resp, bits, stack, (st->complexity+1)>>1);
          /*print_vec(target, st->subframeSize, "after");*/
 
          for (i=0;i<st->subframeSize;i++)
             exc[i] += innov[i]*scale;
 
          if (SUBMODE(double_codebook)) {
-            float *tmp_stack=stack;
-            float *innov2 = PUSH(tmp_stack, st->subframeSize);
+            void *tmp_stack=stack;
+            float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
             for (i=0;i<st->subframeSize;i++)
                innov2[i]=0;
             for (i=0;i<st->subframeSize;i++)
                target[i]*=2.5;
             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
-                                      innov2, bits, tmp_stack, st->complexity);
+                                      innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
             for (i=0;i<st->subframeSize;i++)
                innov2[i]*=scale*(1/2.5);
             for (i=0;i<st->subframeSize;i++)
@@ -600,7 +585,7 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
          }
 
       }
-#if 1
+
          /*Keep the previous memory*/
          for (i=0;i<st->lpcSize;i++)
             mem[i]=st->mem_sp[i];
@@ -608,27 +593,16 @@ void sb_encode(void *state, float *in, SpeexBits *bits)
          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
          
          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
-         /*residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
-         syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
-         */
          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
-#endif
-      
    }
 
 
 #ifndef RELEASE
-   /* Up-sample coded low-band and high-band*/
-   for (i=0;i<st->frame_size;i++)
-   {
-      st->x0[(i<<1)]=st->x0d[i];
-      st->x1[(i<<1)]=st->high[i];
-      st->x0[(i<<1)+1]=0;
-      st->x1[(i<<1)+1]=0;
-   }
+
    /* Reconstruct the original */
-   fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
-   fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
+   fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
+   fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
+
    for (i=0;i<st->full_frame_size;i++)
       in[i]=2*(st->y0[i]-st->y1[i]);
 #endif
@@ -648,55 +622,52 @@ void *sb_decoder_init(SpeexMode *m)
 {
    SBDecState *st;
    SpeexSBMode *mode;
-   st = speex_alloc(sizeof(SBDecState));
+   st = (SBDecState*)speex_alloc(sizeof(SBDecState));
    st->mode = m;
-   mode=m->mode;
+   mode=(SpeexSBMode*)m->mode;
 
-   st->st_low = nb_decoder_init(mode->nb_mode);
+   st->st_low = speex_decoder_init(mode->nb_mode);
    st->full_frame_size = 2*mode->frameSize;
    st->frame_size = mode->frameSize;
-   st->subframeSize = 40;
-   st->nbSubframes = 4;
+   st->subframeSize = mode->subframeSize;
+   st->nbSubframes = mode->frameSize/mode->subframeSize;
    st->lpcSize=8;
 
    st->submodes=mode->submodes;
    st->submodeID=mode->defaultSubmode;
 
    st->first=1;
-   st->stack = speex_alloc(10000*sizeof(float));
+   st->stack = speex_alloc(20000*sizeof(float));
 
-   st->x0=speex_alloc(st->full_frame_size*sizeof(float));
-   st->x1=speex_alloc(st->full_frame_size*sizeof(float));
-   st->x0d=speex_alloc(st->frame_size*sizeof(float));
-   st->x1d=speex_alloc(st->frame_size*sizeof(float));
-   st->high=speex_alloc(st->full_frame_size*sizeof(float));
-   st->y0=speex_alloc(st->full_frame_size*sizeof(float));
-   st->y1=speex_alloc(st->full_frame_size*sizeof(float));
+   st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
+   st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
+   st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
+   st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
+   st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
 
-   st->h0_mem=speex_alloc(QMF_ORDER*sizeof(float));
-   st->h1_mem=speex_alloc(QMF_ORDER*sizeof(float));
-   st->g0_mem=speex_alloc(QMF_ORDER*sizeof(float));
-   st->g1_mem=speex_alloc(QMF_ORDER*sizeof(float));
+   st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
+   st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
+   st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
+   st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
 
-   st->exc=speex_alloc(st->frame_size*sizeof(float));
+   st->exc=(float*)speex_alloc(st->frame_size*sizeof(float));
 
-   st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
-   st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
 
-   st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
+   st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
+   st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
    return st;
 }
 
 void sb_decoder_destroy(void *state)
 {
    SBDecState *st;
-   st = state;
-   nb_decoder_destroy(st->st_low);
-   speex_free(st->x0);
+   st = (SBDecState*)state;
+   speex_decoder_destroy(st->st_low);
    speex_free(st->x0d);
-   speex_free(st->x1);
    speex_free(st->x1d);
    speex_free(st->high);
    speex_free(st->y0);
@@ -711,6 +682,7 @@ void sb_decoder_destroy(void *state)
    speex_free(st->old_qlsp);
    speex_free(st->interp_qlsp);
    speex_free(st->interp_qlpc);
+   speex_free(st->pi_gain);
 
    speex_free(st->mem_sp);
 
@@ -719,7 +691,7 @@ void sb_decoder_destroy(void *state)
    speex_free(state);
 }
 
-static void sb_decode_lost(SBDecState *st, float *out, float *stack)
+static void sb_decode_lost(SBDecState *st, float *out, void *stack)
 {
    int i;
    for (i=0;i<st->frame_size;i++)
@@ -730,17 +702,10 @@ static void sb_decode_lost(SBDecState *st, float *out, float *stack)
    /* Final signal synthesis from excitation */
    iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
    
-   /* Up-sample coded low-band and high-band*/
-   for (i=0;i<st->frame_size;i++)
-   {
-      st->x0[(i<<1)]=st->x0d[i];
-      st->x1[(i<<1)]=st->high[i];
-      st->x0[(i<<1)+1]=0;
-      st->x1[(i<<1)+1]=0;
-   }
    /* Reconstruct the original */
-   fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
-   fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
+   fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
+   fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
+
    for (i=0;i<st->full_frame_size;i++)
       out[i]=2*(st->y0[i]-st->y1[i]);
    
@@ -753,13 +718,14 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
    SBDecState *st;
    int wideband;
    int ret;
-   float *stack;
+   void *stack;
+   float *low_pi_gain, *low_exc, *low_innov;
 
-   st = state;
+   st = (SBDecState*)state;
    stack=st->stack;
 
    /* Decode the low-band */
-   ret = nb_decode(st->st_low, bits, st->x0d);
+   ret = speex_decode(st->st_low, bits, st->x0d);
 
    /* If error decoding the narrowband part, propagate error */
    if (ret!=0)
@@ -800,17 +766,9 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
       /* Final signal synthesis from excitation */
       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
 
-      /* Up-sample coded low-band and high-band*/
-      for (i=0;i<st->frame_size;i++)
-      {
-         st->x0[(i<<1)]=st->x0d[i];
-         st->x1[(i<<1)]=st->high[i];
-         st->x0[(i<<1)+1]=0;
-         st->x1[(i<<1)+1]=0;
-      }
-      /* Reconstruct the original */
-      fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
-      fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
+      fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
+      fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
+
       for (i=0;i<st->full_frame_size;i++)
          out[i]=2*(st->y0[i]-st->y1[i]);
 
@@ -818,6 +776,12 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
 
    }
 
+   low_pi_gain = PUSH(stack, st->nbSubframes, float);
+   low_exc = PUSH(stack, st->frame_size, float);
+   low_innov = PUSH(stack, st->frame_size, float);
+   speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
+   speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
+   speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
 
    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
    
@@ -831,6 +795,7 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
    {
       float *exc, *sp, tmp, filter_ratio, el=0;
       int offset;
+      float rl=0,rh=0;
       
       offset = st->subframeSize*sub;
       sp=st->high+offset;
@@ -852,19 +817,20 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
 
       /* Calculate reponse ratio between the low and high filter in the middle
          of the band (4000 Hz) */
-      {
-         float rl=0, rh=0;
+      
          tmp=1;
+         st->pi_gain[sub]=0;
          for (i=0;i<=st->lpcSize;i++)
          {
             rh += tmp*st->interp_qlpc[i];
             tmp = -tmp;
+            st->pi_gain[sub]+=st->interp_qlpc[i];
          }
-         rl = ((DecState*)st->st_low)->pi_gain[sub];
+         rl = low_pi_gain[sub];
          rl=1/(fabs(rl)+.01);
          rh=1/(fabs(rh)+.01);
          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
-      }
+      
       
       for (i=0;i<st->subframeSize;i++)
          exc[i]=0;
@@ -874,7 +840,7 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
          int quant;
 
          for (i=0;i<st->subframeSize;i++)
-            el+=sqr(((DecState*)st->st_low)->innov[offset+i]);
+            el+=sqr(low_innov[offset+i]);
          quant = speex_bits_unpack_unsigned(bits, 5);
          g= exp(((float)quant-27)/8.0);
          
@@ -883,12 +849,12 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
          
          /* High-band excitation using the low-band excitation and a gain */
          for (i=0;i<st->subframeSize;i++)
-            exc[i]=.8*g*((DecState*)st->st_low)->innov[offset+i];
+            exc[i]=.8*g*low_innov[offset+i];
       } else {
          float gc, scale;
          int qgc = speex_bits_unpack_unsigned(bits, 4);
          for (i=0;i<st->subframeSize;i++)
-            el+=sqr(((DecState*)st->st_low)->exc[offset+i]);
+            el+=sqr(low_exc[offset+i]);
 
 
          gc = exp((1/3.7)*qgc-2);
@@ -902,8 +868,8 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
             exc[i]*=scale;
 
          if (SUBMODE(double_codebook)) {
-            float *tmp_stack=stack;
-            float *innov2 = PUSH(tmp_stack, st->subframeSize);
+            void *tmp_stack=stack;
+            float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
             for (i=0;i<st->subframeSize;i++)
                innov2[i]=0;
             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
@@ -919,17 +885,9 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
 
    }
 
-   /* Up-sample coded low-band and high-band*/
-   for (i=0;i<st->frame_size;i++)
-   {
-      st->x0[(i<<1)]=st->x0d[i];
-      st->x1[(i<<1)]=st->high[i];
-      st->x0[(i<<1)+1]=0;
-      st->x1[(i<<1)+1]=0;
-   }
-   /* Reconstruct the original */
-   fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
-   fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
+   fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
+   fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
+
    for (i=0;i<st->full_frame_size;i++)
       out[i]=2*(st->y0[i]-st->y1[i]);
 
@@ -945,7 +903,7 @@ int sb_decode(void *state, SpeexBits *bits, float *out)
 void sb_encoder_ctl(void *state, int request, void *ptr)
 {
    SBEncState *st;
-   st=state;
+   st=(SBEncState*)state;
    switch(request)
    {
    case SPEEX_GET_FRAME_SIZE:
@@ -962,11 +920,15 @@ void sb_encoder_ctl(void *state, int request, void *ptr)
       break;
    case SPEEX_SET_VBR_QUALITY:
       {
-         int qual = (*(int*)ptr)+1;
+         int q;
+         float qual = (*(float*)ptr)+1;
          if (qual>10)
             qual=10;
+         q=(int)floor(.5+*(float*)ptr);
+         if (q>10)
+            q=10;
          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
-         speex_encoder_ctl(state, SPEEX_SET_QUALITY, ptr);
+         speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
          break;
       }
    case SPEEX_SET_QUALITY:
@@ -1028,6 +990,8 @@ void sb_encoder_ctl(void *state, int request, void *ptr)
    case SPEEX_SET_COMPLEXITY:
       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
       st->complexity = (*(int*)ptr);
+      if (st->complexity<1)
+         st->complexity=1;
       break;
    case SPEEX_GET_COMPLEXITY:
       (*(int*)ptr) = st->complexity;
@@ -1039,6 +1003,34 @@ void sb_encoder_ctl(void *state, int request, void *ptr)
       else
          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
       break;
+   case SPEEX_GET_PI_GAIN:
+      {
+         int i;
+         float *g = (float*)ptr;
+         for (i=0;i<st->nbSubframes;i++)
+            g[i]=st->pi_gain[i];
+      }
+      break;
+   case SPEEX_GET_EXC:
+      {
+         int i;
+         float *e = (float*)ptr;
+         for (i=0;i<st->full_frame_size;i++)
+            e[i]=0;
+         for (i=0;i<st->frame_size;i++)
+            e[2*i]=2*st->exc[i];
+      }
+      break;
+   case SPEEX_GET_INNOV:
+      {
+         int i;
+         float *e = (float*)ptr;
+         for (i=0;i<st->full_frame_size;i++)
+            e[i]=0;
+         for (i=0;i<st->frame_size;i++)
+            e[2*i]=2*st->exc[i];
+      }
+      break;
    default:
       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
    }
@@ -1048,7 +1040,7 @@ void sb_encoder_ctl(void *state, int request, void *ptr)
 void sb_decoder_ctl(void *state, int request, void *ptr)
 {
    SBDecState *st;
-   st=state;
+   st=(SBDecState*)state;
    switch(request)
    {
    case SPEEX_GET_FRAME_SIZE:
@@ -1064,6 +1056,40 @@ void sb_decoder_ctl(void *state, int request, void *ptr)
       else
          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
       break;
+   case SPEEX_SET_HANDLER:
+      speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
+      break;
+   case SPEEX_SET_USER_HANDLER:
+      speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
+      break;
+   case SPEEX_GET_PI_GAIN:
+      {
+         int i;
+         float *g = (float*)ptr;
+         for (i=0;i<st->nbSubframes;i++)
+            g[i]=st->pi_gain[i];
+      }
+      break;
+   case SPEEX_GET_EXC:
+      {
+         int i;
+         float *e = (float*)ptr;
+         for (i=0;i<st->full_frame_size;i++)
+            e[i]=0;
+         for (i=0;i<st->frame_size;i++)
+            e[2*i]=2*st->exc[i];
+      }
+      break;
+   case SPEEX_GET_INNOV:
+      {
+         int i;
+         float *e = (float*)ptr;
+         for (i=0;i<st->full_frame_size;i++)
+            e[i]=0;
+         for (i=0;i<st->frame_size;i++)
+            e[2*i]=2*st->exc[i];
+      }
+      break;
    default:
       fprintf(stderr, "Unknown sb_ctl request: %d\n", request);
    }