wideband VBR seems to (almost) work. Need to adapt it to work on ultra-
[speexdsp.git] / libspeex / nb_celp.c
index 7678185..e3ff6a2 100644 (file)
@@ -1,19 +1,32 @@
 /* Copyright (C) 2002 Jean-Marc Valin 
-   File: speex.c
+   File: nb_celp.c
 
-   This library is free software; you can redistribute it and/or
-   modify it under the terms of the GNU Lesser General Public
-   License as published by the Free Software Foundation; either
-   version 2.1 of the License, or (at your option) any later version.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions
+   are met:
    
-   This library is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-   Lesser General Public License for more details.
+   - Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
    
-   You should have received a copy of the GNU Lesser General Public
-   License along with this library; if not, write to the Free Software
-   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+   - Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+   
+   - Neither the name of the Xiph.org Foundation nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+   
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
+   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <stdlib.h>
 #include "speex_bits.h"
 #include "vbr.h"
 #include "misc.h"
+#include "speex_callbacks.h"
+
+#ifdef SLOW_TRIG
+#include "math_approx.h"
+#define cos speex_cos
+#endif
 
 extern int training_weight;
 #ifndef M_PI
@@ -42,10 +61,12 @@ extern int training_weight;
 float exc_gain_quant_scal3[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
 
 float exc_gain_quant_scal1[2]={-0.35, 0.05};
 
 #define sqr(x) ((x)*(x))
+
+#ifndef min
 #define min(a,b) ((a) < (b) ? (a) : (b))
+#endif
 
 void *nb_encoder_init(SpeexMode *m)
 {
@@ -53,8 +74,8 @@ void *nb_encoder_init(SpeexMode *m)
    SpeexNBMode *mode;
    int i;
 
-   mode=m->mode;
-   st = speex_alloc(sizeof(EncState));
+   mode=(SpeexNBMode *)m->mode;
+   st = (EncState*)speex_alloc(sizeof(EncState));
    st->mode=m;
    /* Codec parameters, should eventually have several "modes"*/
    st->frameSize = mode->frameSize;
@@ -75,67 +96,74 @@ void *nb_encoder_init(SpeexMode *m)
    st->submodeID=mode->defaultSubmode;
    st->pre_mem=0;
    st->pre_mem2=0;
+   st->bounded_pitch = 0;
 
    /* Allocating input buffer */
-   st->inBuf = speex_alloc(st->bufSize*sizeof(float));
+   st->inBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
    st->frame = st->inBuf + st->bufSize - st->windowSize;
    /* Allocating excitation buffer */
-   st->excBuf = speex_alloc(st->bufSize*sizeof(float));
+   st->excBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
    st->exc = st->excBuf + st->bufSize - st->windowSize;
-   st->swBuf = speex_alloc(st->bufSize*sizeof(float));
+   st->swBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
    st->sw = st->swBuf + st->bufSize - st->windowSize;
 
-   st->exc2Buf = speex_alloc(st->bufSize*sizeof(float));
+   st->exc2Buf = (float*)speex_alloc(st->bufSize*sizeof(float));
    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
 
-   st->innov = speex_alloc(st->frameSize*sizeof(float));
+   st->innov = (float*)speex_alloc(st->frameSize*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);
    }
    /* Create the window for autocorrelation (lag-windowing) */
-   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->autocorr = speex_alloc((st->lpcSize+1)*sizeof(float));
+   st->autocorr = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
 
-   st->stack = speex_alloc(20000*sizeof(float));
+   st->stack = (float*)speex_alloc(20000*sizeof(float));
 
-   st->buf2 = speex_alloc(st->windowSize*sizeof(float));
-
-   st->lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
-   st->interp_lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
-   st->interp_qlpc = 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->rc = speex_alloc(st->lpcSize*sizeof(float));
+   st->buf2 = (float*)speex_alloc(st->windowSize*sizeof(float));
+
+   st->lpc = (float*)speex_alloc((st->lpcSize+1)*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->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->rc = (float*)speex_alloc(st->lpcSize*sizeof(float));
    st->first = 1;
+   for (i=0;i<st->lpcSize;i++)
+   {
+      st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
+   }
 
-   st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
-   st->mem_sw = speex_alloc(st->lpcSize*sizeof(float));
+   st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->mem_sw = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->mem_sw_whole = (float*)speex_alloc(st->lpcSize*sizeof(float));
+   st->mem_exc = (float*)speex_alloc(st->lpcSize*sizeof(float));
 
-   st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
+   st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
 
-   st->pitch = speex_alloc(st->nbSubframes*sizeof(int));
+   st->pitch = (int*)speex_alloc(st->nbSubframes*sizeof(int));
 
    if (1) {
-      st->vbr = speex_alloc(sizeof(VBRState));
+      st->vbr = (VBRState*)speex_alloc(sizeof(VBRState));
       vbr_init(st->vbr);
       st->vbr_quality = 8;
       st->vbr_enabled = 0;
@@ -143,20 +171,21 @@ void *nb_encoder_init(SpeexMode *m)
       st->vbr = 0;
    }
    st->complexity=2;
+   st->sampling_rate=8000;
 
    return st;
 }
 
 void nb_encoder_destroy(void *state)
 {
-   EncState *st=state;
+   EncState *st=(EncState *)state;
    /* Free all allocated memory */
    speex_free(st->inBuf);
    speex_free(st->excBuf);
    speex_free(st->swBuf);
    speex_free(st->exc2Buf);
    speex_free(st->innov);
-   speex_free(st->stack);
+   speex_free((float*)st->stack);
 
    speex_free(st->window);
    speex_free(st->buf2);
@@ -178,6 +207,8 @@ void nb_encoder_destroy(void *state)
 
    speex_free(st->mem_sp);
    speex_free(st->mem_sw);
+   speex_free(st->mem_sw_whole);
+   speex_free(st->mem_exc);
    speex_free(st->pi_gain);
    speex_free(st->pitch);
 
@@ -185,21 +216,23 @@ void nb_encoder_destroy(void *state)
    speex_free(st->vbr);
 
    /*Free state memory... should be last*/
-   speex_free(st);
+   speex_free((float*)st);
 }
 
 void nb_encode(void *state, float *in, SpeexBits *bits)
 {
    EncState *st;
    int i, sub, roots;
-   float error;
    int ol_pitch;
    float ol_pitch_coef;
    float ol_gain;
-   float delta_qual=0;
+   float *res, *target, *mem;
+   void *stack;
+   float *syn_resp;
+
+   st=(EncState *)state;
+   stack=st->stack;
 
-   st=state;
-   
    /* Copy new data in input buffer */
    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
@@ -207,41 +240,56 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
    st->pre_mem = in[st->frameSize-1];
 
+   /* Move signals 1 frame towards the past */
    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
 
 
-
    /* Window for analysis */
    for (i=0;i<st->windowSize;i++)
       st->buf2[i] = st->frame[i] * st->window[i];
 
    /* Compute auto-correlation */
-   autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
+   _spx_autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
 
    st->autocorr[0] += 10;        /* prevents NANs */
    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
+
    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
    for (i=0;i<st->lpcSize+1;i++)
       st->autocorr[i] *= st->lagWindow[i];
 
    /* Levinson-Durbin */
-   error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
+   wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
    st->lpc[0]=1;
 
    /* LPC to LSPs (x-domain) transform */
-   roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
-   if (roots!=st->lpcSize)
+   roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
+   /* Check if we found all the roots */
+   if (roots==st->lpcSize)
    {
-      fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
-      exit(1);
+      /* LSP x-domain to angle domain*/
+      for (i=0;i<st->lpcSize;i++)
+         st->lsp[i] = acos(st->lsp[i]);
+   } else {
+      /* Search again if we can afford it */
+      if (st->complexity>1)
+         roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.05, stack);
+      if (roots==st->lpcSize) 
+      {
+         /* LSP x-domain to angle domain*/
+         for (i=0;i<st->lpcSize;i++)
+            st->lsp[i] = acos(st->lsp[i]);
+      } else {
+         /*If we can't find all LSP's, do some damage control and use previous filter*/
+         for (i=0;i<st->lpcSize;i++)
+         {
+            st->lsp[i]=st->old_lsp[i];
+         }
+      }
    }
 
-   /* x-domain to angle domain*/
-   for (i=0;i<st->lpcSize;i++)
-      st->lsp[i] = acos(st->lsp[i]);
-   /*print_vec(st->lsp, 10, "LSP:");*/
    /* LSP Quantization */
    if (st->first)
    {
@@ -249,6 +297,12 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          st->old_lsp[i] = st->lsp[i];
    }
 
+   if (0) {
+      float dd=0;
+      for (i=0;i<st->lpcSize;i++)
+         dd += fabs(st->old_lsp[i] - st->lsp[i]);
+      printf ("lspdist = %f\n", dd);
+   }
 
    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
    {
@@ -260,20 +314,23 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       /* Compute interpolated LPCs (unquantized) for whole frame*/
       for (i=0;i<st->lpcSize;i++)
          st->interp_lsp[i] = cos(st->interp_lsp[i]);
-      lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
+      lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
 
-      bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
-      bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
 
-      residue(st->frame, st->bw_lpc1, st->exc, st->frameSize, st->lpcSize);
-      syn_filt(st->exc, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize);
-      
       /*Open-loop pitch*/
+      if (!st->submodes[st->submodeID] || st->vbr_enabled || SUBMODE(forced_pitch_gain) ||
+          SUBMODE(lbr_pitch) != -1)
       {
          int nol_pitch[4];
          float nol_pitch_coef[4];
+         
+         bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
+         bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
+         
+         filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);
+
          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
-                               nol_pitch, nol_pitch_coef, 4, st->stack);
+                               nol_pitch, nol_pitch_coef, 4, stack);
          ol_pitch=nol_pitch[0];
          ol_pitch_coef = nol_pitch_coef[0];
          /*Try to remove pitch multiples*/
@@ -289,9 +346,12 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          }
          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
          /*printf ("ol_pitch: %d %f\n", ol_pitch, ol_pitch_coef);*/
+      } else {
+         ol_pitch=0;
+         ol_pitch_coef=0;
       }
       /*Compute "real" excitation*/
-      residue(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize);
+      fir_mem2(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc);
 
       /* Compute open-loop excitation gain */
       ol_gain=0;
@@ -304,17 +364,34 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
    /*Experimental VBR stuff*/
    if (st->vbr)
    {
-      delta_qual = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
-      if (delta_qual<0)
-         delta_qual*=.1*(3+st->vbr_quality);
+      st->relative_quality = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
+      /*if (delta_qual<0)*/
+      /*  delta_qual*=.1*(3+st->vbr_quality);*/
       if (st->vbr_enabled) 
       {
-         int qual = (int)floor(st->vbr_quality+delta_qual+.5);
-         if (qual<0)
-            qual=0;
-         if (qual>10)
-            qual=10;
-         speex_encoder_ctl(state, SPEEX_SET_QUALITY, &qual);
+         int mode;
+         mode = 7;
+         while (mode)
+         {
+            int v1;
+            float thresh;
+            v1=(int)floor(st->vbr_quality);
+            if (v1==10)
+               thresh = vbr_nb_thresh[mode][v1];
+            else
+               thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
+            if (st->relative_quality > thresh)
+               break;
+            mode--;
+         }
+         /*fprintf(stderr, "");
+         fprintf (stderr, "encode %f %d\n", st->relative_quality, mode);
+         fprintf(stderr, "encode: %d %d\n",st->submodeID, mode);*/
+
+         speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
+         /*fprintf(stderr, "encode: %d %d\n",st->submodeID, mode);*/
+      } else {
+         st->relative_quality = -1;
       }
    }
    /*printf ("VBR quality = %f\n", vbr_qual);*/
@@ -337,7 +414,7 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       st->first=1;
 
       /* Final signal synthesis from excitation */
-      syn_filt_mem(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
+      iir_mem2(st->exc, st->interp_qlpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
 
       in[0] = st->frame[0] + st->preemph*st->pre_mem2;
       for (i=1;i<st->frameSize;i++)
@@ -393,12 +470,19 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          st->old_qlsp[i] = st->qlsp[i];
    }
 
+   /* Filter response */
+   res = PUSH(stack, st->subframeSize, float);
+   /* Target signal */
+   target = PUSH(stack, st->subframeSize, float);
+   syn_resp = PUSH(stack, st->subframeSize, float);
+   mem = PUSH(stack, st->lpcSize, float);
+
    /* Loop on sub-frames */
    for (sub=0;sub<st->nbSubframes;sub++)
    {
-      float esig, enoise, snr, tmp;
+      float tmp;
       int   offset;
-      float *sp, *sw, *res, *exc, *target, *mem, *exc2;
+      float *sp, *sw, *exc, *exc2;
       int pitch;
 
       /* Offset relative to start of frame */
@@ -412,11 +496,6 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
 
       exc2=st->exc2+offset;
 
-      /* Filter response */
-      res = PUSH(st->stack, st->subframeSize);
-      /* Target signal */
-      target = PUSH(st->stack, st->subframeSize);
-      mem = PUSH(st->stack, st->lpcSize);
 
       /* LSP interpolation (quantized and unquantized) */
       tmp = (1.0 + sub)/st->nbSubframes;
@@ -432,11 +511,11 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       /* Compute interpolated LPCs (quantized and unquantized) */
       for (i=0;i<st->lpcSize;i++)
          st->interp_lsp[i] = cos(st->interp_lsp[i]);
-      lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
+      lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
 
       for (i=0;i<st->lpcSize;i++)
          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
-      lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
+      lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
 
       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
       tmp=1;
@@ -446,7 +525,6 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
          tmp = -tmp;
       }
-     
 
       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
@@ -460,6 +538,12 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
             st->bw_lpc2[i]=0;
       }
 
+      /* Compute impulse response of A(z/g1) / ( A(z)*A(z/g2) )*/
+      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;
@@ -469,25 +553,16 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
       for (i=0;i<st->lpcSize;i++)
          mem[i]=st->mem_sp[i];
-      syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
-      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);
+      iir_mem2(exc, st->interp_qlpc, exc, 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);
-
+      filter_mem2(exc, st->bw_lpc1, 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);
-      
-      esig=0;
-      for (i=0;i<st->subframeSize;i++)
-         esig+=sw[i]*sw[i];
+      filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
       
       /* Compute target signal */
       for (i=0;i<st->subframeSize;i++)
@@ -496,7 +571,7 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       for (i=0;i<st->subframeSize;i++)
          exc[i]=exc2[i]=0;
 
-      /* If we have a long-term predictor (not all sub-modes have one) */
+      /* If we have a long-term predictor (otherwise, something's wrong) */
       if (SUBMODE(ltp_quant))
       {
          int pit_min, pit_max;
@@ -521,47 +596,38 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
             pit_min = st->min_pitch;
             pit_max = st->max_pitch;
          }
+         
+         /* Force pitch to use only the current frame if needed */
+         if (st->bounded_pitch && pit_max>offset)
+            pit_max=offset;
 
+         /* Perform pitch search */
          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
-                                    st->lpcSize, st->subframeSize, bits, st->stack, 
-                                    exc2, st->complexity);
+                                    st->lpcSize, st->subframeSize, bits, stack, 
+                                    exc2, syn_resp, st->complexity);
 
-         /*printf ("cl_pitch: %d\n", pitch);*/
          st->pitch[sub]=pitch;
       } else {
          fprintf (stderr, "No pitch prediction, what's wrong\n");
       }
 
       /* Update target for adaptive codebook contribution */
-      residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
-      syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
-      syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
+      syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, stack);
       for (i=0;i<st->subframeSize;i++)
-        target[i]-=res[i];
-
-      /* Compute noise energy and SNR */
-      enoise=0;
-      for (i=0;i<st->subframeSize;i++)
-         enoise += target[i]*target[i];
-      snr = 10*log10((esig+1)/(enoise+1));
-      /*st->pitch[sub]=(int)snr;*/
-#ifdef DEBUG
-      printf ("pitch SNR = %f\n", snr);
-#endif
+         target[i]-=res[i];
 
 
       /* Quantization of innovation */
       {
          float *innov;
          float ener=0, ener_1;
-         /*innov=PUSH(st->stack, st->subframeSize);*/
+
          innov = st->innov+sub*st->subframeSize;
          for (i=0;i<st->subframeSize;i++)
             innov[i]=0;
-         syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
-         residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
-         residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
+         
+         residue_percep_zero(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize, stack);
          for (i=0;i<st->subframeSize;i++)
             ener+=st->buf2[i]*st->buf2[i];
          ener=sqrt(.1+ener/st->subframeSize);
@@ -569,9 +635,7 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          
          ener /= ol_gain;
 
-         if (0)
-            printf ("ener: %f %f %f\n", ener, ol_gain, ol_pitch_coef);
-
+         /* Calculate gain correction for the sub-frame (if any) */
          if (SUBMODE(have_subframe_gain)) 
          {
             int qe;
@@ -587,13 +651,12 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
                ener=exc_gain_quant_scal1[qe];               
             }
             ener=exp(ener);
-            /*printf ("encode gain: %d %f\n", qe, ener);*/
          } else {
             ener=1;
          }
 
          ener*=ol_gain;
-         /*printf ("transmit gain: %f\n", ener);*/
+
          ener_1 = 1/ener;
 
          if (0) {
@@ -604,15 +667,19 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
             printf ("\n");
          }
          
+         /* Normalize innovation */
          for (i=0;i<st->subframeSize;i++)
             target[i]*=ener_1;
          
+         /* Quantize innovation */
          if (SUBMODE(innovation_quant))
          {
-            /* Normal quantization */
+            /* Codebook search */
             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
-                                      innov, bits, st->stack, st->complexity);
+                                      innov, syn_resp, bits, stack, st->complexity);
+            
+            /* De-normalize innovation and update excitation */
             for (i=0;i<st->subframeSize;i++)
                innov[i]*=ener;
             for (i=0;i<st->subframeSize;i++)
@@ -620,37 +687,39 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          } else {
             fprintf(stderr, "No fixed codebook\n");
          }
-         /*POP(st->stack);*/
+
+         /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
+         if (SUBMODE(double_codebook)) {
+            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.2;
+            SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
+                                      SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
+                                      innov2, syn_resp, bits, tmp_stack, st->complexity);
+            for (i=0;i<st->subframeSize;i++)
+               innov2[i]*=ener*(1/2.2);
+            for (i=0;i<st->subframeSize;i++)
+               exc[i] += innov2[i];
+         }
+
          for (i=0;i<st->subframeSize;i++)
             target[i]*=ener;
 
       }
 
-      /* Compute weighted noise energy and SNR */
-      enoise=0;
-      for (i=0;i<st->subframeSize;i++)
-         enoise += target[i]*target[i];
-      snr = 10*log10((esig+1)/(enoise+1));
-#ifdef DEBUG
-      printf ("seg SNR = %f\n", snr);
-#endif
-
       /*Keep the previous memory*/
       for (i=0;i<st->lpcSize;i++)
          mem[i]=st->mem_sp[i];
       /* Final signal synthesis from excitation */
-      syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
+      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);
       for (i=0;i<st->subframeSize;i++)
          exc2[i]=exc[i];
-
-      POP(st->stack);
-      POP(st->stack);
-      POP(st->stack);
    }
 
    /* Store the LSPs for interpolation in the next frame */
@@ -668,6 +737,10 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
      in[i]=st->frame[i] + st->preemph*in[i-1];
    st->pre_mem2=in[st->frameSize-1];
 
+   if (SUBMODE(innovation_quant) == noise_codebook_quant)
+      st->bounded_pitch = 1;
+   else
+      st->bounded_pitch = 0;
 }
 
 
@@ -677,8 +750,8 @@ void *nb_decoder_init(SpeexMode *m)
    SpeexNBMode *mode;
    int i;
 
-   mode=m->mode;
-   st = speex_alloc(sizeof(DecState));
+   mode=(SpeexNBMode*)m->mode;
+   st = (DecState *)speex_alloc(sizeof(DecState));
    st->mode=m;
 
    st->first=1;
@@ -703,32 +776,39 @@ void *nb_decoder_init(SpeexMode *m)
 
    st->stack = speex_alloc(20000*sizeof(float));
 
-   st->inBuf = speex_alloc(st->bufSize*sizeof(float));
+   st->inBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
    st->frame = st->inBuf + st->bufSize - st->windowSize;
-   st->excBuf = speex_alloc(st->bufSize*sizeof(float));
+   st->excBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
    st->exc = st->excBuf + st->bufSize - st->windowSize;
    for (i=0;i<st->bufSize;i++)
       st->inBuf[i]=0;
    for (i=0;i<st->bufSize;i++)
       st->excBuf[i]=0;
-   st->innov = speex_alloc(st->frameSize*sizeof(float));
+   st->innov = (float*)speex_alloc(st->frameSize*sizeof(float));
 
-   st->interp_qlpc = speex_alloc((st->lpcSize+1)*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->mem_sp = speex_alloc(5*st->lpcSize*sizeof(float));
+   st->interp_qlpc = (float*)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->mem_sp = (float*)speex_alloc(5*st->lpcSize*sizeof(float));
 
-   st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
+   st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
    st->last_pitch = 40;
    st->count_lost=0;
+   st->sampling_rate=8000;
+
+   st->user_callback.func = &speex_default_user_handler;
+   st->user_callback.data = NULL;
+   for (i=0;i<16;i++)
+      st->speex_callbacks[i].func = NULL;
+
    return st;
 }
 
 void nb_decoder_destroy(void *state)
 {
    DecState *st;
-   st=state;
+   st=(DecState*)state;
    speex_free(st->inBuf);
    speex_free(st->excBuf);
    speex_free(st->innov);
@@ -743,7 +823,89 @@ void nb_decoder_destroy(void *state)
    speex_free(state);
 }
 
-void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
+static void nb_decode_lost(DecState *st, float *out, void *stack)
+{
+   int i, sub;
+   float *awk1, *awk2, *awk3;
+   /*float exc_ener=0,g;*/
+   /* Shift all buffers by one frame */
+   speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
+   speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
+
+   awk1=PUSH(stack, (st->lpcSize+1), float);
+   awk2=PUSH(stack, (st->lpcSize+1), float);
+   awk3=PUSH(stack, (st->lpcSize+1), float);
+
+   for (sub=0;sub<st->nbSubframes;sub++)
+   {
+      int offset;
+      float *sp, *exc;
+      /* Offset relative to start of frame */
+      offset = st->subframeSize*sub;
+      /* Original signal */
+      sp=st->frame+offset;
+      /* Excitation */
+      exc=st->exc+offset;
+      /* Excitation after post-filter*/
+
+      /* Calculate perceptually enhanced LPC filter */
+      if (st->lpc_enh_enabled)
+      {
+         float r=.9;
+         
+         float k1,k2,k3;
+         k1=SUBMODE(lpc_enh_k1);
+         k2=SUBMODE(lpc_enh_k2);
+         k3=(1-(1-r*k1)/(1-r*k2))/r;
+         if (!st->lpc_enh_enabled)
+         {
+            k1=k2;
+            k3=0;
+         }
+         bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
+         bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
+         bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
+         
+      }
+        
+      /* Make up a plausible excitation */
+      /* THIS CAN BE IMPROVED */
+      for (i=0;i<st->subframeSize;i++)
+      {
+         exc[i]=st->last_pitch_gain*exc[i-st->last_pitch] + 
+         .8*st->innov[i+offset];
+      }
+
+      for (i=0;i<st->subframeSize;i++)
+         sp[i]=exc[i];
+      
+      /* Signal synthesis */
+      if (st->lpc_enh_enabled)
+      {
+         filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
+                     st->mem_sp+st->lpcSize);
+         filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
+                     st->mem_sp);
+      } else {
+         for (i=0;i<st->lpcSize;i++)
+            st->mem_sp[st->lpcSize+i] = 0;
+         iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
+                     st->mem_sp);
+      }
+  
+   }
+
+   out[0] = st->frame[0] + st->preemph*st->pre_mem;
+   for (i=1;i<st->frameSize;i++)
+      out[i]=st->frame[i] + st->preemph*out[i-1];
+   st->pre_mem=out[st->frameSize-1];
+   
+   st->first = 0;
+   st->count_lost++;
+}
+
+
+int nb_decode(void *state, SpeexBits *bits, float *out)
 {
    DecState *st;
    int i, sub;
@@ -755,26 +917,61 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
    int best_pitch=40;
    float best_pitch_gain=-1;
    int wideband;
-
-   st=state;
-
-   wideband = speex_bits_unpack_unsigned(bits, 1);
-   if (wideband)
+   int m;
+   void *stack;
+   float *awk1, *awk2, *awk3;
+   st=(DecState*)state;
+   stack=st->stack;
+
+   /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
+   if (!bits)
    {
-      int submode;
-      int advance;
-      submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
-      advance = sb_wb_mode.submodes[submode]->bits_per_frame - (SB_SUBMODE_BITS+1);
-      speex_bits_advance(bits, advance);
+      nb_decode_lost(st, out, stack);
+      return 0;
+   }
+
+   /* Search for next narrwoband block (handle requests, skip wideband blocks) */
+   do {
       wideband = speex_bits_unpack_unsigned(bits, 1);
-      if (wideband)
+      if (wideband) /* Skip wideband block (for compatibility) */
       {
-         fprintf (stderr, "Corrupted stream\n");
+         int submode;
+         int advance;
+         submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
+         advance = submode;
+         speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
+         advance -= (SB_SUBMODE_BITS+1);
+         speex_bits_advance(bits, advance);
+         wideband = speex_bits_unpack_unsigned(bits, 1);
+         if (wideband)
+         {
+            fprintf (stderr, "Corrupted stream?\n");
+         }
       }
-   }
+
+      m = speex_bits_unpack_unsigned(bits, 4);
+      if (m==15) /* We found a terminator */
+      {
+         return -1;
+      } else if (m==14) /* Speex in-band request */
+      {
+         int ret = speex_inband_handler(bits, st->speex_callbacks, state);
+         if (ret)
+            return ret;
+      } else if (m==13) /* User in-band request */
+      {
+         int ret = st->user_callback.func(bits, state, st->user_callback.data);
+         if (ret)
+            return ret;
+      } else if (m>7) /* Invalid mode */
+      {
+         return -2;
+      }
+      
+   } while (m>7);
 
    /* Get the sub-mode that was used */
-   st->submodeID = speex_bits_unpack_unsigned(bits, NB_SUBMODE_BITS);
+   st->submodeID = m;
 
    /* Shift all buffers by one frame */
    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
@@ -786,16 +983,16 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
       for (i=0;i<st->frameSize;i++)
          st->exc[i]=0;
       st->first=1;
-      
+
       /* Final signal synthesis from excitation */
-      syn_filt_mem(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
+      iir_mem2(st->exc, st->interp_qlpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
 
       out[0] = st->frame[0] + st->preemph*st->pre_mem;
       for (i=1;i<st->frameSize;i++)
          out[i]=st->frame[i] + st->preemph*out[i-1];
       st->pre_mem=out[st->frameSize-1];
       st->count_lost=0;
-      return;
+      return 0;
    }
 
    /* Unquantize LSPs */
@@ -830,12 +1027,16 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
       /*printf ("decode_ol_gain: %f\n", ol_gain);*/
    }
 
+   awk1=PUSH(stack, st->lpcSize+1, float);
+   awk2=PUSH(stack, st->lpcSize+1, float);
+   awk3=PUSH(stack, st->lpcSize+1, float);
+
    /*Loop on subframes */
    for (sub=0;sub<st->nbSubframes;sub++)
    {
       int offset;
       float *sp, *exc, tmp;
-      float *num, *den;
+
       /* Offset relative to start of frame */
       offset = st->subframeSize*sub;
       /* Original signal */
@@ -849,24 +1050,35 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
       for (i=0;i<st->lpcSize;i++)
          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
 
+      /* Make sure the LSP's are stable */
       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
 
 
       /* Compute interpolated LPCs (unquantized) */
       for (i=0;i<st->lpcSize;i++)
          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
-      lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
+      lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
 
-      num=PUSH(st->stack, ((st->lpcSize<<1)+1));
-      den=PUSH(st->stack, ((st->lpcSize<<1)+1));
+      /* Compute enhanced synthesis filter */
       if (st->lpc_enh_enabled)
       {
-         enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
-                 SUBMODE(lpc_enh_k1), SUBMODE(lpc_enh_k2), st->stack);
-      } else {
-         enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
-                 SUBMODE(lpc_enh_k2), SUBMODE(lpc_enh_k2), st->stack);
+         float r=.9;
+         
+         float k1,k2,k3;
+         k1=SUBMODE(lpc_enh_k1);
+         k2=SUBMODE(lpc_enh_k2);
+         k3=(1-(1-r*k1)/(1-r*k2))/r;
+         if (!st->lpc_enh_enabled)
+         {
+            k1=k2;
+            k3=0;
+         }
+         bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
+         bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
+         bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
+         
       }
+
       /* Compute analysis filter at w=pi */
       tmp=1;
       st->pi_gain[sub]=0;
@@ -884,6 +1096,7 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
       if (SUBMODE(ltp_unquant))
       {
          int pit_min, pit_max;
+         /* Handle pitch constraints if any */
          if (SUBMODE(lbr_pitch) != -1)
          {
             int margin;
@@ -904,33 +1117,17 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
             pit_max = st->max_pitch;
          }
 
+         /* Pitch synthesis */
          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
-                              st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, 0);
-         
+                              st->subframeSize, &pitch, &pitch_gain[0], bits, stack, st->count_lost);
          
-         if (!lost)
+         tmp = (pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
+         if (tmp>best_pitch_gain)
          {
-            /* If the frame was not lost... */
-            tmp = fabs(pitch_gain[0])+fabs(pitch_gain[1])+fabs(pitch_gain[2]);
-            tmp = fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
-            if (tmp>best_pitch_gain)
-            {
-               best_pitch = pitch;
-               while (best_pitch+pitch<st->max_pitch)
-               {
-                  best_pitch+=pitch;
-               }
-               best_pitch_gain = tmp*.9;
-               if (best_pitch_gain>.85)
-                  best_pitch_gain=.85;
-            }
-         } else {
-            /* What to do with pitch if we lost the frame */
-            for (i=0;i<st->subframeSize;i++)
-               exc[i]=0;
-            /*printf ("best_pitch: %d %f\n", st->last_pitch, st->last_pitch_gain);*/
-            for (i=0;i<st->subframeSize;i++)
-               exc[i]=st->last_pitch_gain*exc[i-st->last_pitch];
+            best_pitch = pitch;
+            best_pitch_gain = tmp*.9;
+            if (best_pitch_gain>.85)
+               best_pitch_gain=.85;
          }
       } else {
          fprintf (stderr, "No pitch prediction, what's wrong\n");
@@ -942,11 +1139,11 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
          float ener;
          float *innov;
          
-         /*innov = PUSH(st->stack, st->subframeSize);*/
          innov = st->innov+sub*st->subframeSize;
          for (i=0;i<st->subframeSize;i++)
             innov[i]=0;
 
+         /* Decode sub-frame gain correction */
          if (SUBMODE(have_subframe_gain)==3)
          {
             q_energy = speex_bits_unpack_unsigned(bits, 3);
@@ -964,35 +1161,54 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
          if (SUBMODE(innovation_unquant))
          {
             /*Fixed codebook contribution*/
-            SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, st->stack);
+            SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
          } else {
             fprintf(stderr, "No fixed codebook\n");
          }
 
-         if (st->count_lost)
-            ener*=pow(.8,st->count_lost);
-
+         /* De-normalize innovation and update excitation */
          for (i=0;i<st->subframeSize;i++)
             innov[i]*=ener;
          for (i=0;i<st->subframeSize;i++)
             exc[i]+=innov[i];
 
-         /*POP(st->stack);*/
+         /* Decode second codebook (only for some modes) */
+         if (SUBMODE(double_codebook))
+         {
+            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, bits, tmp_stack);
+            for (i=0;i<st->subframeSize;i++)
+               innov2[i]*=ener*(1/2.2);
+            for (i=0;i<st->subframeSize;i++)
+               exc[i] += innov2[i];
+         }
+
       }
 
       for (i=0;i<st->subframeSize;i++)
          sp[i]=exc[i];
 
+      /* Signal synthesis */
       if (st->lpc_enh_enabled && SUBMODE(comb_gain>0))
          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
                               pitch, pitch_gain, .5);
-      pole_zero_mem(sp, num, den, sp, st->subframeSize, (st->lpcSize<<1), 
-                    st->mem_sp+st->lpcSize, st->stack);
-      syn_filt_mem(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
-        st->mem_sp);
-      
-      POP(st->stack);
-      POP(st->stack);
+      if (st->lpc_enh_enabled)
+      {
+         /* Use enhanced LPC filter */
+         filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
+                     st->mem_sp+st->lpcSize);
+         filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
+                     st->mem_sp);
+      } else {
+         /* Use regular filter */
+         for (i=0;i<st->lpcSize;i++)
+            st->mem_sp[st->lpcSize+i] = 0;
+         iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
+                     st->mem_sp);
+      }
    }
    
    /*Copy output signal*/
@@ -1008,29 +1224,27 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
 
    /* The next frame will not be the first (Duh!) */
    st->first = 0;
-   if (!lost)
-      st->count_lost=0;
-   else
-      st->count_lost++;
-   if (!lost)
-   {
-      st->last_pitch = best_pitch;
-      st->last_pitch_gain = best_pitch_gain;
-   }
+   st->count_lost=0;
+   st->last_pitch = best_pitch;
+   st->last_pitch_gain = best_pitch_gain;
+
+   return 0;
 }
 
 void nb_encoder_ctl(void *state, int request, void *ptr)
 {
    EncState *st;
-   st=state;     
+   st=(EncState*)state;     
    switch(request)
    {
    case SPEEX_GET_FRAME_SIZE:
       (*(int*)ptr) = st->frameSize;
       break;
+   case SPEEX_SET_LOW_MODE:
    case SPEEX_SET_MODE:
       st->submodeID = (*(int*)ptr);
       break;
+   case SPEEX_GET_LOW_MODE:
    case SPEEX_GET_MODE:
       (*(int*)ptr) = st->submodeID;
       break;
@@ -1041,14 +1255,15 @@ void nb_encoder_ctl(void *state, int request, void *ptr)
       (*(int*)ptr) = st->vbr_enabled;
       break;
    case SPEEX_SET_VBR_QUALITY:
-      st->vbr_quality = (*(int*)ptr);
+      st->vbr_quality = (*(float*)ptr);
       break;
    case SPEEX_GET_VBR_QUALITY:
-      (*(int*)ptr) = st->vbr_quality;
+      (*(float*)ptr) = st->vbr_quality;
       break;
    case SPEEX_SET_QUALITY:
       {
          int quality = (*(int*)ptr);
+         /*
          if (quality<=0)
             st->submodeID = 0;
          else if (quality<=1)
@@ -1061,23 +1276,79 @@ void nb_encoder_ctl(void *state, int request, void *ptr)
             st->submodeID = 4;
          else if (quality<=8)
             st->submodeID = 5;
-         else if (quality<=10)
+         else if (quality<=9)
             st->submodeID = 6;
+         else if (quality<=10)
+            st->submodeID = 7;
          else
-            fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);
+         fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);*/
+         if (quality < 0)
+            quality = 0;
+         if (quality > 10)
+            quality = 10;
+         st->submodeID = ((SpeexNBMode*)(st->mode->mode))->quality_map[quality];
       }
       break;
    case SPEEX_SET_COMPLEXITY:
       st->complexity = (*(int*)ptr);
+      if (st->complexity<1)
+         st->complexity=1;
       break;
    case SPEEX_GET_COMPLEXITY:
       (*(int*)ptr) = st->complexity;
       break;
+   case SPEEX_SET_BITRATE:
+      {
+         int i=10, rate, target;
+         target = (*(int*)ptr);
+         while (i>=1)
+         {
+            speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
+            speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
+            if (rate <= target)
+               break;
+            i--;
+         }
+      }
+      break;
    case SPEEX_GET_BITRATE:
       if (st->submodes[st->submodeID])
-         (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
+         (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
       else
-         (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
+         (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
+      break;
+   case SPEEX_SET_SAMPLING_RATE:
+      st->sampling_rate = (*(int*)ptr);
+      break;
+   case SPEEX_GET_SAMPLING_RATE:
+      (*(int*)ptr)=st->sampling_rate;
+      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->frameSize;i++)
+            e[i]=st->exc[i];
+      }
+      break;
+   case SPEEX_GET_INNOV:
+      {
+         int i;
+         float *e = (float*)ptr;
+         for (i=0;i<st->frameSize;i++)
+            e[i]=st->innov[i];
+      }
+      break;
+   case SPEEX_GET_RELATIVE_QUALITY:
+      (*(float*)ptr)=st->relative_quality;
       break;
    default:
       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
@@ -1087,7 +1358,7 @@ void nb_encoder_ctl(void *state, int request, void *ptr)
 void nb_decoder_ctl(void *state, int request, void *ptr)
 {
    DecState *st;
-   st=state;
+   st=(DecState*)state;
    switch(request)
    {
    case SPEEX_SET_ENH:
@@ -1101,9 +1372,55 @@ void nb_decoder_ctl(void *state, int request, void *ptr)
       break;
    case SPEEX_GET_BITRATE:
       if (st->submodes[st->submodeID])
-         (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
+         (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
       else
-         (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
+         (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
+      break;
+   case SPEEX_SET_SAMPLING_RATE:
+      st->sampling_rate = (*(int*)ptr);
+      break;
+   case SPEEX_GET_SAMPLING_RATE:
+      (*(int*)ptr)=st->sampling_rate;
+      break;
+   case SPEEX_SET_HANDLER:
+      {
+         SpeexCallback *c = (SpeexCallback*)ptr;
+         st->speex_callbacks[c->callback_id].func=c->func;
+         st->speex_callbacks[c->callback_id].data=c->data;
+         st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
+      }
+      break;
+   case SPEEX_SET_USER_HANDLER:
+      {
+         SpeexCallback *c = (SpeexCallback*)ptr;
+         st->user_callback.func=c->func;
+         st->user_callback.data=c->data;
+         st->user_callback.callback_id=c->callback_id;
+      }
+      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->frameSize;i++)
+            e[i]=st->exc[i];
+      }
+      break;
+   case SPEEX_GET_INNOV:
+      {
+         int i;
+         float *e = (float*)ptr;
+         for (i=0;i<st->frameSize;i++)
+            e[i]=st->innov[i];
+      }
       break;
    default:
       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);