more sub-band stuff...

git-svn-id: http://svn.xiph.org/trunk/speex@3215 0101bb08-14d6-0310-b084-bc0e0c8e3800

2 files changed, 183 insertions, 20 deletions

diff --git a/libspeex/sb_celp.c b/libspeex/sb_celp.c
index f5e97b6..047412a 100644
--- a/libspeex/sb_celp.c
+++ b/libspeex/sb_celp.c
@@ -21,6 +21,17 @@
 #include "sb_celp.h"
 #include "stdlib.h"
 #include "filters.h"
+#include <math.h>
+#include "lpc.h"
+#include "lsp.h"
+#include <stdio.h>
+
+
+#ifndef M_PI
+#define M_PI           3.14159265358979323846  /* pi */
+#endif
+
+#define sqr(x) ((x)*(x))
 
 static float h0[32] = {
    0.0006910579, -0.001403793,
@@ -62,20 +73,55 @@ static float h1[32] = {
 
 void sb_encoder_init(SBEncState *st, SpeexMode *mode)
 {
+   int i;
    encoder_init(&st->st_low, mode);
-   st->frame_size = 2*st->st_low.frameSize;
-   st->x0=calloc(st->frame_size, sizeof(float));
-   st->x1=calloc(st->frame_size, sizeof(float));
-   st->x0d=calloc(st->frame_size>>1, sizeof(float));
-   st->x1d=calloc(st->frame_size>>1, sizeof(float));
-   st->high=calloc(st->frame_size, sizeof(float));
-   st->y0=calloc(st->frame_size, sizeof(float));
-   st->y1=calloc(st->frame_size, sizeof(float));
+   st->full_frame_size = 2*st->st_low.frameSize;
+   st->frame_size = st->st_low.frameSize;
+   st->subframeSize = 40;
+   st->nbSubframes = 4;
+   st->windowSize = mode->windowSize;
+   st->lpcSize=8;
+
+   st->lag_factor = .01;
+   st->lpc_floor = 1.001;
+   st->first=1;
+   st->stack = calloc(2000, sizeof(float));
+
+   st->x0=calloc(st->full_frame_size, sizeof(float));
+   st->x1=calloc(st->full_frame_size, sizeof(float));
+   st->x0d=calloc(st->frame_size, sizeof(float));
+   st->x1d=calloc(st->frame_size, sizeof(float));
+   st->high=calloc(st->full_frame_size, sizeof(float));
+   st->y0=calloc(st->full_frame_size, sizeof(float));
+   st->y1=calloc(st->full_frame_size, sizeof(float));
 
    st->h0_mem=calloc(32, sizeof(float));
    st->h1_mem=calloc(32, sizeof(float));
    st->g0_mem=calloc(32, sizeof(float));
    st->g1_mem=calloc(32, sizeof(float));
+
+   st->buf=calloc(st->windowSize, sizeof(float));
+   st->window=calloc(st->windowSize, sizeof(float));
+   for (i=0;i<st->windowSize;i++)
+      st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
+
+   st->lagWindow = malloc((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 = malloc(st->lpcSize*sizeof(float));
+   st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
+   st->lpc = malloc((st->lpcSize+1)*sizeof(float));
+   st->lsp = malloc(st->lpcSize*sizeof(float));
+   st->qlsp = malloc(st->lpcSize*sizeof(float));
+   st->old_lsp = malloc(st->lpcSize*sizeof(float));
+   st->old_qlsp = malloc(st->lpcSize*sizeof(float));
+   st->interp_lsp = malloc(st->lpcSize*sizeof(float));
+   st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
+   st->interp_lpc = malloc(st->lpcSize*sizeof(float));
+   st->interp_qlpc = malloc(st->lpcSize*sizeof(float));
+
+
 }
 
 void sb_encoder_destroy(SBEncState *st)
@@ -92,34 +138,125 @@ void sb_encoder_destroy(SBEncState *st)
    free(st->h1_mem);
    free(st->g0_mem);
    free(st->g1_mem);
+   
+   free(st->buf);
+   free(st->window);
+   free(st->lagWindow);
+   free(st->rc);
+   free(st->autocorr);
+   free(st->lpc);
+   free(st->lsp);
+   free(st->qlsp);
+   free(st->old_lsp);
+   free(st->old_qlsp);
+   free(st->interp_lsp);
+   free(st->interp_qlsp);
+   free(st->interp_lpc);
+   free(st->interp_qlpc);
+
+   free(st->stack);
 }
 
 
 void sb_encode(SBEncState *st, float *in, FrameBits *bits)
 {
-   int i;
-   fir_mem(in, h0, st->x0, st->frame_size, 32, st->h0_mem);
-   fir_mem(in, h1, st->x1, st->frame_size, 32, st->h1_mem);
-   for (i=0;i<st->frame_size>>1;i++)
+   int i, roots, sub;
+   /* Compute the two sub-bands by filtering with h0 and h1*/
+   fir_mem(in, h0, st->x0, st->full_frame_size, 32, st->h0_mem);
+   fir_mem(in, h1, st->x1, st->full_frame_size, 32, st->h1_mem);
+   /* Down-sample x0 and x1 */
+   for (i=0;i<st->frame_size;i++)
    {
       st->x0d[i]=st->x0[i<<1];
       st->x1d[i]=st->x1[i<<1];
    }
+   /* Encode the narrowband part*/
    encode(&st->st_low, st->x0d, bits);
-   for (i=0;i<st->frame_size>>1;i++)
+   /* High-band buffering / sync with low band */
+   for (i=0;i<st->frame_size;i++)
+   {
+      st->high[i]=st->high[(st->frame_size)+i];
+      st->high[(st->frame_size)+i]=st->x1d[i];
+   }
+   
+   /* 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);
+
+   st->autocorr[0] += 1;        /* 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 */
+   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)
    {
-      st->high[i]=st->high[(st->frame_size>>1)+i];
-      st->high[(st->frame_size>>1)+i]=st->x1d[i];
+      fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
+      exit(1);
    }
-   for (i=0;i<st->frame_size>>1;i++)
+
+   /* x-domain to angle domain*/
+   for (i=0;i<st->lpcSize;i++)
+      st->lsp[i] = acos(st->lsp[i]);
+   
+   /* FIXME: Need to really quantize the LSPs*/
+   for (i=0;i<st->lpcSize;i++)
+      st->qlsp[i]=st->lsp[i];
+   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];
+   }
+   
+   for (sub=0;sub<st->nbSubframes;sub++)
+   {
+      float *exc, *sp, tmp;
+      int offset;
+      
+      offset = st->subframeSize*sub;
+      sp=st->high+offset;
+      exc=st->exc+offset;
+
+      /* LSP interpolation (quantized and unquantized) */
+      tmp = (.5 + 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];
+
+      /* 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);
+
+      
+   }
+
+   /* 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;
    }
-   fir_mem(st->x0, h0, st->y0, st->frame_size, 32, st->g0_mem);
-   fir_mem(st->x1, h1, st->y1, st->frame_size, 32, st->g1_mem);
-   for (i=0;i<st->frame_size;i++)
+   /* Reconstruct the original */
+   fir_mem(st->x0, h0, st->y0, st->full_frame_size, 32, st->g0_mem);
+   fir_mem(st->x1, h1, st->y1, st->full_frame_size, 32, st->g1_mem);
+   for (i=0;i<st->full_frame_size;i++)
       in[i]=2*(st->y0[i]-st->y1[i]);
+
+   st->first=0;
 }
diff --git a/libspeex/sb_celp.h b/libspeex/sb_celp.h
index 2a0b1a0..1b5767f 100644
--- a/libspeex/sb_celp.h
+++ b/libspeex/sb_celp.h
@@ -27,11 +27,37 @@
 /**Structure representing the full state of the encoder*/
 typedef struct SBEncState {
    EncState st_low;
-   int frame_size;
+   int    full_frame_size;
+   int    frame_size;
+   int    subframeSize;
+   int    nbSubframes;
+   int    windowSize;
+   int    lpcSize;
+   int    first;
+   float  lag_factor;
+   float  lpc_floor;
+
+   float *stack;
    float *x0, *x0d, *x1, *x1d;
    float *high;
    float *y0, *y1;
    float *h0_mem, *h1_mem, *g0_mem, *g1_mem;
+
+   float *exc;
+   float *buf;
+   float *window;
+   float *lagWindow;
+   float *autocorr;
+   float *rc;
+   float *lpc;
+   float *lsp;
+   float *qlsp;
+   float *old_lsp;
+   float *old_qlsp;
+   float *interp_lsp;
+   float *interp_qlsp;
+   float *interp_lpc;
+   float *interp_qlpc;
 } SBEncState;