From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Mon, 23 Aug 2010 06:38:44 +0000 (+0000)
Subject: LSP quantiser unit tests passing
X-Git-Url: http://git.whiteaudio.com/gitweb/?a=commitdiff_plain;h=7424cb4b51887ed7e94d0b81be3715341a446ea3;p=freetel-svn-tracking.git

LSP quantiser unit tests passing

git-svn-id: https://svn.code.sf.net/p/freetel/code@182 01035d8c-6547-0410-b346-abe4f91aad63
---

diff --git a/codec2/src/c2sim.c b/codec2/src/c2sim.c
index 26ac63ec..a84ea591 100644
--- a/codec2/src/c2sim.c
+++ b/codec2/src/c2sim.c
@@ -291,7 +291,7 @@ int main(int argc, char *argv[])
 
     /* option decimation to 20ms rate, which enables interpolation
        routine to synthesise in between frame */
-
+#ifdef FIX_ME_FOR_NEW_INTERP_ROUTINE
     if (decimate) {
 	if (frames%2) {
 
@@ -316,7 +316,7 @@ int main(int argc, char *argv[])
 	model_1 = model;
 	model = model_synth;
     }
-
+#endif
     /* 
        Simulate Wo quantisation noise
        model.Wo += 2.0*(PI/8000)*(1.0 - 2.0*(float)rand()/RAND_MAX);
diff --git a/codec2/src/quantise.c b/codec2/src/quantise.c
index 3e87344c..a21a4735 100644
--- a/codec2/src/quantise.c
+++ b/codec2/src/quantise.c
@@ -28,6 +28,7 @@
 #include <ctype.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>
 #include <math.h>
 
 #include "defines.h"
@@ -44,6 +45,7 @@
 
 typedef struct {
     int   k;        /* dimension of vector                   */
+    int   log2k;    /* number of bits in dimension           */
     int   m;        /* elements in codebook                  */
     char *fn;       /* file name of text file storing the VQ */
 } LSP_CB;
@@ -63,23 +65,38 @@ LSP_CB lsp_q[] = {
 #endif
 
 LSP_CB lsp_q[] = {
-    {1,   16, "../unittest/lsp1.txt"},
-    {1,   16, "../unittest/lsp2.txt"},
-    {1,   16, "../unittest/lsp3.txt"},
-    {1,   16, "../unittest/lsp4.txt"},
-    {1,   16, "../unittest/lsp5.txt"},
-    {1,   16, "../unittest/lsp6.txt"},
-    {1,   16, "../unittest/lsp7.txt"},
-    {1,    8, "../unittest/lsp8.txt"},
-    {1,    8, "../unittest/lsp9.txt"},
-    {1,    4, "../unittest/lsp10.txt"},
-    {0,    0, ""}
+    {1,4,16, "../unittest/lsp1.txt"},
+    {1,4,16, "../unittest/lsp2.txt"},
+    {1,4,16, "../unittest/lsp3.txt"},
+    {1,4,16, "../unittest/lsp4.txt"},
+    {1,4,16, "../unittest/lsp5.txt"},
+    {1,4,16, "../unittest/lsp6.txt"},
+    {1,4,16, "../unittest/lsp7.txt"},
+    {1,3,8, "../unittest/lsp8.txt"},
+    {1,3,8, "../unittest/lsp9.txt"},
+    {1,3,4, "../unittest/lsp10.txt"},
+    {0,0,0, ""}
 };
 
 /* ptr to each codebook */
 
 static float *plsp_cb[MAX_CB];
 
+/*---------------------------------------------------------------------------*\
+									      
+                          FUNCTION HEADERS
+
+\*---------------------------------------------------------------------------*/
+
+float speech_to_uq_lsps(float lsp[], float ak[], float Sn[], float w[], 
+			int order);
+
+/*---------------------------------------------------------------------------*\
+									      
+                             FUNCTIONS
+
+\*---------------------------------------------------------------------------*/
+
 /*---------------------------------------------------------------------------*\
 									      
   quantise_uniform
@@ -212,7 +229,7 @@ void quantise_init()
     i = 0;
     while(lsp_q[i].k) {
 	k = lsp_q[i].k;
-	m = lsp_q[i].m;
+       	m = lsp_q[i].m;
 	plsp_cb[i] = (float*)malloc(sizeof(float)*k*m);
 	assert(plsp_cb[i] != NULL);
 	load_cb(lsp_q[i].fn, plsp_cb[i], k, m);
@@ -445,9 +462,6 @@ void aks_to_M2(
   float Em;		/* energy in band */
   float Am;		/* spectral amplitude sample */
   float signal, noise;
-  float E1,Am1;
-
-  Am1 = model->A[1];
 
   r = TWO_PI/(FFT_DEC);
 
@@ -455,7 +469,8 @@ void aks_to_M2(
 
   for(i=0; i<FFT_DEC; i++) {
     Pw[i].real = 0.0;
-    Pw[i].imag = 0.0;  }
+    Pw[i].imag = 0.0; 
+  }
 
   for(i=0; i<=order; i++)
     Pw[i].real = ak[i];
@@ -484,29 +499,6 @@ void aks_to_M2(
     model->A[m] = Am;
   }
   *snr = 10.0*log10(signal/noise);
-
-  /* 
-     Attenuate fundamental by 30dB if F0 < 150 Hz and LPC modelling
-     error for A[1] is larger than 6dB.
-
-     LPC modelling often makes big errors on 1st harmonic, for example
-     when the fundamental has been removed by analog high pass
-     filtering before sampling.  However on unfiltered speech from
-     high quality sources we would like to keep the fundamental to
-     maintain the speech quality.  So we check the error in A[1] and
-     attenuate it if the error is large to avoid annoying low
-     frequency energy after LPC modelling.
-
-     This will require a single bit to quantise, on top of the other
-     spectral magnitude bits (i.e. LSP bits + 1 total).
-   */
-
-  E1 = fabs(20.0*log10(Am1) - 20.0*log10(model->A[1]));
-  if (E1 > 6.0)
-      if (model->Wo < (PI*150.0/4000)) {
-	  model->A[1] *= 0.032;
-      }
-
 }
 
 /*---------------------------------------------------------------------------*\
@@ -515,29 +507,23 @@ void aks_to_M2(
   AUTHOR......: David Rowe			      
   DATE CREATED: 22/8/2010 
 
-  Encodes Wo using a WO_BITS code and stuffs into bits[] array.
+  Encodes Wo using a WO_LEVELS quantiser.
 
 \*---------------------------------------------------------------------------*/
 
-void encode_Wo(char *bits, int *nbits, float Wo)
+int encode_Wo(float Wo)
 {
-    int   code, bit;
+    int   index;
     float Wo_min = TWO_PI/P_MAX;
     float Wo_max = TWO_PI/P_MIN;
     float norm;
-    int   i;
 
     norm = (Wo - Wo_min)/(Wo_max - Wo_min);
-    code = floor(WO_LEVELS * norm + 0.5);
-    if (code < 0 ) code = 0;
-    if (code > (WO_LEVELS-1)) code = WO_LEVELS-1;
-
-    for(i=0; i<WO_BITS; i++) {
-	bit = (code >> (WO_BITS-i-1)) & 0x1;
-	bits[*nbits+i] = bit;
-    }
+    index = floor(WO_LEVELS * norm + 0.5);
+    if (index < 0 ) index = 0;
+    if (index > (WO_LEVELS-1)) index = WO_LEVELS-1;
 
-    *nbits += WO_BITS;
+    return index;
 }
 
 /*---------------------------------------------------------------------------*\
@@ -546,43 +532,383 @@ void encode_Wo(char *bits, int *nbits, float Wo)
   AUTHOR......: David Rowe			      
   DATE CREATED: 22/8/2010 
 
-  Decodes Wo using a WO_BITS quantiser.
+  Decodes Wo using a WO_LEVELS quantiser.
 
 \*---------------------------------------------------------------------------*/
 
-float decode_Wo(char *bits, int *nbits)
+float decode_Wo(int index)
 {
-    int   code;
     float Wo_min = TWO_PI/P_MAX;
     float Wo_max = TWO_PI/P_MIN;
     float step;
     float Wo;
-    int   i;
 
-    code = 0;
-    for(i=0; i<WO_BITS; i++) {
-	code <<= 1;
-	code |= bits[i];
-    }
-    
     step = (Wo_max - Wo_min)/WO_LEVELS;
-    Wo   = Wo_min + step*(code);
-
-    *nbits += WO_BITS;
+    Wo   = Wo_min + step*(index);
 
     return Wo;
 }
 
-void encode_voicing(char bits[], int *nbits, int voiced1, int voiced2)
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: speech_to_uq_lsps()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Analyse a windowed frame of time domain speech to determine LPCs
+  which are the converted to LSPs for quantisation and transmission
+  over the channel.
+
+\*---------------------------------------------------------------------------*/
+
+float speech_to_uq_lsps(float lsp[],
+			float ak[],
+		        float Sn[], 
+		        float w[],
+		        int   order
+)
+{
+    int   i, roots;
+    float Wn[M];
+    float R[LPC_MAX+1];
+    float E;
+
+    for(i=0; i<M; i++)
+	Wn[i] = Sn[i]*w[i];
+    autocorrelate(Wn, R, M, order);
+    levinson_durbin(R, ak, order);
+  
+    E = 0.0;
+    for(i=0; i<=order; i++)
+	E += ak[i]*R[i];
+ 
+    roots = lpc_to_lsp(ak, order, lsp, 5, LSP_DELTA1);
+    assert(roots == order);
+
+    return E;
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: encode_lsps()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  From a vector of unquantised (floating point) LSPs finds the quantised
+  LSP indexes.
+
+\*---------------------------------------------------------------------------*/
+
+void encode_lsps(int indexes[], float lsp[], int order)
+{
+    int    i,k,m;
+    float  wt[1];
+    float  lsp_hz[LPC_MAX];
+    float *cb, se;
+
+    /* convert from radians to Hz so we can use human readable
+       frequencies */
+
+    for(i=0; i<order; i++)
+	lsp_hz[i] = (4000.0/PI)*lsp[i];
+    
+    /* simple uniform scalar quantisers */
+
+    wt[0] = 1.0;
+    for(i=0; i<order; i++) {
+	k = lsp_q[i].k;
+	m = lsp_q[i].m;
+	cb = plsp_cb[i];
+	indexes[i] = quantise(cb, &lsp_hz[i], wt, k, m, &se);
+    }
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: decode_lsps()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  From a vector of quantised LSP indexes, returns the quantised
+  (floating point) LSPs.
+
+\*---------------------------------------------------------------------------*/
+
+void decode_lsps(float lsp[], int indexes[], int order)
+{
+    int    i,k;
+    float  lsp_hz[LPC_MAX];
+    float *cb;
+
+    for(i=0; i<order; i++) {
+	k = lsp_q[i].k;
+	cb = plsp_cb[i];
+	lsp_hz[i] = cb[indexes[i]*k];
+    }
+
+    /* convert back to radians */
+
+    for(i=0; i<order; i++)
+	lsp[i] = (PI/4000.0)*lsp_hz[i];
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: bw_expand_lsps()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Applies Bandwidth Expansion (BW) to a vector of LSPs.  Prevents any
+  two LSPs getting too close together after quantisation.  We know
+  from experiment that LSP quantisation errors < 12.5Hz (25Hz setp
+  size) are inaudible so we use that as the minimum LSP separation.
+
+\*---------------------------------------------------------------------------*/
+
+void bw_expand_lsps(float lsp[],
+		    int   order
+)
+{
+    int i;
+
+    for(i=1; i<5; i++) {
+	if (lsp[i] - lsp[i-1] < PI*(12.5/4000.0))
+	    lsp[i] = lsp[i-1] + PI*(12.5/4000.0);
+    }
+
+    /* As quantiser gaps increased, larger BW expansion was required
+       to prevent twinkly noises.  This may need more experiment for
+       different quanstisers.
+    */
+
+    for(i=5; i<8; i++) {
+	if (lsp[i] - lsp[i-1] < PI*(25.0/4000.0))
+	    lsp[i] = lsp[i-1] + PI*(25.0/4000.0);
+    }
+    for(i=8; i<order; i++) {
+	if (lsp[i] - lsp[i-1] < PI*(75.0/4000.0))
+	    lsp[i] = lsp[i-1] + PI*(75.0/4000.0);
+    }
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: lpc_correction()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Determine if we need LPC correction of first harmonic.
+
+\*---------------------------------------------------------------------------*/
+
+int need_lpc_correction(MODEL *model, float ak[], float E)
 {
-    bits[0] = voiced1;
-    bits[1] = voiced2;
-    *nbits  += 2;
+    MODEL  tmp;
+    float  snr,E1;
+
+    /* Find amplitudes so we can check if we need LPC correction.
+       TODO: replace call to aks_to_M2() by a single DFT calculation
+       of E/A(exp(jWo)) to make much more efficient.  We only need
+       A[1].
+    */
+
+    memcpy(&tmp, model, sizeof(MODEL));
+    aks_to_M2(ak, LPC_ORD, &tmp, E, &snr);   
+
+    /* 
+       Attenuate fundamental by 30dB if F0 < 150 Hz and LPC modelling
+       error for A[1] is larger than 6dB.
+
+       LPC modelling often makes big errors on 1st harmonic, for example
+       when the fundamental has been removed by analog high pass
+       filtering before sampling.  However on unfiltered speech from
+       high quality sources we would like to keep the fundamental to
+       maintain the speech quality.  So we check the error in A[1] and
+       attenuate it if the error is large to avoid annoying low
+       frequency energy after LPC modelling.
+
+       This requires a single bit to quantise, on top of the other
+       spectral magnitude bits (i.e. LSP bits + 1 total).
+    */
+
+    E1 = fabs(20.0*log10(model->A[1]) - 20.0*log10(tmp.A[1]));
+    if (E1 > 6.0)
+	return 1;
+    else 
+	return 0;
 }
 
-void decode_voicing(int *voiced1, int *voiced2, char bits[], int *nbits);
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: apply_lpc_correction()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Apply first harmonic LPC correction at decoder.
+
+\*---------------------------------------------------------------------------*/
+
+void apply_lpc_correction(MODEL *model, int lpc_correction)
 {
-    *voiced1 = bits[0];
-    *voiced2 = bits[1];
-    *nbits  += 2;
+    if (lpc_correction) {
+	if (model->Wo < (PI*150.0/4000)) {
+	    model->A[1] *= 0.032;
+	}
+    }
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: encode_energy()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Encodes LPC energy using an E_LEVELS quantiser.
+
+\*---------------------------------------------------------------------------*/
+
+int encode_energy(float e)
+{
+    int   index;
+    float e_min = E_MIN_DB;
+    float e_max = E_MAX_DB;
+    float norm;
+
+    norm = (e - e_min)/(e_max - e_min);
+    index = floor(E_LEVELS * norm + 0.5);
+    if (index < 0 ) index = 0;
+    if (index > (E_LEVELS-1)) index = E_LEVELS-1;
+
+    return index;
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: decode_energy()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Decodes energy using a WO_BITS quantiser.
+
+\*---------------------------------------------------------------------------*/
+
+float decode_energy(int index)
+{
+    float e_min = E_MIN_DB;
+    float e_max = E_MAX_DB;
+    float step;
+    float e;
+
+    step = (e_max - e_min)/E_LEVELS;
+    e    = e_min + step*(index);
+
+    return e;
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: encode_amplitudes()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Time domain LPC is used model the amplitudes which are then
+  converted to LSPs and quantised.  So we don't actually encode the
+  amplitudes directly, rather we derive an equivalent representation
+  from the time domain speech.
+
+\*---------------------------------------------------------------------------*/
+
+void encode_amplitudes(int    lsp_indexes[], 
+		       int   *lpc_correction,
+		       int   *energy_index,
+		       MODEL *model, 
+		       float  Sn[], 
+		       float  w[])
+{
+    float lsps[LPC_ORD];
+    float ak[LPC_ORD+1];
+    float e;
+
+    e = speech_to_uq_lsps(lsps, ak, Sn, w, LPC_ORD);
+    encode_lsps(lsp_indexes, lsps, LPC_ORD);
+    *lpc_correction = need_lpc_correction(model, ak, e);
+    *energy_index = encode_energy(e);
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: decode_amplitudes()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 22/8/2010 
+
+  Given the amplitude quantiser indexes recovers the harmonic
+  amplitudes.
+
+\*---------------------------------------------------------------------------*/
+
+float decode_amplitudes(MODEL *model, 
+		        int    lsp_indexes[], 
+		        int    lpc_correction,
+		        int    energy_index
+)
+{
+    float lsps[LPC_ORD];
+    float ak[LPC_ORD+1];
+    float e;
+    float snr;
+
+    decode_lsps(lsps, lsp_indexes, LPC_ORD);
+    bw_expand_lsps(lsps, LPC_ORD);
+    e = decode_energy(energy_index);
+    aks_to_M2(ak, LPC_ORD, model, e, &snr); 
+    apply_lpc_correction(model, lpc_correction);
+
+    return snr;
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: pack()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 23/8/2010 
+
+  Pack a quantiser index into an array of bits.
+
+\*---------------------------------------------------------------------------*/
+
+void pack(char bits[], int *nbit, int index, int index_bits)
+{
+    int i, bit;
+
+    for(i=0; i<index_bits; i++) {
+	bit = (index >> (index_bits-i-1)) & 0x1;
+	bits[*nbit+i] = bit;
+    }
+
+    *nbit += index_bits;
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: unpack()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 23/8/2010 
+
+  Unpack a qunatiser index from an array of bits.
+
+\*---------------------------------------------------------------------------*/
+
+int unpack(char bits[], int *nbit, int index_bits)
+{
+    int index = 0;
+    int i;
+
+    for(i=0; i<index_bits; i++) {
+	index <<= 1;
+	index |= bits[i];
+    }
+    
+    *nbit += index_bits;
+
+    return index;    
 }
diff --git a/codec2/src/quantise.h b/codec2/src/quantise.h
index eb661ad6..86b240f1 100644
--- a/codec2/src/quantise.h
+++ b/codec2/src/quantise.h
@@ -29,6 +29,10 @@
 
 #define WO_BITS   7
 #define WO_LEVELS (1<<WO_BITS)
+#define E_BITS    5
+#define E_LEVELS  (1<<E_BITS)
+#define E_MIN_DB -10.0
+#define E_MAX_DB  40.0
 
 void quantise_init();
 float lpc_model_amplitudes(float Sn[], float w[], MODEL *model, int order,
@@ -36,10 +40,28 @@ float lpc_model_amplitudes(float Sn[], float w[], MODEL *model, int order,
 void aks_to_M2(float ak[], int order, MODEL *model, float E, float *snr);
 float get_gmin(void);
 
-void  encode_Wo(char bits[], int *nbits, float Wo);
-float decode_Wo(char bits[], int *nbits);
+int   encode_Wo(float Wo);
+float decode_Wo(int index);
 
-void encode_voicing(char bits[], int *nbits, int voiced1, int voiced2);
-void decode_voicing(int *voiced1, int *voiced2, char bits[], int *nbits);
+void encode_lsps(int indexes[], float lsp[], int order);
+void decode_lsps(float lsp[], int indexes[], int order);
+
+int encode_energy(float e);
+float decode_energy(int index);
+
+void encode_amplitudes(int    lsp_indexes[], 
+		       int   *lpc_correction, 
+		       int   *energy_index,
+		       MODEL *model, 
+		       float  Sn[], 
+		       float  w[]);
+
+float decode_amplitudes(MODEL *model, 
+		       int lsp_indexes[],
+		       int lpc_correction, 
+		       int energy_index);
+
+void pack(char bits[], int *nbit, int index, int index_bits);
+int unpack(char bits[], int *nbit, int index_bits);
 
 #endif
diff --git a/codec2/unittest/lsp2.txt b/codec2/unittest/lsp2.txt
index 35bdc62f..a5d9860b 100644
--- a/codec2/unittest/lsp2.txt
+++ b/codec2/unittest/lsp2.txt
@@ -14,4 +14,3 @@
 650
 675
 700
-
diff --git a/codec2/unittest/lsp3.txt b/codec2/unittest/lsp3.txt
index 200e11bf..6fde10c8 100644
--- a/codec2/unittest/lsp3.txt
+++ b/codec2/unittest/lsp3.txt
@@ -14,4 +14,3 @@
 1150
 1200
 1250
-
diff --git a/codec2/unittest/lsp4.txt b/codec2/unittest/lsp4.txt
index bce8ffc8..7eae7f8b 100644
--- a/codec2/unittest/lsp4.txt
+++ b/codec2/unittest/lsp4.txt
@@ -13,5 +13,4 @@
 1900
 2000
 2100
-
-
+2200
diff --git a/codec2/unittest/tquant.c b/codec2/unittest/tquant.c
index 04753776..868d55a9 100644
--- a/codec2/unittest/tquant.c
+++ b/codec2/unittest/tquant.c
@@ -36,26 +36,103 @@
 #include "dump.h"
 #include "quantise.h"
 
+int test_Wo_quant();
+int test_lsp_quant();
+int test_lsp(int lsp_number, int levels, float max_error_hz);
+
 int main() {
-    int    i,c,bit;
+    quantise_init();
+    test_Wo_quant();
+    test_lsp_quant();
+
+    return 0;
+}
+
+int test_lsp_quant() {
+    test_lsp( 1, 16,  12.5);
+    test_lsp( 2, 16,  12.5);
+    test_lsp( 3, 16,  25);
+    test_lsp( 4, 16,  50);
+    test_lsp( 5, 16,  50);
+    test_lsp( 6, 16,  50);
+    test_lsp( 7, 16,  50);
+    test_lsp( 8,  8,  50);
+    test_lsp( 9,  8,  50);
+    test_lsp(10,  4, 100);
+
+    return 0;
+}
+
+int test_lsp(int lsp_number, int levels, float max_error_hz) {
+    float lsp[LPC_ORD];
+    int   indexes_in[LPC_ORD];
+    int   indexes_out[LPC_ORD];
+    int   indexes[LPC_ORD];
+    int   i;
+    float lowf, highf, f, error;
+    char  s[MAX_STR];
+    FILE *flsp;
+    float max_error_rads;
+
+    lsp_number--;
+    max_error_rads = max_error_hz*TWO_PI/FS;
+    
+    for(i=0; i<LPC_ORD; i++)
+	indexes_in[i] = 0;
+
+    for(i=0; i<levels; i++) {
+	indexes_in[lsp_number] = i;
+	decode_lsps(lsp, indexes_in, LPC_ORD);
+	encode_lsps(indexes_out, lsp,LPC_ORD);
+	if (indexes_in[lsp_number] != indexes_out[lsp_number]) {
+	    printf("freq: %f index_in: %d index_out: %d\n", 
+		   lsp[lsp_number]+1, indexes_in[lsp_number],
+		   indexes_out[lsp_number]);
+	    exit(0);
+	}	
+    }
+
+    for(i=0; i<LPC_ORD; i++)
+	indexes[i] = 0;
+    indexes[lsp_number] = 0;
+    decode_lsps(lsp, indexes, LPC_ORD);
+    lowf = lsp[lsp_number];
+    indexes[lsp_number] = levels - 1;
+    decode_lsps(lsp, indexes, LPC_ORD);
+    highf = lsp[lsp_number];
+    sprintf(s,"lsp%d_err.txt", lsp_number+1);
+    flsp = fopen(s, "wt");
+
+    for(f=lowf; f<highf; f +=(highf-lowf)/1000.0) {
+	lsp[lsp_number] = f;
+	encode_lsps(indexes, lsp, LPC_ORD);
+	decode_lsps(lsp, indexes, LPC_ORD);
+	error = f - lsp[lsp_number];
+	fprintf(flsp, "%f\n", error);
+	if (fabs(error) > max_error_rads) {
+	    printf("%d error: %f %f\n", lsp_number+1, error, max_error_rads);
+	    exit(0);
+	}
+    }
+
+    fclose(flsp);
+
+    return 0;
+}
+
+int test_Wo_quant() {
+    int    c;
     FILE  *f;
     float  Wo,Wo_dec, error, step_size;
-    char   bits[WO_BITS];
-    int    code, nbits, code_in, code_out;
+    int    index, index_in, index_out;
 
     /* output pitch quant curve for plotting */
 
     f = fopen("quant_pitch.txt","wt");
 
     for(Wo=0.9*(TWO_PI/P_MAX); Wo<=1.1*(TWO_PI/P_MIN); Wo += 0.001) {
-	nbits = 0;
-	encode_Wo(bits, &nbits, Wo);
-        code = 0;
-	for(i=0; i<WO_BITS; i++) {
-	    code <<= 1;
-	    code |= bits[i];
-	}
-	fprintf(f, "%f %d\n", Wo, code);
+	index = encode_Wo(Wo);
+	fprintf(f, "%f %d\n", Wo, index);
     }
 
     fclose(f);
@@ -64,25 +141,12 @@ int main() {
        and decoder Wo levels */
 
     for(c=0; c<WO_LEVELS; c++) {
-	code_in = c;
-	for(i=0; i<WO_BITS; i++) {
-	    bit = (code_in >> (WO_BITS-1-i)) & 0x1;
-	    bits[i] = bit;
-	}
-	nbits = 0;
-	Wo = decode_Wo(bits, &nbits);
-	nbits = 0;
-
-	memset(bits, sizeof(bits), 0);
-        encode_Wo(bits, &nbits, Wo);
-        code_out = 0;
-	for(i=0; i<WO_BITS; i++) {
-	    code_out <<= 1;
-	    code_out |= bits[i];
-	}
-	if (code_in != code_out)
-	    printf("  Wo %f code_in %d code_out %d\n", Wo, 
-		   code_in, code_out);
+	index_in = c;
+	Wo = decode_Wo(index_in);
+        index_out = encode_Wo(Wo);
+	if (index_in != index_out)
+	    printf("  Wo %f index_in %d index_out %d\n", Wo, 
+		   index_in, index_out);
     }
 
     /* measure quantisation error stats and compare to expected.  Also
@@ -92,10 +156,10 @@ int main() {
     step_size = ((TWO_PI/P_MIN) - (TWO_PI/P_MAX))/WO_LEVELS;
 
     for(Wo=TWO_PI/P_MAX; Wo<0.99*TWO_PI/P_MIN; Wo += 0.0001) {
-	nbits = 0; encode_Wo(bits, &nbits, Wo);
-	nbits = 0; Wo_dec = decode_Wo(bits, &nbits);
+	index = encode_Wo(Wo);
+	Wo_dec = decode_Wo(index);
 	error = Wo - Wo_dec;
-	if (error > (step_size/2.0)) {
+	if (fabs(error) > (step_size/2.0)) {
 	    printf("error: %f  step_size/2: %f\n", error, step_size/2.0);
 	    exit(0);
 	}