<|start_filename|>cpp/timing/profile.cpp<|end_filename|>
#include <stdlib.h>
#include <cstdio>
#include <cmath>
#include <sys/time.h>

#include "../src/single_pitch.hpp"

int main(int argc, char **argv){

  int L = 20; //controlled model order
  int ell = 4;
  int N = 1000;
  int F = 5*N*L;
  FTYPE pitch_bounds[] = {0.001, 0.45};
  int i, j;
  struct timeval tim;
  double l1, l2;
  int rpt = 10;

  /* construct solver object */
  gettimeofday(&tim, NULL);
  l1 = tim.tv_sec + (tim.tv_usec/1000000.0);
  single_pitch * sp = new single_pitch(L, F, N, pitch_bounds);
  gettimeofday(&tim, NULL);
  l2 = tim.tv_sec + (tim.tv_usec/1000000.0);
 
  /* construct signal */
  FTYPE * x = new FTYPE [N];
  FTYPE omega = 0.1;

  for( j = 0 ; j < N ; ++j ){
    x[j] = 0;
    for( i = 1 ; i <= ell ; ++i ){
      x[j] += sin(M_PI*i*omega*j);
    }
  }

  /* repeat */
  gettimeofday(&tim, NULL);
  l1 = tim.tv_sec + (tim.tv_usec/1000000.0);

  for( j = 0 ; j < rpt ; ++j){
    sp->compute(x);
  }

  gettimeofday(&tim, NULL);
  l2 = tim.tv_sec + (tim.tv_usec/1000000.0);


  /* repeat with refinement */
  FTYPE * omega_0h;
  
  gettimeofday(&tim, NULL);
  l1 = tim.tv_sec + (tim.tv_usec/1000000.0);

  for( j = 0 ; j < rpt ; ++j){
    sp->compute(x);
    omega_0h = sp->refine(x);
  }

  gettimeofday(&tim, NULL);
  l2 = tim.tv_sec + (tim.tv_usec/1000000.0);


  delete sp;
  delete [] x;
}




<|start_filename|>cpp/src/vector.cpp<|end_filename|>
/*
 Defines usefull constructs (memory constructs) and
  free that supports several memory organization approaches
  using proper usage of defines

  MKL:  uses MKL malloc and free. 
    Define ALIGNMENT32, ALIGNMENT64, ALIGNMENT128, as well
    to define the alignment. 

  else: uses standard C++ allocation via new and delete of arrays.

*/

#include "vector.hpp"

FTYPE * vector(int N){
  FTYPE * x;
  
  #ifdef MKL
  x = (FTYPE*)mkl_malloc(sizeof(FTYPE)*N, ALIGNMENT);
  #else
  x = new FTYPE [N];
  #endif

  if( x == NULL ){
    printf("Vector.cpp: Could not allocate %d bytes of memory", 
           (int)sizeof(FTYPE)*N);
    exit(EXIT_FAILURE);
  }

  return x;
}


void del_vector(FTYPE * x){

  #ifdef MKL
  mkl_free(x);
  #else
  delete [] x;
  #endif
  
}


<|start_filename|>cpp/timing/Makefile<|end_filename|>
# Optional - but remove link option -DMKL
MKLROOT = /opt/intel

#Dynamic linking
MKL = -m64 -I$(MKLROOT)/mkl/include -I$(MKLROOT)/mkl/include/fftw -L$(MKLROOT)/mkl/lib/intel64 -L$(MKLROOT)/compilers_and_libraries/linux/lib/intel64_lin -lmkl_intel_lp64 -lmkl_intel_thread -lmkl_core -liomp5 -lpthread -lm -DALIGNMENT32



FTYPE = -DDOUBLE
#LINK =  -DMKL $(MKL) -DCHEBYSHEV
LINK = -lfftw3 -DCHEBYSHEV


all:
	g++ -O3 -march=native -Wno-write-strings $(FTYPE) ../src/single_pitch.cpp ../src/tools.cpp ../src/th.cpp ../src/vector.cpp timing.cpp $(LINK) -pthread -o timing

	g++ -O3 -g -march=native -Wno-write-strings $(FTYPE) ../src/single_pitch.cpp ../src/tools.cpp ../src/th.cpp ../src/vector.cpp profile.cpp $(LINK) -pthread -o profile



<|start_filename|>cpp/src/tools.cpp<|end_filename|>
/* 

  Various elementwise tools. Developed specifically for the
  use in single pitch ML estimation

  <NAME>, 
  March 2015
*/


#include "tools.hpp"

// Copy y <= x
void CvmdCopy(int n, FTYPE *x, FTYPE *y){

  #ifdef MKL
  COPY(n, x, 1, y, 1);
  #else
  for(int i = 0 ; i < n ; ++i)
    y[i] = x[i];
  #endif

}

// Scal x <= alpha*x
void CvmdScal(int n, FTYPE alpha, FTYPE *x){
  
  #ifdef MKL
  SCAL(n, alpha, x, 1);
  #else
  for(int i = 0 ; i < n ; ++i)
    x[i] = alpha*x[i];
  #endif
}

// returns  y <= alpha x + y
void CvmdAxpy(int n, FTYPE alpha, FTYPE *x, FTYPE *y){

  #ifdef MKL
  AXPY(n, alpha, x, 1, y, 1);
  #else
  for(int i = 0 ; i < n ; ++i)
    y[i] += alpha*x[i];
  #endif
}

// returns inner dot product a^T b
FTYPE CvmdDot(int n, FTYPE *a, FTYPE *b){

  #ifdef MKL
  return DOT(n, a, 1, b, 1);
  #else
  FTYPE c = 0.0;
  for(int i = 0 ; i < n ; ++i)
    c += a[i]*b[i];
  return c;
  #endif


}

// Adds c_i <- a + b_i
void CvmdAddConstant(int n, FTYPE a, FTYPE *b, FTYPE *c){

  for(int i = 0 ; i < n ; ++i)
    c[i] = a + b[i];
}

// Adds c <- a + b
void CvmdAdd(int n, FTYPE *a, FTYPE *b, FTYPE *c){

  #ifdef MKL
  VADD(n, a, b, c);
  #else
  for(int i = 0 ; i < n ; ++i)
    c[i] = a[i] + b[i];
  #endif
}

// Adds c <- a - b
void CvmdSub(int n, FTYPE *a, FTYPE *b, FTYPE *c){

  #ifdef MKL
  VSUB(n, a, b, c);
  #else  
  for(int i = 0 ; i < n ; ++i)
    c[i] = a[i] - b[i];
  #endif
}

// Adds a <- a + b
void CvmdAddInplace(int n, FTYPE *a, FTYPE *b){

  #ifdef MKL
  VADD(n, a, b, a);
  #else  
  for(int i = 0 ; i < n ; ++i)
    a[i] += b[i];
  #endif
}

// Adds a <- a - b
void CvmdSubInplace(int n, FTYPE *a, FTYPE *b){

  #ifdef MKL
  VSUB(n, a, b, a);
  #else    
  for(int i = 0 ; i < n ; ++i)
    a[i] -= b[i];
  #endif
}

//forms reverse vector b <- a_[n:1:-1]
void CvmdReverse(int n, FTYPE *a, FTYPE *b){ 

  for(int i=0 ; i < n ; ++i)
    b[i] = a[n-i-1];

}

//forms symmetric vector b <- [a_[1:n]; a_[n-1:1:-1]]
void CvmdSymmetric(int n, FTYPE *a, FTYPE *b){ 

  for(int i=0 ; i < n ; ++i)
    b[i] = a[i];

  for(int i=0 ; i < n-1 ; ++i)
    b[n+i] = a[n-i-2];
}

//computer c <- a_[1:n] + [0; b_[1:n-1]]
void CvmdShift(int n, FTYPE *a, FTYPE *b, FTYPE *c){ 
  c[0] = a[0];
  for(int i=1 ; i < n; ++i)
    c[i] = a[i] + b[i-1];
}

//Computes elementwise y_i <- a - x_i^2
void CvmdScalSquare(int n, FTYPE a, FTYPE * x, FTYPE * y){
  
  for(int i = 0 ; i < n ; ++i){
    y[i] = a - x[i]*x[i];
  }
}



//Initialize a vector with value a
void CvmdInit(int n, FTYPE a, FTYPE * x){
  
  for(int i = 0 ; i < n ; ++i){
    x[i] = a;
  }
}


//Computes elementwise sin y_i <- sin(a * x_i), where x_i is from min to max
void CvmdSinRange(int min, int max, FTYPE a, FTYPE * y){
  
  FTYPE * yp = y;
  for(int i = min ; i <= max ; ++i){
    *yp++ = sin(a*i);
  }
}

//Computes elementwise sin y_i <- sin(a * x_i), 
 void CvmdSinRange(int n, FTYPE * x, FTYPE a, FTYPE * t, FTYPE * y){

  #ifdef MKL
  CvmdInit(n, 0.0, t);
  AXPY(n, a, x, 1, t, 1);
  VSIN(n, t, y);
  #else
  for(int i = 0 ; i < n ; ++i){
    y[i] = sin(a*x[i]);
  }
  #endif
}


//Computes elementwise complex exponential y_i <- exp(1i*alpha * x_i)
// where x_i is from min to max
// and alpha and xi are real
// y_i is in interleaved complex format
void CvmdExpRange(int min, int max, FTYPE a, FTYPE * y){
  FTYPE v;
  FTYPE *yp = y;

  for(int i = min ; i <= max ; ++i){
    v = a*i;
    *yp++ = cos(v);
    *yp++ = sin(v);
  }
}

//Computes elementwise complex exponential y_i <- exp(1i*alpha * x_i)
// where alpha and xi are real
// y_i is in interleaved complex format
// t1 and t2 are temp vectors of length n
 void CvmdExpRange(int n, FTYPE * x, FTYPE a, 
    FTYPE * t1, FTYPE * t2, FTYPE * y){

  #ifdef MKL
  CvmdInit(n, 0.0, y);
  AXPY(n, a, x, 1, y, 1);
  VSINCOS(n, y, t1, t2);
  VUNPACKI(n, t2, y, 2);
  VUNPACKI(n, t1, y+1, 2);
  #else
  FTYPE v;
  FTYPE *yp = y;

  for(int i = 0 ; i < n ; ++i){
    v = a*x[i];
    *yp++ = cos(v);
    *yp++ = sin(v);
  }
  #endif
}

//Computes elementwise 
//   y_i = cos(alpha * x_i)
// and
//   z_i = sin(alpha * x_i)
// t is a temp vectors of length n
void CvmdCosSinRange(int n, FTYPE * x, FTYPE a, FTYPE *t,
                     FTYPE * y, FTYPE * z){

  #ifdef MKL
  CvmdInit(n, 0.0, t);
  AXPY(n, a, x, 1, t, 1);
  VSINCOS(n, t, z, y);
  #else
  FTYPE v;

  for(int i = 0 ; i < n ; ++i){
    v = a*x[i];
    y[i] = cos(v);
    z[i] = sin(v);
  }
  #endif
}


//Computes elementwise multiplication y_i <- x_i * z_i
void CvmdMul(int n, FTYPE * x, FTYPE * z, FTYPE * y){
  
  #ifdef MKL
  VMUL(n, x, z, y);
  #else
  for(int i = 0 ; i < n ; ++i){
    y[i] = x[i]*z[i];
  }
  #endif
}


//Computes elementwise multiplication v + iw <- x_i * z_(si) 
// with x, z complex and step s
// Real part goes into v, imaginary part into w
// t is a temp vector with n complex number
void CvmdMulz(int n, FTYPE * x, FTYPE * z, int s, FTYPE *t,
              FTYPE * v, FTYPE * w){

  #ifdef MKL
  vzPackI(n, (MKL_COMPLEX*)z, s, (MKL_COMPLEX*)t);
  vzMul(n, (MKL_COMPLEX*)x, (MKL_COMPLEX*)t, (MKL_COMPLEX*)t);
  vdPackI(n, t, 2, v);
  vdPackI(n, t+1, 2, w);
  #else
  int ii = 0;
  for(int i = 0 ; i < n ; ++i){
    v[i] = x[2*i]*z[ii] - x[2*i+1]*z[ii+1];
    w[i] = x[2*i]*z[ii+1] + x[2*i+1]*z[ii];
    ii += 2*s;
  }
  #endif
}

//Computes elementwise multiplication v + iw <- x_i * z_i 
// with x, z complex
// Real part goes into v, imaginary part into w
// t is a temp vector with n complex number
void CvmdMulz(int n, FTYPE * x, FTYPE * z, FTYPE *t,
             FTYPE * v, FTYPE * w){

  #ifdef MKL
  vzMul(n, (MKL_COMPLEX*)x, (MKL_COMPLEX*)z, (MKL_COMPLEX*)t);
  vdPackI(n, t, 2, v);
  vdPackI(n, t+1, 2, w);
  #else
  int ii = 0;
  for(int i = 0 ; i < n ; ++i){
    v[i] = x[2*i]*z[ii] - x[2*i+1]*z[ii+1];
    w[i] = x[2*i]*z[ii+1] + x[2*i+1]*z[ii];
    ii += 2;
  }
  #endif
}



//Computes elementwise division y_i <- x_i / z_i
void CvmdDiv(int n, FTYPE * x, FTYPE * z, FTYPE * y){
  
  #ifdef MKL
  VDIV(n, x, z, y);
  #else
  for(int i = 0 ; i < n ; ++i){
    y[i] = x[i]/z[i];
  }
  #endif
}

//Computes elementwise inverse y_i <- 1.0 / x_i
void CvmdInverse(int n, FTYPE * x, FTYPE * y){
  
  #ifdef MKL
  VINV(n, x, y);
  #else
  for(int i = 0 ; i < n ; ++i){
    y[i] = 1/x[i];
  }
  #endif
}

// packs x from step s to unit stride in y
void CvmdPack(int n, FTYPE * x, int s, FTYPE * y){
  int ii = 0;
  for( int i = 0 ; i < n ; ++i){
    y[i] = x[ii];
    ii += s;
  }
}


int CvmdArgmax(int n, FTYPE * x){

  FTYPE max;
  int arg;

  max = x[0];
  arg = 0;

  for( int k = 1 ; k < n ; ++k){
    if( x[k] > max){
      max = x[k];
      arg =  k;
    }
  }

  return arg;

}


<|start_filename|>cpp/lib/Makefile<|end_filename|>
# Optional - but remove link option -DMKL
MKLROOT = /opt/intel

#Dynamic linking
MKL = -m64 -I$(MKLROOT)/mkl/include -I$(MKLROOT)/mkl/include/fftw -L$(MKLROOT)/mkl/lib/intel64 -L$(MKLROOT)/compilers_and_libraries/linux/lib/intel64_lin -lmkl_intel_lp64 -lmkl_intel_thread -lmkl_core -liomp5 -lpthread -lm -DALIGNMENT32

#FTYPE = -DDOUBLE -DREGULARIZE
FTYPE = -DDOUBLE 
#LINK =  -DMKL $(MKL) -DALIGNMENT32 -DCHEBYSHEV
LINK = -lfftw3 -DCHEBYSHEV


all:
	g++ -O3 -march=native -fPIC --shared -Wl,-soname,single_pitch.so -Wno-write-strings -DLIB $(FTYPE) ../src/single_pitch.cpp ../src/tools.cpp ../src/th.cpp ../src/vector.cpp $(LINK) -pthread  -o single_pitch.so


<|start_filename|>cpp/src/tools.hpp<|end_filename|>
#ifndef __TOOLS_H__
#define __TOOLS_H__

#include <cmath>

#ifdef DOUBLE
#define FTYPE double 

#define ABS fabs

#define FFTW_MALLOC fftw_malloc
#define FFTW_PLAN fftw_plan
#define FFTW_PLAN_DFT_R2C_1D fftw_plan_dft_r2c_1d
#define FFTW_PLAN_DFT_C2R_1D fftw_plan_dft_c2r_1d
#define FFTW_EXECUTE fftw_execute
#define FFTW_DESTROY_PLAN fftw_destroy_plan
#define FFTW_FREE fftw_free
#define FFTW_COMPLEX fftw_complex

#define COPY cblas_dcopy
#define SCAL cblas_dscal
#define AXPY cblas_daxpy
#define DOT cblas_ddot 
#define VADD vdAdd
#define VSUB vdSub
#define VSIN vdSin
#define VSINCOS vdSinCos
#define VUNPACKI vdUnpackI
#define VMUL vdMul
#define VDIV vdDiv
#define VINV vdInv

#define MKL_COMPLEX MKL_Complex16

#else
#define FTYPE float

#define ABS fabsf

#define FFTW_MALLOC fftwf_malloc
#define FFTW_PLAN fftwf_plan
#define FFTW_PLAN_DFT_R2C_1D fftwf_plan_dft_r2c_1d
#define FFTW_PLAN_DFT_C2R_1D fftwf_plan_dft_c2r_1d
#define FFTW_EXECUTE fftwf_execute
#define FFTW_DESTROY_PLAN fftwf_destroy_plan
#define FFTW_FREE fftwf_free
#define FFTW_COMPLEX fftwf_complex

#define COPY cblas_scopy
#define SCAL cblas_sscal
#define AXPY cblas_saxpy
#define DOT cblas_sdot 
#define VADD vsAdd
#define VSUB vsSub
#define VSIN vsSin
#define VSINCOS vsSinCos
#define VUNPACKI vsUnpackI
#define VMUL vsMul
#define VDIV vsDiv
#define VINV vsInv

#define MKL_COMPLEX MKL_Complex8
#endif

#define MAX(A,B) (A > B ? A : B)
#define MIN(A,B) (A < B ? A : B)

#ifdef MKL
#include <mkl.h>
#endif

void CvmdSinRange(int min, int max, FTYPE a, FTYPE * y);

void CvmdSinRange(int n, FTYPE * x, FTYPE a, FTYPE * t, FTYPE * y);

void CvmdExpRange(int min, int max, FTYPE a, FTYPE * y);

void CvmdExpRange(int n, FTYPE * x, FTYPE a, FTYPE *t1, FTYPE *t2, FTYPE * y);

void CvmdCosSinRange(int n, FTYPE * x, FTYPE a, FTYPE *t,
                     FTYPE * y, FTYPE * z);

void CvmdScalSquare(int n, FTYPE a, FTYPE * x, FTYPE * y);

void CvmdInit(int n, FTYPE a, FTYPE * x);

void CvmdDiv(int n, FTYPE * x, FTYPE * z, FTYPE * y);

void CvmdInverse(int n, FTYPE * x, FTYPE * y);

void CvmdMul(int n, FTYPE * x, FTYPE * z, FTYPE * y);

void CvmdMulz(int n, FTYPE * x, FTYPE * z, int s, FTYPE *t, FTYPE * v, FTYPE * w);

void CvmdMulz(int n, FTYPE * x, FTYPE * z, FTYPE *t, FTYPE * v, FTYPE * w);

void CvmdShift(int n, FTYPE *a, FTYPE *b, FTYPE *c);

void CvmdSymmetric(int n, FTYPE *a, FTYPE *b);

void CvmdReverse(int n, FTYPE *a, FTYPE *b);

FTYPE CvmdDot(int n, FTYPE *a, FTYPE *b);

void CvmdAddConstant(int n, FTYPE a, FTYPE *b, FTYPE *c);

void CvmdAdd(int n, FTYPE *a, FTYPE *b, FTYPE *c);

void CvmdSub(int n, FTYPE *a, FTYPE *b, FTYPE *c);

void CvmdAddInplace(int n, FTYPE *a, FTYPE *b);

void CvmdSubInplace(int n, FTYPE *a, FTYPE *b);

void CvmdAxpy(int n, FTYPE alpha, FTYPE *x, FTYPE *y);

void CvmdScal(int n, FTYPE alpha, FTYPE *x);

void CvmdCopy(int n, FTYPE *x, FTYPE *y);

void CvmdPack(int n, FTYPE * x, int s, FTYPE * y);

int CvmdArgmax(int n, FTYPE * x);

#endif


<|start_filename|>cpp/unittest/test.cpp<|end_filename|>
#include <stdlib.h>
#include <algorithm>
#include <math.h>
#include <stdio.h>

#include "hdf5.h"
#include "hdf5_hl.h"
#include "gtest/gtest.h"

#include "../src/single_pitch.hpp"

#ifdef DOUBLE
#define EPS 1e-10 // pure floating
#define EPS2 1e-8 //iterative algs
#else
#define EPS 1e-1
#define EPS2 1e-1
#endif

class RealDataTest : public ::testing::Test{
protected:

  RealDataTest(){
  }

  virtual ~RealDataTest(){
  }

  virtual void SetUp(){
  }

  virtual void TearDown(){
  }
};

TEST_F(RealDataTest, ComputeGamma1) {

  hid_t file;
  herr_t status;


  file = H5Fopen("data_files/unittest1.h5", H5F_ACC_RDONLY, H5P_DEFAULT);

  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  int Mp = F/2+1;
  char label[50];
  
  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);

  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif

  double * cost_function = new double [L*Mp];
  H5LTread_dataset(file, "/costFunction", H5T_NATIVE_DOUBLE, cost_function);

  double * dim = new double [L];
  H5LTread_dataset(file, "/dim", H5T_NATIVE_DOUBLE, dim);

  /* construct object and that will compute gamma1 and gamma2  */
  single_pitch * sp = new single_pitch(L, F, N, pb);


  /* Tjeck that gamma is computed correctly */
  double * gammah;

  for(int ell = 1 ; ell <= L ; ell++){

    gammah = new double[ell*(int)dim[ell-1]];

    /* test for gamma_1 */
    sprintf(label, "/gamma_1_%d", ell);
    H5LTread_dataset(file, label, 
                     H5T_NATIVE_DOUBLE, gammah);

    for(int k = 0 ; k < ell*dim[ell-1] ; ++k ){
      ASSERT_NEAR(gammah[k], sp->Gamma1[k + Mp*(ell-1)], EPS) 
        << "Gamma1: Vectors differ at index " << k << ", gamma level " << ell;
    }

    /* test for gamma_2 */
    sprintf(label, "/gamma_2_%d", ell);
    H5LTread_dataset(file, label, 
                     H5T_NATIVE_DOUBLE, gammah);

    for(int k = 0 ; k < ell*dim[ell-1] ; ++k ){
      ASSERT_NEAR(gammah[k], sp->Gamma2[k + Mp*(ell-1)], EPS) 
        << "Gamma2: Vectors differ at index " << k << ", gamma level " << ell;
    }

    delete [] gammah;

  }
  
  /* close file */
  status = H5Fclose(file);

  del_vector(x);
  delete [] cost_function;
  delete [] dim;
  delete sp;

}

TEST_F(RealDataTest, ComputeCost1) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest1.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  char label[50];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);

  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif


  double * dim = new double [L];
  H5LTread_dataset(file, "/dim", H5T_NATIVE_DOUBLE, dim);

  int M = (int) dim[0];

  double * cost_function = new double [L*M];
  H5LTread_dataset(file, "/costFunction", H5T_NATIVE_DOUBLE, cost_function);

  status = H5Fclose(file);


  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  del_vector(xs);
  #else
  sp->compute(x);
  #endif

  /* Tjeck that the cost function is correctly computed */
  for(int ell = 1 ; ell <= L ; ell++){

    for(int k = 0 ; k < dim[ell-1] ; ++k ){
      ASSERT_NEAR(cost_function[M*(ell-1)+k], 
                  sp->costFunctionMatrix[(ell-1)][k], EPS) 
        << "Cost function differ at index " << k << ", level " << ell;
    }
  }


  
  delete sp;
  del_vector(x);
  delete [] cost_function;
  delete [] dim;
}


TEST_F(RealDataTest, ComputeGamma2) {

  hid_t file;
  herr_t status;


  file = H5Fopen("data_files/unittest2.h5", H5F_ACC_RDONLY, H5P_DEFAULT);

  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  int Mp = F/2+1;
  char label[50];
  
  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);

  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif

  double * cost_function = new double [L*Mp];
  H5LTread_dataset(file, "/costFunction", H5T_NATIVE_DOUBLE, cost_function);

  double * dim = new double [L];
  H5LTread_dataset(file, "/dim", H5T_NATIVE_DOUBLE, dim);

  /* construct object and that will compute gamma1 and gamma2  */
  single_pitch * sp = new single_pitch(L, F, N, pb);


  /* Tjeck that gamma is computed correctly */
  double * gammah;

  for(int ell = 1 ; ell <= L ; ell++){

    gammah = new double[ell*(int)dim[ell-1]];

    /* test for gamma_1 */
    sprintf(label, "/gamma_1_%d", ell);
    H5LTread_dataset(file, label, 
                     H5T_NATIVE_DOUBLE, gammah);

    for(int k = 0 ; k < ell*dim[ell-1] ; ++k ){
      ASSERT_NEAR(gammah[k], sp->Gamma1[k + Mp*(ell-1)], EPS) 
        << "Gamma1: Vectors differ at index " << k << ", gamma level " << ell;
    }

    /* test for gamma_2 */
    sprintf(label, "/gamma_2_%d", ell);
    H5LTread_dataset(file, label, 
                     H5T_NATIVE_DOUBLE, gammah);

    for(int k = 0 ; k < ell*dim[ell-1] ; ++k ){
      ASSERT_NEAR(gammah[k], sp->Gamma2[k + Mp*(ell-1)], EPS) 
        << "Gamma2: Vectors differ at index " << k << ", gamma level " << ell;
    }

    delete [] gammah;

  }
  
  /* close file */
  status = H5Fclose(file);

  del_vector(x);
  delete [] cost_function;
  delete [] dim;
  delete sp;

}

TEST_F(RealDataTest, ComputeCost2) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest2.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  char label[50];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);

  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif


  double * dim = new double [L];
  H5LTread_dataset(file, "/dim", H5T_NATIVE_DOUBLE, dim);

  int M = (int) dim[0];

  double * cost_function = new double [L*M];
  H5LTread_dataset(file, "/costFunction", H5T_NATIVE_DOUBLE, cost_function);

  status = H5Fclose(file);


  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  del_vector(xs);
  #else
  sp->compute(x);
  #endif

  /* Tjeck that the cost function is correctly computed */
  for(int ell = 1 ; ell <= L ; ell++){

    for(int k = 0 ; k < dim[ell-1] ; ++k ){
      ASSERT_NEAR(cost_function[M*(ell-1)+k], 
                  sp->costFunctionMatrix[(ell-1)][k], EPS) 
        << "Cost function differ at index " << k << ", level " << ell;
    }
  }


  
  delete sp;
  del_vector(x);
  delete [] cost_function;
  delete [] dim;
}


TEST_F(RealDataTest, ComputeSingleCost3) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest3.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  char label[50];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);

  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif


  double * dim = new double [L];
  H5LTread_dataset(file, "/dim", H5T_NATIVE_DOUBLE, dim);

  int M = (int) dim[0];

  double * cost_function = new double [L*M];
  H5LTread_dataset(file, "/costFunction", H5T_NATIVE_DOUBLE, cost_function);

  status = H5Fclose(file);


  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  del_vector(xs);
  #else
  sp->compute(x);
  #endif

  /* Tjeck that the cost function is correctly computed */
  for(int ell = 1 ; ell <= L ; ell++){

    for(int k = 0 ; k < dim[ell-1] ; ++k ){
      ASSERT_NEAR(cost_function[M*(ell-1)+k], 
                  sp->costFunctionMatrix[(ell-1)][k], EPS) 
        << "Cost function differ at index " << k << ", level " << ell;
    }
  }

  int ell = 2;

  FTYPE max = *std::max_element(cost_function + M*(ell-1), 
                               cost_function + M*(ell-1) + sp->nPitches(ell)-1);
  FTYPE maxh = sp->max_obj(ell);
  
  ASSERT_NEAR(max, maxh, EPS)  << "max differs";
  

  FTYPE omega = sp->argmax_obj(ell);
  FTYPE maxhh = sp->compute_obj(omega, x, ell);

  ASSERT_NEAR(maxhh, max, EPS)  << "max differs from computed objective";

  delete sp;
  del_vector(x);
  delete [] cost_function;
  delete [] dim;
}


TEST_F(RealDataTest, ComputeSingleCost4) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest4.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  char label[50];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);

  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif


  double * dim = new double [L];
  H5LTread_dataset(file, "/dim", H5T_NATIVE_DOUBLE, dim);

  int M = (int) dim[0];

  double * cost_function = new double [L*M];
  H5LTread_dataset(file, "/costFunction", H5T_NATIVE_DOUBLE, cost_function);

  status = H5Fclose(file);


  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  del_vector(xs)
  #else
  sp->compute(x);
  #endif

  /* Tjeck that the cost function is correctly computed */
  for(int ell = 1 ; ell <= L ; ell++){

    for(int k = 0 ; k < dim[ell-1] ; ++k ){
      ASSERT_NEAR(cost_function[M*(ell-1)+k], 
                  sp->costFunctionMatrix[(ell-1)][k], EPS) 
        << "Cost function differ at index " << k << ", level " << ell;
    }
  }

  int ell = 4;

  FTYPE max = *std::max_element(cost_function + M*(ell-1), 
                               cost_function + M*(ell-1) + sp->nPitches(ell)-1);
  FTYPE maxh = sp->max_obj(ell);
  
  ASSERT_NEAR(max, maxh, EPS)  << "max differs";
  

  FTYPE omega = sp->argmax_obj(ell);
  FTYPE maxhh = sp->compute_obj(omega, x, ell);

  ASSERT_NEAR(maxhh, max, EPS)  << "max differs from computed objective";

  delete sp;
  del_vector(x);
  delete [] cost_function;
  delete [] dim;
}


TEST_F(RealDataTest, Refine4) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest5.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  //H5LTread_dataset(file, "/omega_0", H5T_NATIVE_DOUBLE, d);
  //double omega_0 =  d[0];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);

  double * omega_0h = new double [L];
  H5LTread_dataset(file, "/omega_0h", H5T_NATIVE_DOUBLE, omega_0h);

  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif


  double * dim = new double [L];
  H5LTread_dataset(file, "/dim", H5T_NATIVE_DOUBLE, dim);

  int M = (int) dim[0];

  double * cost_function = new double [L*M];
  H5LTread_dataset(file, "/costFunction", H5T_NATIVE_DOUBLE, cost_function);

  status = H5Fclose(file);


  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  del_vector(xs);
  FTYPE * omega_0hh = sp->refine(xs, 1e-8);
  #else
  sp->compute(x);
  FTYPE * omega_0hh = sp->refine(x, 1e-8);
  #endif

  
  for( int ell = 1 ; ell < L ; ++ell)
    
    ASSERT_NEAR(omega_0h[ell-1], omega_0hh[ell-1], EPS2)  
      << "refinement differs at iOrder = " << ell ;

  delete sp;
  del_vector(x);
  delete [] cost_function;
  delete [] dim;
  delete [] omega_0h;

}


TEST_F(RealDataTest, ModelOrderSelectionNoNoise) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest6.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  H5LTread_dataset(file, "/modelOrder", H5T_NATIVE_DOUBLE, d);
  int modelOrder = (int) d[0];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);


  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif

  status = H5Fclose(file);


  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  del_vector(xs)
  FTYPE * omega_0hh = sp->refine(xs, 1e-8);
  #else
  sp->compute(x);
  FTYPE * omega_0hh = sp->refine(x, 1e-8);
  #endif

  int estModelOrder = sp->model_order_selection(x);


  // Known values
  ASSERT_NEAR(22.971993868687033, sp->lnBF[1], EPS2);
  ASSERT_NEAR(22.4532605908562, sp->lnBF[2], EPS2);

  // Without noise the method should estimate the model order correctly
  ASSERT_EQ(modelOrder, estModelOrder);

  delete sp;
  del_vector(x);

}


TEST_F(RealDataTest, ModelOrderSelectionNoise) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest7.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  H5LTread_dataset(file, "/modelOrder", H5T_NATIVE_DOUBLE, d);
  int modelOrder = (int) d[0];

  
  H5LTread_dataset(file, "/pitch", H5T_NATIVE_DOUBLE, d);
  double pitch = d[0];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);


  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif

  status = H5Fclose(file);

  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  sp->refine(xs, 1e-8);
  del_vector(xs);
  #else
  sp->compute(x);
  sp->refine(x, 1e-8);
  #endif

  int estModelOrder = sp->model_order_selection(x);

  // Control the Bayes Factors
  
  FTYPE lnBF[] = {22.976505179621622, 22.444499748782523, 423.7940152029966, 
                  412.11543962457455, 400.92575920553287};

  for( int k = 1 ; k <= L ; ++k )
    ASSERT_NEAR(lnBF[k-1], sp->lnBF[k], EPS2)  
      << "Bayes Factor differs at order = " << k ;

  // With this realization the model order estimation should work
  ASSERT_EQ(modelOrder, estModelOrder);

  FTYPE omega_0h = sp->est(x);
  ASSERT_NEAR(omega_0h/(2*M_PI), pitch, 1e-6);  

  delete sp;
  del_vector(x);
}

TEST_F(RealDataTest, ModelOrderSelectionNoiseBayes) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest7.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  H5LTread_dataset(file, "/modelOrder", H5T_NATIVE_DOUBLE, d);
  int modelOrder = (int) d[0];

  H5LTread_dataset(file, "/pitch", H5T_NATIVE_DOUBLE, d);
  double pitch = d[0];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);


  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif

  status = H5Fclose(file);

  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  sp->refine(xs, 1e-8);
  del_vector(xs);
  #else
  sp->compute(x);
  sp->refine(x, 1e-8);
  #endif

  int estModelOrder = sp->model_order_selection(x);

  // Control the Bayes Factors
  
  FTYPE lnBF[] = {22.976505179621622, 22.444499748782523, 423.7940152029966, 
                  412.11543962457455, 400.92575920553287};

  for( int k = 1 ; k <= L ; ++k )
    ASSERT_NEAR(lnBF[k-1], sp->lnBF[k], EPS2)  
      << "Bayes Factor differs at order = " << k ;

  // With this realization the model order estimation should work
  ASSERT_EQ(modelOrder, estModelOrder);

  FTYPE omega_0h = sp->est(x, 20.0, 1e-6);
  estModelOrder = sp->modelOrder();
  ASSERT_NEAR(omega_0h/(2*M_PI), pitch, 1e-6);
  ASSERT_EQ(modelOrder, estModelOrder);

  delete sp;
  del_vector(x);
}

TEST_F(RealDataTest, ModelOrderSelectionNoiseBayesRefinementNewOrder) {

  hid_t file;
  herr_t status;

  /* extract data and output */
  file = H5Fopen("data_files/unittest7.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  
  double d[1];
  H5LTread_dataset(file, "/nData", H5T_NATIVE_DOUBLE, d);
  int N = (int) d[0];

  H5LTread_dataset(file, "/L", H5T_NATIVE_DOUBLE, d);
  int L = (int) d[0];

  H5LTread_dataset(file, "/F", H5T_NATIVE_DOUBLE, d);
  int F = (int) d[0];

  H5LTread_dataset(file, "/modelOrder", H5T_NATIVE_DOUBLE, d);
  int modelOrder = (int) d[0];

  H5LTread_dataset(file, "/pitch", H5T_NATIVE_DOUBLE, d);
  double pitch = d[0];

  double * x = vector(N);
  H5LTread_dataset(file, "/data", H5T_NATIVE_DOUBLE, x);


  double pitch_bounds[2];
  H5LTread_dataset(file, "/pitchBounds", H5T_NATIVE_DOUBLE, pitch_bounds);
  #ifdef DOUBLE
  double * pb = pitch_bounds;
  #else
  float pb[2];
  pb[0] = (float)pitch_bounds[0]; pb[1] = (float)pitch_bounds[1];
  #endif

  status = H5Fclose(file);

  /* construct object and compute  */
  single_pitch * sp = new single_pitch(L, F, N, pb);

  #ifdef SINGLE
  float * xs = vector(N);
  for(int l = 0 ; l < N ; ++l)
    xs[l] = (float)x[l];
  sp->compute(xs);
  sp->refine(xs, 1e-8);
  del_vector(xs);
  #else
  sp->compute(x);
  sp->refine(x, 1e-8);
  #endif
  
  FTYPE omega_0h = sp->est_fast(x, 0.0, 1e-3);
  int estModelOrder = sp->modelOrder();
  ASSERT_NEAR(omega_0h/(2*M_PI), pitch, 1e-3); 
  ASSERT_EQ(modelOrder, estModelOrder);

  delete sp;
  del_vector(x);
}

int main(int argc, char **argv){
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}




<|start_filename|>cpp/unittest/test_th.cpp<|end_filename|>
#include "../src/th.hpp"
#include "gtest/gtest.h"
#include <stdlib.h>
#include <math.h>


#ifdef DOUBLE
#define FTYPE double 
#else
#define FTYPE float
#endif


class RealTest : public ::testing::Test{
protected:

  RealTest(){
  }

  virtual ~RealTest(){
  }

  virtual void SetUp(){
  }

  virtual void TearDown(){
  }
};

TEST_F(RealTest, th) {


  int N = 3;

  FTYPE t[] = { 0.2548401 , -1.19102012,  0.93416293, -0.37818981};
  FTYPE h[] = { -0.18860475, -0.33219973,  0.35968834, 
                -2.22949682, -0.3830574, 1.13525328,  0.37186914};

  FTYPE *gammah;
  int K = ((N+2)*(N+1))/2;
  gammah = vector(K);

  FTYPE gamma[] = { 15.0976782, -0.66822683, -0.02905703, 0.36881507, 
                    -0.14570701, -0.19041787, -0.38019016, -0.15277086, 
                    0.16812068,  0.16299686};

  th(N, t, h, gammah);


  for(int i = 0 ; i < K ; ++i){
    ASSERT_NEAR(gamma[i], gammah[i], 1e-8) << "Vectors differ at index " << i;
  }

  del_vector(gammah);
}


TEST_F(RealTest, thp) {


  int N = 3;

  FTYPE t[] = { 0.2548401 , -1.19102012,  0.93416293, -0.37818981};
  FTYPE h[] = { 0.93416293, -0.37818981,  0.35968834, -2.22949682, 
                -0.3830574, 1.13525328,  0.37186914};

  FTYPE *gammah;
  int K = ((N+2)*(N+1))/2;
  gammah = vector(K);

  FTYPE gamma[] = { -1.47205416, -1.37892347,  1.15243512, 1.35466476, 
                    -0.41218666,  1.01869934, 0.52769438,  0.22693268, 
                    0.10547072,  0.26054153};

  for(int k = 0; k < 2*N+1 ; ++k)
    h[k] = -h[k];

  thp(N, t, h, gammah);


  for(int i = 0 ; i < K ; ++i){
    ASSERT_NEAR(gamma[i], gammah[i], 1e-8) << "Vectors differ at index " << i;
  }

  del_vector(gammah);
}

TEST_F(RealTest, solve) {


  int N = 3;

  FTYPE t[] = { 0.2548401 , -1.19102012,  0.93416293, -0.37818981};
  FTYPE h[] = { -0.18860475, -0.33219973,  0.35968834, 
                -2.22949682, -0.3830574, 1.13525328,  0.37186914};
  FTYPE f[] = {-0.13876892,  0.1205414, -1.00767956,  0.05949563};
  FTYPE x[] = { 0.0107896, 0.3009554, -0.00617545, -0.12537081};
  FTYPE * xh = vector(N+1);
    
  solve(N, t, h, f, NULL, xh);

  for(int i = 0 ; i < N + 1 ; ++i){
    ASSERT_NEAR(x[i], xh[i], 1e-8) << "Vectors differ at index " << i;
  }

  del_vector(xh);
}


int main(int argc, char **argv){
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


<|start_filename|>cpp/src/th.cpp<|end_filename|>
/*
Methods for computing the solution to a linear system with coefficient matrix

   R = T + H

where T is symmetric Toeplitz and H is Hankel

Based on:

<NAME> and <NAME>
Efficient Algorithms for Toeplitz Plus Hankel Matrices
Integral Equations and Operator Theory
Vol. 12
1989

<NAME>
March 2015
*/
#include "th.hpp"


void solve(int N, FTYPE *t, FTYPE *h, FTYPE *f, FTYPE *gam, FTYPE *x){
  FTYPE *tp, *t1, lambda_k, zero = 0.0, *gamma;
  int K = ((N+2)*(N+1))/2;

  tp = vector(N+1);
  t1 = vector(N+1);

  if(gam==NULL){
    gamma = vector(K);
    th(N, t, h, gamma);
  }
  else
    gamma = gam;

  
  CvmdReverse(N+1, t, tp);
  CvmdInit(N+1, zero, x);
  
  x[0] = f[0]/(t[0] + h[0]);
  
  for(int k = 1 ; k < N+1 ; ++k){
    CvmdAdd(k, tp+N-k, h+k, t1);
    lambda_k = f[k] - CvmdDot(k, t1, x);

    /*  x[:k+1] += lambda_k*gamma[k] */
    CvmdAxpy(k+1, lambda_k, gamma+((k+1)*k)/2, x);
  }

  del_vector(tp);
  del_vector(t1);

  if(gam == NULL)
    del_vector(gamma);
}


void th(int N, FTYPE *t, FTYPE *h, FTYPE *gamma){

  FTYPE *a, *tp, *alpha, *phi_k, *psi_k, *t1, *r_kp1, *b_k;
  FTYPE zero = 0.0, R00, R01, R11, s, lambda_k, mu_k, nu_kp1, gammap;
  int K, Kp1;

  // test two special cases 
  if( N == 0 ){
    R00 = t[0] + h[0];
    gamma[0] = 1/R00;
    return;
  }
  if( N == 1 ){    
    R00 = t[0] + h[0];
    R01 = t[1] + h[1];
    R11 = t[0] + h[2];
  
    gamma[0] = 1/R00;

    s = 1/(R00*R11 - R01*R01);
    gamma[1] = -s*R01;
    gamma[2] = s*R00;
    return;
  }

  /* allocate */
  a= vector(N); 
  tp = vector(N+1);
  alpha = vector(N+1);
  phi_k = vector(N+1);
  psi_k = vector(N+1);
  t1 = vector(N+1);
  r_kp1 = vector(N+1);
  b_k = vector(N+1);

  CvmdShift(N, t+1, h, a);
  CvmdReverse(N+1, t, tp);

  CvmdInit(N+1, zero, phi_k);
  CvmdInit(N+1, zero, psi_k);
  CvmdInit(N+1, zero, alpha);

  R00 = t[0] + h[0];
  R01 = t[1] + h[1];
  R11 = t[0] + h[2];
  psi_k[0] = 1/R00;
  
  gamma[0] = 1/R00;
  phi_k[0] = a[0]/R00;
  alpha[1] = R01/R00;

  s = 1/(R00*R11 - R01*R01);
  gamma[1] = -s*R01;
  gamma[2] = s*R00;

  Kp1 = 1;

  for(int k = 1 ; k < N ; ++k){

    K = Kp1;
    Kp1 = (k+2)*(k+1)/2;

    /* lambda_k = a[k] - np.dot(tp[N-k:N]+h[k:2*k], phi_k[:k]) */
    CvmdAdd(k, tp+N-k, h+k, t1);
    lambda_k = a[k] - CvmdDot(k, t1, phi_k);
    
    /* mu_k = - np.dot(tp[N-k:N]+h[k:2*k], psi_k[:k]) */
    mu_k = -CvmdDot(k, t1, psi_k);

    /* phi_k[:k+1] += lambda_k*gamma[k] */
    CvmdAxpy(k+1, lambda_k, gamma+K, phi_k);

    /* psi_k[:k+1] += mu_k*gamma[k] */
    CvmdAxpy(k+1, mu_k, gamma+K, psi_k);

    /*r_kp1 = tp[N-k-1:N] + h[k+1:2*k+2]*/
    CvmdAdd(k+1, tp + N - k - 1, h + k + 1, r_kp1);
    
    /* alpha[k+1] = np.dot(tp[N-k-1:N]+h[k+1:2*(k+1)], gamma[k]) */
    alpha[k+1] = CvmdDot(k+1, r_kp1, gamma + K);

    /* b_k = ((alpha[k] - alpha[k+1])*gamma[k] + np.hstack([0, gamma[k][:k]])
               + np.hstack([gamma[k][1:k+1], 0])
               -np.hstack([gamma[k-1], 0])
               + psi_k[k]*phi_k[:k+1] - phi_k[k]*psi_k[:k+1]) */
    CvmdInit(k+1, 0.0, b_k);
    CvmdAxpy(k+1, alpha[k]-alpha[k+1], gamma + K, b_k);
    CvmdAxpy(k, 1.0, gamma + K, b_k + 1);
    CvmdAxpy(k, 1.0, gamma + K + 1, b_k);
    CvmdAxpy(k, -1.0, gamma + ((k)*(k-1))/2, b_k);
    CvmdAxpy(k, psi_k[k], phi_k, b_k);
    CvmdAxpy(k, -phi_k[k], psi_k, b_k);

    /* nu_kp1 = (1/gamma[k][k]) * np.dot(tp[N-k-1:N]+h[k+1:2*(k+1)], b_k) */
    nu_kp1 = (1/gamma[K + k]) 
      * CvmdDot(k+1, r_kp1, b_k);

    gammap = 1.0/(nu_kp1 + tp[N] + h[2*k+2]);

    /* gamma.append(np.hstack([(gammap/gamma[k][k])*b_k, gammap])) */
    CvmdScal(k+1, gammap/gamma[K + k], b_k);
    CvmdCopy(k+1, b_k, gamma + Kp1);
    gamma[Kp1 + k + 1] = gammap;
  }

  /* de-allocate */
  del_vector(a);
  del_vector(tp);
  del_vector(alpha);
  del_vector(phi_k);
  del_vector(psi_k);
  del_vector(t1);
  del_vector(r_kp1);
  del_vector(b_k);
}


void thp(int N, FTYPE *t, FTYPE *h, FTYPE *gamma){

  FTYPE *tp, *alpha, *r_kp1, *b_k;
  FTYPE zero = 0.0, R00, R01, R11, s, nu_kp1, gammap;
  int K, Kp1;

  // test two special cases 
  if( N == 0 ){
    R00 = t[0] + h[0];
    gamma[0] = 1/R00;
    return;
  }
  if( N == 1 ){    
    R00 = t[0] + h[0];
    R01 = t[1] + h[1];
    R11 = t[0] + h[2];
  
    gamma[0] = 1/R00;

    s = 1/(R00*R11 - R01*R01);
    gamma[1] = -s*R01;
    gamma[2] = s*R00;
    return;
  }

    
  /* allocate */
  tp = vector(N+1);
  alpha = vector(N+1);
  r_kp1 = vector(N+1);
  b_k = vector(N+1);

  CvmdReverse(N+1, t, tp);

  CvmdInit(N+1, zero, alpha);

  R00 = t[0] + h[0];
  R01 = t[1] + h[1];
  R11 = t[0] + h[2];
  
  gamma[0] = 1/R00;
  alpha[1] = R01/R00;

  s = 1/(R00*R11 - R01*R01);
  gamma[1] = -s*R01;
  gamma[2] = s*R00;

  Kp1 = 1;

  for(int k = 1 ; k < N ; ++k){

    K = Kp1;
    Kp1 = (k+2)*(k+1)/2;

    /*r_kp1 = tp[N-k-1:N] + h[k+1:2*k+2]*/
    CvmdAdd(k+1, tp + N - k - 1, h + k + 1, r_kp1);
    
    /* alpha[k+1] = np.dot(tp[N-k-1:N]+h[k+1:2*(k+1)], gamma[k]) */
    alpha[k+1] = CvmdDot(k+1, r_kp1, gamma + ((k+1)*k)/2);

    /* b_k = ((alpha[k] - alpha[k+1])*gamma[k] + np.hstack([0, gamma[k][:k]])
               + np.hstack([gamma[k][1:k+1], 0])
               -np.hstack([gamma[k-1], 0])
                */
    CvmdInit(k+1, 0.0, b_k);
    CvmdAxpy(k+1, alpha[k]-alpha[k+1], gamma + ((k+1)*k)/2, b_k);
    CvmdAxpy(k, 1.0, gamma + K, b_k + 1);
    CvmdAxpy(k, 1.0, gamma + K + 1, b_k);
    CvmdAxpy(k, -1.0, gamma + ((k)*(k-1))/2, b_k);

    /* nu_kp1 = (1/gamma[k][k]) * np.dot(tp[N-k-1:N]+h[k+1:2*(k+1)], b_k) */
    nu_kp1 = (1/gamma[K + k]) 
      * CvmdDot(k+1, r_kp1, b_k);

    gammap = 1.0/(nu_kp1 + tp[N] + h[2*k+2]);

    /* gamma.append(np.hstack([(gammap/gamma[k][k])*b_k, gammap])) */
    CvmdScal(k+1, gammap/gamma[K + k], b_k);
    CvmdCopy(k+1, b_k, gamma + Kp1);
    gamma[Kp1 + k + 1] = gammap;
  }

  /* de-allocate */
  del_vector(tp);
  del_vector(alpha);
  del_vector(r_kp1);
  del_vector(b_k);
}


<|start_filename|>cpp/src/single_pitch.hpp<|end_filename|>
#ifndef __SINGLE_PITCH_H__
#define __SINGLE_PITCH_H__


#include <math.h>
#include <complex.h>
#include <limits>

#ifdef MKL
#include "fftw3.h"
#else
#include <fftw3.h>
#endif

#include "tools.hpp"
#include "th.hpp"
#include "vector.hpp"

#ifdef REGULARIZE
#define T0_REG 1e-8
#else
#define T0_REG 0.0
#endif

#define DIM int


class single_pitch{
public:
  single_pitch(int maxModelOrder, int nFftGrid, int nData, 
                 FTYPE * pitchBounds);

  ~single_pitch();

  void compute(FTYPE * x);
  void update_gamma(int iOrder, int nPitches, int nPitchesOld, 
                    int nPitchesOldOld, FTYPE * phi, 
                    FTYPE * psi, FTYPE * alpha, FTYPE * lambda, 
                    FTYPE * mu, FTYPE * Gamma);

  void update_gamma_p(int iOrder, int nPitches, int nPitchesOld, 
                      int nPitchesOldOld, FTYPE * alpha, FTYPE * Gamma);

  void update_ls_sol(int iOrder, int nPitches, int nPitchesOld, 
                     bool add, FTYPE * dftData, 
                     FTYPE * Gamma, FTYPE * lsSol);

  void compute_cc();

  FTYPE compute_obj(FTYPE omega, FTYPE * x, int iOrder);
  FTYPE compute_obj(FTYPE omega, FTYPE * x, int iOrder, FTYPE * ac, FTYPE * as);

  FTYPE * refine(FTYPE * x);
  FTYPE * refine(FTYPE * x, FTYPE eps);
  FTYPE refine_single(FTYPE *x, int iOrder, FTYPE eps);
  void compute_max_on_grid(void);
    
  FTYPE golden(FTYPE * x, int iOrder, FTYPE omega_L, FTYPE omega_U, FTYPE eps);

  int model_order_selection(FTYPE * x);
  int model_order_selection(FTYPE * x, FTYPE lnBFZeroOrder);

  FTYPE est(FTYPE * x);
  FTYPE est(FTYPE * x, FTYPE lnBFZeroOrder, FTYPE eps);
  FTYPE est_fast(FTYPE * x, FTYPE lnBFZeroOrder, FTYPE eps);

  FTYPE max_obj(int iOrder);
  FTYPE argmax_obj(int iOrder);

  FTYPE * Gamma1;
  FTYPE * Gamma2;
  FTYPE ** costFunctionMatrix;
  FTYPE * omega_0h;
  FTYPE * lnBF;

  int nPitches(int iOrder){ return nPitchesAll[iOrder-1];};
  int modelOrder(){return estOrder;};

private:
  
  int maxFftIndexOld;
//int M, N, L, F;
  int maxModelOrder, nData, nFftGrid;
  int estOrder;

  int t;
  int k;
  int M;
  int Mp;
  int minFftIndex;

  FTYPE pitchBounds[2];

  int * nPitchesAll;

  FTYPE ** crossCorrelationVectors;
  FTYPE ** tf;
  FTYPE ** hf;

  FTYPE * dftData1;
  FTYPE * dftData2;
  FTYPE * x;
  FTYPE * y;
  FTYPE * yt;

  FTYPE * t1;
  FTYPE * t2;
  FTYPE * t3;
  FTYPE * t4;
  FTYPE * tp;
 
  FTYPE * fftShiftVector;
  FTYPE * lsSol1;
  FTYPE * lsSol2;

  FTYPE * range;

  FTYPE * xp;
  FTYPE * dftData;
  FFTW_PLAN p1;
};
  

#ifdef LIB
// The class is wrapped using these C-style functions
extern "C"{

  single_pitch * single_pitch_new(int maxModelOrder, int nFftGrid, 
                                  int nData, FTYPE * pitchBounds){
    return new single_pitch(maxModelOrder, nFftGrid, 
                            nData, pitchBounds);
  }
  
  FTYPE single_pitch_est(single_pitch * sp, FTYPE * x, FTYPE lnBFZeroOrder, FTYPE eps){
    return sp->est(x, lnBFZeroOrder, eps);
  }

  FTYPE single_pitch_est_fast(single_pitch * sp, FTYPE * x, FTYPE lnBFZeroOrder, FTYPE eps){
    return sp->est_fast(x, lnBFZeroOrder, eps);
  }
  
  void single_pitch_del(single_pitch * sp){delete sp;}

  int single_pitch_model_order(single_pitch * sp){return sp->modelOrder();}
}
#endif

#endif


<|start_filename|>cpp/unittest/Makefile<|end_filename|>

# Mandatory 
GTEST = /usr/lib/
HDF5 = /usr/lib/x86_64-linux-gnu/hdf5/serial

# Optional - but remove link option -DMKL
MKLROOT = /opt/intel

EXTLIB = -L$(HDF5)
INCLUDE = -I$(HDF5)/include
LIBSHDF = $(EXTLIB) -lhdf5_hl -lhdf5 -lz 

#Dynamic linking
MKL = -m64 -I$(MKLROOT)/mkl/include -I$(MKLROOT)/mkl/include/fftw -L$(MKLROOT)/mkl/lib/intel64 -L$(MKLROOT)/compilers_and_libraries/linux/lib/intel64_lin -lmkl_intel_lp64 -lmkl_intel_thread -lmkl_core -liomp5 -lpthread -lm -DALIGNMENT32

FTYPE = -DDOUBLE

#LINK =  -DMKL $(MKL) -DCHEBYSHEV
#LINK = -DMKL $(MKL)
LINK = -lfftw3 -DCHEBYSHEV

all:
	g++ -g -fno-inline -Wno-write-strings $(FTYPE) $(INCLUDE) -isystem $(GTEST)/include ../src/single_pitch.cpp ../src/tools.cpp ../src/th.cpp ../src/vector.cpp test.cpp $(GTEST)/libgtest.a $(INCLUDE) $(LIBSHDF) $(LINK) -pthread -o test

	g++ -g -Wno-write-strings -DDOUBLE -isystem $(GTEST)/include ../src/th.cpp ../src/tools.cpp ../src/vector.cpp test_th.cpp $(GTEST)/libgtest.a $(LINK) -pthread -o test_th


<|start_filename|>cpp/src/vector.hpp<|end_filename|>
#ifndef __VECTOR_H__
#define __VECTOR_H__

#include <stdio.h>
#include <stdlib.h>

#include "tools.hpp"

#ifdef ALIGNMENT32
#define ALIGNMENT 32
#endif

#ifdef ALIGNMENT64
#define ALIGNMENT 64
#endif

#ifdef ALIGNMENT128
#define ALIGNMENT 128
#endif

FTYPE * vector(int N);

void del_vector(FTYPE * x);


#endif 


<|start_filename|>cpp/src/single_pitch.cpp<|end_filename|>
#include "single_pitch.hpp"


single_pitch::single_pitch(int L, int F, int N, FTYPE * pB){

  maxModelOrder = L;
  nFftGrid = F;
  nData = N;

  pitchBounds[0] = pB[0];
  pitchBounds[1] = pB[1];

  int iOrder;
  int nPitchesOld=-1;
  int nPitchesOldOld=-1;

  // Compute some values for later
  minFftIndex = (int)ceil(nFftGrid*pitchBounds[0]);
  int maxFftIndex = (int)floor(nFftGrid*pitchBounds[1]);
  int minPitchGrid = minFftIndex;
  int maxPitchGrid = maxFftIndex;

  int nPitches = maxFftIndex - minFftIndex + 1;

  omega_0h = new FTYPE [maxModelOrder];
  lnBF = new FTYPE [maxModelOrder+1]; //include the NULL order
  nPitchesAll = new int [maxModelOrder];
  M = nPitches;
  Mp = nFftGrid/2+1;

  /* allocate */
  Gamma1 = vector(maxModelOrder*Mp); //Could be smaller
  Gamma2 = vector(maxModelOrder*Mp); //Could be smaller

  lsSol1 = vector(Mp);    //Could be smaller
  lsSol2 = vector(Mp);    //Could be smaller


  #ifdef MKL
  t1 = vector(2*Mp); //can be used as a complex array of length Mp
  t2 = vector(Mp); 
  #else
  t1 = vector(Mp);   //Could be smaller
  t2 = vector(Mp);   //Could be smaller
  #endif
  t3 = vector(Mp);   //Could be smaller
  t4 = vector(Mp);   //Could be smaller

  range = vector(Mp);
  for( int k = 0 ; k < Mp; ++k )
    range[k] = (FTYPE) k;

  /* Prepare the dft */
  xp = vector(F);
  CvmdInit(F, 0.0, xp); // make everything zero for zero padding later
  dftData =  (FTYPE*)FFTW_MALLOC(sizeof(FFTW_COMPLEX)*F);
  p1 = FFTW_PLAN_DFT_R2C_1D(F, xp, (FFTW_COMPLEX*)dftData, FFTW_MEASURE);

  /*
  fftShiftVector = ...
    exp(1i*2*pi*(0:ceil(nFftGrid/2)-1)'*(nData-1)/(2*nFftGrid));
    dftData = X(1:ceil(nFftGrid/2)).*fftShiftVector;
  */
  // just using Mp = nFftGrid/2 + 1 would be sufficient
  fftShiftVector = vector(2*Mp);

  CvmdExpRange(nFftGrid/2+1, range, 
               (2*M_PI*(nData-1))/(2*nFftGrid), 
               t1, t2, fftShiftVector);

  dftData1  = vector(Mp);
  dftData2 = vector(Mp);

  /* Compute crossCorrelationVectors */
  compute_cc();
  
  /* precompute gamma */
  FTYPE * phi1 = vector(Mp);    // nPitches at iOrder = 1 is sufficient
  FTYPE * psi1 = vector(Mp);    //Could be smaller
  FTYPE * alpha1 = vector(Mp);  //Could be smaller
  FTYPE * alpha2 = vector(Mp);  //Could be smaller
  FTYPE * lambda1 = vector(Mp); //Could be smaller
  FTYPE * mu1 = vector(Mp);     //Could be smaller

  
  costFunctionMatrix = new FTYPE*[maxModelOrder]; 

  for( iOrder = 1 ; iOrder <= maxModelOrder ; ++iOrder){
    //precompute some quantities
    maxFftIndexOld = maxFftIndex;
    maxFftIndex = (int)floor(MIN(nFftGrid*pitchBounds[1], 
                                 ((FTYPE)nFftGrid)/(2*iOrder)-1));

    nPitchesOldOld = nPitchesOld;
    nPitchesOld = nPitches;
    nPitches = nPitches - (maxFftIndexOld - maxFftIndex);
    nPitchesAll[iOrder-1] = nPitches;

    maxPitchGrid = minPitchGrid + nPitches -1;

    //allocate for special sized structure
    costFunctionMatrix[iOrder-1] = vector(nPitches);
    
    

    update_gamma(iOrder, nPitches, nPitchesOld, nPitchesOldOld,
                 phi1, psi1, alpha1, lambda1, 
                 mu1, Gamma1);

    update_gamma_p(iOrder, nPitches, nPitchesOld, nPitchesOldOld, 
                   alpha2, Gamma2);
    
  }//end of iOrder for-loop

  del_vector(phi1);
  del_vector(psi1);

  del_vector(alpha1);
  del_vector(alpha2);
  del_vector(lambda1);
  del_vector(mu1);

}


void single_pitch::compute_cc(){

  int maxFftIndex = (int)floor(nFftGrid*pitchBounds[1]);
  int minPitchGrid = minFftIndex;
  int maxPitchGrid = maxFftIndex;

  int nPitches = maxFftIndex - minFftIndex + 1;

  int iOrder = 0; 
  int k;

  crossCorrelationVectors = new FTYPE*[2*(maxModelOrder+1)+1];

  crossCorrelationVectors[0] = vector(nPitches);
  CvmdInit(nPitches, (1+T0_REG)*nData*0.5, crossCorrelationVectors[0]);

  for( int k = 1 ; k < 2*(maxModelOrder+1) + 1; k += 2 ){
    //precompute some quantities
    iOrder++;
    int maxFftIndexOld = maxFftIndex;
    maxFftIndex = (int)floor(MIN(nFftGrid*pitchBounds[1], 
                                 ((FTYPE)nFftGrid)/(2*iOrder)-1));
    
    nPitches = nPitches - (maxFftIndexOld - maxFftIndex);
    
    maxPitchGrid = minPitchGrid + nPitches -1;

    crossCorrelationVectors[k] = vector(nPitches);
    #ifdef MKL
    CvmdSinRange(nPitches, range+minPitchGrid, 
                 (M_PI*nData*k)/nFftGrid, t4, t1);
    CvmdSinRange(nPitches, range+minPitchGrid, 
                 (M_PI*k)/nFftGrid, t4, t2);
    #else
    CvmdSinRange(minPitchGrid, maxPitchGrid, 
                 (M_PI*nData*k)/nFftGrid, t1);
    CvmdSinRange(minPitchGrid, maxPitchGrid, 
                 (M_PI*k)/nFftGrid, t2);
    #endif

    
    CvmdScal(nPitches, 0.5, t1);
    CvmdDiv(nPitches, t1, t2, crossCorrelationVectors[k]);

    crossCorrelationVectors[k+1] = vector(nPitches);
    #ifdef MKL
    CvmdSinRange(nPitches, range+minPitchGrid, 
                 (M_PI*nData*(k+1))/nFftGrid, t4, t1);
    CvmdSinRange(nPitches, range+minPitchGrid, 
                 (M_PI*(k+1))/nFftGrid, t4, t2);
    #else
    CvmdSinRange(minPitchGrid, maxPitchGrid, 
                 (M_PI*nData*(k+1))/nFftGrid, t1);
    CvmdSinRange(minPitchGrid, maxPitchGrid, 
                 (M_PI*(k+1))/nFftGrid, t2);
    #endif
    CvmdScal(nPitches, 0.5, t1);
    CvmdDiv(nPitches, t1, t2, crossCorrelationVectors[k+1]);
    
  }

  hf = new FTYPE*[2*(maxModelOrder)];
  tf = new FTYPE*[2*(maxModelOrder)];

  for( k = 0; k < 2*maxModelOrder ; ++k){
    tf[k] = crossCorrelationVectors[k];
    hf[k] = crossCorrelationVectors[k+2];
  }
  
}


void single_pitch::compute(FTYPE * x){

  /* Initialize */
  int nPitchesOld=-1;
  int iOrder;

  int maxFftIndex = (int)floor(nFftGrid*pitchBounds[1]);
  int minPitchGrid = minFftIndex;
  int maxPitchGrid = maxFftIndex;
  int nPitches = maxFftIndex - minFftIndex + 1;

  /* The dft of the data */
  CvmdCopy(nData, x, xp);
  FFTW_EXECUTE(p1); //the result is in dftData
  
  CvmdMulz(nFftGrid/2+1, fftShiftVector, dftData, t1, dftData1, dftData2);
  //dftData1 and dftData2 now contains the real and imaginary part
  //of the time shifted dft coefficients (phase rotation in frequency)


  for( iOrder = 1 ; iOrder <= maxModelOrder ; ++iOrder ){
    
    //Update the considered frequency range
    maxFftIndexOld = maxFftIndex;
    maxFftIndex = (int)floor(MIN(nFftGrid*pitchBounds[1], 
                                 ((FTYPE)nFftGrid)/(2*iOrder)-1));

    nPitchesOld = nPitches;
    nPitches = nPitches - (maxFftIndexOld - maxFftIndex);

    maxPitchGrid = minPitchGrid + nPitches -1;
    
    /*
    dftDataMatrix1(iOrder, validPitchIndices) = ...
            real(dftData(validFftIndices*iOrder+1))';
    dftDataMatrix2(iOrder, validPitchIndices) = ...
            imag(dftData(validFftIndices*iOrder+1))';
    No longer saves dftDataMatrix1/dftDataMatrix2 explicit
       just get pulled out of dftData1 and dftData2
    */
    
    if( iOrder == 1 ){
      /*
        lsSol1 = dftDataMatrix1(iOrder, validPitchIndices).*gamma1;
        lsSol2 = dftDataMatrix2(iOrder, validPitchIndices).*gamma2;
      */
      CvmdPack(nPitches, dftData1 + minFftIndex*iOrder,
               iOrder, t3);
      CvmdMul(nPitches, t3, Gamma1, lsSol1);

      CvmdPack(nPitches, dftData2 + minFftIndex*iOrder,
               iOrder, t3);
      CvmdMul(nPitches, t3, Gamma2, lsSol2);
    }
    else{
      CvmdPack(nPitches, dftData1 + minFftIndex*iOrder,
               iOrder, t3); //update_ls_sol does not use t3
      update_ls_sol(iOrder, nPitches, nPitchesOld, true, 
                    t3, Gamma1, lsSol1);

      CvmdPack(nPitches, dftData2 + minFftIndex*iOrder,
               iOrder, t3); //update_ls_sol does not use t3
      update_ls_sol(iOrder, nPitches, nPitchesOld, false, 
                    t3, Gamma2, lsSol2);

    }

    /* Compute the cost function
       costFunctionMatrix(iOrder,validPitchIndices) = ...
       sum([dftDataMatrix1(1:iOrder, validPitchIndices);...
       dftDataMatrix2(1:iOrder, validPitchIndices)].* ...
       [lsSol1;lsSol2], 1); 
    */

    CvmdInit(nPitches, 0.0, t4);
    for( k = 0 ; k < iOrder  ; k++){
      CvmdPack(nPitches, dftData1 + minFftIndex*(k+1), k+1, t2);
      CvmdMul(nPitches, t2, lsSol1 + k*nPitches, t1);
      CvmdAddInplace(nPitches, t4, t1);

      CvmdPack(nPitches, dftData2 + minFftIndex*(k+1), k+1, t2);
      CvmdMul(nPitches, t2, lsSol2 + k*nPitches, t1);
      CvmdAddInplace(nPitches, t4, t1);
    }
    CvmdCopy(nPitches, t4, costFunctionMatrix[iOrder-1]);
  }
}


single_pitch::~single_pitch(){
  
  delete [] nPitchesAll;

  del_vector(dftData1);
  del_vector(dftData2);

  del_vector(t1);
  del_vector(t2);
  del_vector(t3);
  del_vector(t4);

  del_vector(xp);
  FFTW_DESTROY_PLAN(p1);
  FFTW_FREE(dftData);

  for(int k = 0 ; k < 2*(maxModelOrder +1) + 1; ++k){
    del_vector(crossCorrelationVectors[k]);
  }
  delete [] crossCorrelationVectors;

  delete [] hf;
  delete [] tf;
    
  del_vector(Gamma1);
  del_vector(Gamma2);
  
  for(int k = 0 ; k < maxModelOrder; ++k){
    del_vector(costFunctionMatrix[k]);
  }
  delete [] costFunctionMatrix;

  del_vector(lsSol1);
  del_vector(lsSol2);

  del_vector(fftShiftVector);

  delete [] omega_0h;
  delete [] lnBF;

  del_vector(range);
}


void single_pitch::update_ls_sol(int iOrder, int nPitches, int nPitchesOld, 
                                 bool add, FTYPE * dftData, 
                                 FTYPE * Gamma, FTYPE * lsSol){

  /*
    lambda= (ones(iOrder,1)*(dftDataMatrix(iOrder, validPitchIndices)-...
    sum(R(1:end-1,:).*lsSol(:, validPitchIndices),1)));
    
    lsSol = [lsSol(:,validPitchIndices);zeros(1,nPitches)]...
    + lambda.*gamma;
  */

  CvmdInit(nPitches, 0.0, t4);
  for( k = 0 ; k < iOrder-1 ; ++k ){

    if(add)
      CvmdAdd(nPitches, tf[iOrder-1-k], hf[k+iOrder-1], t1);
    else
      CvmdSub(nPitches, tf[iOrder-1-k], hf[k+iOrder-1], t1);

    CvmdMul(nPitches, t1, lsSol + k*nPitchesOld, t2);
    CvmdAddInplace(nPitches, t4, t2);
  }
  CvmdSub(nPitches, dftData, t4, t1); 
  //lambda rep is now in t1
  
  CvmdCopy((iOrder-1)*nPitchesOld, lsSol, t4); //could copy less
  for( k = 0 ; k < iOrder - 1 ; ++k ){
    CvmdMul(nPitches, t1, Gamma + Mp*(iOrder-1) + k*nPitches, t2);
    CvmdAdd(nPitches, t4 + k*nPitchesOld, t2, lsSol + k*nPitches);
  }
  CvmdMul(nPitches, t1, Gamma + Mp*(iOrder-1) + (iOrder-1)*nPitches, 
          lsSol + (iOrder-1)*nPitches);  
}


void single_pitch::update_gamma(int iOrder, int nPitches, int nPitchesOld, 
                                int nPitchesOldOld, FTYPE * phi, 
                                FTYPE * psi, FTYPE * alpha, FTYPE * lambda, 
                                FTYPE * mu, FTYPE * Gamma){

    if( iOrder == 1 ){
      CvmdAdd(nPitches, tf[0], hf[0], t1);
      CvmdInverse(nPitches, t1, Gamma);     
      CvmdCopy(nPitches, Gamma, psi);
      CvmdMul(nPitches, tf[1], Gamma, phi);
    }
    else if( iOrder == 2 ){
      CvmdAdd(nPitches, tf[0], hf[0], t1); // R00 = t[0] + h[0];
      CvmdAdd(nPitches, tf[1], hf[1], t2); // R01 = t[1] + h[1];
      CvmdAdd(nPitches, tf[0], hf[2], t3); // R11 = t[0] + h[2];
      CvmdMul(nPitches, t1, t3, t3);
      CvmdMul(nPitches, t2, t2, t4);
      CvmdSubInplace(nPitches, t3, t4);
      CvmdInverse(nPitches, t3, t4);

      CvmdMul(nPitches, t2, Gamma, alpha);

      CvmdScal(nPitches, -1.0, t2);
      CvmdMul(nPitches, t2, t4, Gamma + Mp);
      CvmdMul(nPitches, t1, t4, Gamma + Mp + nPitches);      
    }
    else{
      /* lambda = a-sum(ROld.*phi,1); */
      CvmdInit(nPitches*(iOrder-2), 0.0, t4);
      for(k = 0 ; k < iOrder-2 ; ++k){
        CvmdAdd(nPitches, tf[iOrder-2-k], hf[k+iOrder-2], t1);
        CvmdMul(nPitches, phi + k*nPitchesOld, t1, t2);
        CvmdAddInplace(nPitches, t4, t2);
      }
      CvmdAdd(nPitches, tf[iOrder-1], hf[iOrder-3], t2); //a is now in t2
      CvmdSub(nPitches, t2, t4, lambda);

      /* mu = -sum(ROld.*psi,1); */
      CvmdInit(nPitches*(iOrder-2), 0.0, mu);
      for(k = 0 ; k < iOrder-2 ; ++k){
        CvmdAdd(nPitches, tf[iOrder-2-k], hf[k+iOrder-2], t1);
        CvmdMul(nPitches, psi + k*nPitchesOld, t1, t2);
        CvmdSubInplace(nPitches, mu, t2);
      }


      /* phi = [phi;zeros(1,nPitches)]+(ones(iOrder-1,1)*lambda).*gammaNew; */
      CvmdCopy((iOrder-2)*nPitchesOld, phi, t1); //could copy less
      for(k = 0 ; k < iOrder-2 ; ++k){
        CvmdMul(nPitches, lambda, Gamma + Mp*(iOrder-2) + nPitchesOld*k, t2);
        CvmdAdd(nPitches, t1 + nPitchesOld*k, t2, phi + nPitches*k);
      }
      CvmdMul(nPitches, lambda, 
              Gamma + Mp*(iOrder-2) + nPitchesOld*(iOrder-2), 
              phi + nPitches*(iOrder-2));
      
      /* psi = [psi;zeros(1,nPitches)]+(ones(iOrder-1,1)*mu).*gammaNew; */
      CvmdCopy((iOrder-2)*nPitchesOld, psi, t1); //could copy less
      for(k = 0 ; k < iOrder-2 ; ++k){
        CvmdMul(nPitches, mu, Gamma + Mp*(iOrder-2) + nPitchesOld*k, t2);
        CvmdAdd(nPitches, t1 + nPitchesOld*k, t2, psi + nPitches*k);
      }
      CvmdMul(nPitches, mu, 
              Gamma + Mp*(iOrder-2) + nPitchesOld*(iOrder-2), 
              psi + nPitches*(iOrder-2));

      
      /* alphaNew = sum(RNew(1:end-1,:).*gammaNew,1); */
      CvmdInit(nPitches, 0.0, t1);
      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdAdd(nPitches, tf[iOrder-1-k], hf[k + iOrder-1], t2);
        CvmdMul(nPitches, t2, Gamma + Mp*(iOrder-2) + nPitchesOld*k, t3);
        CvmdAddInplace(nPitches, t1, t3);
      }
      
      /*     b = (ones(iOrder-1,1)*(alphaOld-alphaNew)).*gammaNew+...
             [zeros(1,nPitches);gammaNew(1:iOrder-2,:)]+...
             [gammaNew(2:end,:);zeros(1,nPitches)]-...
             [gammaOld(1:iOrder-2,:);zeros(1,nPitches)]+...
             (ones(iOrder-1,1)*psi(end,:)).*phi-...
             (ones(iOrder-1,1)*phi(end,:)).*psi;
      */
      CvmdSub(nPitches, alpha, t1, t2);
      //make alpha1 -> alphaOld in the next iteration
      CvmdCopy(nPitches, t1, alpha);

      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdMul(nPitches, t2, Gamma + Mp*(iOrder-2) + nPitchesOld*k, 
                t3 + k*nPitches);
        if(k != 0){
          CvmdAddInplace(nPitches, t3 + k*nPitches, 
                         Gamma + Mp*(iOrder-2) + nPitchesOld*(k-1));
        }
        if(k != iOrder - 2){
          CvmdAddInplace(nPitches, t3 + k*nPitches, 
                         Gamma + Mp*(iOrder-2) + nPitchesOld*(k+1));

          CvmdSubInplace(nPitches, t3 + k*nPitches, 
                         Gamma + Mp*(iOrder-3) + nPitchesOldOld*k);
        }
        CvmdMul(nPitches, psi + nPitches*(iOrder-2), phi + nPitches*k, t1);
        CvmdAddInplace(nPitches, t3 + k*nPitches, t1);
        CvmdMul(nPitches, phi + nPitches*(iOrder-2), psi + nPitches*k, t1);
        CvmdSubInplace(nPitches, t3 + k*nPitches, t1);
      }
      // b is now in t3

      /* nu = sum(RNew(1:end-1,:).*b)./gammaNew(end,:); */
      CvmdInit(nPitches, 0.0, t1);
      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdAdd(nPitches, tf[iOrder-1-k], hf[k + iOrder-1], t2);
        CvmdMul(nPitches, t2, t3 + k*nPitches, t4);
        CvmdAddInplace(nPitches, t1, t4);
      }
      CvmdDiv(nPitches, t1, Gamma + Mp*(iOrder-2) + nPitchesOld*(iOrder-2), t2);
      //nu is now in t2

      /*      gammaOld = gammaNew;
              gammaNew = nan(iOrder,nPitches);
              gammaNew(iOrder,:) = 1./(nu+RNew(iOrder,:));
              gammaNew(1:iOrder-1,:) = (ones(iOrder-1,1)*...
                       (gammaNew(iOrder,:)./gammaOld(end,:))).*b;
      */
      CvmdAdd(nPitches, tf[0], hf[2*(iOrder-1)], t1);
      CvmdAddInplace(nPitches, t2, t1);
      CvmdInverse(nPitches, t2, Gamma + Mp*(iOrder-1) + nPitches*(iOrder-1));
      CvmdDiv(nPitches, Gamma + Mp*(iOrder-1) + nPitches*(iOrder-1), 
              Gamma + Mp*(iOrder-2) + nPitchesOld*(iOrder-2), t4);
      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdMul(nPitches, t3 + nPitches*k, t4, 
                Gamma + Mp*(iOrder-1) + nPitches*k);
      }      
    }
}

/*
Implements a special version for the system
 
   R = T - H

where 
  
  t_2 = h_0, t_3 = h_1, ....

  In this case phi, psi, mu, lambda and a can be left out.
*/

void single_pitch::update_gamma_p(int iOrder, int nPitches, int nPitchesOld, 
                                  int nPitchesOldOld,  FTYPE * alpha, 
                                  FTYPE * Gamma){

    if( iOrder == 1 ){
      CvmdSub(nPitches, tf[0], hf[0], t2);
      CvmdInverse(nPitches, t2, Gamma);
    }
    else if( iOrder == 2 ){
      CvmdSub(nPitches, tf[0], hf[0], t1);     // R00 = t[0] - h[0];
      CvmdSub(nPitches, tf[1], hf[1], t2); // R01 = t[1] - h[1];
      CvmdSub(nPitches, tf[0], hf[2], t3); // R11 = t[0] - h[2];
      CvmdMul(nPitches, t1, t3, t3);
      CvmdMul(nPitches, t2, t2, t4);
      CvmdSubInplace(nPitches, t3, t4);
      CvmdInverse(nPitches, t3, t4);

      CvmdMul(nPitches, t2, Gamma, alpha);

      CvmdScal(nPitches, -1.0, t2);
      CvmdMul(nPitches, t2, t4, Gamma + Mp);
      CvmdMul(nPitches, t1, t4, Gamma + Mp + nPitches);
    }
    else{

      /* alphaNew = sum(RNew(1:end-1,:).*gammaNew,1); */
      CvmdInit(nPitches, 0.0, t1);
      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdSub(nPitches, tf[iOrder-1-k], hf[k + iOrder-1], t2);
        CvmdMul(nPitches, t2, Gamma + Mp*(iOrder-2) + nPitchesOld*k, t3);
        CvmdAddInplace(nPitches, t1, t3);
      }
      
      /*     b = (ones(iOrder-1,1)*(alphaOld-alphaNew)).*gammaNew+...
             [zeros(1,nPitches);gammaNew(1:iOrder-2,:)]+...
             [gammaNew(2:end,:);zeros(1,nPitches)]-...
             [gammaOld(1:iOrder-2,:);zeros(1,nPitches)]+...
      */
      CvmdSub(nPitches, alpha, t1, t2);
      //make alpha1 -> alphaOld in the next iteration
      CvmdCopy(nPitches, t1, alpha);

      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdMul(nPitches, t2, Gamma + Mp*(iOrder-2) + nPitchesOld*k, 
                t3 + k*nPitches);
        if(k != 0){
          CvmdAddInplace(nPitches, t3 + k*nPitches, 
                         Gamma + Mp*(iOrder-2) + nPitchesOld*(k-1));
        }
        if(k != iOrder - 2){
          CvmdAddInplace(nPitches, t3 + k*nPitches, 
                         Gamma + Mp*(iOrder-2) + nPitchesOld*(k+1));

          CvmdSubInplace(nPitches, t3 + k*nPitches, 
                         Gamma + Mp*(iOrder-3) + nPitchesOldOld*k);
        }
      }
      // b is now in t3

      /* nu = sum(RNew(1:end-1,:).*b)./gammaNew(end,:); */
      CvmdInit(nPitches, 0.0, t1);
      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdSub(nPitches, tf[iOrder-1-k], hf[k + iOrder-1], t2);
        CvmdMul(nPitches, t2, t3 + k*nPitches, t4);
        CvmdAddInplace(nPitches, t1, t4);
      }
      CvmdDiv(nPitches, t1, Gamma + Mp*(iOrder-2) + nPitchesOld*(iOrder-2), t2);
      //nu is now in t2

      /*      gammaOld = gammaNew;
              gammaNew = nan(iOrder,nPitches);
              gammaNew(iOrder,:) = 1./(nu+RNew(iOrder,:));
              gammaNew(1:iOrder-1,:) = (ones(iOrder-1,1)*...
                       (gammaNew(iOrder,:)./gammaOld(end,:))).*b;
      */
      CvmdSub(nPitches, tf[0], hf[2*(iOrder-1)], t1);
      CvmdAddInplace(nPitches, t2, t1);
      CvmdInverse(nPitches, t2, Gamma + Mp*(iOrder-1) + nPitches*(iOrder-1));
      CvmdDiv(nPitches, Gamma + Mp*(iOrder-1) + nPitches*(iOrder-1), 
              Gamma + Mp*(iOrder-2) + nPitchesOld*(iOrder-2), t4);
      for(k = 0 ; k < iOrder-1 ; ++k){
        CvmdMul(nPitches, t3 + nPitches*k, t4, 
                Gamma + Mp*(iOrder-1) + nPitches*k);
      }      
    }
}


FTYPE single_pitch::compute_obj(FTYPE omega, FTYPE * x, 
                                int iOrder, FTYPE * ac, FTYPE * as){

  FTYPE * ccVectors = vector(2*(iOrder+1));

  /*
    crossCorrelationVectors = 0.5*[nData;...
                    sin((1:2*iOrder+1)'*0.5*omega*nData)./...
                    (sin((1:2*iOrder+1)'*0.5*omega))];
  */
  ccVectors[0] = nData*(1+T0_REG);

  #ifdef MKL
  CvmdSinRange(2*iOrder+1, range+1, 0.5*omega*nData, t4, t1);
  CvmdSinRange(2*iOrder+1, range+1, 0.5*omega, t4, t2);
  #else
  CvmdSinRange(1, 2*iOrder+1, 0.5*omega*nData, t1);
  CvmdSinRange(1, 2*iOrder+1, 0.5*omega, t2);
  #endif
  CvmdDiv(2*iOrder+1, t1, t2, ccVectors + 1);
  CvmdScal(2*(iOrder+1), 0.5, ccVectors);

  /* n = -(nData-1)/2:-(nData-1)/2+nData-1; */
  FTYPE * n = vector(nData);
  CvmdAddConstant(nData, -(nData-1)*0.5, range, n);

  /* C = cos(omega*n'*(1:iOrder));
     S = sin(omega*n'*(1:iOrder));
     
     bc = C'*x;
     bs = S'*x;
  */
  FTYPE * bc = vector(iOrder);
  FTYPE * bs = vector(iOrder);
  
  #ifdef CHEBYSHEV
  /* Implementation using Chebyshev recursive method */
  FTYPE * cn = vector(nData);
  FTYPE * sn = vector(nData);
  FTYPE * cnm1 = vector(nData);
  FTYPE * snm1 = vector(nData);
  FTYPE * cnm2 = vector(nData);
  FTYPE * snm2 = vector(nData);
  FTYPE * tN = vector(nData);
  FTYPE * tw;
  
  CvmdCosSinRange(nData, n, omega, tN, cn, sn);
  bc[0] = CvmdDot(nData, cn, x);
  bs[0] = CvmdDot(nData, sn, x);
  
  if( iOrder > 1 ){
    CvmdInit(nData, 1.0, cnm2);
    CvmdMul(nData, cn, cn, tN);
    CvmdAxpy(nData, -2, tN, cnm2);
    CvmdScal(nData, -1.0, cnm2);
    bc[1] = CvmdDot(nData, cnm2, x);
  
    CvmdMul(nData, cn, sn, snm2);
    CvmdScal(nData, 2, snm2);
    bs[1] = CvmdDot(nData, snm2, x);
  
    if( iOrder > 2 ){
      CvmdCopy(nData, cnm2, cnm1);
      CvmdCopy(nData, snm2, snm1);
      CvmdCopy(nData, cn, cnm2);
      CvmdCopy(nData, sn, snm2);
      for( int l = 3 ; l <= iOrder ; ++l ){
        CvmdMul(nData, cn, cnm1, tN);
        CvmdAxpy(nData, -2, tN, cnm2);
        CvmdScal(nData, -1, cnm2);
        bc[l-1] = CvmdDot(nData, cnm2, x);
        
        CvmdMul(nData, cn, snm1, tN);
        CvmdAxpy(nData, -2, tN, snm2);
        CvmdScal(nData, -1, snm2);
        bs[l-1] = CvmdDot(nData, snm2, x);
        
        /* update using swap of pointers */
        tw = cnm1;
        cnm1 = cnm2;
        cnm2 = tw;
        
        tw = snm1;
        snm1 = snm2;
        snm2 = tw;
      }
    }
  }
  del_vector(cn);
  del_vector(sn);
  del_vector(cnm1);
  del_vector(snm1);
  del_vector(cnm2);
  del_vector(snm2);
  del_vector(tN);
  #else
  // standard impl
  for( int l = 1 ; l <= iOrder ; ++l ){
    CvmdCosSinRange(nData, n, omega*l, t1, t2, t3);
    bc[l-1] = CvmdDot(nData, t2, x);
    bs[l-1] = CvmdDot(nData, t3, x);
  }
  #endif

  /*
    t = crossCorrelationVectors(1:iOrder);
    h = crossCorrelationVectors(3:2*iOrder+1);
  */

  FTYPE * t = ccVectors;
  FTYPE * h = vector(2*(iOrder-1)+1);
  CvmdCopy(2*(iOrder-1)+1, ccVectors + 2, h);

  /*
  T = toeplitz(t);
  H = hankel(h(1:iOrder), h(iOrder:end));
  
  ac = (T + H)\bc; 
  as = (T - H)\bs; 
  */

  /* Solve the system using the TH algorithm */
  int K = ((iOrder+1)*(iOrder))/2;
  FTYPE * gamma = vector(K);

  th(iOrder-1, t, h, gamma);
  solve(iOrder-1, t, h, bc, gamma, ac);
  
  for(int k = 0; k < 2*(iOrder-1)+1 ; ++k)
    h[k] = -h[k];

  thp(iOrder-1, t, h, gamma);
  solve(iOrder-1, t, h, bs, gamma, as);
 
  /* obj = bc'*ac + bs'*as */
  FTYPE obj = CvmdDot(iOrder, bc, ac) + CvmdDot(iOrder, bs, as);

  del_vector(ccVectors);
  del_vector(n);
  del_vector(bc);
  del_vector(bs);
  del_vector(h);
  del_vector(gamma);

  return obj;
}


/*

 Compute the objective at omega, for data x, with length nData 
  and order iOrder
*/
FTYPE single_pitch::compute_obj(FTYPE omega, FTYPE * x, int iOrder){

  FTYPE * ac = new FTYPE [iOrder];
  FTYPE * as = new FTYPE [iOrder];

  FTYPE obj = compute_obj(omega, x, iOrder, ac, as);

  delete [] ac;
  delete [] as;

  return obj;
}


/* returns the maximum objective at order iOrder for the 
  last computed objective
*/
FTYPE single_pitch::max_obj(int iOrder){
  
  FTYPE max = 0;
  FTYPE * obj = costFunctionMatrix[iOrder-1];

  for( int k = 0 ; k < nPitchesAll[iOrder-1] ; ++k){
   max = MAX(obj[k], max);
  }

  return max;
}


/* returns the omega that maximes the objective at order iOrder
   for the lates computed objetive   
*/
FTYPE single_pitch::argmax_obj(int iOrder){
  FTYPE max = 0;
  int arg;
  FTYPE * obj = costFunctionMatrix[iOrder-1];

  for( int k = 0 ; k < nPitchesAll[iOrder-1] ; ++k){
    if( obj[k] > max){
      max = obj[k];
      arg =  k;
    }
  }
  return (2*M_PI*(arg+minFftIndex))/nFftGrid;
}


/* refine of the grid solution that was latest computed 
   returns a pointer to an internal length maxModelOrder array

  eps is the size of of the last interval measured in radian per samples
*/
FTYPE * single_pitch::refine(FTYPE *x, FTYPE eps){
  FTYPE res = 2*M_PI/nFftGrid;
  FTYPE omega, omega_l, omega_u;
  /* eps accuracy could be modified to follow CRLB or % of a note */

  for(int iOrder = 1 ; iOrder <= maxModelOrder ; ++iOrder){
    omega = argmax_obj(iOrder);
    omega_l = omega - res;
    omega_u = omega + res;
    omega_0h[iOrder-1] = golden(x, iOrder, omega_l, omega_u, eps);
  }
  return omega_0h;
}

void single_pitch::compute_max_on_grid(void){

  for(int iOrder = 1 ; iOrder <= maxModelOrder ; ++iOrder){
    omega_0h[iOrder-1] = argmax_obj(iOrder);
  }
}

/* refine of the grid solution that was latest computed 
   returns an estimate in radians per sample

  eps is the size of of the last interval measured in radian per samples
*/
FTYPE single_pitch::refine_single(FTYPE *x, int iOrder, FTYPE eps){
  FTYPE res = 2*M_PI/nFftGrid;
  FTYPE omega, omega_l, omega_u;
  /* eps accuracy could be modified to follow CRLB or % of a note */

  if (iOrder >= 1){
    omega = argmax_obj(iOrder);
    omega_l = omega - res;
    omega_u = omega + res;
    return golden(x, iOrder, omega_l, omega_u, eps);
  }
  return -1;
}

/* refine of the grid solution that was latest computed 
   returns a pointer to an internal length maxModelOrder array

  default eps = 1e-4
*/
FTYPE * single_pitch::refine(FTYPE *x){
  return refine(x, 1e-4);
}

/* 
   Applies the golden section search to the objective defined
   via the function compute_obj()

   The folden section search follows the description in

   <NAME> and <NAME>
   Practical Optimization
   Algorithms and Engineering Applications
   2007
   Springer
*/
FTYPE single_pitch::golden(FTYPE * x, int iOrder, 
                           FTYPE omega_L, FTYPE omega_U, FTYPE eps){
  FTYPE K = 1.618033988749895;
  
  FTYPE omega_l = omega_L;
  FTYPE omega_u = omega_U;

  FTYPE Ik = (omega_U - omega_L)/K;
  
  FTYPE omega_a = omega_u - Ik;
  FTYPE omega_b = omega_l + Ik;

  FTYPE fa = -compute_obj(omega_a, x, iOrder);
  FTYPE fb = -compute_obj(omega_b, x, iOrder);

  while( Ik > eps or omega_u < omega_l ){

    Ik = Ik/K;
   
    if( fa >= fb ){
      omega_l = omega_a;
      omega_a = omega_b;
      omega_b = omega_l + Ik;
      fa = fb;
      fb = -compute_obj(omega_b, x, iOrder);
    }
    else{
      omega_u = omega_b;
      omega_b = omega_a;
      omega_a = omega_u - Ik;
      fb = fa;
      fa = -compute_obj(omega_a, x, iOrder);
      }
  }

  if( fa > fb )
    return 0.5*(omega_b + omega_u);
  else if( fa < fb )
    return 0.5*(omega_l + omega_a);
  else
    return 0.5*(omega_a + omega_b);
}

/* perform model order selection on the data x at 
    the frequencies currently located in omega_h
    as obtained using the refine procedure

  This model order selection method is outlined in
 
  <NAME>, <NAME>, <NAME> and <NAME>
  Bayesian Model Comparision with the g-Prior
  IEEE Transaction on Signal Processing
  Vol 62, pp. 225-238, 2014

*/
int single_pitch::model_order_selection(FTYPE * x){
  return single_pitch::model_order_selection(x, 0);
}
  
int single_pitch::model_order_selection(FTYPE * x, FTYPE lnBFZeroOrder){
  
  int r = 2;
  int rho = 1;
  int v = 1;
  FTYPE delta = r*1.5;

  FTYPE w;
  FTYPE u = ((FTYPE)nData)/r;
  FTYPE omega, Jomega, alpha_tau, beta_tau, lngh, gh, eS;
  FTYPE lngamma, sigma2, ms, lnH, lnBFg, lnW, R2;

  FTYPE * ac = vector(maxModelOrder);
  FTYPE * as = vector(maxModelOrder);

  FTYPE energy = CvmdDot(nData, x, x);
  int order = 0;
  lnBF[0] = lnBFZeroOrder;

  for( int k = 1 ; k <= maxModelOrder ; ++k ){

    lnW = log(MIN((0.5/k), pitchBounds[1])-pitchBounds[0]);
    omega = omega_0h[k-1];
    Jomega = compute_obj(omega, x, k, ac, as);
    // cosine and sine part of the solution is in ac and as
    
    R2 = Jomega/energy;
    w = (nData-2*k-delta)/r;
    alpha_tau = (1-R2)*(v+w-u);
    beta_tau = (u-v)*R2+2*v+w-u;
    
    lngh = log(beta_tau + sqrt(beta_tau*beta_tau-4*alpha_tau*v)) 
             - log(-2*alpha_tau);
    gh = exp(lngh);

    eS = gh/(1+gh); //estimated shrinkage

    if( ABS(1-eS) > 1e-14 ){

      lngamma = -log((gh*u*(1-R2))/pow(1+gh*(1-R2), 2)
                     - gh*w/pow(1+gh, 2));

      sigma2 = energy/nData - eS*Jomega/nData;

      ms = 0.0;
      for( int ell = 1 ; ell <= k ; ++ell )
        ms += ac[ell-1]*ac[ell-1]*ell*ell + as[ell-1]*as[ell-1]*ell*ell;

      lnH = log(ABS(-gh*nData*(nData*nData-1)/(r*r*6*(1+gh)*sigma2))) + log(ms);
      
      lnBFg = log(1+gh)*(-k) + log((energy/nData)/sigma2)*(((FTYPE)nData)/r);
     
      lnBF[k] = lnBFg + log(gh*(delta-r)) - log(r) -log(1+gh)*(delta/r) 
        - lnW + log(2*M_PI)*((rho+1)*0.5) + lngamma/2 - lnH/2;

      //printf("lnBF %d = %f\n", k, lnBF[k]);
    }
    else{
      /* The model is very accuracte and computing Bayes factors
         will be numerical problematic.
         The simpliest model is the most likely model.
      */
      order = k;
      
      break;
    }
  }

  if( order == 0 ){
    order = CvmdArgmax(maxModelOrder + 1, lnBF); 
  }

  del_vector(ac);
  del_vector(as);

  return order;
}

/*
   The single step function that performs the three steps

   1. Calculate the objective function for all candidate model
       order and on the Fourier grid
   2. Refine the best estimates for each model order
   3. Perform model order selection

   and return the estimated frequency in radians per sample
 */
FTYPE single_pitch::est(FTYPE * x){

  compute(x);
  refine(x, 1e-6);
  estOrder = model_order_selection(x, 0.0);

  if( estOrder >= 0 )
    return omega_0h[estOrder-1];
  else
    return 0.0;
}

FTYPE single_pitch::est(FTYPE * x, FTYPE lnBFZeroOrder, FTYPE eps){

  compute(x);
  refine(x, eps);
  estOrder = model_order_selection(x, lnBFZeroOrder);

  if( estOrder >= 0 )
    return omega_0h[estOrder-1];
  else
    return 0.0;
}

/*
   The single step function that performs the three steps

   1. Calculate the objective function for all candidate model
       order and on the Fourier grid
   2. Perform model order selection
   3. Refine the for the selected model order

   and return the estimated frequency in radians per sample
 */
FTYPE single_pitch::est_fast(FTYPE * x, FTYPE lnBFZeroOrder, FTYPE eps){

  compute(x);
  compute_max_on_grid();
  estOrder = model_order_selection(x, lnBFZeroOrder);
  
  if( estOrder >= 0 )
    return refine_single(x, estOrder, eps);
  else
    return 0.0;
}


<|start_filename|>cpp/src/th.hpp<|end_filename|>
/*
Methods for computing the solution to a linear system with coefficient matrix

   R = T + H

where T is symmetric Toeplitz and H is Hankel

Based on:

<NAME> and <NAME>
Efficient Algorithms for Toeplitz Plus Hankel Matrices
Integral Equations and Operator Theory
Vol. 12
1989

<NAME>
March 2015
*/
#ifndef __TH__H_
#define __TH__H_

#include <stdlib.h>

#include "tools.hpp"
#include "vector.hpp"

/* Solves the system with f as right hand side. 
   t: length N+1
   h: lenght 2*N+1
   f: lenght N+1
   R is N+1 x N+1 
 
   returns the solution in x. 
*/
void solve(int N, FTYPE *t, FTYPE *h, FTYPE *f, FTYPE *gamma, FTYPE *x);
 
/* Computes the gamma values for use in solve
   t: length N+1
   h: lenght 2*N+1
   gamma: length (N+2)*(N+1)/2
          gamma_0: lenght 1
          gamma_1: length 2
          gamma_2: length 3
          
          In general,
          gamma_k: starts at (k+1)*k/2 and has length k+1
*/
void th(int N, FTYPE *t, FTYPE *h, FTYPE *gamma);


/* Computes the gamma values for use in solve with coefficient matrix
 
   R = T - H

   t: length N+1
   h: lenght 2*N+1 (should be called with -h)
   gamma: length (N+2)*(N+1)/2
          gamma_0: lenght 1
          gamma_1: length 2
          gamma_2: length 3
          
          In general,
          gamma_k: starts at (k+1)*k/2 and has length k+1

  Special case with t_2 = h_0, t_3 = h_1, etc
*/
void thp(int N, FTYPE *t, FTYPE *h, FTYPE *gamma);

#endif
