From 7d43c4efd38794fff40dd447971e40ea5dc89724 Mon Sep 17 00:00:00 2001
From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Mon, 9 Jan 2017 10:15:38 +0000
Subject: [PATCH] all newamp signal proc functions ported to C and working OK
 compared to Octave.  Next step is some refactoring and integration into a C
 codec 2 mode

git-svn-id: https://svn.code.sf.net/p/freetel/code@2954 01035d8c-6547-0410-b346-abe4f91aad63
---
 codec2-dev/octave/mag_to_phase.m        |  26 ++++-
 codec2-dev/octave/newamp.m              |  54 ++++++++-
 codec2-dev/octave/newamp1_batch.m       |  39 +------
 codec2-dev/octave/tnewamp1.m            | 106 ++++++++++++++++-
 codec2-dev/src/codebooknewamp1_energy.c |  39 +++++++
 codec2-dev/src/phase.c                  | 103 +++++++++++++++++
 codec2-dev/src/phase.h                  |   2 +
 codec2-dev/src/quantise.c               | 147 ++++++++++++++++++++++++
 codec2-dev/src/quantise.h               |   3 +
 codec2-dev/unittest/tnewamp1.c          | 109 +++++++++++++++++-
 10 files changed, 578 insertions(+), 50 deletions(-)
 create mode 100644 codec2-dev/src/codebooknewamp1_energy.c

diff --git a/codec2-dev/octave/mag_to_phase.m b/codec2-dev/octave/mag_to_phase.m
index 43c04e85..0eac133f 100644
--- a/codec2-dev/octave/mag_to_phase.m
+++ b/codec2-dev/octave/mag_to_phase.m
@@ -9,8 +9,7 @@
 % is rather dim, but a working example is good place to start!
 
 
-function [phase s] = mag_to_phase(Gdbfk, verbose_en = 0)
-  Nfft    = 512;  % FFT size to use 
+function [phase s] = mag_to_phase(Gdbfk, Nfft = 512, verbose_en = 0)
 
   Ns = length(Gdbfk); if Ns~=Nfft/2+1, error("confusion"); end
   Sdb = [Gdbfk,Gdbfk(Ns-1:-1:2)]; % install negative-frequencies
@@ -31,7 +30,27 @@ function [phase s] = mag_to_phase(Gdbfk, verbose_en = 0)
     end
   end
 
+  printf("  Sdb..: ");
+  for i=1:5
+      printf("%5.2f ", real(Sdb(i)));
+  end
+  printf("\n         ");
+  for i=1:5
+      printf("%5.2f ", imag(Sdb(i)));
+  end
+  printf("\n");
+
   c = ifft(Sdb); % compute real cepstrum from log magnitude spectrum
+ 
+  printf("  c....: ");
+  for i=1:5
+      printf("%5.2f ", real(c(i)));
+  end
+  printf("\n         ");
+  for i=1:5
+      printf("%5.2f ", imag(c(i)));
+  end
+  printf("\n");
 
   % Check aliasing of cepstrum (in theory there is always some):
 
@@ -50,10 +69,11 @@ function [phase s] = mag_to_phase(Gdbfk, verbose_en = 0)
   % Fold cepstrum to reflect non-min-phase zeros inside unit circle:
 
   cf = [c(1), c(2:Ns-1)+c(Nfft:-1:Ns+1), c(Ns), zeros(1,Nfft-Ns)];
+
   Cf = fft(cf); % = dB_magnitude + j * minimum_phase
 
   % The maths says we are meant to be using log(x), not 20*log10(x),
-  % so we need to sclae the phase toa ccount for this:
+  % so we need to scale the phase to account for this:
   % log(x) = 20*log10(x)/scale;
 
   scale = (20/log(10));
diff --git a/codec2-dev/octave/newamp.m b/codec2-dev/octave/newamp.m
index a55af918..60b358ff 100644
--- a/codec2-dev/octave/newamp.m
+++ b/codec2-dev/octave/newamp.m
@@ -149,6 +149,17 @@ function [phase Gdbfk s Aw] = determine_phase(model, f, Nfft=512, ak)
   rate_L_sample_freqs_kHz = (1:L)*Wo*4/pi;
   Gdbfk = interp_para(rate_L_sample_freqs_kHz, AmdB, sample_freqs_kHz);    
 
+  printf("  AmdB.: ");
+  for m=1:5
+    printf("%5.2f ", AmdB(m));
+  end
+  printf("\n");
+  printf("  Gdbfk: ");
+  for m=1:5
+    printf("%5.2f ", Gdbfk(m));
+  end
+  printf("\n");
+
   % Gdbfk = resample_mask(model, f, mask_sample_freqs_kHz);
 
   % optional input of aks for testing
@@ -228,7 +239,7 @@ function [model_ AmdB_] = resample_rate_L(model, rate_K_surface, rate_K_sample_f
   max_amp = 80;
   [frames col] = size(model);
 
-  model_ = zeros(frames, max_amp+3);
+  model_ = zeros(frames, max_amp+2);
   for f=1:frames
     Wo = model(f,1);
     L = model(f,2);
@@ -309,6 +320,47 @@ function save_vq(vqset, filenameprefix)
 endfunction
 
 
+% Decoder side interpolation of Wo and voicing, to go from 25 Hz
+% sample rate used over channel to 100Hz internal sample rate of Codec
+% 2.
+
+function [Wo_ voicing_] = interp_Wo_v(Wo1, Wo2, voicing1, voicing2)
+    M = 4;
+    max_amp = 80;
+
+    Wo_ = zeros(1,M); 
+    voicing_ = zeros(1,M);
+    if !voicing1 && !voicing2
+       Wo_(1:M) = 2*pi/100;
+    end
+
+    if voicing1 && !voicing2
+       Wo_(1:M/2) = Wo1;
+       Wo_(M/2+1:M) = 2*pi/100;
+       voicing_(1:M/2) = 1;
+    end
+
+    if !voicing1 && voicing2
+       Wo_(1:M/2) = 2*pi/100;
+       Wo_(M/2+1:M) = Wo2;
+       voicing_(M/2+1:M) = 1;
+    end
+
+    if voicing1 && voicing2
+      Wo_samples = [Wo1 Wo2];
+      Wo_(1:M) = interp_linear([1 M+1], Wo_samples, 1:M);
+      voicing_(1:M) = 1;
+    end
+
+    #{
+    printf("f: %d f+M/2: %d Wo: %f %f (%f %%) v: %d %d \n", f, f+M/2, model(f,1), model(f+M/2,1), 100*abs(model(f,1) - model(f+M/2,1))/model(f,1), voicing(f), voicing(f+M/2));
+    for i=f:f+M/2-1
+      printf("  f: %d v: %d v_: %d Wo: %f Wo_: %f\n", i, voicing(i), voicing_(i), model(i,1),  model_(i,1));
+    end
+    #}
+endfunction
+
+
 % --------------------------------------------------------------------------------
 % Experimental functions used for masking, piecewise models, not part of newamp1
 % --------------------------------------------------------------------------------
diff --git a/codec2-dev/octave/newamp1_batch.m b/codec2-dev/octave/newamp1_batch.m
index 90346ee6..a59ff027 100644
--- a/codec2-dev/octave/newamp1_batch.m
+++ b/codec2-dev/octave/newamp1_batch.m
@@ -208,7 +208,7 @@ function [model_ voicing_ indexes] = experiment_rate_K_dec(model, voicing)
   mesh(surface_no_mean_);
     
   surface_ = zeros(frames, K);
-  energy_q = 10 + 40/16*(0:15);
+  energy_q = create_energy_q;
   for f=1:frames   
     [mean_f_ indx] = quantise(energy_q, mean_f(f));
     indexes(f,3) = indx - 1;
@@ -249,11 +249,9 @@ function [model_ voicing_ indexes] = experiment_rate_K_dec(model, voicing)
         index = 1;
       end
       indexes(f,4) = index;
-      voicing(f) = 1;
       model_(f,1) = decode_log_Wo(indexes(f,4), 6);
     else
       indexes(f,4) = 0;
-      voicing(f) = 0;
       model_(f,1) = 2*pi/100;
     end
   end      
@@ -485,41 +483,6 @@ function [model_ voicing_] = model_from_indexes_fbf(indexes)
 endfunction
 
 
-function [Wo_ voicing_] = interp_Wo_v(Wo1, Wo2, voicing1, voicing2)
-    M = 4;
-    max_amp = 80;
-
-    Wo_ = zeros(1,M); 
-    voicing_ = zeros(1,M);
-    if !voicing1 && !voicing2
-       Wo_(1:M) = 2*pi/100;
-    end
-
-    if voicing1 && !voicing2
-       Wo_(1:M/2) = Wo1;
-       Wo_(M/2+1:M) = 2*pi/100;
-       voicing_(1:M/2) = 1;
-    end
-
-    if !voicing1 && voicing2
-       Wo_(1:M/2) = 2*pi/100;
-       Wo_(M/2+1:M) = Wo2;
-       voicing_(M/2+1:M) = 1;
-    end
-
-    if voicing1 && voicing2
-      Wo_samples = [Wo1 Wo2];
-      Wo_(1:M) = interp_linear([1 M+1], Wo_samples, 1:M);
-      voicing_(1:M) = 1;
-    end
-
-    #{
-    printf("f: %d f+M/2: %d Wo: %f %f (%f %%) v: %d %d \n", f, f+M/2, model(f,1), model(f+M/2,1), 100*abs(model(f,1) - model(f+M/2,1))/model(f,1), voicing(f), voicing(f+M/2));
-    for i=f:f+M/2-1
-      printf("  f: %d v: %d v_: %d Wo: %f Wo_: %f\n", i, voicing(i), voicing_(i), model(i,1),  model_(i,1));
-    end
-    #}
-endfunction
 
 
 % ---------------------------------------------------------------------------------------
diff --git a/codec2-dev/octave/tnewamp1.m b/codec2-dev/octave/tnewamp1.m
index af41622a..10ee9be3 100644
--- a/codec2-dev/octave/tnewamp1.m
+++ b/codec2-dev/octave/tnewamp1.m
@@ -34,6 +34,7 @@
 
 function tnewamp1(input_prefix)
   newamp;
+  autotest;
   more off;
 
   max_amp = 80;
@@ -58,6 +59,7 @@ function tnewamp1(input_prefix)
   % Load in C vectors and compare -----------------------------------------
  
   load("../build_linux/unittest/tnewamp1_out.txt");
+  
   K = 20;
   [frames tmp] = size(rate_K_surface_c);
   [rate_K_surface sample_freqs_kHz] = resample_const_rate_f_mel(model(1:frames,:), K);
@@ -77,20 +79,116 @@ function tnewamp1(input_prefix)
   end
     
   rate_K_surface_ = zeros(frames, K);
+  interpolated_surface_ = zeros(frames, K);
   energy_q = create_energy_q;
+  M = 4;
   for f=1:frames   
     [mean_f_ indx] = quantise(energy_q, mean_f(f));
     indexes(f,3) = indx - 1;
     rate_K_surface_(f,:) = rate_K_surface_no_mean_(f,:) + mean_f_;
   end
 
+  % simulated decoder
+  % break into segments of M frames.  We have 2 samples spaced M apart
+  % and interpolate the rest.
+
+  Nfft_phase = 128;
+  model_ = zeros(frames, max_amp+2);
+  voicing_ = zeros(1,frames);
+  Hm = zeros(frames, max_amp);
+  phi = zeros(1,max_amp);
+  for f=1:M:frames   
+    if voicing(f)
+      index = encode_log_Wo(model(f,1), 6);
+      if index == 0
+        index = 1;
+      end
+      model_(f,1) = decode_log_Wo(index, 6);
+    else
+      model_(f,1) = 2*pi/100;
+    end
+
+    if f > M
+      Wo1 = model_(f-M,1);
+      Wo2 = model_(f,1);
+      [Wo_ avoicing_] = interp_Wo_v(Wo1, Wo2, voicing(f-M), voicing(f));
+      model_(f-M:f-1,1) = Wo_;
+      voicing_(f-M:f-1) = avoicing_;
+      model_(f-M:f-1,2) = floor(pi ./ model_(f-M:f-1,1)); % calculate L for each interpolated Wo
+
+      left_vec = rate_K_surface_(f-M,:);
+      right_vec = rate_K_surface_(f,:);
+      sample_points = [f-M f];
+      resample_points = f-M:f-1;
+      for k=1:K
+        interpolated_surface_(resample_points,k) = interp_linear(sample_points, [left_vec(k) right_vec(k)], resample_points);
+      end
+      
+      for k=f-M:f-1
+        model_(k,:) = resample_rate_L(model_(k,:), interpolated_surface_(k,:), sample_freqs_kHz);
+        printf("\n");
+        printf("frame: %d Wo: %4.3f L: %d\n", k, model_(k,1), model_(k,2));
+        phase = determine_phase(model_, k, Nfft_phase);
+        printf("  phase: ");
+        for i=1:5
+          printf("%5.2f ", phase(i));
+        end
+        printf("\n");
+        printf("  b....: ");
+        for m=1:model_(k,2)
+          b = round(m*model_(k,1)*Nfft_phase/(2*pi));  % map harmonic centre to DFT bin
+          if m <= 5
+            printf("%5d ", b);
+          end
+          phi(m) = phase(b+1);
+          Hm(k,m) = exp(-j*phi(m));
+        end  
+        printf("\n");
+        printf("  phi..: ");
+        for m=1:5
+          printf("%5.2f ", phi(m));
+        end
+        printf("\n");
+        %if k == 2
+        %  xx
+        %end
+     
+      end
+    end
+  end
+
+  %model_(1,1:77)
+  %model__c(1,1:77)
+  %sum(model_(1,1:77)-model__c(1,1:77))
+  %[mx mxi] = max(model_(1,1:77)-model__c(1,1:77))
+
+  %interpolated_surface_(1,:)
+  %interpolated_surface__c(1,:)
+  %sum(interpolated_surface_(1,:) - interpolated_surface__c(1,:))
+
+
+  %Hm(2,:) - Hm_c(2,:)
+  for f=1:frames
+    s = abs(sum(Hm(f,:) - Hm_c(f,:)));
+    printf("f: %d s: %f \n", f, s);
+  end
+  
   figure(1);
-  mesh(rate_K_surface_);
+  mesh(angle(Hm));
   figure(2);
-  mesh(rate_K_surface__c);
+  mesh(angle(Hm_c));
   figure(3);
-  mesh(rate_K_surface_ - rate_K_surface__c);
-  axis([1 K 1 frames -1 1])
+  mesh(abs(Hm - Hm_c));
+
+  check(rate_K_surface, rate_K_surface_c, 'rate_K_surface', 0.01);
+  check(mean_f, mean_c, 'mean', 0.01);
+  check(rate_K_surface_, rate_K_surface__c, 'rate_K_surface_', 0.01);
+  check(interpolated_surface_, interpolated_surface__c, 'interpolated_surface_', 0.01);
+  check(model_(:,1), model__c(:,1), 'interpolated Wo_', 0.001);
+  check(voicing_, voicing__c, 'interpolated voicing');
+  check(model_(:,3:max_amp+2), model__c(:,3:max_amp+2), 'rate L surface at dec', 0.1);
+  check(Hm, Hm_c, 'phase surface');
+
   #{
 
   for f=1:frames
diff --git a/codec2-dev/src/codebooknewamp1_energy.c b/codec2-dev/src/codebooknewamp1_energy.c
new file mode 100644
index 00000000..6ecdd7c1
--- /dev/null
+++ b/codec2-dev/src/codebooknewamp1_energy.c
@@ -0,0 +1,39 @@
+/* THIS IS A GENERATED FILE. Edit generate_codebook.c and its input */
+
+/*
+ * This intermediary file and the files that used to create it are under 
+ * The LGPL. See the file COPYING.
+ */
+
+#include "defines.h"
+
+  /* /home/david/codec2-dev/src/codebook/newamp1_energy_q.txt */
+static const float codes0[] = {
+  10,
+  12.5,
+  15,
+  17.5,
+  20,
+  22.5,
+  25,
+  27.5,
+  30,
+  32.5,
+  35,
+  37.5,
+  40,
+  42.5,
+  45,
+  47.5
+};
+
+const struct lsp_codebook newamp1_energy_cb[] = {
+  /* /home/david/codec2-dev/src/codebook/newamp1_energy_q.txt */
+  {
+    1,
+    4,
+    16,
+    codes0
+  },
+  { 0, 0, 0, 0 }
+};
diff --git a/codec2-dev/src/phase.c b/codec2-dev/src/phase.c
index fa5998c4..0268bc31 100644
--- a/codec2-dev/src/phase.c
+++ b/codec2-dev/src/phase.c
@@ -29,6 +29,7 @@
 #include "phase.h"
 #include "kiss_fft.h"
 #include "comp.h"
+#include "comp_prim.h"
 #include "sine.h"
 
 #include <assert.h>
@@ -212,3 +213,105 @@ void phase_synth_zero_order(
 
 }
 
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: mag_to_phase
+  AUTHOR......: David Rowe
+  DATE CREATED: Jan 2017
+
+  Algorithm for http://www.dsprelated.com/showcode/20.php ported to C.  See
+  also Octave function mag_to_phase.m
+
+  Given a magnitude spectrum in dB, returns a minimum-phase phase
+  spectra.
+
+\*---------------------------------------------------------------------------*/
+
+void mag_to_phase(float phase[],             /* Nfft/2+1 output phase samples in radians       */
+                  float Gdbfk[],             /* Nfft/2+1 postive freq amplitudes samples in dB */
+                  int Nfft, 
+                  codec2_fft_cfg fft_fwd_cfg,
+                  codec2_fft_cfg fft_inv_cfg
+                  )
+{
+    COMP Sdb[Nfft], c[Nfft], cf[Nfft], Cf[Nfft];
+    int  Ns = Nfft/2+1;
+    int  i;
+
+    /* install negative frequency components, 1/Nfft takes into
+       account kiss fft lack of scaling on ifft */
+
+    Sdb[0].real = Gdbfk[0];
+    Sdb[Ns].real = Gdbfk[Ns];
+    Sdb[0].imag = Sdb[Ns].imag = 0.0;
+    for(i=1; i<Ns; i++) {
+        Sdb[i].real = Sdb[Nfft-i].real = Gdbfk[i];
+        Sdb[i].imag = Sdb[Nfft-i].imag = 0.0;
+    }
+
+    printf("  Sdb..: ");
+    for(i=0; i<5; i++) {
+        printf("%5.2f ", Sdb[i].real);
+    }
+    printf("\n         ");
+    for(i=0; i<5; i++) {
+        printf("%5.2f ", Sdb[i].imag);
+    }
+    printf("\n");
+
+    /* compute real cepstrum from log magnitude spectrum */
+
+    codec2_fft(fft_inv_cfg, Sdb, c);
+    for(i=0; i<Nfft; i++) {
+        c[i].real /= (float)Nfft;
+        c[i].imag /= (float)Nfft;
+    }
+
+    printf("  c....: ");
+    for(i=0; i<5; i++) {
+        printf("%5.2f ", c[i].real);
+    }
+    printf("\n         ");
+    for(i=0; i<5; i++) {
+        printf("%5.2f ", c[i].imag);
+    }
+    printf("\n");
+    /*
+    for(i=0; i<Nfft; i++) {
+        printf("i: %d c: %f %f\n", i, c[i].real, c[i].imag);
+    }
+    */
+
+    /* Fold cepstrum to reflect non-min-phase zeros inside unit circle */
+
+    cf[0] = c[0];
+    for(i=1; i<Ns-1; i++) {
+        cf[i] = cadd(c[i],c[Nfft-i]);
+    }
+    cf[Ns] = c[Ns];
+    for(i=Ns; i<Nfft; i++) {
+        cf[i].real = 0.0;
+        cf[i].imag = 0.0;
+    }
+
+    /* Cf = dB_magnitude + j * minimum_phase */
+
+    codec2_fft(fft_fwd_cfg, cf, Cf);
+
+    /*  The maths says we are meant to be using log(x), not 20*log10(x),
+        so we need to scale the phase to account for this:
+        log(x) = 20*log10(x)/scale */
+                          
+    float scale = (20.0/logf(10.0));
+    
+    for(i=0; i<Ns; i++) {
+        phase[i] = Cf[i].imag/scale;
+    }
+    
+    printf("  phase: ");
+    for(i=0; i<5; i++) {
+        printf("%5.2f ", phase[i]);
+    }
+    printf("\n");
+}
diff --git a/codec2-dev/src/phase.h b/codec2-dev/src/phase.h
index 358cf9c2..a01a3d0a 100644
--- a/codec2-dev/src/phase.h
+++ b/codec2-dev/src/phase.h
@@ -34,4 +34,6 @@
 void sample_phase(MODEL *model, COMP filter_phase[], COMP A[]);
 void phase_synth_zero_order(MODEL *model, float *ex_phase, COMP filter_phase[]);
 
+void mag_to_phase(float phase[], float Gdbfk[], int Nfft, codec2_fft_cfg fwd_cfg, codec2_fft_cfg inv_cfg);
+
 #endif
diff --git a/codec2-dev/src/quantise.c b/codec2-dev/src/quantise.c
index 05de19a5..ca9f615a 100644
--- a/codec2-dev/src/quantise.c
+++ b/codec2-dev/src/quantise.c
@@ -37,6 +37,7 @@
 #include "lpc.h"
 #include "lsp.h"
 #include "codec2_fft.h"
+#include "phase.h"
 #undef PROFILE
 #include "machdep.h"
 
@@ -2384,3 +2385,149 @@ void post_filter_newamp1(float vec[], float sample_freq_kHz[], int K, float pf_g
         vec[k] -= pre[k];
     }
 }
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: interp_Wo_v
+  AUTHOR......: David Rowe
+  DATE CREATED: Jan 2017
+
+  Decoder side interpolation of Wo and voicing, to go from 25 Hz
+  sample rate used over channle to 100Hz internal sample rate of Codec 2.
+
+\*---------------------------------------------------------------------------*/
+
+void interp_Wo_v(float Wo_[], int voicing_[], float Wo1, float Wo2, int voicing1, int voicing2)
+{
+    int i;
+    int M = 4;
+
+    for(i=0; i<M; i++)
+        voicing_[i] = 0;
+
+    if (!voicing1 && !voicing2) {
+        for(i=0; i<M; i++)
+            Wo_[i] = 2.0*M_PI/100.0;
+    }
+
+    if (voicing1 && !voicing2) {
+       Wo_[0] = Wo_[1] = Wo1;
+       Wo_[2] = Wo_[3] = 2.0*M_PI/100.0;
+       voicing_[0] = voicing_[1] = 1;
+    }
+
+    if (!voicing1 && voicing2) {
+       Wo_[0] = Wo_[1] = 2.0*M_PI/100.0;
+       Wo_[2] = Wo_[3] = Wo2;
+       voicing_[2] = voicing_[3] = 1;
+    }
+
+    if (voicing1 && voicing2) {
+        float c;
+        for(i=0,c=1.0; i<M; i++,c-=1.0/M) {
+            Wo_[i] = Wo1*c + Wo2*(1.0-c);
+            voicing_[i] = 1;
+        }
+    }
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: resample_rate_L
+  AUTHOR......: David Rowe
+  DATE CREATED: Jan 2017
+
+  Decoder side conversion of rate K vector back to rate L.
+
+\*---------------------------------------------------------------------------*/
+
+void resample_rate_L(MODEL *model, float rate_K_vec[], float rate_K_sample_freqs_kHz[], int K)
+{
+   float rate_K_vec_term[K+2], rate_K_sample_freqs_kHz_term[K+2];
+   float AmdB[MAX_AMP+1], rate_L_sample_freqs_kHz[MAX_AMP+1];
+   int m;
+
+   /* terminate either end of the rate K vecs with 0dB points */
+
+   rate_K_vec_term[0] = rate_K_vec_term[K+1] = 0.0;
+   rate_K_sample_freqs_kHz_term[0] = 0.0;
+   rate_K_sample_freqs_kHz_term[K+1] = 4.0;
+
+   for(int k=0; k<K; k++) {
+       rate_K_vec_term[k+1] = rate_K_vec[k];
+       rate_K_sample_freqs_kHz_term[k+1] = rate_K_sample_freqs_kHz[k];
+  
+       //printf("k: %d f: %f rate_K: %f\n", k, rate_K_sample_freqs_kHz[k], rate_K_vec[k]);
+   }
+
+   for(m=1; m<=model->L; m++) {
+       rate_L_sample_freqs_kHz[m] = m*model->Wo*4.0/M_PI;
+   }
+
+   interp_para(&AmdB[1], rate_K_sample_freqs_kHz_term, rate_K_vec_term, K+2, &rate_L_sample_freqs_kHz[1], model->L);    
+   for(m=1; m<=model->L; m++) {
+       model->A[m] = pow(10.0,  AmdB[m]/20.0);
+       // printf("m: %d f: %f AdB: %f A: %f\n", m, rate_L_sample_freqs_kHz[m], AmdB[m], model->A[m]);
+   }
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: determine_phase
+  AUTHOR......: David Rowe
+  DATE CREATED: Jan 2017
+
+  Given a magnitude spectrum determine a phase spectrum, used for
+  phase synthesis with newamp1.
+
+\*---------------------------------------------------------------------------*/
+
+void determine_phase(MODEL *model, int Nfft, codec2_fft_cfg fwd_cfg, codec2_fft_cfg inv_cfg)
+{
+    int i,m,b;
+    int Ns = Nfft/2+1;
+    float Gdbfk[Ns], sample_freqs_kHz[Ns], phase[Ns];
+    float AmdB[MAX_AMP+1], rate_L_sample_freqs_kHz[MAX_AMP+1];
+
+    printf("  AmdB.: ");
+    for(m=1; m<=model->L; m++) {
+        AmdB[m] = 20.0*log10(model->A[m]);
+        rate_L_sample_freqs_kHz[m] = (float)m*model->Wo*4.0/M_PI;
+        if (m <=5) {
+            printf("%5.2f ", AmdB[m]);
+        }
+    }
+    printf("\n");
+    
+    for(i=0; i<Ns; i++) {
+        sample_freqs_kHz[i] = (FS/1000.0)*(float)i/Nfft;
+    }
+
+    interp_para(Gdbfk, &rate_L_sample_freqs_kHz[1], &AmdB[1], model->L, sample_freqs_kHz, Ns);
+
+    printf("  Gdbfk: ");
+    for(i=0; i<5; i++) {
+        printf("%5.2f ", Gdbfk[i]);
+    }
+    printf("\n");
+
+    mag_to_phase(phase, Gdbfk, Nfft, fwd_cfg, inv_cfg);
+
+    printf("  b....: ");
+    for(m=1; m<=model->L; m++) {
+        b = floorf(0.5+m*model->Wo*Nfft/(2.0*M_PI));
+        model->phi[m] = phase[b];
+        if (m <= 5) {
+            printf("%5d ", b);
+        }
+    }
+    printf("\n");
+    printf("  phi..: ");
+    for(m=1; m<=5; m++) {
+        printf("% 5.2f ", model->phi[m]);
+    }
+    printf("\n");
+}
diff --git a/codec2-dev/src/quantise.h b/codec2-dev/src/quantise.h
index 765b6efc..399a523a 100644
--- a/codec2-dev/src/quantise.h
+++ b/codec2-dev/src/quantise.h
@@ -141,5 +141,8 @@ void mel_sample_freqs_kHz(float rate_K_sample_freqs_kHz[], int K);
 void resample_const_rate_f(MODEL *model, float rate_K_vec[], float rate_K_sample_freqs_kHz[], int K);
 float rate_K_mbest_encode(int *indexes, float *x, float *xq, int ndim, int mbest_entries);
 void post_filter_newamp1(float vec[], float sample_freq_kHz[], int K, float pf_gain);
+void interp_Wo_v(float Wo_[], int voicing_[], float Wo1, float Wo2, int voicing1, int voicing2);
+void resample_rate_L(MODEL *model, float rate_K_vec[], float rate_K_sample_freqs_kHz[], int K);
+void determine_phase(MODEL *model, int Nfft, codec2_fft_cfg fwd_cfg, codec2_fft_cfg inv_cfg);
 
 #endif
diff --git a/codec2-dev/unittest/tnewamp1.c b/codec2-dev/unittest/tnewamp1.c
index eda9a18c..5af420e5 100644
--- a/codec2-dev/unittest/tnewamp1.c
+++ b/codec2-dev/unittest/tnewamp1.c
@@ -37,6 +37,7 @@
 #include "quantise.h"
 
 #define FRAMES 100
+#define PHASE_NFFT 128
 
 int main(int argc, char *argv[]) {
     short buf[N_SAMP];	        /* input/output buffer                   */
@@ -47,6 +48,7 @@ int main(int argc, char *argv[]) {
     COMP  W[FFT_ENC];	        /* DFT of w[]                            */
     MODEL model;
     void *nlp_states;
+    codec2_fft_cfg phase_fft_fwd_cfg, phase_fft_inv_cfg;
     float pitch, prev_uq_Wo;
     int   i,m,f,k;
     
@@ -59,6 +61,9 @@ int main(int argc, char *argv[]) {
     fft_fwd_cfg = codec2_fft_alloc(FFT_ENC, 0, NULL, NULL); 
     make_analysis_window(fft_fwd_cfg, w, W);
 
+    phase_fft_fwd_cfg = codec2_fft_alloc(PHASE_NFFT, 0, NULL, NULL);
+    phase_fft_inv_cfg = codec2_fft_alloc(PHASE_NFFT, 1, NULL, NULL);
+
     for(i=0; i<M_PITCH; i++) {
 	Sn[i] = 1.0;
     }
@@ -72,10 +77,25 @@ int main(int argc, char *argv[]) {
     float mean[FRAMES];
     float mean_[FRAMES];
     float rate_K_surface_[FRAMES][K];         // quantised rate K vecs for each frame
+    float interpolated_surface_[FRAMES][K];   // dec/interpolated surface
+    int   voicing[FRAMES];
+    int   voicing_[FRAMES];
+    float model_octave_[FRAMES][MAX_AMP+2];
+    COMP  Hm[FRAMES][MAX_AMP];
 
-    for(f=0; f<FRAMES; f++)
-        for(m=0; m<MAX_AMP+2; m++)
+    for(f=0; f<FRAMES; f++) {
+        for(m=0; m<MAX_AMP+2; m++) {
             model_octave[f][m] = 0.0;
+            model_octave_[f][m] = 0.0;
+        }
+        for(m=0; m<MAX_AMP; m++) {
+            Hm[f][m].real = 0.0;
+            Hm[f][m].imag = 0.0;
+        }
+        for(k=0; m<K; k++)
+            interpolated_surface_[f][k] = 0.0;
+        voicing_[f] = 0;
+    }
 
     mel_sample_freqs_kHz(rate_K_sample_freqs_kHz, K);
 
@@ -88,6 +108,8 @@ int main(int argc, char *argv[]) {
         exit(1);
     }
 
+    int M = 4; 
+
     for(f=0; f<FRAMES; f++) {
         assert(fread(buf,sizeof(short),N_SAMP,fin) == N_SAMP);
 
@@ -108,6 +130,8 @@ int main(int argc, char *argv[]) {
 	dft_speech(fft_fwd_cfg, Sw, Sn, w);
 	two_stage_pitch_refinement(&model, Sw);
 	estimate_amplitudes(&model, Sw, W, 1);
+        est_voicing_mbe(&model, Sw, W);
+        voicing[f] = model.voiced;
 
         /* newamp1 processing ----------------------------------------*/
 
@@ -144,7 +168,80 @@ int main(int argc, char *argv[]) {
         for(m=1; m<=model.L; m++) {
             model_octave[f][m+1] = model.A[m];
         }        
-     }
+    }
+
+ 
+    /* Decoder */
+
+    MODEL model_;
+
+    for(f=0; f<FRAMES; f+=M) {
+
+        /* Quantise Wo. V/UV flag is coded using a zero index for Wo,
+           this means we need to adjust Wo index slightly for the
+           lowest Wo V frames */
+
+        if (voicing[f]) {
+            int index = encode_log_Wo(model_octave[f][0], 6);
+            if (index == 0) {
+                index = 1;
+            }
+            model_octave_[f][0] = decode_log_Wo(index, 6);
+         }
+        else {
+            model_octave_[f][0] = 2.0*M_PI/100.0;
+        }
+
+        float c;
+        if (f >= M) {
+
+            /* interpolate 25Hz amplitude vectors back to 100Hz */
+
+            float *left_vec = &rate_K_surface_[f-M][0];
+            float *right_vec = &rate_K_surface_[f][0];
+            for(i=f-M,c=1.0; i<f; i++,c-=1.0/M) {
+                for(k=0; k<K; k++) {
+                    interpolated_surface_[i][k] = left_vec[k]*c + right_vec[k]*(1.0-c);
+                }
+            }
+            
+            /* interpolate 25Hz v and Wo back to 100Hz */
+
+            float aWo_[M];
+            int avoicing_[M], m;
+            float Wo1 = model_octave_[f-M][0];
+            float Wo2 = model_octave_[f][0];
+            interp_Wo_v(aWo_, avoicing_, Wo1, Wo2, voicing[f-M], voicing[f]);
+            for(i=f-M, m=0; i<f; i++,m++) {
+                model_octave_[i][0] = aWo_[m];
+                model_octave_[i][1] = floorf(M_PI/model_octave_[i][0]); 
+                voicing_[i] = avoicing_[m];
+            }
+
+            /* back to rate L, synth phase */
+
+            for(i=f-M; i<f; i++) {
+                model_.Wo = model_octave_[i][0];
+                model_.L  = model_octave_[i][1];
+                resample_rate_L(&model_, &interpolated_surface_[i][0], rate_K_sample_freqs_kHz, K);
+                printf("\n");
+                printf("frame: %d Wo: %4.3f L: %d\n", i+1, model_.Wo, model_.L);
+                determine_phase(&model_, PHASE_NFFT, phase_fft_fwd_cfg, phase_fft_inv_cfg);
+                //if (i == 1) {
+                //    exit(0);
+                //}
+                        
+                for(m=1; m<=model_.L; m++) {
+                    model_octave_[i][m+1] = model_.A[m];
+                    Hm[i][m-1].real = cos(model_.phi[m]);
+                    Hm[i][m-1].imag = -sin(model_.phi[m]);
+                    //printf("m: %d Hm: %f %f\n", m, Hm[i][m].real, Hm[i][m].imag);
+                }   
+            }
+
+        }
+
+    }
 
     fclose(fin);
 
@@ -154,12 +251,16 @@ int main(int argc, char *argv[]) {
     assert(fout != NULL);
     fprintf(fout, "# Created by tnewamp1.c\n");
     octave_save_float(fout, "rate_K_surface_c", (float*)rate_K_surface, FRAMES, K, K);
-    octave_save_float(fout, "mean_c", (float*)mean, FRAMES, 1, 1);
+    octave_save_float(fout, "mean_c", (float*)mean, 1, FRAMES, 1);
     octave_save_float(fout, "rate_K_surface_no_mean_c", (float*)rate_K_surface_no_mean, FRAMES, K, K);
     octave_save_float(fout, "rate_K_surface_no_mean__c", (float*)rate_K_surface_no_mean_, FRAMES, K, K);
     octave_save_float(fout, "mean__c", (float*)mean_, FRAMES, 1, 1);
     octave_save_float(fout, "rate_K_surface__c", (float*)rate_K_surface_, FRAMES, K, K);
+    octave_save_float(fout, "interpolated_surface__c", (float*)interpolated_surface_, FRAMES, K, K);
     octave_save_float(fout, "model_c", (float*)model_octave, FRAMES, MAX_AMP+2, MAX_AMP+2);
+    octave_save_float(fout, "model__c", (float*)model_octave_, FRAMES, MAX_AMP+2, MAX_AMP+2);
+    octave_save_int(fout, "voicing__c", (int*)voicing_, 1, FRAMES);
+    octave_save_complex(fout, "Hm_c", (COMP*)Hm, FRAMES, MAX_AMP, MAX_AMP);
     fclose(fout);
 
     printf("Done! Now run\n  octave:1> tnewamp1(\"../build_linux/src/hts1a\")\n");
-- 
2.25.1