Completed VBR for 0.5.0 release
[speexdsp.git] / libspeex / nb_celp.c
index 003eb18..76ecb98 100644 (file)
 #include "vq.h"
 #include "speex_bits.h"
 #include "post_filter.h"
+#include "vbr.h"
 
 #ifndef M_PI
 #define M_PI           3.14159265358979323846  /* pi */
 #endif
 
+#define SUBMODE(x) st->submodes[st->submodeID]->x
+
+float exc_gain_quant_scal[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
 
 #define sqr(x) ((x)*(x))
 #define min(a,b) ((a) < (b) ? (a) : (b))
@@ -45,14 +49,13 @@ void *nb_encoder_init(SpeexMode *m)
    EncState *st;
    SpeexNBMode *mode;
    int i;
-   float tmp;
 
    mode=m->mode;
    st = malloc(sizeof(EncState));
    st->mode=m;
    /* Codec parameters, should eventually have several "modes"*/
    st->frameSize = mode->frameSize;
-   st->windowSize = mode->windowSize;
+   st->windowSize = st->frameSize*3/2;
    st->nbSubframes=mode->frameSize/mode->subframeSize;
    st->subframeSize=mode->subframeSize;
    st->lpcSize = mode->lpcSize;
@@ -65,38 +68,11 @@ void *nb_encoder_init(SpeexMode *m)
    st->lpc_floor = mode->lpc_floor;
    st->preemph = mode->preemph;
   
-
-   st->lsp_quant = mode->lsp_quant;
-   st->ltp_quant = mode->ltp_quant;
-   st->ltp_params = mode->ltp_params;
-   st->innovation_quant = mode->innovation_quant;
-   st->innovation_params = mode->innovation_params;
-
+   st->submodes=mode->submodes;
+   st->submodeID=mode->defaultSubmode;
    st->pre_mem=0;
    st->pre_mem2=0;
 
-   /* Over-sampling filter (fractional pitch)*/
-   st->os_fact=4;
-   st->os_filt_ord2=4*st->os_fact;
-   st->os_filt = malloc((1+2*st->os_filt_ord2)*sizeof(float));
-   st->os_filt[st->os_filt_ord2] = 1;
-   for (i=1;i<=st->os_filt_ord2;i++)
-   {
-      float x=M_PI*i/st->os_fact;
-      st->os_filt[st->os_filt_ord2-i] = st->os_filt[st->os_filt_ord2+i]=sin(x)/x*(.5+.5*cos(M_PI*i/st->os_filt_ord2));
-   }
-   /* Normalizing the over-sampling filter */
-   tmp=0;
-   for (i=0;i<2*st->os_filt_ord2+1;i++)
-      tmp += st->os_filt[i];
-   tmp=1/tmp;
-   for (i=0;i<2*st->os_filt_ord2+1;i++)
-      st->os_filt[i] *= tmp;
-
-   /*for (i=0;i<2*st->os_filt_ord2+1;i++)
-      printf ("%f ", st->os_filt[i]);
-      printf ("\n");*/
-
    /* Allocating input buffer */
    st->inBuf = calloc(st->bufSize,sizeof(float));
    st->frame = st->inBuf + st->bufSize - st->windowSize;
@@ -109,11 +85,17 @@ void *nb_encoder_init(SpeexMode *m)
    st->exc2Buf = calloc(st->bufSize,sizeof(float));
    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
 
-   /* Hanning window */
-   st->window = malloc(st->windowSize*sizeof(float));
-   for (i=0;i<st->windowSize;i++)
-      st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
-
+   /* Asymetric "pseudo-Hamming" window */
+   {
+      int part1, part2;
+      part1 = st->subframeSize*7/2;
+      part2 = st->subframeSize*5/2;
+      st->window = malloc(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 = malloc((st->lpcSize+1)*sizeof(float));
    for (i=0;i<st->lpcSize+1;i++)
@@ -139,13 +121,23 @@ void *nb_encoder_init(SpeexMode *m)
    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
    st->rc = malloc(st->lpcSize*sizeof(float));
    st->first = 1;
-   
+
    st->mem_sp = calloc(st->lpcSize, sizeof(float));
    st->mem_sw = calloc(st->lpcSize, sizeof(float));
 
    st->pi_gain = calloc(st->nbSubframes, sizeof(float));
 
    st->pitch = calloc(st->nbSubframes, sizeof(int));
+
+   if (1) {
+      st->vbr = malloc(sizeof(VBRState));
+      vbr_init(st->vbr);
+      st->vbr_quality = 8;
+      st->vbr_enabled = 0;
+   } else {
+      st->vbr = 0;
+   }
+
    return st;
 }
 
@@ -156,7 +148,6 @@ void nb_encoder_destroy(void *state)
    free(st->inBuf);
    free(st->excBuf);
    free(st->swBuf);
-   free(st->os_filt);
    free(st->exc2Buf);
    free(st->stack);
 
@@ -182,7 +173,11 @@ void nb_encoder_destroy(void *state)
    free(st->mem_sw);
    free(st->pi_gain);
    free(st->pitch);
-   
+
+   vbr_destroy(st->vbr);
+   free(st->vbr);
+
+   /*Free state memory... should be last*/
    free(st);
 }
 
@@ -191,8 +186,13 @@ 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;
+
    st=state;
+   
    /* Copy new data in input buffer */
    memmove(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;
@@ -234,24 +234,107 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       st->lsp[i] = acos(st->lsp[i]);
    /*print_vec(st->lsp, 10, "LSP:");*/
    /* LSP Quantization */
-   st->lsp_quant(st->lsp, st->qlsp, st->lpcSize, bits);
+   if (st->first)
+   {
+      for (i=0;i<st->lpcSize;i++)
+         st->old_lsp[i] = st->lsp[i];
+   }
 
-   /*for (i=0;i<st->lpcSize;i++)
-     st->qlsp[i]=st->lsp[i];*/
-   /*printf ("LSP ");
-   for (i=0;i<st->lpcSize;i++)
-      printf ("%f ", st->lsp[i]);
-   printf ("\n");
-   printf ("QLSP ");
+
+   /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
+   {
+      for (i=0;i<st->lpcSize;i++)
+         st->interp_lsp[i] = .5*st->old_lsp[i] + .5*st->lsp[i];
+
+      lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
+
+      /* 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);
+
+      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*/
+      open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
+                            &ol_pitch, &ol_pitch_coef, 1, st->stack);
+
+      /*Compute "real" excitation*/
+      residue(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize);
+
+      /* Compute open-loop excitation gain */
+      ol_gain=0;
+      for (i=0;i<st->frameSize;i++)
+         ol_gain += st->exc[i]*st->exc[i];
+      
+      ol_gain=sqrt(1+ol_gain/st->frameSize);
+   }
+
+   /*Experimental VBR stuff*/
+   if (st->vbr)
+   {
+      delta_qual = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
+      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);
+      }
+   }
+   /*printf ("VBR quality = %f\n", vbr_qual);*/
+
+   /* First, transmit the sub-mode we use for this frame */
+   speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
+
+
+   /*Quantize LSPs*/
+#if 1 /*0 for unquantized*/
+   SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
+#else
    for (i=0;i<st->lpcSize;i++)
-      printf ("%f ", st->qlsp[i]);
-   printf ("\n");*/
+     st->qlsp[i]=st->lsp[i];
+#endif
+
+   /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
+   if (SUBMODE(lbr_pitch)!=-1 && SUBMODE(ltp_params))
+   {
+      speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
+   } else if (SUBMODE(lbr_pitch)==0)
+   {
+      int quant;
+      speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
+      quant = (int)floor(.5+15*ol_pitch_coef);
+      if (quant>15)
+         quant=0;
+      if (quant<0)
+         quant=0;
+      speex_bits_pack(bits, quant, 4);
+      ol_pitch_coef=0.066667*quant;
+   }
+   
+   
+   /*Quantize and transmit open-loop excitation gain*/
+   {
+      int qe = (int)(floor(3.5*log(ol_gain)));
+      if (qe<0)
+         qe=0;
+      if (qe>31)
+         qe=31;
+      ol_gain = exp(qe/3.5);
+      speex_bits_pack(bits, qe, 5);
+   }
+
    /* Special case for first frame */
    if (st->first)
    {
       for (i=0;i<st->lpcSize;i++)
-         st->old_lsp[i] = st->lsp[i];
-      for (i=0;i<st->lpcSize;i++)
          st->old_qlsp[i] = st->qlsp[i];
    }
 
@@ -281,25 +364,16 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       mem = PUSH(st->stack, st->lpcSize);
 
       /* LSP interpolation (quantized and unquantized) */
-      tmp = (.5 + sub)/st->nbSubframes;
+      tmp = (1.0 + sub)/st->nbSubframes;
       for (i=0;i<st->lpcSize;i++)
          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
       for (i=0;i<st->lpcSize;i++)
          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
 
-      if (0) {
-         float *h=PUSH(st->stack, 8);
-         for (i=0;i<8;i++)
-            h[i]=0;
-         h[0]=1;
-         
-         residue_zero(h, st->bw_lpc1, h, 8, st->lpcSize);
-         syn_filt_zero(h, st->interp_qlpc, h, 8, st->lpcSize);
-         syn_filt_zero(h, st->bw_lpc2, h, 8, st->lpcSize);
-         print_vec(h, 8, "lpc_resp");
-         POP(st->stack);
-      }
-      
+      /* Make sure the filters are stable */
+      lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
+      lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
+
       /* Compute interpolated LPCs (quantized and unquantized) */
       for (i=0;i<st->lpcSize;i++)
          st->interp_lsp[i] = cos(st->interp_lsp[i]);
@@ -309,6 +383,7 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
 
+      /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
       tmp=1;
       st->pi_gain[sub]=0;
       for (i=0;i<=st->lpcSize;i++)
@@ -329,18 +404,7 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          for (i=2;i<=st->lpcSize;i++)
             st->bw_lpc2[i]=0;
       }
-#ifdef DEBUG
-      printf ("\nlpc0 ");
-      for (i=0;i<=st->lpcSize;i++)
-         printf ("%f ", st->interp_lpc[i]);
-      printf ("\nlpc1 ");
-      for (i=0;i<=st->lpcSize;i++)
-         printf ("%f ", st->bw_lpc1[i]);
-      printf ("\nlpc2 ");
-      for (i=0;i<=st->lpcSize;i++)
-         printf ("%f ", st->bw_lpc2[i]);
-      printf ("\n\n");
-#endif
+
       /* Reset excitation */
       for (i=0;i<st->subframeSize;i++)
          exc[i]=0;
@@ -375,13 +439,41 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
          target[i]=sw[i]-res[i];
 
       for (i=0;i<st->subframeSize;i++)
-         exc[i]=0;
+         exc[i]=exc2[i]=0;
 
-      /* Long-term prediction */
-      pitch = st->ltp_quant(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
-                    exc, st->ltp_params, st->min_pitch, st->max_pitch, 
-                    st->lpcSize, st->subframeSize, bits, st->stack, exc2);
-      st->pitch[sub]=pitch;
+      /* If we have a long-term predictor (not all sub-modes have one) */
+      if (SUBMODE(ltp_params))
+      {
+         /* Long-term prediction */
+         if (SUBMODE(lbr_pitch) != -1)
+         {
+            /* Low bit-rate pitch handling */
+            int pit_min, pit_max;
+            int margin;
+            margin = SUBMODE(lbr_pitch);
+            if (ol_pitch < st->min_pitch+margin-1)
+               ol_pitch=st->min_pitch+margin-1;
+            if (ol_pitch > st->max_pitch-margin)
+               ol_pitch=st->max_pitch-margin;
+            pit_min = ol_pitch-margin+1;
+            pit_max = ol_pitch+margin;
+            pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
+                                       exc, SUBMODE(ltp_params), pit_min, pit_max, 
+                                       st->lpcSize, st->subframeSize, bits, st->stack, exc2);
+         } else {
+            /* Normal pitch handling */
+            pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
+                                       exc, SUBMODE(ltp_params), st->min_pitch, st->max_pitch, 
+                                       st->lpcSize, st->subframeSize, bits, st->stack, exc2);
+         }
+         /*printf ("cl_pitch: %d\n", pitch);*/
+         st->pitch[sub]=pitch;
+      } else if (SUBMODE(lbr_pitch==0)) {
+         for (i=0;i<st->subframeSize;i++)
+         {
+            exc[i]=exc[i-ol_pitch]*ol_pitch_coef;
+         }
+      }
 
       /* Update target for adaptive codebook contribution */
       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
@@ -441,14 +533,7 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       for (i=0;i<st->subframeSize;i++)
          exc[i]+=st->buf2[i];
 #else
-      if (0)
-      {
-      /* Perform innovation search */
-      st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
-                           st->innovation_params, st->lpcSize,
-                           st->subframeSize, exc, bits, st->stack);
-      }
-      else
+      /* Quantization of innovation */
       {
          float *innov;
          float ener=0, ener_1;
@@ -462,30 +547,41 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
             ener+=st->buf2[i]*st->buf2[i];
          ener=sqrt(.1+ener/st->subframeSize);
 
+         ener /= ol_gain;
+         if (SUBMODE(have_subframe_gain)) 
          {
-            int qe = (int)(floor(7*log(ener)));
-            if (qe<0)
-               qe=0;
-            if (qe>63)
-               qe=63;
-            ener = exp(qe/7.0);
-            speex_bits_pack(bits, qe, 6);
+            int qe;
+            ener=log(ener);
+            qe = vq_index(&ener, exc_gain_quant_scal, 1, 8);
+            speex_bits_pack(bits, qe, 3);
+            ener=exc_gain_quant_scal[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;
          
          for (i=0;i<st->subframeSize;i++)
             target[i]*=ener_1;
-#if 1
-         st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
-                                st->innovation_params, st->lpcSize, st->subframeSize, 
-                                innov, bits, st->stack);
          
-         for (i=0;i<st->subframeSize;i++)
-            exc[i] += innov[i]*ener;
-#else
-         for (i=0;i<st->subframeSize;i++)
-            exc[i] += st->buf2[i];
-#endif
+         if (SUBMODE(innovation_quant))
+         {
+            /* Normal quantization */
+            SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
+                                      SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
+                                      innov, bits, st->stack);
+            
+            for (i=0;i<st->subframeSize;i++)
+               exc[i] += innov[i]*ener;
+         } else {
+            /* This is the "real" (cheating) excitation in the encoder but the decoder will
+               use white noise */
+            for (i=0;i<st->subframeSize;i++)
+               exc[i] += st->buf2[i];
+         }
          POP(st->stack);
          for (i=0;i<st->subframeSize;i++)
             target[i]*=ener;
@@ -497,8 +593,9 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
       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++)
@@ -548,6 +645,7 @@ void nb_encode(void *state, float *in, SpeexBits *bits)
    for (i=1;i<st->frameSize;i++)
      in[i]=st->frame[i] + st->preemph*in[i-1];
    st->pre_mem2=in[st->frameSize-1];
+
 }
 
 
@@ -564,7 +662,7 @@ void *nb_decoder_init(SpeexMode *m)
    st->first=1;
    /* Codec parameters, should eventually have several "modes"*/
    st->frameSize = mode->frameSize;
-   st->windowSize = mode->windowSize;
+   st->windowSize = st->frameSize*3/2;
    st->nbSubframes=mode->frameSize/mode->subframeSize;
    st->subframeSize=mode->subframeSize;
    st->lpcSize = mode->lpcSize;
@@ -575,16 +673,10 @@ void *nb_decoder_init(SpeexMode *m)
    st->max_pitch=mode->pitchEnd;
    st->preemph = mode->preemph;
 
-   st->pre_mem=0;
-   st->lsp_unquant = mode->lsp_unquant;
-   st->ltp_unquant = mode->ltp_unquant;
-   st->ltp_params = mode->ltp_params;
+   st->submodes=mode->submodes;
+   st->submodeID=mode->defaultSubmode;
 
-   st->innovation_unquant = mode->innovation_unquant;
-   st->innovation_params = mode->innovation_params;
-
-   st->post_filter_func = mode->post_filter_func;
-   st->post_filter_params = mode->post_filter_params;
+   st->pre_mem=0;
    st->pf_enabled=0;
 
    st->stack = calloc(10000, sizeof(float));
@@ -607,9 +699,12 @@ void *nb_decoder_init(SpeexMode *m)
    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
    st->mem_sp = calloc(st->lpcSize, sizeof(float));
+   st->mem_pf = calloc(st->lpcSize, sizeof(float));
+   st->mem_pf2 = calloc(st->lpcSize, sizeof(float));
 
    st->pi_gain = calloc(st->nbSubframes, sizeof(float));
-   
+   st->last_pitch = 40;
+   st->count_lost=0;
    return st;
 }
 
@@ -626,6 +721,8 @@ void nb_decoder_destroy(void *state)
    free(st->interp_qlsp);
    free(st->stack);
    free(st->mem_sp);
+   free(st->mem_pf);
+   free(st->mem_pf2);
    free(st->pi_gain);
    
    free(state);
@@ -637,21 +734,52 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
    int i, sub;
    int pitch;
    float pitch_gain[3];
-
+   float ol_gain;
+   int ol_pitch=0;
+   float ol_pitch_coef=0;
+   int best_pitch=40;
+   float best_pitch_gain=-1;
    st=state;
 
+   /* Get the sub-mode that was used */
+   st->submodeID = speex_bits_unpack_unsigned(bits, NB_SUBMODE_BITS);
+
+   /* Shift all buffers by one frame */
    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
    memmove(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
 
+   /* Unquantize LSPs */
+   SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
 
-   st->lsp_unquant(st->qlsp, st->lpcSize, bits);
-   if (st->first)
+   /* Handle first frame and lost-packet case */
+   if (st->first || st->count_lost)
    {
       for (i=0;i<st->lpcSize;i++)
          st->old_qlsp[i] = st->qlsp[i];
    }
 
+   /* Get open-loop pitch estimation for low bit-rate pitch coding */
+   if (SUBMODE(lbr_pitch)!=-1 && SUBMODE(ltp_params))
+   {
+      ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
+      speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
+   } else if (SUBMODE(lbr_pitch)==0)
+   {
+      int quant;
+      ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
+      quant = speex_bits_unpack_unsigned(bits, 4);
+      ol_pitch_coef=0.066667*quant;
+   }
+   
+   /* Get global excitation gain */
+   {
+      int qe;
+      qe = speex_bits_unpack_unsigned(bits, 5);
+      ol_gain = exp(qe/3.5);
+      /*printf ("decode_ol_gain: %f\n", ol_gain);*/
+   }
+
    /*Loop on subframes */
    for (sub=0;sub<st->nbSubframes;sub++)
    {
@@ -668,15 +796,20 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
       exc2=st->exc2+offset;
 
       /* LSP interpolation (quantized and unquantized) */
-      tmp = (.5 + sub)/st->nbSubframes;
+      tmp = (1.0 + sub)/st->nbSubframes;
       for (i=0;i<st->lpcSize;i++)
          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
 
+      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);
 
+
+      /* Compute analysis filter at w=pi */
       tmp=1;
       st->pi_gain[sub]=0;
       for (i=0;i<=st->lpcSize;i++)
@@ -690,9 +823,56 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
          exc[i]=0;
 
       /*Adaptive codebook contribution*/
-      st->ltp_unquant(exc, st->min_pitch, st->max_pitch, st->ltp_params, st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, lost);
+      if (SUBMODE(ltp_unquant))
+      {
+         if (SUBMODE(lbr_pitch) != -1)
+         {
+            int pit_min, pit_max;
+            int margin;
+            margin = SUBMODE(lbr_pitch);
+            if (ol_pitch < st->min_pitch+margin-1)
+               ol_pitch=st->min_pitch+margin-1;
+            if (ol_pitch > st->max_pitch-margin)
+               ol_pitch=st->max_pitch-margin;
+            pit_min = ol_pitch-margin+1;
+            pit_max = ol_pitch+margin;
+            SUBMODE(ltp_unquant)(exc, pit_min, pit_max, SUBMODE(ltp_params), st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, 0);
+         } else {
+            SUBMODE(ltp_unquant)(exc, st->min_pitch, st->max_pitch, SUBMODE(ltp_params), st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, 0);
+         }
+         
+         if (!lost)
+         {
+            /* 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];
+         }
+      } else if (SUBMODE(lbr_pitch==0)) {
+         for (i=0;i<st->subframeSize;i++)
+         {
+            exc[i]=exc[i-ol_pitch]*ol_pitch_coef;
+         }
+      }
       
-
+      /* Unquantize the innovation */
       {
          int q_energy;
          float ener;
@@ -702,12 +882,30 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
          for (i=0;i<st->subframeSize;i++)
             innov[i]=0;
 
-         q_energy = speex_bits_unpack_unsigned(bits, 6);
-         ener = exp(q_energy/7.0);
+         if (SUBMODE(have_subframe_gain))
+         {
+            q_energy = speex_bits_unpack_unsigned(bits, 3);
+            ener = ol_gain*exp(exc_gain_quant_scal[q_energy]);
+         } else {
+            ener = ol_gain;
+         }
+         
          /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
          
-         /*Fixed codebook contribution*/
-         st->innovation_unquant(innov, st->innovation_params, st->subframeSize, bits, st->stack);
+         if (SUBMODE(innovation_unquant))
+         {
+            /*Fixed codebook contribution*/
+            SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, st->stack);
+         } else {
+            float scale;
+            scale = 3*sqrt(1.2-ol_pitch_coef);
+            for (i=0;i<st->subframeSize;i++)
+               innov[i] = scale*((((float)rand())/RAND_MAX)-.5);
+            
+         }
+
+         if (st->count_lost)
+            ener*=pow(.8,st->count_lost);
 
          for (i=0;i<st->subframeSize;i++)
             exc[i]+=ener*innov[i];
@@ -718,11 +916,13 @@ void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
       for (i=0;i<st->subframeSize;i++)
          exc2[i]=exc[i];
 
-      if (st->pf_enabled)
-         st->post_filter_func(exc, exc2, st->interp_qlpc, st->lpcSize, st->subframeSize,
-                              pitch, pitch_gain, st->post_filter_params, st->stack);
-
-      /*Compute decoded signal*/
+      /* Apply post-filter */
+      if (st->pf_enabled && SUBMODE(post_filter_func))
+         SUBMODE(post_filter_func)(exc, exc2, st->interp_qlpc, st->lpcSize, st->subframeSize,
+                              pitch, pitch_gain, SUBMODE(post_filter_params), st->mem_pf, 
+                              st->mem_pf2, st->stack);
+      
+      /* Apply synthesis filter */
       syn_filt_mem(exc2, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
 
    }
@@ -743,22 +943,83 @@ 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;
+   }
 }
 
-
-void nb_ctl(void *state, int request, void *ptr)
+void nb_encoder_ctl(void *state, int request, void *ptr)
 {
+   EncState *st;
+   st=state;     
    switch(request)
    {
-   case SPEEX_SET_PF:
+   case SPEEX_GET_FRAME_SIZE:
+      (*(int*)ptr) = st->frameSize;
+      break;
+   case SPEEX_SET_MODE:
+      st->submodeID = (*(int*)ptr);
+      break;
+   case SPEEX_GET_MODE:
+      (*(int*)ptr) = st->submodeID;
+      break;
+   case SPEEX_SET_VBR:
+      st->vbr_enabled = (*(int*)ptr);
+      break;
+   case SPEEX_GET_VBR:
+      (*(int*)ptr) = st->vbr_enabled;
+      break;
+   case SPEEX_SET_VBR_QUALITY:
+      st->vbr_quality = (*(int*)ptr);
+      break;
+   case SPEEX_GET_VBR_QUALITY:
+      (*(int*)ptr) = st->vbr_quality;
+      break;
+   case SPEEX_SET_QUALITY:
       {
-         DecState *st;
-         st=state;     
-         st->pf_enabled = *((int*)ptr);
+         int quality = (*(int*)ptr);
+         if (quality<=0)
+            st->submodeID = 1;
+         else if (quality<=1)
+            st->submodeID = 1;
+         else if (quality<=2)
+            st->submodeID = 2;
+         else if (quality<=4)
+            st->submodeID = 3;
+         else if (quality<=6)
+            st->submodeID = 4;
+         else if (quality<=8)
+            st->submodeID = 5;
+         else if (quality<=10)
+            st->submodeID = 6;
+         else
+            fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);
       }
       break;
    default:
       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
    }
 }
+
+void nb_decoder_ctl(void *state, int request, void *ptr)
+{
+   DecState *st;
+   st=state;
+   switch(request)
+   {
+   case SPEEX_SET_PF:
+      st->pf_enabled = *((int*)ptr);
+      break;
+   case SPEEX_GET_FRAME_SIZE:
+      (*(int*)ptr) = st->frameSize;
+      break;
+   default:
+      fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
+   }
+}