/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for EOBNRv2HM reduced order model (non-spinning version).
 * See CQG 31 195010, 2014, arXiv:1402.4146 for details on the reduced order method.
 * See arXiv:1106.1021 for the EOBNRv2HM model.
 *
 * Borrows from the SEOBNR ROM LAL code written by Michael Puerrer and John Veitch.
 *
 * Put the untared data in the directory designated by the environment variable ROM_DATA_PATH.
 *
 * Parameter ranges:
 *   q = 1-12 (almost)
 *   No spin
 *   Mtot >= 10Msun for fstart=8Hz
 *
 */

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "EOBNRv2HMROMstruct.h"
#include "EOBNRv2HMROM.h"

/*************************************************/
/********* Some general definitions **************/

/* Number and list of modes to generate - to be modified ultimately to allow for a selection of the desired mode(s) */
/* By convention the first mode of the list is used to set phiRef */
/* nbmodemax = 5 has been defined in the header */
const int listmode[nbmodemax][2] = { {2,2}, {2,1}, {3,3}, {4,4}, {5,5} };

/* Maximal mass ratio covered by the model - q=12 (almost) for now */
static const double q_max = 11.9894197212;
/* Minimal geometric frequency covered by the model - f=8Hz for M=10Msol for now */
static const double Mf_ROM_min = 0.0003940393857519091;
/* Maximal geometric frequency covered by the model - Mf=0.285 for the 55 mode - used as default */
static const double Mf_ROM_max = 0.285;
/* Total mass (in units of solar mass) used to generate the ROM - used to restore the correct amplitude (global factor M) when coming back to physical units */
static const double M_ROM = 10.;

/* Define the number of points in frequency used by the SVD, identical for all modes */
static const int nbfreq = 300;
/* Define the number of training waveforms used by the SVD, identical for all modes */
static const int nbwf = 301;

/******************************************************************/
/********* Definitions for the persistent structures **************/

/* SEOBNR-ROM structures are generalized to lists */
ListmodesEOBNRHMROMdata* __EOBNRv2HMROM_data_init = NULL; /* for initialization only */
ListmodesEOBNRHMROMdata** const __EOBNRv2HMROM_data = &__EOBNRv2HMROM_data_init;
ListmodesEOBNRHMROMdata_interp* __EOBNRv2HMROM_interp_init = NULL; /* for initialization only */
ListmodesEOBNRHMROMdata_interp** const __EOBNRv2HMROM_interp = &__EOBNRv2HMROM_interp_init;
/* Tag indicating whether the data has been loaded and interpolated */
int __EOBNRv2HMROM_setup = FAILURE; /* To be set to SUCCESS after initialization*/

/********************* Miscellaneous ********************/

/* Return the closest higher power of 2 */
static size_t NextPow2(const size_t n) {
  return 1 << (size_t) ceil(log2(n));
}

/* Arbitrary tuned q-dependent functions by which the frequencies for the 44 and 55 modes have been multiplied (to put the ringdowns at the same Mf). The frequencies stored in the data for the 44 and 55 modes are the rescaled ones, not the original ones. */
static double Scaling44(const double q) {
  return 1.-1./4.*exp(-(q-1.)/5.);
}
static double Scaling55(const double q) {
  return 1.-1./3.*exp(-(q-1.)/5.);
}

/* Function evaluating eta as a function of q */
static double EtaOfq(const double q) {
  return q/(1.+q)/(1.+q);
}
/* Function evaluating delta m/m = (m1-m2)/(m1+m2) as a function of q */
static double DeltaOfq(const double q) {
  return( (q-1.)/(q+1.) );
}

/* Fit of the frequency of the 22 mode at the peak amplitude - from table III in the EOBNRv2HM paper, Pan&al 1106 */
static double omega22peakOfq(const double q) {
  double eta = EtaOfq(q);
  return 0.2733 + 0.2316*eta + 0.4463*eta*eta;
}

/* Amplitude factors scaled out for each mode; this does not include the global amplitude factor scaled out from all modes. */
static double ModeAmpFactor(const int l, const int m, const double q) {
  double eta = EtaOfq(q);
  if( l==2 && m==2 ) return(sqrt(eta));
  else if( l==2 && m==1 ) return( sqrt(eta)*DeltaOfq(q) );
  else if( l==3 && m==3 ) return( sqrt(eta)*DeltaOfq(q) );
  else if( l==4 && m==4 ) return( sqrt(Scaling44(q))*sqrt(eta)*(1.-3.*eta) );
  else if( l==5 && m==5 ) return( pow(Scaling55(q), 1./6)*sqrt(eta)*DeltaOfq(q)*(1.-2.*eta) );
  else {
    fprintf(stderr, "Unknown mode (%d,%d) for the amplitude factor.\n", l, m);
    return(FAILURE);
  }
}

/********************* Functions to initialize and cleanup contents of data structures ********************/
void EOBNRHMROMdata_Init(EOBNRHMROMdata **data) {
  if(!data) exit(1);
  /* Create storage for structures */
  if(!*data) *data=malloc(sizeof(EOBNRHMROMdata));
  else
  {
    EOBNRHMROMdata_Cleanup(*data);
  }
  gsl_set_error_handler(&Err_Handler);
  (*data)->q = gsl_vector_alloc(nbwf);
  (*data)->freq = gsl_vector_alloc(nbfreq);
  (*data)->Camp = gsl_matrix_alloc(nk_amp,nbwf);
  (*data)->Cphi = gsl_matrix_alloc(nk_phi,nbwf);
  (*data)->Bamp = gsl_matrix_alloc(nbfreq,nk_amp);
  (*data)->Bphi = gsl_matrix_alloc(nbfreq,nk_phi);
  (*data)->shifttime = gsl_vector_alloc(nbwf);
  (*data)->shiftphase = gsl_vector_alloc(nbwf);
}
void EOBNRHMROMdata_interp_Init(EOBNRHMROMdata_interp **data_interp) {
  if(!data_interp) exit(1);
  /* Create storage for structures */
  if(!*data_interp) *data_interp=malloc(sizeof(EOBNRHMROMdata_interp));
  else
  {
    EOBNRHMROMdata_interp_Cleanup(*data_interp);
  }
  (*data_interp)->Camp_interp = NULL;
  (*data_interp)->Cphi_interp = NULL;
  (*data_interp)->shifttime_interp = NULL;
  (*data_interp)->shiftphase_interp = NULL;
}
void EOBNRHMROMdata_coeff_Init(EOBNRHMROMdata_coeff **data_coeff) {
  if(!data_coeff) exit(1);
  /* Create storage for structures */
  if(!*data_coeff) *data_coeff=malloc(sizeof(EOBNRHMROMdata_coeff));
  else
  {
    EOBNRHMROMdata_coeff_Cleanup(*data_coeff);
  }
  gsl_set_error_handler(&Err_Handler);
  (*data_coeff)->Camp_coeff = gsl_vector_alloc(nk_amp);
  (*data_coeff)->Cphi_coeff = gsl_vector_alloc(nk_phi);
  (*data_coeff)->shifttime_coeff = malloc(sizeof(double));
  (*data_coeff)->shiftphase_coeff = malloc(sizeof(double));
}
void EOBNRHMROMdata_Cleanup(EOBNRHMROMdata *data /* data to destroy */) {
  if(data->q) gsl_vector_free(data->q);
  if(data->freq) gsl_vector_free(data->freq);
  if(data->Camp) gsl_matrix_free(data->Camp);
  if(data->Cphi) gsl_matrix_free(data->Cphi);
  if(data->Bamp) gsl_matrix_free(data->Bamp);
  if(data->Bphi) gsl_matrix_free(data->Bphi);
  if(data->shifttime) gsl_vector_free(data->shifttime);
  if(data->shiftphase) gsl_vector_free(data->shiftphase);
  free(data);
}
void EOBNRHMROMdata_coeff_Cleanup(EOBNRHMROMdata_coeff *data_coeff) {
  if(data_coeff->Camp_coeff) gsl_vector_free(data_coeff->Camp_coeff);
  if(data_coeff->Cphi_coeff) gsl_vector_free(data_coeff->Cphi_coeff);
  if(data_coeff->shifttime_coeff) free(data_coeff->shifttime_coeff);
  if(data_coeff->shiftphase_coeff) free(data_coeff->shiftphase_coeff);
  free(data_coeff);
}
void EOBNRHMROMdata_interp_Cleanup(EOBNRHMROMdata_interp *data_interp) {
  if(data_interp->Camp_interp) SplineList_Destroy(data_interp->Camp_interp);
  if(data_interp->Cphi_interp) SplineList_Destroy(data_interp->Cphi_interp);
  if(data_interp->shifttime_interp) SplineList_Destroy(data_interp->shifttime_interp);
  if(data_interp->shiftphase_interp) SplineList_Destroy(data_interp->shiftphase_interp);
  free(data_interp);
}

/* Read binary ROM data for frequency vectors, coefficients matrices, basis functions matrices, and shiftvectors in time and phase */
int Read_Data_Mode(const char dir[], const int mode[2], EOBNRHMROMdata *data) {
  /* Load binary data for amplitude and phase spline coefficients as computed in Mathematica */
  int ret = SUCCESS;
  char* file_q = malloc(strlen(dir)+64);
  char* file_freq = malloc(strlen(dir)+64);
  char* file_Camp = malloc(strlen(dir)+64);
  char* file_Cphi = malloc(strlen(dir)+64);
  char* file_Bamp = malloc(strlen(dir)+64);
  char* file_Bphi = malloc(strlen(dir)+64);
  char* file_shifttime = malloc(strlen(dir)+64);
  char* file_shiftphase = malloc(strlen(dir)+64);
  sprintf(file_q, "%s", "EOBNRv2HMROM_q.dat"); /* The q vector is the same for all modes */
  sprintf(file_freq, "%s%d%d%s", "EOBNRv2HMROM_freq_", mode[0], mode[1], ".dat");
  sprintf(file_Camp, "%s%d%d%s", "EOBNRv2HMROM_Camp_", mode[0], mode[1], ".dat");
  sprintf(file_Cphi, "%s%d%d%s", "EOBNRv2HMROM_Cphi_", mode[0], mode[1], ".dat");
  sprintf(file_Bamp, "%s%d%d%s", "EOBNRv2HMROM_Bamp_", mode[0], mode[1], ".dat");
  sprintf(file_Bphi, "%s%d%d%s", "EOBNRv2HMROM_Bphi_", mode[0], mode[1], ".dat");
  sprintf(file_shifttime, "%s%d%d%s", "EOBNRv2HMROM_shifttime_", mode[0], mode[1], ".dat");
  sprintf(file_shiftphase, "%s%d%d%s", "EOBNRv2HMROM_shiftphase_", mode[0], mode[1], ".dat");
  ret |= Read_Vector(dir, file_q, data->q);
  ret |= Read_Vector(dir, file_freq, data->freq);
  ret |= Read_Matrix(dir, file_Camp, data->Camp);
  ret |= Read_Matrix(dir, file_Cphi, data->Cphi);
  ret |= Read_Matrix(dir, file_Bamp, data->Bamp);
  ret |= Read_Matrix(dir, file_Bphi, data->Bphi);
  ret |= Read_Vector(dir, file_shifttime, data->shifttime);
  ret |= Read_Vector(dir, file_shiftphase, data->shiftphase);
  free(file_q);
  free(file_freq);
  free(file_Camp);
  free(file_Cphi);
  free(file_Bamp);
  free(file_Bphi);
  free(file_shifttime);
  free(file_shiftphase);
  return(ret);
}

/* Function interpolating the data in matrix/vector form produces an interpolated data in the form of SplineLists */
int Interpolate_Spline_Data(const EOBNRHMROMdata *data, EOBNRHMROMdata_interp *data_interp) {

  gsl_set_error_handler(&Err_Handler);
  SplineList* splinelist;
  gsl_spline* spline;
  gsl_interp_accel* accel;
  gsl_vector* matrixline;
  gsl_vector* vector;
  int j;

  /* Interpolating Camp */
  splinelist = data_interp->Camp_interp;
  for (j = 0; j<nk_amp; j++) {
    matrixline = gsl_vector_alloc(nbwf);
    gsl_matrix_get_row(matrixline, data->Camp, j);

    accel = gsl_interp_accel_alloc();
    spline = gsl_spline_alloc(gsl_interp_cspline, nbwf);
    gsl_spline_init(spline, gsl_vector_const_ptr(data->q, 0), gsl_vector_const_ptr(matrixline, 0), nbwf);

    splinelist = SplineList_AddElementNoCopy(splinelist, spline,  accel, j);
    gsl_vector_free(matrixline);
  }
  data_interp->Camp_interp = splinelist;

  /* Interpolating Cphi */
  splinelist = data_interp->Cphi_interp;
  for (j = 0; j<nk_phi; j++) {
    matrixline = gsl_vector_alloc(nbwf);
    gsl_matrix_get_row(matrixline, data->Cphi, j);

    accel = gsl_interp_accel_alloc();
    spline = gsl_spline_alloc(gsl_interp_cspline, nbwf);
    gsl_spline_init(spline, gsl_vector_const_ptr(data->q, 0), gsl_vector_const_ptr(matrixline, 0), nbwf);

    splinelist = SplineList_AddElementNoCopy(splinelist, spline,  accel, j);
    gsl_vector_free(matrixline);
  }
  data_interp->Cphi_interp = splinelist;

  /* Interpolating shifttime */
  splinelist = data_interp->shifttime_interp;
  vector = data->shifttime;

  accel = gsl_interp_accel_alloc();
  spline = gsl_spline_alloc(gsl_interp_cspline, nbwf);
  gsl_spline_init(spline, gsl_vector_const_ptr(data->q, 0), gsl_vector_const_ptr(vector, 0), nbwf);

  splinelist = SplineList_AddElementNoCopy(NULL, spline,  accel, 0); /* This SplineList has only 1 element */
  data_interp->shifttime_interp = splinelist;

  /* Interpolating shiftphase */
  splinelist = data_interp->shiftphase_interp;
  vector = data->shiftphase;

  accel = gsl_interp_accel_alloc();
  spline = gsl_spline_alloc(gsl_interp_cspline, nbwf);
  gsl_spline_init(spline, gsl_vector_const_ptr(data->q, 0), gsl_vector_const_ptr(vector, 0), nbwf);

  splinelist = SplineList_AddElementNoCopy(NULL, spline,  accel, 0); /* This SplineList has only 1 element */
  data_interp->shiftphase_interp = splinelist;

  return SUCCESS;
}

/* Function taking as input interpolated data in the form of SplineLists
 * evaluates for a given q the projection coefficients and shifts in time and phase
*/
int Evaluate_Spline_Data(const double q, const EOBNRHMROMdata_interp* data_interp, EOBNRHMROMdata_coeff* data_coeff){

  SplineList* splinelist;
  /* Evaluating the vector of projection coefficients for the amplitude */
  for (int j=0; j<nk_amp; j++) {
    splinelist = SplineList_GetElement(data_interp->Camp_interp, j);
    gsl_vector_set(data_coeff->Camp_coeff, j, gsl_spline_eval(splinelist->spline, q, splinelist->accel));
  }
  /* Evaluating the vector of projection coefficients for the phase */
  for (int j=0; j<nk_phi; j++) {
    splinelist = SplineList_GetElement(data_interp->Cphi_interp, j);
    gsl_vector_set(data_coeff->Cphi_coeff, j, gsl_spline_eval(splinelist->spline, q, splinelist->accel));
  }
  /* Evaluating the shift in time */
  splinelist = SplineList_GetElement(data_interp->shifttime_interp, 0); /* This SplineList has only one element */
  *(data_coeff->shifttime_coeff) = gsl_spline_eval(splinelist->spline, q, splinelist->accel);
  /* Evaluating the shift in phase */
  splinelist = SplineList_GetElement(data_interp->shiftphase_interp, 0); /* This SplineList has only one element */
  *(data_coeff->shiftphase_coeff) = gsl_spline_eval(splinelist->spline, q, splinelist->accel);

  return SUCCESS;
}

/*
 * Core function for computing the ROM waveform.
 * Evaluates projection coefficients and shifts in time and phase at desired q.
 * Construct 1D splines for amplitude and phase.
 * Compute strain waveform from amplitude and phase.
*/
int EOBNRv2HMROMCore(
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,
  int nbmode,
  double deltatRef,
  double phiRef,
  double fRef,
  double Mtot_sec,
  double q,
  double distance,
  int setphiRefatfRef)
{
  int ret = SUCCESS;
  int j;
  double tpeak22estimate = 0;
  /* Check output arrays */
  if(!listhlm) exit(1);
  if(*listhlm)
  {
    printf("Error: (*listhlm) is supposed to be NULL, but got %p\n",(*listhlm));
    exit(1);
  }
  /* Check number of modes */
  if(nbmode<1 || nbmode>nbmodemax) {
    printf("Error: incorrect number of modes: %d", nbmode);
    exit(1);
  }

  /* Check if the data has been set up */
  if(__EOBNRv2HMROM_setup) {
    printf("Error: the ROM data has not been set up\n");
    exit(1);
  }
  /* Set the global pointers to data */
  ListmodesEOBNRHMROMdata* listdata = *__EOBNRv2HMROM_data;
  ListmodesEOBNRHMROMdata_interp* listdata_interp = *__EOBNRv2HMROM_interp;

  /* Global amplitude prefactor - includes total mass scaling, Fourier scaling, distance scaling, and undoing an additional arbitrary scaling */
  double Mtot_msol = Mtot_sec / MTSUN_SI; /* Mtot_msol and M_ROM in units of solar mass */
  double amp0 = (Mtot_msol/M_ROM) * Mtot_sec * 1.E-16 * 1.E6 * PC_SI / distance;

  /* Highest allowed geometric frequency for the first mode of listmode in the ROM - used for fRef
   * by convention, we use the first mode of listmode (presumably the 22) for phiref */
  ListmodesEOBNRHMROMdata* listdata_ref = ListmodesEOBNRHMROMdata_GetMode(listdata, listmode[0][0], listmode[0][1]);
  EOBNRHMROMdata* data_ref = listdata_ref->data;
  double Mf_ROM_max_ref = gsl_vector_get(data_ref->freq, nbfreq-1);
  /* Convert to geometric units the reference time and frequency */
  double deltatRef_geom = deltatRef / Mtot_sec;
  double fRef_geom = fRef * Mtot_sec;

  /* Enforce allowed geometric frequency range for fRef */
  /* In case the user asks for a reference frequency higher than covered by the ROM, we keep it that way as we will just 0-pad the waveform (and do it anyway for some modes) */
  if (fRef_geom > Mf_ROM_max_ref || fRef_geom == 0)
    fRef_geom = Mf_ROM_max_ref; /* If fRef > fhigh or 0 we reset fRef to default value of cutoff frequency for the first mode of the list (presumably the 22 mode) */
  if (0 < fRef_geom && fRef_geom < Mf_ROM_min) {
    //printf("WARNING: Reference frequency Mf_ref=%g is smaller than lowest frequency in ROM Mf=%g. Setting it to the lowest frequency in ROM.\n", fRef_geom, Mf_ROM_min);
    fRef_geom = Mf_ROM_min;
  }

  /* Internal storage for the projection coefficients and shifts in time and phase */
  /* Initialized only once, and reused for the different modes */
  EOBNRHMROMdata_coeff *data_coeff = NULL;
  EOBNRHMROMdata_coeff_Init(&data_coeff);

  /* The phase change imposed by phiref, from the phase of the first mode in the list - to be set in the first step of the loop on the modes */
  double phase_change_ref = 0;

  /* Main loop over the modes */
  for(int i=0; i<nbmode; i++ ){
    int l = listmode[i][0];
    int m = listmode[i][1];

    /* Getting the relevant modes in the lists of data */
    ListmodesEOBNRHMROMdata* listdata_mode = ListmodesEOBNRHMROMdata_GetMode(listdata, l, m);
    ListmodesEOBNRHMROMdata_interp* listdata_interp_mode = ListmodesEOBNRHMROMdata_interp_GetMode(listdata_interp, l, m);

    /* Evaluating the projection coefficients and shift in time and phase */
    ret |= Evaluate_Spline_Data(q, listdata_interp_mode->data_interp, data_coeff);

    /* Evaluating the unnormalized amplitude and unshifted phase vectors for the mode */
    /* Notice a change in convention: B matrices are transposed with respect to the B matrices in SEOBNRROM */
    /* amp_pts = Bamp . Camp_coeff */
    /* phi_pts = Bphi . Cphi_coeff */
    gsl_vector* amp_f = gsl_vector_alloc(nbfreq);
    gsl_vector* phi_f = gsl_vector_alloc(nbfreq);
    //clock_t begblas = clock();
    gsl_blas_dgemv(CblasNoTrans, 1.0, listdata_mode->data->Bamp, data_coeff->Camp_coeff, 0.0, amp_f);
    gsl_blas_dgemv(CblasNoTrans, 1.0, listdata_mode->data->Bphi, data_coeff->Cphi_coeff, 0.0, phi_f);
    //clock_t endblas = clock();
    //printf("Mode (%d,%d) Blas time: %g s\n", l, m, (double)(endblas - begblas) / CLOCKS_PER_SEC);

     /* The downsampled frequencies for the mode - we undo the rescaling of the frequency for the 44 and 55 modes */
    gsl_vector* freq_ds = gsl_vector_alloc(nbfreq);
    gsl_vector_memcpy(freq_ds, listdata_mode->data->freq);
    if ( l==4 && m==4) gsl_vector_scale( freq_ds, 1./Scaling44(q));
    if ( l==5 && m==5) gsl_vector_scale( freq_ds, 1./Scaling55(q));

    /* Evaluating the shifts in time and phase - conditional scaling for the 44 and 55 modes */
    /* Note: the stored values of 'shifttime' correspond actually to 2pi*Deltat */
    SplineList* shifttime_splinelist = listdata_interp_mode->data_interp->shifttime_interp;
    SplineList* shiftphase_splinelist = listdata_interp_mode->data_interp->shiftphase_interp;
    double twopishifttime;
    if( l==4 && m==4) {
      twopishifttime = gsl_spline_eval(shifttime_splinelist->spline, q, shifttime_splinelist->accel) * Scaling44(q);
    }
    else if( l==5 && m==5) {
      twopishifttime = gsl_spline_eval(shifttime_splinelist->spline, q, shifttime_splinelist->accel) * Scaling55(q);
    }
    else {
      twopishifttime = gsl_spline_eval(shifttime_splinelist->spline, q, shifttime_splinelist->accel);
    }
    double shiftphase = gsl_spline_eval(shiftphase_splinelist->spline, q, shiftphase_splinelist->accel);

    /* If first mode in the list, assumed to be the 22 mode, set totalshifttime and phase_change_ref */
    if( i==0 ) {
      if(l==2 && m==2) {
      /* Setup 1d cubic spline for the phase of the 22 mode */
      gsl_interp_accel* accel_phi22 = gsl_interp_accel_alloc();
      gsl_spline* spline_phi22 = gsl_spline_alloc(gsl_interp_cspline, nbfreq);
      gsl_spline_init(spline_phi22, gsl_vector_const_ptr(freq_ds,0), gsl_vector_const_ptr(phi_f,0), nbfreq);
      /* Compute the shift in time needed to set the peak of the 22 mode roughly at deltatRef */
      /* We use the SPA formula tf = -(1/2pi)*dPsi/df to estimate the correspondence between frequency and time */
      /* The frequency corresponding to the 22 peak is omega22peak/2pi, with omega22peak taken from the fit to NR in Pan&al 1106 EOBNRv2HM paper */
      double f22peak = fmin(omega22peakOfq(q)/(2*PI), Mf_ROM_max_ref); /* We ensure we evaluate the spline within its range */
      /* Note : twopishifttime is almost 0 (order 1e-8) by construction for the 22 mode, so it does not intervene here */
      tpeak22estimate = -1./(2*PI) * gsl_spline_eval_deriv(spline_phi22, f22peak, accel_phi22);
      /* Determine the change in phase (to be propagated to all modes) required to have phi22(fRef) = 2*phiRef */
      if(setphiRefatfRef) {
        phase_change_ref = 2*phiRef + (gsl_spline_eval(spline_phi22, fRef_geom, accel_phi22) - (twopishifttime - 2*PI*tpeak22estimate + 2*PI*deltatRef_geom) * fRef_geom - shiftphase);
      }
      else {
        phase_change_ref = 2*phiRef;
      }
      gsl_spline_free(spline_phi22);
      gsl_interp_accel_free(accel_phi22);
      }
      else {
      	printf("Error: the first mode in listmode must be the 22 mode to set the changes in phase and time \n");
      	return FAILURE;
      }
    }
    /* Total shift in time, and total change in phase for this mode */
    double totaltwopishifttime = twopishifttime - 2*PI*tpeak22estimate + 2*PI*deltatRef_geom;
    double constphaseshift = m/2. * phase_change_ref + shiftphase;

    //
    //printf("deltatRef_geom: %g\n", deltatRef_geom);
    //printf("freq_ds 0: %g\n", gsl_vector_get(freq_ds, 0));

    //printf("2*PI*deltatRef_geom * gsl_vector_get(freq_ds, 0), phase_change_ref: %g, %g\n", 2*PI*deltatRef_geom * gsl_vector_get(freq_ds, 0), phase_change_ref);

    /* Initialize the complex series for the mode */
    CAmpPhaseFrequencySeries *modefreqseries = NULL;
    int len = (int) freq_ds->size;
    CAmpPhaseFrequencySeries_Init(&modefreqseries, len);

    /* Mode-dependent complete amplitude prefactor */
    double amp_pre = amp0 * ModeAmpFactor( l, m, q);

    /* Final result for the mode */
    /* Scale and set the amplitudes (amplitudes are real at this stage)*/
    gsl_vector_scale(amp_f, amp_pre);
    gsl_vector_memcpy(modefreqseries->amp_real, amp_f);
    gsl_vector_set_zero(modefreqseries->amp_imag); /* Amplitudes are real at this stage */
    /* Add the linear term and the constant (including the shift to phiRef), and set the phases */
    gsl_vector_scale(phi_f, -1.); /* Change the sign of the phases: ROM convention Psi=-phase */
    gsl_blas_daxpy(totaltwopishifttime, freq_ds, phi_f); /*Beware: here freq_ds must still be in geometric units*/
    gsl_vector_add_constant(phi_f, constphaseshift);
    gsl_vector_memcpy(modefreqseries->phase, phi_f);

//
//printf("first phi_f: %g\n", gsl_vector_get(phi_f, 0 ));
//printf("last phi_f: %g\n", gsl_vector_get(phi_f, phi_f->size -1 ));

    /* Scale (to physical units) and set the frequencies */
    gsl_vector_scale(freq_ds, 1./Mtot_sec);
    gsl_vector_memcpy(modefreqseries->freq, freq_ds);

    /* Append the computed mode to the ListmodesCAmpPhaseFrequencySeries structure */
    *listhlm = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*listhlm, modefreqseries, l, m);

    //
    //printf("Mode (%d,%d) generated:\n", l, m);
    //for( int j=0; j<len; j++ ){
    //  printf("%g %g %g %g\n", gsl_vector_get(modefreqseries->freq, j), gsl_vector_get(modefreqseries->amp_real, j), gsl_vector_get(modefreqseries->amp_imag, j), gsl_vector_get(modefreqseries->phase, j));
    //}

    /* Cleanup for the mode */
    gsl_vector_free(freq_ds);
    gsl_vector_free(amp_f);
    gsl_vector_free(phi_f);
  }

  /* Cleanup of the coefficients data structure */
  EOBNRHMROMdata_coeff_Cleanup(data_coeff);

  return(SUCCESS);
}

/* Compute waveform in downsampled frequency-amplitude-phase format */
int SimEOBNRv2HMROM(
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,  /* Output: list of modes in Frequency-domain amplitude and phase form */
  int nbmode,                                    /* Number of modes to generate (starting with the 22) */
  double deltatRef,                              /* Time shift so that the peak of the 22 mode occurs at deltatRef */
  double phiRef,                                 /* Phase at reference frequency */
  double fRef,                                   /* Reference frequency (Hz); 0 defaults to fLow */
  double m1SI,                                   /* Mass of companion 1 (kg) */
  double m2SI,                                   /* Mass of companion 2 (kg) */
  double distance,                               /* Distance of source (m) */
  int setphiRefatfRef)                           /* Flag for adjusting the FD phase at phiRef at the given fRef, which depends also on tRef - if false, treat phiRef simply as an orbital phase shift (minus an observer phase shift) */
{
  /* Get masses in terms of solar mass */
  double mass1 = m1SI / MSUN_SI;
  double mass2 = m2SI / MSUN_SI;
  double Mtot = mass1 + mass2;
  double q = fmax(mass1/mass2, mass2/mass1);    /* Mass-ratio >1 by convention*/
  double Mtot_sec = Mtot * MTSUN_SI; /* Total mass in seconds */

  if ( q > q_max ) {
    //printf( "Error - %s: q out of range!\nEOBNRv2HMROM is only available for a mass ratio in the range q <= %g.\n", __func__, q_max);
    return FAILURE;
  }

  /* Set up (load and build interpolation) ROM data if not setup already */
  //clock_t beg = clock();
  EOBNRv2HMROM_Init_DATA();
  //clock_t end = clock();
  //printf("Initialization time: %g s\n", (double)(end - beg) / CLOCKS_PER_SEC);

  //beg = clock();
  int retcode = EOBNRv2HMROMCore(listhlm, nbmode, deltatRef, phiRef, fRef, Mtot_sec, q, distance, setphiRefatfRef);
  //end = clock();
  //printf("ROM evaluation time: %g s\n", (double)(end - beg) / CLOCKS_PER_SEC);

  return(retcode);
}

/* Setup EOBNRv2HMROM model using data files installed in $ROM_DATA_PATH */
int EOBNRv2HMROM_Init_DATA(void) {
  if (!__EOBNRv2HMROM_setup) return SUCCESS;

  int ret=FAILURE;
  char *envpath=NULL;
  char path[32768];
  char *brkt,*word;
  envpath=getenv("ROM_DATA_PATH");
  if(!envpath) {
    printf("Error: the environment variable ROM_DATA_PATH, giving the path to the ROM data, seems undefined\n");
    return(FAILURE);
  }
  strncpy(path,envpath,sizeof(path));

#pragma omp critical
  {
    for(word=strtok_r(path,":",&brkt); word; word=strtok_r(NULL,":",&brkt))
      {
	ret = EOBNRv2HMROM_Init(word);
	if(ret == SUCCESS) break;
      }
    if(ret!=SUCCESS) {
      printf("Error: unable to find EOBNRv2HMROM data files in $ROM_DATA_PATH\n");
      exit(FAILURE);
    }
    __EOBNRv2HMROM_setup = ret;
  }
  return(ret);
}

/* Setup EOBNRv2HMROM model using data files installed in dir */
int EOBNRv2HMROM_Init(const char dir[]) {
  if(!__EOBNRv2HMROM_setup) {
    printf("Error: EOBNRHMROMdata was already set up!");
    exit(1);
  }

  int ret = SUCCESS;
  ListmodesEOBNRHMROMdata* listdata = *__EOBNRv2HMROM_data;
  ListmodesEOBNRHMROMdata_interp* listdata_interp = *__EOBNRv2HMROM_interp;

  for (int j=0; j<nbmodemax; j++) { /* At setup, we initialize all available modes anyway */

    EOBNRHMROMdata* data = NULL;
    EOBNRHMROMdata_Init(&data);
    ret |= Read_Data_Mode( dir, listmode[j], data);
    if (!ret) {
      listdata = ListmodesEOBNRHMROMdata_AddModeNoCopy( listdata, data, listmode[j][0], listmode[j][1]);

      EOBNRHMROMdata_interp* data_interp = NULL;
      EOBNRHMROMdata_interp_Init(&data_interp);
      ret |= Interpolate_Spline_Data(data, data_interp);
      if (!ret) listdata_interp = ListmodesEOBNRHMROMdata_interp_AddModeNoCopy( listdata_interp, data_interp, listmode[j][0], listmode[j][1]);
    }
  }

  __EOBNRv2HMROM_setup = ret;
  if (!ret) {
    *__EOBNRv2HMROM_data = listdata;
    *__EOBNRv2HMROM_interp = listdata_interp;
  }
  return(ret);
}

/* Non-spinning merger TaylorF2 waveform, copied and condensed from LAL */
/* Used by SimEOBNRv2HMROMExtTF2 to extend the signal to arbitrarily low frequencies */
static void TaylorF2nonspin(
		double *amp,                            /**< FD waveform amplitude (modulus)*/
		double *phase,                          /**< FD waveform phase */
		const double *freqs,                    /**< frequency points at which to evaluate the waveform (Hz) */
		const int size,                         /** number of freq samples */
		const double m1_SI,                     /**< mass of companion 1 (kg) */
		const double m2_SI,                     /**< mass of companion 2 (kg) */
		const double distance,                  /** distance (m) */
		const double imatch                     /**< index at which to match phase;
							   assumes arrays are preloaded at imatch and imatch+1
							   with the required result */
		     )
{
  //The meat of this computation is copied from LAL: XLALSimInspiralPNPhasing_F2
  //We dont need the spin terms
  double m1 = m1_SI / MSUN_SI;
  double m2 = m2_SI / MSUN_SI;
  double mtot = m1 + m2;
  double d = (m1 - m2) / (m1 + m2);
  double eta = m1*m2/mtot/mtot;
  double m1M = m1/mtot;
  double m2M = m2/mtot;
  double m_sec = mtot * MTSUN_SI;
  double piM = PI * m_sec;

  double pfaN = 3.L/(128.L * eta);

  /* Non-spin phasing terms - see arXiv:0907.0700, Eq. 3.18 */
  double pfav0 = 1.L;
  double pfav2 = 5.L*(743.L/84.L + 11.L * eta)/9.L;
  double pfav3 = -16.L*PI;
  double pfav4 = 5.L*(3058.673L/7.056L + 5429.L/7.L * eta
		+ 617.L * eta*eta)/72.L;
  double pfav5 = 5.L/9.L * (7729.L/84.L - 13.L * eta) * PI;
  double pfalogv5 = 5.L/3.L * (7729.L/84.L - 13.L * eta) * PI;
  double pfav6 = (11583.231236531L/4.694215680L
	    - 640.L/3.L * PI * PI - 6848.L/21.L*GAMMA)
    + eta * (-15737.765635L/3.048192L
	     + 2255./12. * PI * PI)
    + eta*eta * 76055.L/1728.L
    - eta*eta*eta * 127825.L/1296.L;
  pfav6 += (-6848.L/21.L)*log(4.);
  double pfalogv6 = -6848.L/21.L;
  double pfav7 = PI * ( 77096675.L/254016.L
		      + 378515.L/1512.L * eta - 74045.L/756.L * eta*eta);

  /* Non-spin 2-2 amplitude terms (Blanchet LRR)*/
  double a2 = ( -107 + 55*eta ) / 42.;
  double a3 = 2*PI;
  double a4 = ( ( 2047.*eta - 7483. ) * eta - 2173. ) / 1512.;
  /* Blanchet has more terms, but there should diminishing returns:
     expect v^5 ~ 1e-5 and the higher terms are more complicated and, indeed, complex */

  //Lead coefficients
  //double amp0 = -4. * m1 * m2 / distance * C_SI * MTSUN_SI * MTSUN_SI * sqrt(PI/12.L); //(from LAL)
  double amp0B = 2. * m1 * m2 / distance * C_SI * MTSUN_SI * MTSUN_SI * sqrt(16*PI/5.L); //Based on Blanchet-LRR (327)
  //Note: amp0B = -4 * sqrt( 3/5) * amp0;
  double FTaN =  32.0 * eta*eta / 5.0;
  //printf("eta=%g\n",eta);
  //Compute raw TaylorF2
  int i;
  for (i = 0; i < size; i++) {
    double f = freqs[i];
    double v = cbrt(piM*f);
    double logv = log(v);
    double v2 = v*v;
    double v5 = v2*v2*v;
    double v10 = v5*v5;

    //printf("taylorf2: f=%g  v=%g\n",f,v);
    double phasing=0;
    phasing = pfav7 * v;
    phasing = (phasing + pfav6 + pfalogv6 * logv) * v;
    phasing = (phasing + pfav5 + pfalogv5 * logv) * v;
    phasing = (phasing + pfav4) * v;
    phasing = (phasing + pfav3) * v;
    phasing = (phasing + pfav2) * v2;
    phasing += 1;
    phasing *=pfaN;

    double amp22fac;
    amp22fac = a4*v;
    amp22fac = ( amp22fac + a3 ) * v;
    amp22fac = ( amp22fac + a2 ) * v2;
    amp22fac += 1.0;

    phasing /= v5;
    double flux = FTaN * v10;
    double dEnergy = -eta * v;
    phase[i]=phasing;
    //Notes for amplitude: Blanchet at leading order:
    /* mf=x^(3/2); fdot=3/2/m x^(1/2) xdot ~ 3/2/m x^(1/2) * (-1/16)*(4x)^5(-eta/5/m) = 96/5*eta/m^2 * x^(11/2) */
    /*-flux/dEnergy = 32.0 * eta*eta / 5.0 / eta *v^9 */
    /*--> -flux/dEnergy =  fdot / (3*v^2) [using x=v^2]*/
    //amp[i] = amp0 * sqrt(-dEnergy/flux) * v;  (Based on LAL)
    amp[i] = amp0B * amp22fac * v2 / sqrt(-flux/dEnergy * 3 * v2 );
    //printf("v=%g: a=%g ph=%g;  amp22fac=%g sqrt(v^-9)=%g\n",v,amp[i],phase[i],amp22fac,sqrt(v/v10));
    //Note ampB = - amp * 4*sqrt(3/5) * / sqrt(3) + higher-order = -4/sqrt(5)*( 1 + higher-order )
    //  ...possibly related to sph-harm normalization
    // HACK: Strangely it seems that an additional factor of 4/sqrt(5) is just right to nearly match the EOB wf FT
    amp[i] *= 4/sqrt(5);   //HACK

    //Here we depart from LAL, referencing phase and time-shift to two neighboring freq points
    //First we match the freq derivative
  }
}

/*Wrapper for waveform generation with possibly a combination of EOBNRv2HMROM and TaylorF2*/
/* Note: GenerateWaveform accepts masses and distances in SI units, whereas LISA params is in solar masses and Mpc */
/* Note: the extended waveform will now a different number of frequency points for each mode */
/* Note: phiRef is readjusted after the extension -- for the case where fRef is below the band covered by ROM, in which case the core function defaults fRef to the max geometric freq of the ROM */
int SimEOBNRv2HMROMExtTF2(
  ListmodesCAmpPhaseFrequencySeries **listhlm,   /* Output: list of modes in Frequency-domain amplitude and phase form */
  int nbmode,                                    /* Number of modes to generate (starting with the 22) */
  double Mf_match,                               /* Minimum frequency using EOBNRv2HMROM in inverse total mass units*/
  double minf,                                   /* Minimum frequency required */
  int tagexthm,                                  /* Tag to decide whether or not to extend the higher modes as well */
  double deltatRef,                              /* Time shift so that the peak of the 22 mode occurs at deltatRef */
  double phiRef,                                 /* Phase at reference frequency */
  double fRef,                                   /* Reference frequency (Hz); 0 defaults to fLow */
  double m1SI,                                   /* Mass of companion 1 (kg) */
  double m2SI,                                   /* Mass of companion 2 (kg) */
  double distance,                               /* Distance of source (m) */
  int setphiRefatfRef)                           /* Flag for adjusting the FD phase at phiRef at the given fRef, which depends also on tRef - if false, treat phiRef simply as an orbital phase shift (minus an observer phase shift) */
{//
  //printf("calling SimEOBNRv2HMROMExtTF2 with minf=%g\n", minf);
  //printf("params: %d %g %g %g %g %g %g %g %g\n", nbmode, Mf_match, minf, deltatRef, phiRef, fRef, m1SI, m2SI, distance);

  int ret,i;
  ListmodesCAmpPhaseFrequencySeries* listROM = NULL;
  //int lout=-1,mout=-1;
  int lout=-1,mout=-1;

  /* Generate the waveform with the ROM */
  ret = SimEOBNRv2HMROM(&listROM, nbmode, deltatRef, phiRef, fRef, m1SI, m2SI, distance, setphiRefatfRef);

  /* If the ROM waveform generation failed (e.g. parameters were out of bounds) return FAILURE */
  //if(ret==FAILURE)printf("SimEOBNRv2HMROMExtTF2: Generation of ROM for injection failed!\n");
  if(ret==FAILURE) return FAILURE;

  /* Main loop over the modes (as linked list) to perform the extension */
  /* The 2-2 mode will be extended by TaylorF2 model with the phase and time offset
  determined by matching conditions. All other modes will be extended as some
  sort of power-law fall-off in amplitude and power-law growth in phase.          */
  ListmodesCAmpPhaseFrequencySeries* listelement = listROM;
  while(listelement) {    // For each l-m (ie each listelement)

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;

    //NOTE: temporary hack to allow avoiding power-law extension of higher modes which seems problematic
    //    if((l==2&&m==2) || tagexthm) {

    /* First we must compute a new frequency grid including a possible extension to lower frequencies*/
    gsl_vector *freq_new;
    gsl_vector* freq = listelement->freqseries->freq;
    int len = (int) freq->size;
    // Construct frequency grid extension on the geometric mean of the lowest few ROM frequencies after the matching point
    const int Navg=3;
    int imatch=-1;
    double f_match;
    if(Mf_match<=0) {
      f_match = Mf_ROM_min/(m1SI+m2SI)*MSUN_SI/MTSUN_SI;
    }
    else if(Mf_match>Mf_ROM_min) {
      f_match = Mf_match/(m1SI+m2SI)*MSUN_SI/MTSUN_SI;
    }
    else {
      printf("WARNING: Mf_match lower than the lowest geometric frequency of the ROM model - used instead\n");
      f_match = Mf_ROM_min/(m1SI+m2SI)*MSUN_SI/MTSUN_SI;
    }
    f_match = f_match*m/2; //Shift the matching frequency for non-22 modes to correspond to a comparable orbital freq.
    //printf("f_match=%g\n",f_match);
    for(i=0;i<len-Navg;i++){
      if(gsl_vector_get(freq,i)>f_match){
        imatch=i;
        break;
      }
    }
    if(imatch<0){
      printf("WARNING: f_match exceeds high-freq range of the ROM model\n");
      imatch=len-Navg-1;
    }
    double lnfmatch=log(gsl_vector_get(freq,imatch));
    double lnfmin=log(minf);
    double dlnf=(log(gsl_vector_get(freq,imatch+Navg))-lnfmatch)/Navg;
    double dffac=exp(dlnf);
    //The new grid will include the old grid, from imatch and after, plus adequate lower-freq
    int len_add = (lnfmatch-lnfmin)/dlnf;
    if(len_add<0) len_add=0;
    int len_new = len - imatch + len_add;
    //printf("extending waveform: len_add + len - imatch = len_new: %i + %i - %i = %i\n",len_add,len,imatch,len_new);
    /* construct the extended freq grid */
    freq_new=gsl_vector_alloc(len_new);
    for(i=len_add;i<len_new;i++)gsl_vector_set(freq_new,i,gsl_vector_get(freq,len-len_new+i));
    for(i=len_add;i>0;i--)gsl_vector_set(freq_new,i-1,gsl_vector_get(freq_new,i)/dffac);
    //for(i=0;i<len_new;i++)printf("%i: %g %g\n",i,(i>=len_new-len?freq->data[i-len_new+len]:0),freq_new->data[i]);

    //copy the old freqseries data to a new one and extend with power-law
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    if(l==lout&&m==mout){  //write to file for debugging
      printf("Writing waveform-before for (%i,%i)\n",l,m);
      FILE *f = fopen("waveform-before.dat", "w");
      for(i=0;i<freqseries->freq->size;i++){
        fprintf(f,"%i %i %g  %g  %g  %g  %i\n",l,m,
        gsl_vector_get(freqseries->freq,i),
        gsl_vector_get(freqseries->amp_real,i),
        gsl_vector_get(freqseries->amp_imag,i),
        gsl_vector_get(freqseries->phase,i),
        i);
      }
      fclose(f);
    }
    CAmpPhaseFrequencySeries* freqseries_new=0;     //New result will be assembled here
    CAmpPhaseFrequencySeries_Init(&freqseries_new,len_new);
    //set the new freqs
    for(i=0;i<len_new;i++){
      gsl_vector_set(freqseries_new->freq,i,gsl_vector_get(freq_new,i));
    }
    //copy in the high-freq ROM-model data
    //printf("l,m = %i,%i;  lenghts=%i,%i\n",l,m,freqseries->freq->size, freqseries_new->freq->size);
    for(i=len_add;i<len_new;i++){
      //printf("i, len-len_new+i: %i, %i\n",i,len-len_new+i);
      gsl_vector_set(freqseries_new->amp_real,i,gsl_vector_get(freqseries->amp_real,len-len_new+i));
      gsl_vector_set(freqseries_new->amp_imag,i,gsl_vector_get(freqseries->amp_imag,len-len_new+i));
      gsl_vector_set(freqseries_new->phase,i,gsl_vector_get(freqseries->phase,len-len_new+i));
      //printf("%i: copying %g  %g  %g  %g\n",i,freqseries_new->freq->data[i],freqseries_new->amp_real->data[i],freqseries_new->amp_imag->data[i],freqseries_new->phase->data[i]);
    }
    //extend
    if(l==2&&m==2&&len_add>0) {//extend 2-2 with TaylorF2
      //Assemble data for matching
      double f0=freq_new->data[len_add],f1=freq_new->data[len_add+1];
      double ph0=freqseries_new->phase->data[len_add],ph1=freqseries_new->phase->data[len_add+1];
      double amp=sqrt(freqseries_new->amp_real->data[len_add]*freqseries_new->amp_real->data[len_add]
        +freqseries_new->amp_imag->data[len_add]*freqseries_new->amp_imag->data[len_add]);
      double amp1=sqrt(freqseries_new->amp_real->data[len_add+1]*freqseries_new->amp_real->data[len_add+1]
        +freqseries_new->amp_imag->data[len_add+1]*freqseries_new->amp_imag->data[len_add+1]);
      double amprfac=freqseries_new->amp_real->data[len_add]/amp,ampifac=freqseries_new->amp_imag->data[len_add]/amp;
      //Compute raw TaylorF2
      TaylorF2nonspin(freqseries_new->amp_real->data,freqseries_new->phase->data,freq_new->data,len_add+2,m1SI,m2SI,distance,imatch);
      //Compute offsets in phase, first phase derivative, and amplitude argument
      double dphase0tf2=(freqseries_new->phase->data[len_add+1]-freqseries_new->phase->data[len_add])/(f1-f0);
      double phase0tf2=freqseries_new->phase->data[len_add];
      double dphase0eob=(ph1-ph0)/(f1-f0);
      double dphase0=dphase0eob-dphase0tf2;
      double phase0=ph0 - phase0tf2 - f0*dphase0;
      double amp0bcoeff = amp / freqseries_new->amp_real->data[len_add] - 1.0; //Compute correction for continuity matching.
      double amp0ccoeff = ((amp1/freqseries_new->amp_real->data[len_add+1]-1.0)/amp0bcoeff/(f1*f1/f0/f0)-1.0)/(f1/f0-1.0);
      //TESTING
      //printf("%g, %g\n", amp0bcoeff, amp0ccoeff);
      //Compute correction for continuity matching.
      /*
      printf("ph0eob,dph0eob= %g,  %g\n",ph0,dphase0eob);
      printf("ph0tf2,dph0tf2= %g,  %g\n",phase0tf2,dphase0tf2);
      printf("ph0,dph0= %g,  %g\n",phase0,dphase0);
      printf("f0,f0*dph0= %g,  %g\n",f0,dphase0*f0);
      printf("imatch=%i\n",imatch);
      */
      //Apply offsets
      for (i = 0; i < len_add+2; i++){
        double f=freqseries_new->freq->data[i];
        //printf("%i<%i,%g\n",i,len_add,len_add-i);
        //printf("f,ph0+f*dph0= %g, %g\n",freqseries_new->freq->data[i],phase0 + dphase0*f);
        freqseries_new->phase->data[i] += phase0 + dphase0*f;
        //First apply continuity matching
        // amp -> amp * ( 1 + b*f^2/f0^2 * ( 1 + c*(f/f0 -1) )
        //(starts at order f^2 since we only keep 2PN order ampl corrections in TaylorF2 code below; could change to f^3 if higher order terms are used)
        freqseries_new->amp_real->data[i] *= 1.0 + amp0bcoeff*f*f/f0/f0*(1+amp0ccoeff*(f/f0-1));
        freqseries_new->amp_imag->data[i] = freqseries_new->amp_real->data[i]*ampifac;
        freqseries_new->amp_real->data[i] *= amprfac;
        //printf("%i: extending TF2 %g  %g  %g  %g\n",i,freqseries_new->freq->data[i],freqseries_new->amp_real->data[i],freqseries_new->amp_imag->data[i],freqseries_new->phase->data[i]);
      }
    } else { //extend other modes with power-law
      //The results are many cycles out of phase almost immediately, so this definitely is not an accurate
      //waveform, but the results are reasonably smooth and of plausible structure.
      //Alternatively, we could also extend these with TaylorF2, btu we are mostly assuming this part of the WF is small
      double phref=freqseries->phase->data[len-1];//For phase we extend by a power-law referenced to zero phase at end of ROM
      for(i=0;i<len;i++)if(phref>freqseries->phase->data[i])phref=freqseries->phase->data[i];//get the smallest value of phi to use as ref.
      phref=phref+1.0;//add one more
      double ldArfac = 0;
      if(gsl_vector_get(freqseries->amp_real,imatch)>0) //avoid div0 in trivial cases
      //dArfac=exp(-log( gsl_vector_get(freqseries->amp_real,imatch+Navg)
      //		 /gsl_vector_get(freqseries->amp_real,imatch) ) / Navg);
      ldArfac=(log( gsl_vector_get(freqseries->amp_real,imatch+Navg)
      /gsl_vector_get(freqseries->amp_real,imatch) ) /
      log( gsl_vector_get(freqseries->freq,imatch+Navg)
      /gsl_vector_get(freqseries->freq,imatch) )  );
      //double dphfac = exp(-log( (gsl_vector_get(freqseries->phase,imatch+Navg)-phref)
      //			/(gsl_vector_get(freqseries->phase,imatch) -phref)) / Navg);
      double ldphfac = (log( (gsl_vector_get(freqseries->phase,imatch+Navg)-phref)
      /(gsl_vector_get(freqseries->phase,imatch) -phref)) /
      log( gsl_vector_get(freqseries->freq,imatch+Navg)
      /gsl_vector_get(freqseries->freq,imatch) )  );
      if(1&&l==lout&&m==mout)printf("ldphfac(%i,%i)=%g\n",l,m,ldphfac);
      //double f0=gsl_vector_get(freqseries->freq,imatch);
      for(i=len_add;i>0;i--){
        double fratio=gsl_vector_get(freqseries_new->freq,i-1)/gsl_vector_get(freqseries_new->freq,i);
        double dArfac=pow(fratio,ldArfac);
        gsl_vector_set(freqseries_new->amp_real,i-1,gsl_vector_get(freqseries_new->amp_real,i)*dArfac);
        gsl_vector_set(freqseries_new->amp_imag,i-1,gsl_vector_get(freqseries_new->amp_imag,i)*dArfac);
        double dphfac=pow(fratio,ldphfac);
        gsl_vector_set(freqseries_new->phase,i-1,(gsl_vector_get(freqseries_new->phase,i)-phref)*dphfac+phref);
        //printf("%i: extending %g  %g  %g  %g\n",i,freqseries_new->freq->data[i-1],freqseries_new->amp_real->data[i-1],freqseries_new->amp_imag->data[i-1],freqseries_new->phase->data[i-1]);
      }
      //printf("Extended (%i,%i) down to f=%g, ampR=%g, ampI=%g, phase=%g\n",l,m,freqseries_new->freq->data[0],freqseries_new->amp_real->data[0],freqseries_new->amp_imag->data[0],freqseries_new->phase->data[0]);
    }
    //delete the old content data and replace with the new
    CAmpPhaseFrequencySeries_Cleanup(freqseries);
    listelement->freqseries=freqseries_new;

    //TESTING
    //printf("In ext: len freqseries: %d\n", freqseries_new->freq->size);
    //for(int i=0; i<freqseries_new->freq->size; i++) {
    //printf("%g %g %g %g\n", gsl_vector_get(freqseries_new->freq, i), gsl_vector_get(freqseries_new->amp_real, i), gsl_vector_get(freqseries_new->amp_imag, i), gsl_vector_get(freqseries_new->phase, i));
    //}

    //debugging
    //   freqseries=listelement->freqseries;
    //   if(1&&l==lout&&m==mout){  //write to file for debugging
    //     FILE *f = fopen("waveform.dat", "w");
    //     for(i=0;i<freqseries->freq->size;i++){
    // fprintf(f,"%i %i %g  %g %g  %g %i\n",l,m,
    // 	freqseries->freq->data[i],
    // 	freqseries->amp_real->data[i],
    // 	freqseries->amp_imag->data[i],
    // 	freqseries->phase->data[i],
    // 	i);
    //     }
    //     fclose(f);
    //   }

    gsl_vector_free(freq_new);
    listelement=listelement->next;
  }

  /* Compute phase shift to set phiRef at fRef */
  /* Covers the case where input fRef is outside the range of the ROM, in which case the Core function defaulted to Mfmax_ROM */
  /* Not very clean and a bit redundant */
  if (setphiRefatfRef) {
    CAmpPhaseFrequencySeries* h22 = ListmodesCAmpPhaseFrequencySeries_GetMode(listROM, 2, 2)->freqseries;
    gsl_vector* freq22 = h22->freq;
    gsl_vector* phase22 = h22->phase;
    int nbfreq = freq22->size;
    gsl_interp_accel* accel_phi22 = gsl_interp_accel_alloc();
    gsl_spline* spline_phi22 = gsl_spline_alloc(gsl_interp_cspline, nbfreq);
    gsl_spline_init(spline_phi22, gsl_vector_const_ptr(freq22,0), gsl_vector_const_ptr(phase22,0), nbfreq);
    /* If fRef was not set (fRef=0), use the last frequency generated for 22 mode -- as is done internally in Core */
    if (fRef==0.) fRef = freq22->data[freq22->size - 1];
    /* Compute 22 phase at fRef before adjustment, check the extended range */
    if ( (fRef<freq22->data[0]) || (fRef>freq22->data[freq22->size - 1]) ) {
      printf("Error: fRef is not covered by the frequency range of the waveform after extension.\n");
      return FAILURE;
    }
    double phi22atfRef = gsl_spline_eval(spline_phi22, fRef, accel_phi22);
    /* Phase shift, as an orbital or observer phase shift (change in 22 is 2*phaseshift) */
    double phaseshift = (2*phiRef - phi22atfRef)/2.;

    /* Propagate phase shift to full list of modes */
    listelement = listROM;
    while(listelement) {
      int l = listelement->l;
      int m = listelement->m;

      double phaseshiftlm = m/2. * phaseshift;
      gsl_vector* philm = listelement->freqseries->phase;
      gsl_vector_add_constant(philm, phaseshiftlm);

      listelement = listelement->next;
    }

    /* Cleanup */
    gsl_spline_free(spline_phi22);
    gsl_interp_accel_free(accel_phi22);
  }

  /* Output */
  *listhlm = listROM;

      /*
      printf("generated listROM: n=%i l=%i m=%i\n",(*listhlm)->freqseries->amp_real->size,listROM->l,listROM->m);
      for(i=0;i<(*listhlm)->freqseries->freq->size;i++){
      printf("%i:  %g  %g  %g  %g\n",i,(*listhlm)->freqseries->freq->data[i],(*listhlm)->freqseries->amp_real->data[i],(*listhlm)->freqseries->amp_imag->data[i],(*listhlm)->freqseries->phase->data[i]);
    }
    printf("listlhm=%x\n",listhlm);
    printf("*listlhm=%x\n",*listhlm);
    */
  return SUCCESS;
}


#include "common/c_math/gsl_linalg_extra.h"

#include <assert.h>
#include <float.h>
#include <math.h>
#include <stdint.h>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_blas_types.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_vector.h>

// TODO(kennyjensen): Remove these and allocate arrays in calling
// library.
#define MAX_VECTOR_SIZE (32U)
#define MAX_MATRIX_SIZE (16U * 16U)

// Solves triangular matrix equations of the form:
//
//   op(T) * X = B   or   X * op(T) = B
//
// using forward or back substitution.  For rectangular T
// (i.e. underdetermined or overdetermined systems), this only solves
// the square submatrix of T.  For underdetermined systems, the
// remaining elements of X are set to zeros.  If the triangular matrix
// T is singular, then this will only solve the part of the matrix up
// to the first zero diagonal element.
//
// Args:
//   side: Which side of the multiplication op(T) appears.
//   uplo: Whether T is upper or lower triangular.
//   transpose: Whether op(.) is a transpose or not.
//   T: Triangular matrix (m x n).
//   B: Right hand side matrix (m x k).
//   X: Solution matrix (n x k).
//
// Returns:
//   GSL_ESING if T is singular and GSL_SUCCESS otherwise.
int32_t GslTriangularSolve(CBLAS_SIDE_t side, CBLAS_UPLO_t uplo,
                           CBLAS_TRANSPOSE_t transpose, const gsl_matrix *T,
                           const gsl_matrix *B, gsl_matrix *X) {
  assert(T != NULL && B != NULL && X != NULL);
  assert(T != X && B != X);
  assert(side != CblasLeft ||
         (X->size2 == B->size2 &&
          (transpose == CblasNoTrans ? T->size1 : T->size2) == B->size1 &&
          (transpose == CblasNoTrans ? T->size2 : T->size1) == X->size1));
  assert(side != CblasRight ||
         (X->size1 == B->size1 &&
          (transpose == CblasNoTrans ? T->size1 : T->size2) == X->size2 &&
          (transpose == CblasNoTrans ? T->size2 : T->size1) == B->size2));

  int32_t err = GSL_SUCCESS;
  size_t small_dim = (T->size1 < T->size2) ? T->size1 : T->size2;

  gsl_matrix_set_zero(X);
  // Calling this "rank" here is assuming that the diagonal elements
  // of T are in descending order.
  size_t rank = small_dim;
  for (size_t i = 0U; i < small_dim; ++i) {
    if (fabs(gsl_matrix_get(T, i, i)) <= DBL_EPSILON) {
      rank = i;
      err = GSL_ESING;
      break;
    }
    gsl_vector_const_view row = gsl_matrix_const_row(B, i);
    gsl_matrix_set_row(X, i, &row.vector);
  }
  if (rank == 0U) return err;

  // Make a square sub-matrix of T that has all non-zero elements
  // along the diagonal and a reduced B matrix with the same number of
  // rows.
  gsl_matrix_const_view T_rank =
      gsl_matrix_const_submatrix(T, 0U, 0U, rank, rank);
  gsl_matrix_view X_rank = gsl_matrix_submatrix(X, 0U, 0U, rank, X->size2);

  // Solve the triangular system op(T) * X = B or X * op(T) = B.
  gsl_blas_dtrsm(side, uplo, transpose, CblasNonUnit, 1.0, &T_rank.matrix,
                 &X_rank.matrix);
  return err;
}

// Converts a trapezoidal matrix R with m < n to a pure upper
// triangular matrix with zeros outside of the square portion of the
// matrix using orthogonal transformations from the right:
//
//   R = [T, 0] * Z
//
// Here Z = Z_1 * Z_2 * ... * Z_m with Z_k = I - tau_k * v_k * v_k'
// where v_k is the Householder vector which cancels the
// non-triangular elements of the kth row of T.  This is similar to
// the DTZRZF function from LAPACK.
//
// Args:
//   R: Right trapezoidal matrix with m < n.  This function will
//       happily ignore non-zero components in the lower portion of
//       the R matrix; however these will also appear in the T matrix.
//   T: Upper triangular matrix output (m x n).  The "zero" portion of
//       the T matrix is used to store the Householder vectors that,
//       with tau, form Z.
//   tau: Vector of length m of Householder coefficients.
void GslTrapezoidalToTriangular(const gsl_matrix *R, gsl_matrix *T,
                                gsl_vector *tau) {
  assert(R != NULL && T != NULL && tau != NULL);
  assert(R->size1 == T->size1 && R->size2 == T->size2);
  assert(tau->size == T->size1);
  assert(R->size1 > 0U && R->size1 < R->size2);

  if (T != R) gsl_matrix_memcpy(T, R);

  size_t i_max = R->size1 - 1U;
  for (size_t k = 0U; k < R->size1; ++k) {
    size_t i = i_max - k;

    // hh is used to calculate the Householder vector; however, the
    // Household vector itself is only stored in hh1.
    gsl_vector_view hh = gsl_matrix_subrow(T, i, i_max, R->size2 - i_max);
    gsl_vector_view hh1 =
        gsl_matrix_subrow(T, i, R->size1, R->size2 - R->size1);

    // GSL assumes that the elements you want to cancel are adjacent
    // to the diagonal element, which isn't the case here, so we have
    // to swap elements around.
    gsl_vector_view row = gsl_matrix_row(T, i);
    gsl_vector_swap_elements(&row.vector, i, i_max);
    double tau_k = gsl_linalg_householder_transform(&hh.vector);
    gsl_vector_set(tau, k, tau_k);
    gsl_vector_swap_elements(&row.vector, i, i_max);

    // Apply Householder reflections to the right hand side of the
    // triangular matrix.  We don't apply the reflection to the ith
    // row itself because GSL's Householder transform sets the
    // diagonal element for us.  (Doing this with the
    // gsl_linalg_householder_mh function would require swapping
    // columns because of assumptions GSL makes about the diagonal
    // element being adjacent to the elements that the Householder
    // vector cancels.)
    for (size_t j = 0U; j < i; ++j) {
      gsl_vector_view T_jm =
          gsl_matrix_subrow(T, j, R->size1, R->size2 - R->size1);
      double alpha, T_ji = gsl_matrix_get(T, j, i);
      gsl_blas_ddot(&hh1.vector, &T_jm.vector, &alpha);
      alpha += T_ji;
      gsl_blas_daxpy(-tau_k * alpha, &hh1.vector, &T_jm.vector);
      gsl_matrix_set(T, j, i, -tau_k * alpha + T_ji);
    }
  }
}

// Applies the inverse of the Householder transformations output by
// GslTrapezoidalToTriangular to the matrix X.  More specifically, for
// the matrix Z defined by:
//
//   R = [T, 0] * Z
//
// this function applies Z' to the input matrix X.
//
// Args:
//   T: The R matrix of a QR decomposition after complete
//       orthogonalization.  The elements that should be zero from the
//       QR decomposition are ignored.  The elements that should be
//       zero from the complete orthogonalization must contain the
//       Householder vectors used in the orthogonalization.
//   tau: The factors to multiply the Householder vectors by which is
//       output by the GslTrapezoidalToTriangular function.
//   X: The input matrix to which we apply Z'.
//   Zt_X: Output matrix with Householder transformations applied  Z' * X.
void GslTrapezoidalToTriangularZTMat(const gsl_matrix *T, const gsl_vector *tau,
                                     const gsl_matrix *X, gsl_matrix *Zt_X) {
  assert(X != NULL && tau != NULL && Zt_X != NULL && T != NULL);
  assert(T != X && T != Zt_X);
  assert(T->size1 == tau->size && T->size2 == X->size1);
  assert(X->size1 == Zt_X->size1 && X->size2 == Zt_X->size2);
  assert(T->size1 <= T->size2);

  if (X != Zt_X) gsl_matrix_memcpy(Zt_X, X);

  for (size_t i = 0U; i < T->size1; ++i) {
    // Grab the Householder vector from the columns past the square
    // section of the T matrix.
    gsl_vector_const_view hh1 =
        gsl_matrix_const_subrow(T, i, T->size1, T->size2 - T->size1);

    // Apply Householder reflections to the left hand side of X.
    // (Doing this with the gsl_linalg_householder_hm function would
    // require swapping rows because of assumptions GSL makes about
    // the diagonal element being adjacent to the elements that the
    // Householder vector cancels.)
    //
    // TODO(kennyjensen): If we need this non-standard
    // gsl_linalg_householder_hm in other places, we should make a
    // helper function.
    double tau_k = gsl_vector_get(tau, T->size1 - i - 1U);
    for (size_t j = 0U; j < Zt_X->size2; ++j) {
      gsl_vector_view Zt_X_mj =
          gsl_matrix_subcolumn(Zt_X, j, T->size1, T->size2 - T->size1);
      double alpha, Zt_X_ij = gsl_matrix_get(Zt_X, i, j);
      gsl_blas_ddot(&hh1.vector, &Zt_X_mj.vector, &alpha);
      alpha += Zt_X_ij;
      gsl_blas_daxpy(-tau_k * alpha, &hh1.vector, &Zt_X_mj.vector);
      gsl_matrix_set(Zt_X, i, j, -tau_k * alpha + Zt_X_ij);
    }
  }
}

// Left or right matrix divide.  Finds a solution, X, to the equations
// A*X = B or X*A = B for left and right divide respectively.  If the
// system is overdetermined or underdetermined, then this returns a
// least-squares solution that minimizes |A*X - B| or |X*A - B|,
// similar to MATLAB's "\" or "/" operators.  The solution is found
// using QR decomposition with column pivoting followed by back
// substitution.  For matrix left divide, the solution proceeds as:
//
//   A*X = B
//   Q*R*P' * X = B
//   R * P'*X = Q'*B
//
// For matrix right divide, the solution proceeds as:
//
//   X*A = B
//   A'*X' = B'
//   Q*R*P' * X' = B'
//   R * (X*P)' = Q'*B'
//
// Args:
//   side: CblasLeft or CblasRight for left or right divide.
//   A: Left hand side matrix.
//   B: Right hand side matrix.
//   X: Solution matrix.
//
// Returns:
//   GSL_ESING A is rank deficient and thus the R matrix is singular and
//   GSL_SUCCESS otherwise.
int32_t GslMatrixDivide(CBLAS_SIDE_t side, const gsl_matrix *A,
                        const gsl_matrix *B, gsl_matrix *X) {
  assert(A != NULL && B != NULL && X != NULL);
  assert(A != X && B != X);
  assert(side != CblasLeft || (A->size1 == B->size1 && A->size2 == X->size1 &&
                               X->size2 == B->size2));
  assert(side != CblasRight || (A->size2 == B->size2 && A->size1 == X->size2 &&
                                X->size1 == B->size1));
  assert(A->size1 <= MAX_VECTOR_SIZE && A->size2 <= MAX_VECTOR_SIZE);
  assert(A->size1 * A->size2 <= MAX_MATRIX_SIZE);
  assert(B->size1 * B->size2 <= MAX_MATRIX_SIZE);
  assert(X->size1 * X->size2 <= MAX_MATRIX_SIZE);

  int32_t err = GSL_SUCCESS;
  size_t m = (side == CblasLeft) ? A->size1 : A->size2;
  size_t n = (side == CblasLeft) ? A->size2 : A->size1;
  size_t k = (side == CblasLeft) ? B->size2 : B->size1;

  // Allocate workspace variables.
  double workspace[MAX_MATRIX_SIZE];
  double qr_data[MAX_MATRIX_SIZE];
  double tau_data[MAX_VECTOR_SIZE];
  size_t perm_data[MAX_VECTOR_SIZE];
  int signum;

  // Decompose op(A) into Q*R*P'.
  gsl_matrix_view QR = gsl_matrix_view_array(qr_data, m, n);
  gsl_vector_view tau = gsl_vector_view_array(tau_data, m < n ? m : n);
  gsl_permutation p = {n, perm_data};
  gsl_vector_view norm = gsl_vector_view_array(workspace, n);
  if (side == CblasLeft) {
    gsl_matrix_memcpy(&QR.matrix, A);
  } else {
    gsl_matrix_transpose_memcpy(&QR.matrix, A);
  }
  gsl_linalg_QRPT_decomp(&QR.matrix, &tau.vector, &p, &signum, &norm.vector);

  // Calculate Q'*op(B) matrix.
  gsl_matrix_view Qt_opB = gsl_matrix_view_array(workspace, m, k);
  if (side == CblasLeft) {
    gsl_matrix_memcpy(&Qt_opB.matrix, B);
  } else {
    gsl_matrix_transpose_memcpy(&Qt_opB.matrix, B);
  }
  gsl_linalg_QR_QTmat(&QR.matrix, &tau.vector, &Qt_opB.matrix);

  // Determine the rank of the R matrix.
  //
  // TODO(tobenkin): DGELSY uses a different iterative condition
  // number estimation technique to determine the rank of the
  // submatrix R(1:rank, 1:rank).  See http://www.netlib.org/lapack/
  // lapack-3.1.1/html/dgelsy.f.html#DGEQP3.269.
  double R_00 = fabs(gsl_matrix_get(&QR.matrix, 0U, 0U));
  uint32_t rank = 0U;
  if (R_00 > DBL_EPSILON) {
    for (rank = 1U; rank < m && rank < n; ++rank) {
      if (fabs(gsl_matrix_get(&QR.matrix, rank, rank)) < 1e-9 * R_00) {
        err = GSL_ESING;
        break;
      }
    }
  } else {
    gsl_matrix_set_zero(X);
    return GSL_ESING;
  }

  // Solve the triangular system R * Z * P' * op(X) = Q' * op(B) and
  // multiply the solution by Z' to get P' * op(X).
  gsl_matrix_view Z_Pt_opX = gsl_matrix_view_array(X->data, n, k);
  gsl_matrix_set_zero(&Z_Pt_opX.matrix);

  // For underdetermined or rank deficient matrices, use complete
  // orthogonalization to provide the minimal 2-norm solution.
  gsl_vector_view tau_trap = gsl_vector_view_array(tau_data, rank);
  gsl_matrix_view QR_trap = gsl_matrix_submatrix(&QR.matrix, 0U, 0U, rank, n);
  if (rank < n) {
    GslTrapezoidalToTriangular(&QR_trap.matrix, &QR_trap.matrix,
                               &tau_trap.vector);
  }

  // Solve the non-singular square triangular system.
  gsl_matrix_const_view QR_rank =
      gsl_matrix_const_submatrix(&QR.matrix, 0U, 0U, rank, rank);
  gsl_matrix_const_view Qt_opB_rank =
      gsl_matrix_const_submatrix(&Qt_opB.matrix, 0U, 0U, rank, k);
  gsl_matrix_view Z_Pt_opX_rank =
      gsl_matrix_submatrix(&Z_Pt_opX.matrix, 0U, 0U, rank, k);
  gsl_matrix_memcpy(&Z_Pt_opX_rank.matrix, &Qt_opB_rank.matrix);
  gsl_blas_dtrsm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0,
                 &QR_rank.matrix, &Z_Pt_opX_rank.matrix);

  // For full-rank square and overdetermined matrices, Z = I.
  if (rank < n) {
    GslTrapezoidalToTriangularZTMat(&QR_trap.matrix, &tau_trap.vector,
                                    &Z_Pt_opX.matrix, &Z_Pt_opX.matrix);
  }

  // Apply the permutation matrix to get op(X).
  for (uint32_t i = 0U; i < k; ++i) {
    gsl_vector_view col = gsl_matrix_column(&Z_Pt_opX.matrix, i);
    gsl_permute_vector_inverse(&p, &col.vector);
  }

  if (side == CblasRight) {
    // We can't transpose within the same memory, hence the
    // unfortunate extra memcpy.
    gsl_matrix_view X_copy = gsl_matrix_view_array(qr_data, k, n);
    gsl_matrix_transpose_memcpy(&X_copy.matrix, &Z_Pt_opX.matrix);
    gsl_matrix_memcpy(X, &X_copy.matrix);
  }

  return err;
}


/* integration/qagp.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

static int
qagp (const gsl_function *f,
      const double *pts, const size_t npts,
      const double epsabs, const double epsrel, const size_t limit,
      gsl_integration_workspace * workspace,
      double *result, double *abserr,
      gsl_integration_rule * q);

#include "initialise.c"
#include "qpsrt.c"
#include "util.c"
#include "append.c"
#include "reset.c"
#include "qelg.c"
#include "qpsrt2.c"
#include "ptsort.c"
#include "positivity.c"

int
gsl_integration_qagp (const gsl_function *f,
                      double * pts, size_t npts,
                      double epsabs, double epsrel, size_t limit,
                      gsl_integration_workspace * workspace,
                      double * result, double * abserr)
{
  int status = qagp (f, pts, npts,
                     epsabs, epsrel, limit,
                     workspace,
                     result, abserr,
                     &gsl_integration_qk21) ;

  return status ;
}

static int
qagp (const gsl_function * f,
      const double *pts, const size_t npts,
      const double epsabs, const double epsrel,
      const size_t limit,
      gsl_integration_workspace * workspace,
      double *result, double *abserr,
      gsl_integration_rule * q)
{
  double area, errsum;
  double res_ext, err_ext;
  double result0, abserr0, resabs0;
  double tolerance;

  double ertest = 0;
  double error_over_large_intervals = 0;
  double reseps = 0, abseps = 0, correc = 0;
  size_t ktmin = 0;
  int roundoff_type1 = 0, roundoff_type2 = 0, roundoff_type3 = 0;
  int error_type = 0, error_type2 = 0;

  size_t iteration = 0;

  int positive_integrand = 0;
  int extrapolate = 0;
  int disallow_extrapolation = 0;

  struct extrapolation_table table;

  const size_t nint = npts - 1; /* number of intervals */

  size_t *ndin = workspace->level; /* temporarily alias ndin to level */

  size_t i;

  /* Initialize results */

  *result = 0;
  *abserr = 0;

  /* Test on validity of parameters */

  if (limit > workspace->limit)
    {
      GSL_ERROR ("iteration limit exceeds available workspace", GSL_EINVAL) ;
    }

  if (npts > workspace->limit)
    {
      GSL_ERROR ("npts exceeds size of workspace", GSL_EINVAL);
    }

  if (epsabs <= 0 && (epsrel < 50 * GSL_DBL_EPSILON || epsrel < 0.5e-28))
    {
      GSL_ERROR ("tolerance cannot be acheived with given epsabs and epsrel",
                 GSL_EBADTOL);
    }

  /* Check that the integration range and break points are an
     ascending sequence */

  for (i = 0; i < nint; i++)
    {
      if (pts[i + 1] < pts[i])
        {
          GSL_ERROR ("points are not in an ascending sequence", GSL_EINVAL);
        }
    }

  /* Perform the first integration */

  result0 = 0;
  abserr0 = 0;
  resabs0 = 0;

  initialise (workspace, 0.0, 0.0) ;

  for (i = 0; i < nint; i++)
    {
      double area1, error1, resabs1, resasc1;
      const double a1 = pts[i];
      const double b1 = pts[i + 1];

      q (f, a1, b1, &area1, &error1, &resabs1, &resasc1);

      result0 = result0 + area1;
      abserr0 = abserr0 + error1;
      resabs0 = resabs0 + resabs1;

      append_interval (workspace, a1, b1, area1, error1);

      if (error1 == resasc1 && error1 != 0.0)
        {
          ndin[i] = 1;
        }
      else
        {
          ndin[i] = 0;
        }
    }

  /* Compute the initial error estimate */

  errsum = 0.0;

  for (i = 0; i < nint; i++)
    {
      if (ndin[i])
        {
          workspace->elist[i] = abserr0;
        }

      errsum = errsum + workspace->elist[i];

    }

  for (i = 0; i < nint; i++)
    {
      workspace->level[i] = 0;
    }

  /* Sort results into order of decreasing error via the indirection
     array order[] */

  sort_results (workspace);

  /* Test on accuracy */

  tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (result0));

  if (abserr0 <= 100 * GSL_DBL_EPSILON * resabs0 && abserr0 > tolerance)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("cannot reach tolerance because of roundoff error"
                 "on first attempt", GSL_EROUND);
    }
  else if (abserr0 <= tolerance)
    {
      *result = result0;
      *abserr = abserr0;

      return GSL_SUCCESS;
    }
  else if (limit == 1)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("a maximum of one iteration was insufficient", GSL_EMAXITER);
    }

  /* Initialization */

  initialise_table (&table);
  append_table (&table, result0);

  area = result0;

  res_ext = result0;
  err_ext = GSL_DBL_MAX;

  error_over_large_intervals = errsum;
  ertest = tolerance;

  positive_integrand = test_positivity (result0, resabs0);

  iteration = nint - 1;

  do
    {
      size_t current_level;
      double a1, b1, a2, b2;
      double a_i, b_i, r_i, e_i;
      double area1 = 0, area2 = 0, area12 = 0;
      double error1 = 0, error2 = 0, error12 = 0;
      double resasc1, resasc2;
      double resabs1, resabs2;
      double last_e_i;

      /* Bisect the subinterval with the largest error estimate */

      retrieve (workspace, &a_i, &b_i, &r_i, &e_i);

      current_level = workspace->level[workspace->i] + 1;

      a1 = a_i;
      b1 = 0.5 * (a_i + b_i);
      a2 = b1;
      b2 = b_i;

      iteration++;

      q (f, a1, b1, &area1, &error1, &resabs1, &resasc1);
      q (f, a2, b2, &area2, &error2, &resabs2, &resasc2);

      area12 = area1 + area2;
      error12 = error1 + error2;
      last_e_i = e_i;

      /* Improve previous approximations to the integral and test for
         accuracy.

         We write these expressions in the same way as the original
         QUADPACK code so that the rounding errors are the same, which
         makes testing easier. */

      errsum = errsum + error12 - e_i;
      area = area + area12 - r_i;

      tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (area));

      if (resasc1 != error1 && resasc2 != error2)
        {
          double delta = r_i - area12;

          if (fabs (delta) <= 1.0e-5 * fabs (area12) && error12 >= 0.99 * e_i)
            {
              if (!extrapolate)
                {
                  roundoff_type1++;
                }
              else
                {
                  roundoff_type2++;
                }
            }

          if (i > 10 && error12 > e_i)
            {
              roundoff_type3++;
            }
        }

      /* Test for roundoff and eventually set error flag */

      if (roundoff_type1 + roundoff_type2 >= 10 || roundoff_type3 >= 20)
        {
          error_type = 2;       /* round off error */
        }

      if (roundoff_type2 >= 5)
        {
          error_type2 = 1;
        }

      /* set error flag in the case of bad integrand behaviour at
         a point of the integration range */

      if (subinterval_too_small (a1, a2, b2))
        {
          error_type = 4;
        }

      /* append the newly-created intervals to the list */

      update (workspace, a1, b1, area1, error1, a2, b2, area2, error2);

      if (errsum <= tolerance)
        {
          goto compute_result;
        }

      if (error_type)
        {
          break;
        }

      if (iteration >= limit - 1)
        {
          error_type = 1;
          break;
        }

      if (disallow_extrapolation)
        {
          continue;
        }

      error_over_large_intervals += -last_e_i;

      if (current_level < workspace->maximum_level)
        {
          error_over_large_intervals += error12;
        }

      if (!extrapolate)
        {
          /* test whether the interval to be bisected next is the
             smallest interval. */
          if (large_interval (workspace))
            continue;

          extrapolate = 1;
          workspace->nrmax = 1;
        }

      /* The smallest interval has the largest error.  Before
         bisecting decrease the sum of the errors over the larger
         intervals (error_over_large_intervals) and perform
         extrapolation. */

      if (!error_type2 && error_over_large_intervals > ertest)
        {
          if (increase_nrmax (workspace))
            continue;
        }

      /* Perform extrapolation */

      append_table (&table, area);

      if (table.n < 3)
        {
          goto skip_extrapolation;
        }

      qelg (&table, &reseps, &abseps);

      ktmin++;

      if (ktmin > 5 && err_ext < 0.001 * errsum)
        {
          error_type = 5;
        }

      if (abseps < err_ext)
        {
          ktmin = 0;
          err_ext = abseps;
          res_ext = reseps;
          correc = error_over_large_intervals;
          ertest = GSL_MAX_DBL (epsabs, epsrel * fabs (reseps));
          if (err_ext <= ertest)
            break;
        }

      /* Prepare bisection of the smallest interval. */

      if (table.n == 1)
        {
          disallow_extrapolation = 1;
        }

      if (error_type == 5)
        {
          break;
        }

    skip_extrapolation:

      reset_nrmax (workspace);
      extrapolate = 0;
      error_over_large_intervals = errsum;

    }
  while (iteration < limit);

  *result = res_ext;
  *abserr = err_ext;

  if (err_ext == GSL_DBL_MAX)
    goto compute_result;

  if (error_type || error_type2)
    {
      if (error_type2)
        {
          err_ext += correc;
        }

      if (error_type == 0)
        error_type = 3;

      if (result != 0 && area != 0)
        {
          if (err_ext / fabs (res_ext) > errsum / fabs (area))
            goto compute_result;
        }
      else if (err_ext > errsum)
        {
          goto compute_result;
        }
      else if (area == 0.0)
        {
          goto return_error;
        }
    }

  /*  Test on divergence. */

  {
    double max_area = GSL_MAX_DBL (fabs (res_ext), fabs (area));

    if (!positive_integrand && max_area < 0.01 * resabs0)
      goto return_error;
  }

  {
    double ratio = res_ext / area;

    if (ratio < 0.01 || ratio > 100 || errsum > fabs (area))
      error_type = 6;
  }

  goto return_error;

compute_result:

  *result = sum_results (workspace);
  *abserr = errsum;

return_error:

  if (error_type > 2)
    error_type--;

  if (error_type == 0)
    {
      return GSL_SUCCESS;
    }
  else if (error_type == 1)
    {
      GSL_ERROR ("number of iterations was insufficient", GSL_EMAXITER);
    }
  else if (error_type == 2)
    {
      GSL_ERROR ("cannot reach tolerance because of roundoff error",
                 GSL_EROUND);
    }
  else if (error_type == 3)
    {
      GSL_ERROR ("bad integrand behavior found in the integration interval",
                 GSL_ESING);
    }
  else if (error_type == 4)
    {
      GSL_ERROR ("roundoff error detected in the extrapolation table",
                 GSL_EROUND);
    }
  else if (error_type == 5)
    {
      GSL_ERROR ("integral is divergent, or slowly convergent",
                 GSL_EDIVERGE);
    }
  else
    {
      GSL_ERROR ("could not integrate function", GSL_EFAILED);
    }
}


/* To execute:
 * ./driver <n> # for some positive integer n
 * */
#include "q_incs.h"
#include <lapacke.h>
#include "matrix_helpers.h"
#include "positive_solver.h"

/* testing and benchmarking for AxEqualsB solver.
 * generates a random matrix A, and tests the positive semidefinite solver
 * by running iton AtA and tests the full solver by running it on A.
 *
 * the current method of generating random matrices isn't perfect, but it's
 * better than it seems because solving Ax = b is independent of any scaling
 * factor, so the size of the matrix is irrelevant.
 */

static int _bench_drop_n = 0;
static int _bench_iters = 1;
static bool _verbose = false;

static void
_print_input(
            double **A,
            double *x,
            double *b,
            int n
            )
{
  for ( int i = 0; i < n; i++ ) {
    fprintf(stderr, "[ ");
    for ( int j = 0; j < n; j++ ) {
      if ( j == 0 ) {
        fprintf(stderr, " %2d ", (int)index_symm_matrix(A, i, j));
      }
      else {
        fprintf(stderr, ", %2d ", (int)index_symm_matrix(A, i, j));
      }
    }
    fprintf(stderr, "] ");
    // print x
    fprintf(stderr, " [ ");
    fprintf(stderr, " %2d ", (int)x[i]);
    fprintf(stderr, "] ");
    // print b
    fprintf(stderr, "  = [ ");
    fprintf(stderr, " %3d ", (int)b[i]);
    fprintf(stderr, "] ");

    fprintf(stderr, "\n");
  }
}

/* assembly code to read the TSC */
static inline uint64_t
_RDTSC(void)
{
  unsigned int hi, lo;
  __asm__ volatile("rdtsc" : "=a" (lo), "=d" (hi));
  return ((uint64_t)hi << 32) | lo;
}

static void
_print_result(double *x_expected, double *x_returned,
              double *b_expected, double *b_returned,
              char *name, int n, double runtime, bool checker_success)
{
  bool error = false;
  fprintf(stderr, "CHECKING %s RESULTS\n", name);
  if (_verbose) {
    fprintf(stderr, "x(expect), x(%s), b(expect), b(%s)\n", name, name);
  }
  for (int i=0; i < n; i++) {
    if (_verbose) {
      fprintf(stderr, " %lf, %lf, %lf, %lf",
              x_expected[i], x_returned[i], b_expected[i], b_returned[i]);
    }

    if ( abs(b_returned[i] - b_expected[i]) < 0.001 ) {
      if (_verbose) {
        fprintf(stderr, " %s_MATCH\n", name);
      }
    }
    else {
      error = true;
      if (_verbose) {
        fprintf(stderr, " %s_ERROR\n", name);
      }
    }
  }
  fprintf(stderr, "%s %s in %.3e cycles, checker said %s.\n\n",
          name,
          error ? "FAILED" : "SUCCEEDED",
          runtime,
          checker_success ? "SUCCEEDED" : "FAILED");
}

static int
_run_our_tests(
    double **A,
    double *x_expected,
    double *b,
    double **A_posdef,
    double *b_posdef,
    int n
    )
{
  int status = 0;

  double **A_posdef_copy = NULL;
  double **A_posdef_full = NULL;
  double *b_posdef_copy = NULL;
  double *b_returned = NULL;
  double *x_returned = NULL;
  uint64_t begin, total;
  double avg_cycles;
  bool checker_success;

  status = alloc_symm_matrix(&A_posdef_copy, n); cBYE(status);
  status = alloc_matrix(&A_posdef_full, n); cBYE(status);
  b_posdef_copy = malloc(n * sizeof(double)); return_if_malloc_failed(b_posdef_copy);
  b_returned = malloc(n * sizeof(double)); return_if_malloc_failed(b_returned);
  x_returned = malloc(n * sizeof(double)); return_if_malloc_failed(x_returned);

  for (int i = 0; i < n; i++) {
    b_posdef_copy[i] = b_posdef[i];
    for(int j = 0; j < n; j++) {
      A_posdef_full[i][j] = index_symm_matrix(A_posdef, i, j);
      if (j < n - i) {
        A_posdef_copy[i][j] = A_posdef[i][j];
      }
    }
  }

  // time computations for each solver
  total = 0;
  for (int i = 0; i < _bench_iters; i++) {
    begin = _RDTSC();
    status = posdef_positive_solver(A_posdef, x_returned, b_posdef, n); cBYE(status);
    if (i >= _bench_drop_n) {
      total += _RDTSC() - begin;
    }
  }
  avg_cycles = (double)total / (_bench_iters - _bench_drop_n);
  multiply_symm_matrix_vector(A_posdef, x_returned, n, b_returned);
  checker_success = symm_positive_solver_check(A_posdef, x_returned, b_posdef, n, 0.0);
  _print_result(x_expected, x_returned, b_posdef, b_returned, "POSDEF_SLOW", n, avg_cycles, checker_success);

  total = 0;
  for (int i = 0; i < _bench_iters; i++) {
    for (int i = 0; i < n; i++) {
      b_posdef_copy[i] = b_posdef[i];
      for(int j = 0; j < n - i; j++) {
        A_posdef_copy[i][j] = A_posdef[i][j];
      }
    }
    begin = _RDTSC();
    status = posdef_positive_solver_fast(A_posdef_copy, x_returned, b_posdef_copy, n); cBYE(status); cBYE(status);
    if (i >= _bench_drop_n) {
      total += _RDTSC() - begin;
    }
  }
  avg_cycles = (double)total / (_bench_iters - _bench_drop_n);
  multiply_symm_matrix_vector(A_posdef, x_returned, n, b_returned);
  checker_success = symm_positive_solver_check(A_posdef, x_returned, b_posdef, n, 0.0);
  _print_result(x_expected, x_returned, b_posdef, b_returned, "POSDEF_FAST", n, avg_cycles, checker_success);

  total = 0;
  for (int i = 0; i < _bench_iters; i++) {
    for (int i = 0; i < n; i++) {
      for(int j = 0; j < n; j++) {
        A_posdef_full[i][j] = index_symm_matrix(A_posdef, i, j);
      }
    }
    begin = _RDTSC();
    status = full_posdef_positive_solver(A_posdef_full, x_returned, b_posdef, n); cBYE(status);
    if (i >= _bench_drop_n) {
      total += _RDTSC() - begin;
    }
  }
  avg_cycles = (double)total / (_bench_iters - _bench_drop_n);
  multiply_symm_matrix_vector(A_posdef, x_returned, n, b_returned);
  checker_success = symm_positive_solver_check(A_posdef, x_returned, b_posdef, n, 0.0);
  _print_result(x_expected, x_returned, b_posdef, b_returned, "FULL_POSDEF_SLOW", n, avg_cycles, checker_success);

  total = 0;
  for (int i = 0; i < _bench_iters; i++) {
    for (int i = 0; i < n; i++) {
      b_posdef_copy[i] = b_posdef[i];
      for(int j = 0; j < n; j++) {
        A_posdef_full[i][j] = index_symm_matrix(A_posdef, i, j);
      }
    }
    begin = _RDTSC();
    status = full_posdef_positive_solver_fast(A_posdef_full, x_returned, b_posdef_copy, n); cBYE(status);
    if (i >= _bench_drop_n) {
      total += _RDTSC() - begin;
    }
  }
  avg_cycles = (double)total / (_bench_iters - _bench_drop_n);
  multiply_symm_matrix_vector(A_posdef, x_returned, n, b_returned);
  checker_success = symm_positive_solver_check(A_posdef, x_returned, b_posdef, n, 0.0);
  _print_result(x_expected, x_returned, b_posdef, b_returned, "FULL_POSDEF_FAST", n, avg_cycles, checker_success);

  total = 0;
  for (int i = 0; i < _bench_iters; i++) {
    begin = _RDTSC();
    status = positive_solver(A, x_returned, b, n); cBYE(status);
    if (i >= _bench_drop_n) {
      total += _RDTSC() - begin;
    }
  }
  avg_cycles = (double)total / (_bench_iters - _bench_drop_n);
  multiply_matrix_vector(A, x_returned, n, b_returned);
  checker_success = full_positive_solver_check(A, x_returned, b, n, 0.0);
  _print_result(x_expected, x_returned, b, b_returned, "FULL", n, avg_cycles, checker_success);

BYE:
  free_matrix(A_posdef_copy, n);
  free_matrix(A_posdef_full, n);
  free_if_non_null(b_posdef_copy);
  free_if_non_null(b_returned);
  free_if_non_null(x_returned);

  return status;
}

static int
_run_lapack_tests(
    double **A,
    double *x_expected,
    double *b,
    double **A_posdef,
    double *b_posdef,
    int n
    )
{
  int status = 0;
  lapack_int N = n;
  lapack_int NRHS = 1;
  lapack_int LDA = N;
  lapack_int LDB = N;

  double *A_unrolled = NULL;
  double *A_posdef_unrolled = NULL;
  double *b_copy = NULL;
  double *b_posdef_copy = NULL;
  double *b_returned = NULL;
  lapack_int *ipiv = NULL;

  uint64_t begin, total;
  double avg_cycles;
  bool checker_success;

  A_unrolled = malloc(n * n * sizeof(double)); return_if_malloc_failed(A_unrolled);
  A_posdef_unrolled = malloc(n * n * sizeof(double)); return_if_malloc_failed(A_posdef_unrolled);
  b_copy = malloc(n * sizeof(double)); return_if_malloc_failed(b_copy);
  b_posdef_copy = malloc(n * sizeof(double)); return_if_malloc_failed(b_posdef_copy);
  b_returned = malloc(n * sizeof(double)); return_if_malloc_failed(b_returned);
  ipiv = malloc(n * sizeof(lapack_int)); return_if_malloc_failed(ipiv);

  total = 0;
  for (int i = 0; i < _bench_iters; i++) {
    for (int i = 0; i < n; i++) {
      b_copy[i] = b[i];
      for (int j = 0; j < n; j++) {
        A_unrolled[n * i + j] = A[i][j];
      }
    }
    begin = _RDTSC();
    LAPACKE_dgesv(LAPACK_COL_MAJOR, N, NRHS, A_unrolled, LDA, ipiv, b_copy, LDB);
    if (i >= _bench_drop_n) {
      total += _RDTSC() - begin;
    }
  }
  avg_cycles = (double)total / (_bench_iters - _bench_drop_n);
  multiply_matrix_vector(A, b_copy, n, b_returned);
  checker_success = full_positive_solver_check(A, b_copy, b, n, 0.0);
  _print_result(x_expected, b_copy, b, b_returned, "LAPACK_FULL", n, avg_cycles, checker_success);

  total = 0;
  for (int i = 0; i < _bench_iters; i++) {
    for (int i = 0; i < n; i++) {
      b_posdef_copy[i] = b_posdef[i];
      for (int j = 0; j <= i; j++) {
        A_posdef_unrolled[n * i + j] = A_posdef[j][i - j];
      }
      for (int j = i + 1; j < n; j++) {
        A_posdef_unrolled[n * i + j] = 0;
      }
    }
    begin = _RDTSC();
    LAPACKE_dposv(LAPACK_COL_MAJOR, 'U', N, NRHS, A_posdef_unrolled, LDA, b_posdef_copy, LDB);
    if (i >= _bench_drop_n) {
      total += _RDTSC() - begin;
    }
  }
  avg_cycles = (double)total / (_bench_iters - _bench_drop_n);
  multiply_symm_matrix_vector(A_posdef, b_posdef_copy, n, b_returned);
  checker_success = symm_positive_solver_check(A_posdef, b_posdef_copy, b_posdef, n, 0.0);
  _print_result(x_expected, b_posdef_copy, b_posdef, b_returned, "LAPACK_POSDEF", n, avg_cycles, checker_success);

BYE:
  free_if_non_null(A_unrolled);
  free_if_non_null(A_posdef_unrolled);
  free_if_non_null(b_copy);
  free_if_non_null(b_posdef_copy);
  free_if_non_null(b_returned);
  free_if_non_null(ipiv);

  return status;
}

int
main(
    int argc,
    char **argv
    )
{
  int status = 0;
  int n = 0;                 // dimension of matrices
  double **A = NULL;         // randomly generated n * n matrix
  double **A_posdef = NULL;  // randomly generated positive definite matrix (right now just A transpose * A)
  double *x_expected = NULL; // randomly generated solution
  double *b_posdef = NULL;   // A_posdef * x_expected
  double *b_full = NULL;     // A * x_expected

  srand48(_RDTSC());

  bool print_usage = false, bench = false;
  switch ( argc ) {
  case 3 :
    if (strcmp("-vb", argv[2]) == 0 || strcmp("-bv", argv[2]) == 0) {
      _verbose = true;
      bench = true;
    } else if (strcmp("-v", argv[2]) == 0) {
      _verbose = true;
    } else if (strcmp("-b", argv[2]) == 0) {
      bench = true;
    } else {
      print_usage = true;
    } // fall through
  case 2 :
    n = atoi(argv[1]);
    break;
  default :
    print_usage = true;
    break;
  }
  if (print_usage || n <= 0) {
    printf("Usage: ./test_driver <n> [-v]\n");
    printf("where n is a positive integer and -v provides verbose output.\n");
    go_BYE(-1);
  }
  if (bench == true) {
    _bench_iters = 15;
    _bench_drop_n = 3;
  }

  status = alloc_matrix(&A, n); cBYE(status);
  status = alloc_symm_matrix(&A_posdef, n); cBYE(status);

  for ( int i = 0; i < n; i++ )  {
    for ( int j = 0; j < n; j++ ) {
      A[i][j] = (drand48() - 0.5) * 100;
    }
  }
  transpose_and_multiply(A, A_posdef, n);
  b_posdef = (double *) malloc(n * sizeof(double)); return_if_malloc_failed(b_posdef);
  b_full = (double *) malloc(n * sizeof(double)); return_if_malloc_failed(b_full);
  x_expected = (double *) malloc(n * sizeof(double)); return_if_malloc_failed(x_expected);

  // Initialize x and b
  for ( int i = 0; i < n; i++ )  {
    x_expected[i] = (lrand48() % 16) - 16/2;
  }
  multiply_symm_matrix_vector(A_posdef, x_expected, n, b_posdef);
  multiply_matrix_vector(A, x_expected, n, b_full);

  if (_verbose) {
    _print_input(A_posdef, x_expected, b_posdef, n);
  }

  _run_our_tests(A, x_expected, b_full, A_posdef, b_posdef, n);

  _run_lapack_tests(A, x_expected, b_full, A_posdef, b_posdef, n);

BYE:
  free_matrix(A, n);
  free_matrix(A_posdef, n);
  free_if_non_null(x_expected);
  free_if_non_null(b_posdef);
  free_if_non_null(b_full);

  return status;
}


/*
 * resampling.c
 *
 *  Created on: 5.5.2017
 *      Author: heine
 */
#define _XOPEN_SOURCE 600

#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <mpi.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
//#include "ziggurat.h"

const gsl_rng_type *T; // Generator type
const gsl_rng *rgen;   // Generator

void set_resamplingseed( int id ) {

  struct timespec spec;

  clock_gettime( CLOCK_REALTIME, &spec );

  gsl_rng_env_setup();
  T = gsl_rng_default;
  rgen = gsl_rng_alloc( T );

  gsl_rng_set( rgen, ( unsigned long int ) ( round( spec.tv_nsec / 1.0e6 ) + 223 * id ) );

}

double depth_first_sum(double *x, long len) {

  long n = len;
  double *x_tmp, out;
  x_tmp = malloc(sizeof(double) * len);
  for (long i = 0; i < len; i++)
    x_tmp[i] = x[i];

  while (n > 1) {
    for (long i = 0; i < n / 2; i++) {
      x_tmp[i] = x_tmp[2 * i] + x_tmp[2 * i + 1];
    }
    if (n % 2 != 0) {
      x_tmp[n / 2] = x_tmp[n - 1];
    }
    n = n / 2 + n % 2;
  }
  out = x_tmp[0];
  free(x_tmp);
  return out;
}

double *depth_first_cumsum(double *x, long len) {

  double *W;
  W = malloc(sizeof(double) * len);

  for (long i = 0; i < len; i++) {
    W[i] = depth_first_sum(x, i);
  }
  return W;
}

double *exclusive_prefix_sum(double *w, long len) {
  double *W;
  W = malloc(sizeof(double) * len);

  W[0] = 0;
  for (long i = 1; i < len; i++) {
    W[i] = W[i - 1] + w[i - 1];
  }

  return W;
}

double *combining_exclusive_prefix_sum(double *w, double *w_recv, long n) {

  double *W;
  W = malloc(sizeof(double) * 2 * n);

  W[0] = 0;

  for (long i = 1; i <= n; i++) {
    W[i] = W[i - 1] + w[i - 1];
  }

  for (long i = 0; i < n-1; i++) {
    W[n + i + 1] = W[n + i] + w_recv[i];
  }

  return W;
}

double serial_multinomial_resample(long n, double *w, double *w_recv, long *inds) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = 2 * n - 1;

  W = combining_exclusive_prefix_sum(w, w_recv, n);

  total_weight = log(W[2 * n - 1] + w_recv[n - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  free(W);

  return exp(total_weight);
}

double serial_multinomial_resample_sng(long n, double *w, long *inds) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = n - 1;

  W = exclusive_prefix_sum(w, n);

  total_weight = log(W[n - 1] + w[n - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  free(W);

  return exp(total_weight);
}

double serial_multinomial_resample_sng_noutk(long n, double *w, long *inds, long k) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = k - 1;

  W = exclusive_prefix_sum(w, k);

  total_weight = log( W[ k - 1 ] + w[ k - 1 ] );

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  free(W);

  return exp(total_weight);
}

double serial_multinomial_resample_noutk(long n, double *w, double* W, long *inds, long k) {

  double total_weight, lnmax = 0, u;
  long j = k - 1;

  total_weight = log(W[k - 1] + w[k - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  return -1;
}

void binary_resampler(long n, double w, long *counts)  {

  counts[0] = 0;
  counts[1] = 0;

  for ( long i = 0; i < n; i++ )
    counts[  gsl_rng_uniform_pos( rgen ) < w ? 0 : 1 ]++;

}

void
simplified_resample(
		    long n,
		    long *inds,
		    long k)
{

  for ( long i = 0; i < n; i++ )
    inds[ i ] = ( long ) ( gsl_rng_uniform_int( rgen, ( unsigned long int ) k ) );
}

void serial_multinomial_resample_counts(long n, long k, double *w, long *cnts) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = k - 1;

  for( long i = 0; i < k ; i++ )
    cnts[ i ] = 0;

  W = exclusive_prefix_sum(w, k);

  total_weight = log(W[k - 1] + w[k - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    cnts[j]++;
  }

  free(W);

}