#include <math.h>
#include <unistd.h>
#include <stdio.h>
#include <stdbool.h>
#include <ctype.h>
#include <stdlib.h>
#include <time.h>
#include <string.h>
//#include <pthread.h>
#include <omp.h>
#include <complex.h>
#include <fftw3.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

#include "21cmFAST.h"
#include "exceptions.h"
#include "logger.h"
#include "Constants.h"
#include "Globals.h"
#include "indexing.c"
#include "UsefulFunctions.c"
#include "ps.c"
#include "dft.c"
#include "PerturbField.c"
#include "bubble_helper_progs.c"
#include "elec_interp.c"
#include "heating_helper_progs.c"
#include "recombinations.c"
#include "IonisationBox.c"
#include "SpinTemperatureBox.c"
#include "BrightnessTemperatureBox.c"
#include "FindHaloes.c"
#include "PerturbHaloField.c"

void adj_complex_conj(fftwf_complex *HIRES_box, struct UserParams *user_params, struct CosmoParams *cosmo_params){
    /*****  Adjust the complex conjugate relations for a real array  *****/

    int i, j, k;

    // corners
    HIRES_box[C_INDEX(0,0,0)] = 0;
    HIRES_box[C_INDEX(0,0,MIDDLE)] = crealf(HIRES_box[C_INDEX(0,0,MIDDLE)]);
    HIRES_box[C_INDEX(0,MIDDLE,0)] = crealf(HIRES_box[C_INDEX(0,MIDDLE,0)]);
    HIRES_box[C_INDEX(0,MIDDLE,MIDDLE)] = crealf(HIRES_box[C_INDEX(0,MIDDLE,MIDDLE)]);
    HIRES_box[C_INDEX(MIDDLE,0,0)] = crealf(HIRES_box[C_INDEX(MIDDLE,0,0)]);
    HIRES_box[C_INDEX(MIDDLE,0,MIDDLE)] = crealf(HIRES_box[C_INDEX(MIDDLE,0,MIDDLE)]);
    HIRES_box[C_INDEX(MIDDLE,MIDDLE,0)] = crealf(HIRES_box[C_INDEX(MIDDLE,MIDDLE,0)]);
    HIRES_box[C_INDEX(MIDDLE,MIDDLE,MIDDLE)] = crealf(HIRES_box[C_INDEX(MIDDLE,MIDDLE,MIDDLE)]);

    // do entire i except corners
#pragma omp parallel shared(HIRES_box) private(i,j,k) num_threads(user_params->N_THREADS)
    {
#pragma omp for
        for (i=1; i<MIDDLE; i++){
            // just j corners
            for (j=0; j<=MIDDLE; j+=MIDDLE){
                for (k=0; k<=MIDDLE; k+=MIDDLE){
                    HIRES_box[C_INDEX(i,j,k)] = conjf(HIRES_box[C_INDEX((user_params->DIM)-i,j,k)]);
                }
            }

            // all of j
            for (j=1; j<MIDDLE; j++){
                for (k=0; k<=MIDDLE; k+=MIDDLE){
                    HIRES_box[C_INDEX(i,j,k)] = conjf(HIRES_box[C_INDEX((user_params->DIM)-i,(user_params->DIM)-j,k)]);
                    HIRES_box[C_INDEX(i,(user_params->DIM)-j,k)] = conjf(HIRES_box[C_INDEX((user_params->DIM)-i,j,k)]);
                }
            }
        } // end loop over i
    }

    // now the i corners
#pragma omp parallel shared(HIRES_box) private(i,j,k) num_threads(user_params->N_THREADS)
    {
#pragma omp for
        for (i=0; i<=MIDDLE; i+=MIDDLE){
            for (j=1; j<MIDDLE; j++){
                for (k=0; k<=MIDDLE; k+=MIDDLE){
                    HIRES_box[C_INDEX(i,j,k)] = conjf(HIRES_box[C_INDEX(i,(user_params->DIM)-j,k)]);
                }
            }
        } // end loop over remaining j
    }
}

// Re-write of init.c for original 21cmFAST

int ComputeInitialConditions(
    unsigned long long random_seed, struct UserParams *user_params,
    struct CosmoParams *cosmo_params, struct InitialConditions *boxes
){

//     Generates the initial conditions: gaussian random density field (user_params->DIM^3) as well as the equal or lower resolution velocity fields, and smoothed density field (user_params->HII_DIM^3).
//
//     Author: Andrei Mesinger
//     Date: 9/29/06

    int status;

    Try{ // This Try wraps the entire function so we don't indent.

    // Makes the parameter structs visible to a variety of functions/macros
    // Do each time to avoid Python garbage collection issues
    Broadcast_struct_global_PS(user_params,cosmo_params);
    Broadcast_struct_global_UF(user_params,cosmo_params);

    unsigned long long ct;
    int n_x, n_y, n_z, i, j, k, ii, thread_num, dimension;
    float k_x, k_y, k_z, k_mag, p, a, b, k_sq;
    double pixel_deltax;
    float p_vcb, vcb_i;

    float f_pixel_factor;

    gsl_rng * r[user_params->N_THREADS];
    gsl_rng * rseed = gsl_rng_alloc(gsl_rng_mt19937); // An RNG for generating seeds for multithreading

    gsl_rng_set(rseed, random_seed);

    omp_set_num_threads(user_params->N_THREADS);

    switch(user_params->PERTURB_ON_HIGH_RES) {
        case 0:
            dimension = user_params->HII_DIM;
            break;
        case 1:
            dimension = user_params->DIM;
            break;
    }

    // ************  INITIALIZATION ********************** //
    unsigned int seeds[user_params->N_THREADS];

    // For multithreading, seeds for the RNGs are generated from an initial RNG (based on the input random_seed) and then shuffled (Author: Fred Davies)
    int num_int = INT_MAX/16;
    unsigned int *many_ints = (unsigned int *)malloc((size_t)(num_int*sizeof(unsigned int))); // Some large number of possible integers
    for (i=0; i<num_int; i++) {
        many_ints[i] = i;
    }

    gsl_ran_choose(rseed, seeds, user_params->N_THREADS, many_ints, num_int, sizeof(unsigned int)); // Populate the seeds array from the large list of integers
    gsl_ran_shuffle(rseed, seeds, user_params->N_THREADS, sizeof(unsigned int)); // Shuffle the randomly selected integers

    int checker;

    checker = 0;
    // seed the random number generators
    for (thread_num = 0; thread_num < user_params->N_THREADS; thread_num++){
        switch (checker){
            case 0:
                r[thread_num] = gsl_rng_alloc(gsl_rng_mt19937);
                gsl_rng_set(r[thread_num], seeds[thread_num]);
                break;
            case 1:
                r[thread_num] = gsl_rng_alloc(gsl_rng_gfsr4);
                gsl_rng_set(r[thread_num], seeds[thread_num]);
                break;
            case 2:
                r[thread_num] = gsl_rng_alloc(gsl_rng_cmrg);
                gsl_rng_set(r[thread_num], seeds[thread_num]);
                break;
            case 3:
                r[thread_num] = gsl_rng_alloc(gsl_rng_mrg);
                gsl_rng_set(r[thread_num], seeds[thread_num]);
                break;
            case 4:
                r[thread_num] = gsl_rng_alloc(gsl_rng_taus2);
                gsl_rng_set(r[thread_num], seeds[thread_num]);
                break;
        } // end switch

        checker += 1;

        if(checker==5) {
            checker = 0;
        }
    }

    free(many_ints);

    // allocate array for the k-space and real-space boxes
    fftwf_complex *HIRES_box = (fftwf_complex *) fftwf_malloc(sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);
    fftwf_complex *HIRES_box_saved = (fftwf_complex *) fftwf_malloc(sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);

    // allocate array for the k-space and real-space boxes for vcb
    fftwf_complex *HIRES_box_vcb_saved;
    // HIRES_box_vcb_saved may be needed if FFTW_Wisdom doesn't exist -- currently unused
    // but I am not going to allocate it until I am certain I needed it.

    // find factor of HII pixel size / deltax pixel size
    f_pixel_factor = user_params->DIM/(float)user_params->HII_DIM;

    // ************  END INITIALIZATION ****************** //
    LOG_DEBUG("Finished initialization.");
    // ************ CREATE K-SPACE GAUSSIAN RANDOM FIELD *********** //

    init_ps();

#pragma omp parallel shared(HIRES_box,r) \
                    private(n_x,n_y,n_z,k_x,k_y,k_z,k_mag,p,a,b,p_vcb) num_threads(user_params->N_THREADS)
    {
#pragma omp for
        for (n_x=0; n_x<user_params->DIM; n_x++){
            // convert index to numerical value for this component of the k-mode: k = (2*pi/L) * n
            if (n_x>MIDDLE)
                k_x =(n_x-user_params->DIM) * DELTA_K;  // wrap around for FFT convention
            else
                k_x = n_x * DELTA_K;

            for (n_y=0; n_y<user_params->DIM; n_y++){
                // convert index to numerical value for this component of the k-mode: k = (2*pi/L) * n
                if (n_y>MIDDLE)
                    k_y =(n_y-user_params->DIM) * DELTA_K;
                else
                    k_y = n_y * DELTA_K;

                // since physical space field is real, only half contains independent modes
                for (n_z=0; n_z<=MIDDLE; n_z++){
                    // convert index to numerical value for this component of the k-mode: k = (2*pi/L) * n
                    k_z = n_z * DELTA_K;

                    // now get the power spectrum; remember, only the magnitude of k counts (due to issotropy)
                    // this could be used to speed-up later maybe
                    k_mag = sqrt(k_x*k_x + k_y*k_y + k_z*k_z);
                    p = power_in_k(k_mag);

                    // ok, now we can draw the values of the real and imaginary part
                    // of our k entry from a Gaussian distribution
                    if(user_params->NO_RNG) {
                        a = 1.0;
                        b = -1.0;
                    }
                    else {
                        a = gsl_ran_ugaussian(r[omp_get_thread_num()]);
                        b = gsl_ran_ugaussian(r[omp_get_thread_num()]);
                    }

                    HIRES_box[C_INDEX(n_x, n_y, n_z)] = sqrt(VOLUME*p/2.0) * (a + b*I);

                }
            }
        }
    }
    LOG_DEBUG("Drawn random fields.");

    // *****  Adjust the complex conjugate relations for a real array  ***** //
    adj_complex_conj(HIRES_box,user_params,cosmo_params);

    memcpy(HIRES_box_saved, HIRES_box, sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);

    // FFT back to real space
    int stat = dft_c2r_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, HIRES_box);
    if(stat>0) Throw(stat);
    LOG_DEBUG("FFT'd hires boxes.");

#pragma omp parallel shared(boxes,HIRES_box) private(i,j,k) num_threads(user_params->N_THREADS)
    {
#pragma omp for
        for (i=0; i<user_params->DIM; i++){
            for (j=0; j<user_params->DIM; j++){
                for (k=0; k<user_params->DIM; k++){
                    *((float *)boxes->hires_density + R_INDEX(i,j,k)) = *((float *)HIRES_box + R_FFT_INDEX(i,j,k))/VOLUME;
                }
            }
        }
    }

    // *** If required, let's also create a lower-resolution version of the density field  *** //
    memcpy(HIRES_box, HIRES_box_saved, sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);

    // Only filter if we are perturbing on the low-resolution grid
    if(!user_params->PERTURB_ON_HIGH_RES) {
        if (user_params->DIM != user_params->HII_DIM) {
            filter_box(HIRES_box, 0, 0, L_FACTOR*user_params->BOX_LEN/(user_params->HII_DIM+0.0));
        }

        // FFT back to real space
        dft_c2r_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, HIRES_box);

        // Renormalise the FFT'd box (sample the high-res box if we are perturbing on the low-res grid)
#pragma omp parallel shared(boxes,HIRES_box,f_pixel_factor) private(i,j,k) num_threads(user_params->N_THREADS)
        {
#pragma omp for
            for (i=0; i<user_params->HII_DIM; i++){
                for (j=0; j<user_params->HII_DIM; j++){
                    for (k=0; k<user_params->HII_DIM; k++){
                        boxes->lowres_density[HII_R_INDEX(i,j,k)] =
                        *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                           (unsigned long long)(j*f_pixel_factor+0.5),
                                                           (unsigned long long)(k*f_pixel_factor+0.5)))/VOLUME;
                    }
                }
            }
        }
    }

    // ******* Relative Velocity part ******* //
  if(user_params->USE_RELATIVE_VELOCITIES){
    //JBM: We use the memory allocated to HIRES_box as it's free.

      for(ii=0;ii<3;ii++) {

        memcpy(HIRES_box, HIRES_box_saved, sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);

#pragma omp parallel shared(HIRES_box,ii) private(n_x,n_y,n_z,k_x,k_y,k_z,k_mag,p,p_vcb) num_threads(user_params->N_THREADS)
        {
#pragma omp for
            for (n_x=0; n_x<user_params->DIM; n_x++){
                if (n_x>MIDDLE)
                    k_x =(n_x-user_params->DIM) * DELTA_K;  // wrap around for FFT convention
                else
                    k_x = n_x * DELTA_K;

                for (n_y=0; n_y<user_params->DIM; n_y++){
                    if (n_y>MIDDLE)
                        k_y =(n_y-user_params->DIM) * DELTA_K;
                    else
                        k_y = n_y * DELTA_K;

                    for (n_z=0; n_z<=MIDDLE; n_z++){
                        k_z = n_z * DELTA_K;

                        k_mag = sqrt(k_x*k_x + k_y*k_y + k_z*k_z);
                        p = power_in_k(k_mag);
                        p_vcb = power_in_vcb(k_mag);

                        // now set the velocities
                        if ((n_x==0) && (n_y==0) && (n_z==0)){ // DC mode
                            HIRES_box[0] = 0;
                        }
                        else{
                            if(ii==0) {
                                HIRES_box[C_INDEX(n_x,n_y,n_z)] *= I * k_x/k_mag * sqrt(p_vcb/p) * C_KMS;
                            }
                            if(ii==1) {
                                HIRES_box[C_INDEX(n_x,n_y,n_z)] *= I * k_y/k_mag * sqrt(p_vcb/p) * C_KMS;
                            }
                            if(ii==2) {
                                HIRES_box[C_INDEX(n_x,n_y,n_z)] *= I * k_z/k_mag * sqrt(p_vcb/p) * C_KMS;
                            }
                        }
                    }
                }
            }
        }

//we only care about the lowres vcb box, so we filter it directly.
      if (user_params->DIM != user_params->HII_DIM) {
          filter_box(HIRES_box, 0, 0, L_FACTOR*user_params->BOX_LEN/(user_params->HII_DIM+0.0));
      }

//fft each velocity component back to real space
      dft_c2r_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, HIRES_box);

      #pragma omp parallel shared(boxes,HIRES_box,f_pixel_factor,ii) private(i,j,k,vcb_i) num_threads(user_params->N_THREADS)
              {
      #pragma omp for
                  for (i=0; i<user_params->HII_DIM; i++){
                      for (j=0; j<user_params->HII_DIM; j++){
                          for (k=0; k<user_params->HII_DIM; k++){
                            vcb_i = *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                             (unsigned long long)(j*f_pixel_factor+0.5),
                                                             (unsigned long long)(k*f_pixel_factor+0.5)));
                            boxes->lowres_vcb[HII_R_INDEX(i,j,k)] += vcb_i*vcb_i;
                          }
                      }
                  }
              }

    }

//now we take the sqrt of that and normalize the FFT
    for (i=0; i<user_params->HII_DIM; i++){
        for (j=0; j<user_params->HII_DIM; j++){
            for (k=0; k<user_params->HII_DIM; k++){
              boxes->lowres_vcb[HII_R_INDEX(i,j,k)] = sqrt(boxes->lowres_vcb[HII_R_INDEX(i,j,k)])/VOLUME;
            }
        }
    }

  }
    LOG_DEBUG("Completed Relative velocities.");
    // ******* End of Relative Velocity part ******* //

    // Now look at the velocities

    for(ii=0;ii<3;ii++) {

        memcpy(HIRES_box, HIRES_box_saved, sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);
        // Now let's set the velocity field/dD/dt (in comoving Mpc)

#pragma omp parallel shared(HIRES_box,ii) private(n_x,n_y,n_z,k_x,k_y,k_z,k_sq) num_threads(user_params->N_THREADS)
        {
#pragma omp for
            for (n_x=0; n_x<user_params->DIM; n_x++){
                if (n_x>MIDDLE)
                    k_x =(n_x-user_params->DIM) * DELTA_K;  // wrap around for FFT convention
                else
                    k_x = n_x * DELTA_K;

                for (n_y=0; n_y<user_params->DIM; n_y++){
                    if (n_y>MIDDLE)
                        k_y =(n_y-user_params->DIM) * DELTA_K;
                    else
                        k_y = n_y * DELTA_K;

                    for (n_z=0; n_z<=MIDDLE; n_z++){
                        k_z = n_z * DELTA_K;

                        k_sq = k_x*k_x + k_y*k_y + k_z*k_z;

                        // now set the velocities
                        if ((n_x==0) && (n_y==0) && (n_z==0)){ // DC mode
                            HIRES_box[0] = 0;
                        }
                        else{
                            if(ii==0) {
                                HIRES_box[C_INDEX(n_x,n_y,n_z)] *= k_x*I/k_sq/VOLUME;
                            }
                            if(ii==1) {
                                HIRES_box[C_INDEX(n_x,n_y,n_z)] *= k_y*I/k_sq/VOLUME;
                            }
                            if(ii==2) {
                                HIRES_box[C_INDEX(n_x,n_y,n_z)] *= k_z*I/k_sq/VOLUME;
                            }
                        }
                    }
                }
            }
        }

        // Filter only if we require perturbing on the low-res grid
        if(!user_params->PERTURB_ON_HIGH_RES) {
            if (user_params->DIM != user_params->HII_DIM) {
                filter_box(HIRES_box, 0, 0, L_FACTOR*user_params->BOX_LEN/(user_params->HII_DIM+0.0));
            }
        }

        dft_c2r_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, HIRES_box);

        // now sample to lower res
        // now sample the filtered box
#pragma omp parallel shared(boxes,HIRES_box,f_pixel_factor,ii,dimension) private(i,j,k) num_threads(user_params->N_THREADS)
        {
#pragma omp for
            for (i=0; i<dimension; i++){
                for (j=0; j<dimension; j++){
                    for (k=0; k<dimension; k++){
                        if(user_params->PERTURB_ON_HIGH_RES) {
                            if(ii==0) {
                                boxes->hires_vx[R_INDEX(i,j,k)] =
                                *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                                   (unsigned long long)(j),
                                                                   (unsigned long long)(k)));
                            }
                            if(ii==1) {
                                boxes->hires_vy[R_INDEX(i,j,k)] =
                                *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                                   (unsigned long long)(j),
                                                                   (unsigned long long)(k)));
                            }
                            if(ii==2) {
                                boxes->hires_vz[R_INDEX(i,j,k)] =
                                *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                                   (unsigned long long)(j),
                                                                   (unsigned long long)(k)));
                            }
                        }
                        else {
                            if(ii==0) {
                                boxes->lowres_vx[HII_R_INDEX(i,j,k)] =
                                *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                                   (unsigned long long)(j*f_pixel_factor+0.5),
                                                                   (unsigned long long)(k*f_pixel_factor+0.5)));
                            }
                            if(ii==1) {
                                boxes->lowres_vy[HII_R_INDEX(i,j,k)] =
                                *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                                   (unsigned long long)(j*f_pixel_factor+0.5),
                                                                   (unsigned long long)(k*f_pixel_factor+0.5)));
                            }
                            if(ii==2) {
                                boxes->lowres_vz[HII_R_INDEX(i,j,k)] =
                                *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                                   (unsigned long long)(j*f_pixel_factor+0.5),
                                                                   (unsigned long long)(k*f_pixel_factor+0.5)));
                            }
                        }
                    }
                }
            }
        }
    }

    LOG_DEBUG("Done Inverse FT.");

    // * *************************************************** * //
    // *              BEGIN 2LPT PART                        * //
    // * *************************************************** * //

    // Generation of the second order Lagrangian perturbation theory (2LPT) corrections to the ZA
    // reference: Scoccimarro R., 1998, MNRAS, 299, 1097-1118 Appendix D

    // Parameter set in ANAL_PARAMS.H
    if(user_params->USE_2LPT){

        // use six supplementary boxes to store the gradients of phi_1 (eq. D13b)
        // Allocating the boxes
#define PHI_INDEX(i, j) ((int) ((i) - (j)) + 3*((j)) - ((int)(j))/2  )
        // ij -> INDEX
        // 00 -> 0
        // 11 -> 3
        // 22 -> 5
        // 10 -> 1
        // 20 -> 2
        // 21 -> 4

        fftwf_complex *phi_1 = (fftwf_complex *) fftwf_malloc(sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);

        // First generate the ii,jj phi_1 boxes

        int phi_component;

        float component_ii,component_jj,component_ij;

        // Indexing for the various phy components
        int phi_directions[3][2] = {{0,1},{0,2},{1,2}};

#pragma omp parallel shared(HIRES_box,phi_1) private(i,j,k) num_threads(user_params->N_THREADS)
        {
#pragma omp for
            for (i=0; i<user_params->DIM; i++){
                for (j=0; j<user_params->DIM; j++){
                    for (k=0; k<user_params->DIM; k++){
                        *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                           (unsigned long long)(j),
                                                           (unsigned long long)(k)) ) = 0.;
                    }
                }
            }
        }

        // First iterate over the i = j components to phi
        // We'll also save these temporarily to the hires_vi_2LPT boxes which will get
        // overwritten later with the correct 2LPT velocities
        for(phi_component=0;phi_component<3;phi_component++) {

            i = j = phi_component;

                // generate the phi_1 boxes in Fourier transform
#pragma omp parallel shared(HIRES_box,phi_1,i,j) private(n_x,n_y,n_z,k_x,k_y,k_z,k_sq,k) num_threads(user_params->N_THREADS)
                {
#pragma omp for
                for (n_x=0; n_x<user_params->DIM; n_x++){
                    if (n_x>MIDDLE)
                        k_x =(n_x-user_params->DIM) * DELTA_K;  // wrap around for FFT convention
                    else
                        k_x = n_x * DELTA_K;

                    for (n_y=0; n_y<user_params->DIM; n_y++){
                        if (n_y>MIDDLE)
                            k_y =(n_y-user_params->DIM) * DELTA_K;
                        else
                            k_y = n_y * DELTA_K;

                        for (n_z=0; n_z<=MIDDLE; n_z++){
                            k_z = n_z * DELTA_K;

                            k_sq = k_x*k_x + k_y*k_y + k_z*k_z;

                            float k[] = {k_x, k_y, k_z};
                            // now set the velocities
                            if ((n_x==0) && (n_y==0) && (n_z==0)){ // DC mode
                                phi_1[0] = 0;
                            }
                            else{
                                phi_1[C_INDEX(n_x,n_y,n_z)] = -k[i]*k[j]*HIRES_box_saved[C_INDEX(n_x, n_y, n_z)]/k_sq/VOLUME;
                                // note the last factor of 1/VOLUME accounts for the scaling in real-space, following the FFT
                            }
                        }
                    }
                }
            }

            dft_c2r_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, phi_1);

            // Temporarily store in the allocated hires_vi_2LPT boxes
#pragma omp parallel shared(boxes,phi_1,phi_component) private(i,j,k) num_threads(user_params->N_THREADS)
            {
#pragma omp for
                for (i=0; i<user_params->DIM; i++){
                    for (j=0; j<user_params->DIM; j++){
                        for (k=0; k<user_params->DIM; k++){
                            if(phi_component==0) {
                                boxes->hires_vx_2LPT[R_INDEX(i,j,k)] = *((float *)phi_1 + R_FFT_INDEX((unsigned long long)(i),
                                                                                (unsigned long long)(j),
                                                                                (unsigned long long)(k)));
                            }
                            if(phi_component==1) {
                                boxes->hires_vy_2LPT[R_INDEX(i,j,k)] = *((float *)phi_1 + R_FFT_INDEX((unsigned long long)(i),
                                                                                (unsigned long long)(j),
                                                                                (unsigned long long)(k)));
                            }
                            if(phi_component==2) {
                                boxes->hires_vz_2LPT[R_INDEX(i,j,k)] = *((float *)phi_1 + R_FFT_INDEX((unsigned long long)(i),
                                                                                (unsigned long long)(j),
                                                                                (unsigned long long)(k)));
                            }
                        }
                    }
                }
            }
        }

        for(phi_component=0;phi_component<3;phi_component++) {
            // Now calculate the cross components and start evaluating the 2LPT field
            i = phi_directions[phi_component][0];
            j = phi_directions[phi_component][1];

            // generate the phi_1 boxes in Fourier transform
#pragma omp parallel shared(HIRES_box,phi_1) private(n_x,n_y,n_z,k_x,k_y,k_z,k_sq,k) num_threads(user_params->N_THREADS)
            {
#pragma omp for
                for (n_x=0; n_x<user_params->DIM; n_x++){
                    if (n_x>MIDDLE)
                        k_x =(n_x-user_params->DIM) * DELTA_K;  // wrap around for FFT convention
                    else
                        k_x = n_x * DELTA_K;

                    for (n_y=0; n_y<user_params->DIM; n_y++){
                        if (n_y>MIDDLE)
                            k_y =(n_y-user_params->DIM) * DELTA_K;
                        else
                            k_y = n_y * DELTA_K;

                        for (n_z=0; n_z<=MIDDLE; n_z++){
                            k_z = n_z * DELTA_K;

                            k_sq = k_x*k_x + k_y*k_y + k_z*k_z;

                            float k[] = {k_x, k_y, k_z};
                            // now set the velocities
                            if ((n_x==0) && (n_y==0) && (n_z==0)){ // DC mode
                                phi_1[0] = 0;
                            }
                            else{
                                phi_1[C_INDEX(n_x,n_y,n_z)] = -k[i]*k[j]*HIRES_box_saved[C_INDEX(n_x, n_y, n_z)]/k_sq/VOLUME;
                                // note the last factor of 1/VOLUME accounts for the scaling in real-space, following the FFT
                            }
                        }
                    }
                }
            }

            dft_c2r_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, phi_1);

            // Then we will have the laplacian of phi_2 (eq. D13b)
            // After that we have to return in Fourier space and generate the Fourier transform of phi_2
#pragma omp parallel shared(HIRES_box,phi_1,phi_component) private(i,j,k,component_ii,component_jj,component_ij) num_threads(user_params->N_THREADS)
            {
#pragma omp for
                for (i=0; i<user_params->DIM; i++){
                    for (j=0; j<user_params->DIM; j++){
                        for (k=0; k<user_params->DIM; k++){
                            // Note, I have temporarily stored the components into other arrays to minimise memory usage
                            // phi - {0, 1, 2} -> {hires_vx_2LPT, hires_vy_2LPT, hires_vz_2LPT}
                            // This may be opaque to the user, but this shouldn't need modification
                            if(phi_component==0) {
                                component_ii = boxes->hires_vx_2LPT[R_INDEX(i,j,k)];
                                component_jj = boxes->hires_vy_2LPT[R_INDEX(i,j,k)];
                                component_ij = *((float *)phi_1 + R_FFT_INDEX((unsigned long long)(i),
                                                                              (unsigned long long)(j),
                                                                              (unsigned long long)(k)));
                            }
                            if(phi_component==1) {
                                component_ii = boxes->hires_vx_2LPT[R_INDEX(i,j,k)];
                                component_jj = boxes->hires_vz_2LPT[R_INDEX(i,j,k)];
                                component_ij = *((float *)phi_1 + R_FFT_INDEX((unsigned long long)(i),
                                                                              (unsigned long long)(j),
                                                                              (unsigned long long)(k)));
                            }
                            if(phi_component==2) {
                                component_ii = boxes->hires_vy_2LPT[R_INDEX(i,j,k)];
                                component_jj = boxes->hires_vz_2LPT[R_INDEX(i,j,k)];
                                component_ij = *((float *)phi_1 + R_FFT_INDEX((unsigned long long)(i),
                                                                              (unsigned long long)(j),
                                                                              (unsigned long long)(k)));
                            }

                            // Kept in this form to maintain similar (possible) rounding errors
                            *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                               (unsigned long long)(j),
                                                               (unsigned long long)(k)) ) += \
                            ( component_ii * component_jj );

                            *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                               (unsigned long long)(j),
                                                               (unsigned long long)(k)) ) -= \
                            ( component_ij * component_ij );
                        }
                    }
                }
            }
        }

#pragma omp parallel shared(HIRES_box,phi_1) private(i,j,k) num_threads(user_params->N_THREADS)
        {
#pragma omp for
            for (i=0; i<user_params->DIM; i++){
                for (j=0; j<user_params->DIM; j++){
                    for (k=0; k<user_params->DIM; k++){
                        *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),(unsigned long long)(j),(unsigned long long)(k)) ) /= TOT_NUM_PIXELS;
                    }
                }
            }
        }

        // Perform FFTs
        dft_r2c_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, HIRES_box);

        memcpy(HIRES_box_saved, HIRES_box, sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);

        // Now we can store the content of box in a back-up array
        // Then we can generate the gradients of phi_2 (eq. D13b and D9)

        // ***** Store back-up k-box RHS eq. D13b ***** //

        // For each component, we generate the velocity field (same as the ZA part)

        // Now let's set the velocity field/dD/dt (in comoving Mpc)

        // read in the box
        // TODO correct free of phi_1

        for(ii=0;ii<3;ii++) {

            if(ii>0) {
                memcpy(HIRES_box, HIRES_box_saved, sizeof(fftwf_complex)*KSPACE_NUM_PIXELS);
            }

#pragma omp parallel shared(HIRES_box,ii) private(n_x,n_y,n_z,k_x,k_y,k_z,k_sq) num_threads(user_params->N_THREADS)
            {
#pragma omp for
            // set velocities/dD/dt
                for (n_x=0; n_x<user_params->DIM; n_x++){
                    if (n_x>MIDDLE)
                        k_x =(n_x-user_params->DIM) * DELTA_K;  // wrap around for FFT convention
                    else
                        k_x = n_x * DELTA_K;

                    for (n_y=0; n_y<user_params->DIM; n_y++){
                        if (n_y>MIDDLE)
                            k_y =(n_y-user_params->DIM) * DELTA_K;
                        else
                            k_y = n_y * DELTA_K;

                        for (n_z=0; n_z<=MIDDLE; n_z++){
                            k_z = n_z * DELTA_K;

                            k_sq = k_x*k_x + k_y*k_y + k_z*k_z;

                            // now set the velocities
                            if ((n_x==0) && (n_y==0) && (n_z==0)){ // DC mode
                                HIRES_box[0] = 0;
                            }
                            else{
                                if(ii==0) {
                                    HIRES_box[C_INDEX(n_x,n_y,n_z)] *= k_x*I/k_sq;
                                }
                                if(ii==1) {
                                    HIRES_box[C_INDEX(n_x,n_y,n_z)] *= k_y*I/k_sq;
                                }
                                if(ii==2) {
                                    HIRES_box[C_INDEX(n_x,n_y,n_z)] *= k_z*I/k_sq;
                                }
                            }
                        }
                        // note the last factor of 1/VOLUME accounts for the scaling in real-space, following the FFT
                    }
                }
            }

            // Filter only if we require perturbing on the low-res grid
            if(!user_params->PERTURB_ON_HIGH_RES) {
                if (user_params->DIM != user_params->HII_DIM) {
                    filter_box(HIRES_box, 0, 0, L_FACTOR*user_params->BOX_LEN/(user_params->HII_DIM+0.0));
                }
            }

            dft_c2r_cube(user_params->USE_FFTW_WISDOM, user_params->DIM, user_params->N_THREADS, HIRES_box);

            // now sample to lower res
            // now sample the filtered box
#pragma omp parallel shared(boxes,HIRES_box,f_pixel_factor,ii,dimension) private(i,j,k) num_threads(user_params->N_THREADS)
            {
#pragma omp for
                for (i=0; i<dimension; i++){
                    for (j=0; j<dimension; j++){
                        for (k=0; k<dimension; k++){
                            if(user_params->PERTURB_ON_HIGH_RES) {
                                if(ii==0) {
                                    boxes->hires_vx_2LPT[R_INDEX(i,j,k)] =
                                    *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                                       (unsigned long long)(j),
                                                                       (unsigned long long)(k)));
                                }
                                if(ii==1) {
                                    boxes->hires_vy_2LPT[R_INDEX(i,j,k)] =
                                    *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                                       (unsigned long long)(j),
                                                                       (unsigned long long)(k)));
                                }
                                if(ii==2) {
                                    boxes->hires_vz_2LPT[R_INDEX(i,j,k)] =
                                    *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i),
                                                                       (unsigned long long)(j),
                                                                       (unsigned long long)(k)));
                                }
                            }
                            else {
                                if(ii==0) {
                                    boxes->lowres_vx_2LPT[HII_R_INDEX(i,j,k)] =
                                    *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                                       (unsigned long long)(j*f_pixel_factor+0.5),
                                                                       (unsigned long long)(k*f_pixel_factor+0.5)));
                                }
                                if(ii==1) {
                                    boxes->lowres_vy_2LPT[HII_R_INDEX(i,j,k)] =
                                    *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                                       (unsigned long long)(j*f_pixel_factor+0.5),
                                                                       (unsigned long long)(k*f_pixel_factor+0.5)));
                                }
                                if(ii==2) {
                                    boxes->lowres_vz_2LPT[HII_R_INDEX(i,j,k)] =
                                    *((float *)HIRES_box + R_FFT_INDEX((unsigned long long)(i*f_pixel_factor+0.5),
                                                                       (unsigned long long)(j*f_pixel_factor+0.5),
                                                                       (unsigned long long)(k*f_pixel_factor+0.5)));
                                }
                            }
                        }
                    }
                }
            }
        }

        // deallocate the supplementary boxes
        fftwf_free(phi_1);

    }
    LOG_DEBUG("Done 2LPT.");

    // * *********************************************** * //
    // *               END 2LPT PART                     * //
    // * *********************************************** * //
    fftwf_cleanup_threads();
    fftwf_cleanup();
    fftwf_forget_wisdom();

    // deallocate
    fftwf_free(HIRES_box);
    fftwf_free(HIRES_box_saved);

    free_ps();

    for (i=0; i<user_params->N_THREADS; i++) {
        gsl_rng_free (r[i]);
    }
    gsl_rng_free(rseed);
    LOG_DEBUG("Cleaned Up.");
    } // End of Try{}

    Catch(status){
        return(status);
    }
    return(0);
}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for the implementation of the Fourier domain response for LISA-like detectors.
 *
 */

#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 "LISAgeometry.h"
#include "LISAFDresponse.h"

/***************************************/
/********* Core functions **************/

/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LISA response, for given values of the inclination, position in the sky and polarization angle - here simplified version for just the y_21 observable */
/* Older version of th FD response: two stages, first for the orbital delay (Bessel phase) and second for the constellation delay/modulation */
int LISASimFDResponse21(
  LISAconstellation *variant,                                 /* Provides specifics on the variant of LISA */
  struct tagListmodesCAmpPhaseFrequencySeries **list,  /* Input/Output: list of modes in Frequency-domain amplitude and phase form as produced by the ROM, and output after FD response processing */
  const double inclination,                                   /* Inclination of the source */
  const double lambda,                                        /* First angle for the position in the sky */
  const double beta,                                          /* Second angle for the position in the sky */
  const double psi)                                           /* Polarization angle */
{
  /* Computing the complicated trigonometric coefficients */
  //clock_t begsetcoeffs = clock();
  SetCoeffsG(lambda, beta, psi);
  //clock_t endsetcoeffs = clock();
  //printf("Set Coeffs time: %g s\n", (double)(endsetcoeffs - begsetcoeffs) / CLOCKS_PER_SEC);

  /* Main loop over the modes - goes through all the modes present */
  ListmodesCAmpPhaseFrequencySeries* listelement = *list;
  while(listelement) {

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    gsl_vector* freq = freqseries->freq;
    gsl_vector* amp_real = freqseries->amp_real;
    gsl_vector* amp_imag = freqseries->amp_imag;
    gsl_vector* phase = freqseries->phase;
    int len = (int) freq->size;
    double f, tf, bphi;
    double complex camp;

    /* Computing the Ylm combined factors for plus and cross for this mode */
    /* Capital Phi is set to 0 by convention */
    double complex Yfactorplus;
    double complex Yfactorcross;
    if (!(l%2)) {
      Yfactorplus = 1/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }
    else {
      Yfactorplus = 1/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }

    /* First step of the processing: orbital delay */
    /* Initializing spline for the phase */
    gsl_spline* spline_phi = gsl_spline_alloc(gsl_interp_cspline, len);
    gsl_interp_accel* accel_phi = gsl_interp_accel_alloc();
    gsl_spline_init(spline_phi, gsl_vector_const_ptr(freq, 0), gsl_vector_const_ptr(phase, 0), len);
    /* Vector keeping track of the Bessel Phase correction - to be used in the next step */
    gsl_vector* besselphi = gsl_vector_alloc(len);

    /* Loop over the frequencies - computing the correction due to the orbital delay */
    for(int j=0; j<len; j++) {
      f = gsl_vector_get(freq, j);
      tf =  gsl_spline_eval_deriv(spline_phi, f, accel_phi)/(2*PI);
      bphi = -2*PI*f*variant->OrbitR/C_SI*cos(beta) * cos( variant->OrbitOmega*tf + variant->OrbitPhi0 - lambda );
      camp = gsl_vector_get(amp_real, j) * cexp(I*bphi); /* Amplitude is real before applying this first delay */
      gsl_vector_set(amp_real, j, creal(camp));
      gsl_vector_set(amp_imag, j, cimag(camp));
      gsl_vector_set(besselphi, j, bphi);
    }

    /* Second step of the processing: constellation delay/modulation */
    /* Initializing spline for the bessel phase */
    gsl_spline* spline_besselphi = gsl_spline_alloc(gsl_interp_cspline, len);
    gsl_interp_accel* accel_besselphi = gsl_interp_accel_alloc();
    gsl_spline_init(spline_besselphi, gsl_vector_const_ptr(freq, 0), gsl_vector_const_ptr(besselphi, 0), len);
    /* Loop over the frequencies - computing the correction due to the orbital delay */
    for(int j=0; j<len; j++) {
      f = gsl_vector_get(freq, j);
      tf =  (gsl_spline_eval_deriv(spline_phi, f, accel_phi) + gsl_spline_eval_deriv(spline_besselphi, f, accel_besselphi))/(2*PI);
      camp = G21mode(variant, f, tf, Yfactorplus, Yfactorcross) * (gsl_vector_get(amp_real, j) + I * gsl_vector_get(amp_imag, j));
      /**/
      gsl_vector_set(amp_real, j, creal(camp));
      gsl_vector_set(amp_imag, j, cimag(camp));
    }

    listelement = listelement->next;

    /* Clean up */
    gsl_spline_free(spline_phi);
    gsl_interp_accel_free(accel_phi);
    gsl_vector_free(besselphi);
    gsl_spline_free(spline_besselphi);
    gsl_interp_accel_free(accel_besselphi);
  }

  return SUCCESS;
}

//WARNING: tRef is ignored for now in the response - i.e. set to 0
/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LISA response, for given values of the inclination, position in the sky and polarization angle */
int LISASimFDResponsey12(
  LISAconstellation *variant,                                 /* Provides specifics on the variant of LISA */
  struct tagListmodesCAmpPhaseFrequencySeries **list,      /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  struct tagListmodesCAmpPhaseFrequencySeries **listy12,   /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the y12 observable */
  const double torb,                                       /* Reference orbital time - tf as read from the hlm gives t-tinj, this arg allows to pass tinj to the response */
  const double lambda,                                        /* First angle for the position in the sky */
  const double beta,                                          /* Second angle for the position in the sky */
  const double inclination,                                   /* Inclination of the source */
  const double psi,                                           /* Polarization angle */
  const int tagfrozenLISA,                                    /* Tag to treat LISA as frozen at its torb configuration  */
  const ResponseApproxtag responseapprox)                     /* Tag to select possible low-f approximation level in FD response */
{
  /* Computing the complicated trigonometric coefficients */
  //clock_t begsetcoeffs = clock();
  SetCoeffsG(lambda, beta, psi);
  //clock_t endsetcoeffs = clock();
  //printf("Set Coeffs time: %g s\n", (double)(endsetcoeffs - begsetcoeffs) / CLOCKS_PER_SEC);

  /* Main loop over the modes - goes through all the modes present, stopping when encountering NULL */
  ListmodesCAmpPhaseFrequencySeries* listelement = *list;
  while(listelement) {

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    gsl_vector* freq = freqseries->freq;
    gsl_vector* amp_real = freqseries->amp_real;
    gsl_vector* amp_imag = freqseries->amp_imag;
    gsl_vector* phase = freqseries->phase;
    int len = (int) freq->size;
    double f, tf, tforb;
    double complex g12mode = 0.;
    double complex g21mode = 0.;
    double complex g23mode = 0.;
    double complex g32mode = 0.;
    double complex g31mode = 0.;
    double complex g13mode = 0.;
    double complex camp_y;

    /* Computing the Ylm combined factors for plus and cross for this mode */
    /* Capital Phi is set to 0 by convention */
    double complex Yfactorplus;
    double complex Yfactorcross;
    if (!(l%2)) {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }
    else {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }

    /* Initializing spline for the phase */
    gsl_spline* spline_phi = gsl_spline_alloc(gsl_interp_cspline, len);
    gsl_interp_accel* accel_phi = gsl_interp_accel_alloc();
    gsl_spline_init(spline_phi, gsl_vector_const_ptr(freq, 0), gsl_vector_const_ptr(phase, 0), len);

    /* Orbital delay, constellation delay/modulation */
    /* Initializing frequency series structure for this mode, for each of the TDI observables */
    CAmpPhaseFrequencySeries *modefreqseries = NULL;
    CAmpPhaseFrequencySeries_Init(&modefreqseries, len);
    gsl_vector* freq_y = modefreqseries->freq;
    gsl_vector* amp_real_y = modefreqseries->amp_real;
    gsl_vector* amp_imag_y = modefreqseries->amp_imag;
    gsl_vector* phase_y = modefreqseries->phase;
    /* Loop over the frequencies - computing the correction due to the constellation delay/modulation */
    //clock_t tbegcontesllation = clock();
    //double timingcumulativeGABmode = 0;
    for(int j=0; j<len; j++) {
      f = gsl_vector_get(freq, j);
      tf = (gsl_spline_eval_deriv(spline_phi, f, accel_phi))/(2*PI);
      /* tf read from hlm is t-tinj - here convert to orbital time */
      if(!(tagfrozenLISA)) {
        tforb = tf + torb;
      } else {
        tforb = torb;
      }
      //clock_t tbegGAB = clock();
	  EvaluateGABmode(variant, &g12mode, &g21mode, &g23mode, &g32mode, &g31mode, &g13mode, f, tforb, Yfactorplus, Yfactorcross, 1, responseapprox); /* does include the R-delay term */
      //clock_t tendGAB = clock();
      //timingcumulativeGABmode += (double) (tendGAB-tbegGAB) /CLOCKS_PER_SEC;
      /**/
      camp_y = (gsl_vector_get(amp_real, j) + I * gsl_vector_get(amp_imag, j)) * g12mode;
      /**/
      gsl_vector_set(amp_real_y, j, creal(camp_y));
      gsl_vector_set(amp_imag_y, j, cimag(camp_y));
    }
    //clock_t tendcontesllation = clock();
  //printf("Set constellation time: %g s\n", (double)(tendcontesllation - tbegcontesllation) / CLOCKS_PER_SEC);
  //printf("GAB cumulated time: %g s\n", timingcumulativeGABmode);

    /* Copying the vectors of frequencies and phases */
    gsl_vector_memcpy(freq_y, freq);
    gsl_vector_memcpy(phase_y, phase);

    /* Append the modes to the ouput list-of-modes structures */
    *listy12 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*listy12, modefreqseries, l, m);

    /* Going to the next mode in the list */
    listelement = listelement->next;

    /* Clean up */
    gsl_spline_free(spline_phi);
    gsl_interp_accel_free(accel_phi);
  }

  return SUCCESS;
}

//WARNING: tRef is ignored for now in the response - i.e. set to 0
/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LISA response, for given values of the inclination, position in the sky and polarization angle */
int LISASimFDResponseTDI3Chan(
  int tagtRefatLISA,                                          /* 0 to measure Tref from SSB arrival, 1 at LISA guiding center */
  LISAconstellation *variant,                                 /* Provides specifics on the variant of LISA */
  struct tagListmodesCAmpPhaseFrequencySeries **list,      /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  struct tagListmodesCAmpPhaseFrequencySeries **listTDI1,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the TDI channel 1 */
  struct tagListmodesCAmpPhaseFrequencySeries **listTDI2,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the TDI channel 2 */
  struct tagListmodesCAmpPhaseFrequencySeries **listTDI3,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the TDI channel 3 */
  const double torb,                                       /* Reference orbital time - tf as read from the hlm gives t-tinj, this arg allows to pass tinj to the response */
  const double lambda,                                     /* First angle for the position in the sky */
  const double beta,                                       /* Second angle for the position in the sky */
  const double inclination,                                /* Inclination of the source */
  const double psi,                                        /* Polarization angle */
  const double m1,                                         /* m1 in solar masses - used for resampling */
  const double m2,                                         /* m2 in solar masses - used for resampling */
  const double maxf,                                       /* Maximal frequency to consider - used to ignore hard-to-resolve response at f>1Hz - NOTE: for now, no recomputation of the boundary, so when not resampling can lose a bit of support between the last frequency point covered and maxf */
  const TDItag tditag,                                     /* Selector for the set of TDI observables */
  const int tagfrozenLISA,                                 /* Tag to treat LISA as frozen at its torb configuration  */
  const ResponseApproxtag responseapprox)                  /* Tag to select possible low-f approximation level in FD response */
{
  /* Computing the complicated trigonometric coefficients */
  //clock_t begsetcoeffs = clock();
  SetCoeffsG(lambda, beta, psi);
  //clock_t endsetcoeffs = clock();
  //printf("Set Coeffs time: %g s\n", (double)(endsetcoeffs - begsetcoeffs) / CLOCKS_PER_SEC);

  /* Chirp mass for resampling */
  double mchirp = Mchirpofm1m2(m1, m2);

  /* Main loop over the modes - goes through all the modes present, stopping when encountering NULL */
  ListmodesCAmpPhaseFrequencySeries* listelement = *list;
  while(listelement) {

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    gsl_vector* freq = freqseries->freq;
    gsl_vector* amp_real = freqseries->amp_real;
    gsl_vector* amp_imag = freqseries->amp_imag;
    gsl_vector* phase = freqseries->phase;
    int len = (int) freq->size;

    //
    //printf("len: %d\n", len);
    //
    //if(l==2&&m==1) printf("no resampling 21\n");
    //      if(l==2&&m==1) {for(int i=0; i<len; i++) printf("%d, %g, %g, %g, %g\n", i, gsl_vector_get(freq, i), gsl_vector_get(amp_real, i), gsl_vector_get(amp_imag, i), gsl_vector_get(phase, i));};

    double f, tf, tforb;
    double complex g21mode = 0.;
    double complex g12mode = 0.;
    double complex g32mode = 0.;
    double complex g23mode = 0.;
    double complex g13mode = 0.;
    double complex g31mode = 0.;
    double complex camp;
    double complex camp1;
    double complex camp2;
    double complex camp3;
    double complex factor1 = 0.;
    double complex factor2 = 0.;
    double complex factor3 = 0.;

    /* Computing the Ylm combined factors for plus and cross for this mode */
    /* Capital Phi is set to 0 by convention */
    double complex Yfactorplus;
    double complex Yfactorcross;
    if (!(l%2)) {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }
    else {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }

    /* Initializing spline for the phase - will be used to compute tf */
    gsl_spline* spline_phi = gsl_spline_alloc(gsl_interp_cspline, len);
    gsl_interp_accel* accel_phi = gsl_interp_accel_alloc();
    gsl_spline_init(spline_phi, gsl_vector_const_ptr(freq, 0), gsl_vector_const_ptr(phase, 0), len);

    /* Resampling at high f to achieve a deltaf of at most 0.002 Hz */
    /* Resample linearly at this deltaf when this threshold is reached */
    /* NOTE: Assumes input frequencies are logarithmic (except maybe first interval) to evaluate when to resample */
    gsl_vector* freqrhigh = NULL;
    SetMaxdeltafResampledFrequencies(&freqrhigh, freq, maxf, 0.002); /* Use 0.002Hz as a default maximal deltaf */

    /* Resampling at low f to achieve a deltat of at most 2 weeks */
    /* Resample linearly in time until this threshold is reached */
    /* NOTE: uses Newtonian estimates - also requires the chirp mass as extra information */
    /* It would also be possible to use t(f) (more accurate), but we would also need to interpolate f(t) */
    /* Accuracy in time of this resampling is not critical, estimate should be enough */
    gsl_vector* freqr = NULL;
    SetMaxdeltatResampledFrequencies(&freqr, freqrhigh, 1./24, mchirp, m); /* Use half a month as a default maximal deltaft */

    /* Evaluate resampled waveform */
    CAmpPhaseFrequencySeries* freqseriesr = NULL;
    CAmpPhaseFrequencySeries_Resample(&freqseriesr, freqseries, freqr);
    gsl_vector* freq_resample = freqseriesr->freq;
    gsl_vector* amp_real_resample = freqseriesr->amp_real;
    gsl_vector* amp_imag_resample = freqseriesr->amp_imag;
    gsl_vector* phase_resample = freqseriesr->phase;
    int len_resample = (int) freq_resample->size;

    // /* Determine frequencies to use */
    // /* Because we will need the interpolation on the Re/Im amplitude to resolve the structure of the L-response at high fequencies, we add points beyond 0.01 Hz, with a linear sampling */
    // /* WARNING : It seemed 600 points between 0.1 and 3 Hz (deltaf=0.005) should give interpolation errors below 1e-4 - considering a simple sin(2 pi f L) */
    // /* But first test with TDIA show the sampling should be reduced 10-fold - possibly large increase in cost */
    // /* We use here deltaf=0.0005 until the cause is better understood */
    // /* NOTE: the structure in the response due to the R-delay gives much larger interpolation errors - here we assume the R-delay term is now treated as a phase */
    // int resampled = 0; /* Keeps track of wether or not we resampled and allocated new resources we need to free */
    // double maxfsignal = gsl_vector_get(freq, len-1);
    // double fHigh = fmin(maxf, maxfsignal);
    // /* BEWARE : as Mathematica tests show, this is way too pessimistic - normally deltaf=0.002Hz sould work */
    // /* To be investigated */
    // double fHigh_log_samp = 0.002;
    // double deltaflineartarget = 0.00002; //1e-5 better, but slower
    // int ifmax = len-1; /* last index to be covered in original sampling overall */
    // while((gsl_vector_get(freq, ifmax)>fHigh) && ifmax>0) ifmax--;
    // int imaxlogsampling = ifmax; /* last index to be covered with original sampling */
    // while((gsl_vector_get(freq, imaxlogsampling)>fHigh_log_samp) && imaxlogsampling>0) imaxlogsampling--;
    // gsl_vector* freq_resample = NULL; /*  */
    // gsl_vector* amp_real_resample = NULL;
    // gsl_vector* amp_imag_resample = NULL;
    // gsl_vector* phase_resample = NULL;
    // int len_resample;
    //
    // if((fHigh>fHigh_log_samp) && ((fHigh - gsl_vector_get(freq, imaxlogsampling))/deltaflineartarget)>ifmax-imaxlogsampling) { /* condition to check if the linear sampling will add points - if not, do nothing */
    //
    //   resampled = 1;
    //   /* Number of pts in resampled part */
    //   int nbfreqlinear = ceil((fHigh - gsl_vector_get(freq, imaxlogsampling))/deltaflineartarget);
    //   double deltaflinear = (fHigh - gsl_vector_get(freq, imaxlogsampling))/(nbfreqlinear + 1);
    //   /* Initialize new vectors */
    //   len_resample = imaxlogsampling + 1 + nbfreqlinear;
    //   freq_resample = gsl_vector_alloc(len_resample);
    //   amp_real_resample = gsl_vector_alloc(len_resample);
    //   amp_imag_resample = gsl_vector_alloc(len_resample);
    //   phase_resample = gsl_vector_alloc(len_resample);
    //   /* Build interpolation for original amp_real, amp_imag and phase */
    //   /* NOTE: we could use spline_phi here, written this way for clarity */
    //   gsl_spline* spline_amp_real = gsl_spline_alloc(gsl_interp_cspline, len);
    //   gsl_spline* spline_amp_imag = gsl_spline_alloc(gsl_interp_cspline, len);
    //   gsl_spline* spline_phase = gsl_spline_alloc(gsl_interp_cspline, len);
    //   gsl_interp_accel* accel_amp_real = gsl_interp_accel_alloc();
    //   gsl_interp_accel* accel_amp_imag = gsl_interp_accel_alloc();
    //   gsl_interp_accel* accel_phase = gsl_interp_accel_alloc();
    //   gsl_spline_init(spline_amp_real, gsl_vector_const_ptr(freq, 0), gsl_vector_const_ptr(amp_real, 0), len);
    //   gsl_spline_init(spline_amp_imag, gsl_vector_const_ptr(freq, 0), gsl_vector_const_ptr(amp_imag, 0), len);
    //   gsl_spline_init(spline_phase, gsl_vector_const_ptr(freq, 0), gsl_vector_const_ptr(phase, 0), len);
    //   /* Set resampled frequencies and values */
    //   for(int j=0; j<=imaxlogsampling; j++) {
    //     gsl_vector_set(freq_resample, j, gsl_vector_get(freq, j));
    //     gsl_vector_set(amp_real_resample, j, gsl_vector_get(amp_real, j));
    //     gsl_vector_set(amp_imag_resample, j, gsl_vector_get(amp_imag, j));
    //     gsl_vector_set(phase_resample, j, gsl_vector_get(phase, j));
    //   }
    //   double fimax = gsl_vector_get(freq, imaxlogsampling);
    //   for(int j=imaxlogsampling+1; j<len_resample; j++) {
    //     f = fimax + (j-imaxlogsampling) * deltaflinear;
    //     gsl_vector_set(freq_resample, j, f);
    //     gsl_vector_set(amp_real_resample, j, gsl_spline_eval(spline_amp_real, f, accel_amp_real));
    //     gsl_vector_set(amp_imag_resample, j, gsl_spline_eval(spline_amp_imag, f, accel_amp_imag));
    //     gsl_vector_set(phase_resample, j, gsl_spline_eval(spline_phase, f, accel_phase));
    //   }
    //   /* Free interpolation functions */
    //   gsl_spline_free(spline_amp_real);
    //   gsl_spline_free(spline_amp_imag);
    //   gsl_spline_free(spline_phase);
    //   gsl_interp_accel_free(accel_amp_real);
    //   gsl_interp_accel_free(accel_amp_imag);
    //   gsl_interp_accel_free(accel_phase);
    // }
    // else { /* If no resampling, use the values we had as input */
    //   resampled = 0;
    //   len_resample = imaxlogsampling + 1; /* If maxf < maxfsignal, we will cut the signal above maxf by simply adjusting the range of indices included (NOTE: without recomputing the exact boundary, so some support is lost due to discretization) */
    //   freq_resample = freq;
    //   amp_real_resample = amp_real;
    //   amp_imag_resample = amp_imag;
    //   phase_resample = phase;
    // }

    /* Initializing frequency series structure for this mode, for each of the TDI observables */
    CAmpPhaseFrequencySeries *modefreqseries1 = NULL;
    CAmpPhaseFrequencySeries *modefreqseries2 = NULL;
    CAmpPhaseFrequencySeries *modefreqseries3 = NULL;
    CAmpPhaseFrequencySeries_Init(&modefreqseries1, len_resample);
    CAmpPhaseFrequencySeries_Init(&modefreqseries2, len_resample);
    CAmpPhaseFrequencySeries_Init(&modefreqseries3, len_resample);
    gsl_vector* freq1 = modefreqseries1->freq;
    gsl_vector* amp_real1 = modefreqseries1->amp_real;
    gsl_vector* amp_imag1 = modefreqseries1->amp_imag;
    gsl_vector* phase1 = modefreqseries1->phase;
    gsl_vector* freq2 = modefreqseries2->freq;
    gsl_vector* amp_real2 = modefreqseries2->amp_real;
    gsl_vector* amp_imag2 = modefreqseries2->amp_imag;
    gsl_vector* phase2 = modefreqseries2->phase;
    gsl_vector* freq3 = modefreqseries3->freq;
    gsl_vector* amp_real3 = modefreqseries3->amp_real;
    gsl_vector* amp_imag3 = modefreqseries3->amp_imag;
    gsl_vector* phase3 = modefreqseries3->phase;

    /* Loop over the frequencies */
    //clock_t tbegcontesllation = clock();
    //double timingcumulativeGABmode = 0;
    for(int j=0; j<len_resample; j++) {
      f = gsl_vector_get(freq_resample, j);
      tf = (gsl_spline_eval_deriv(spline_phi, f, accel_phi))/(2*PI);
      /* tf read from hlm is t-tinj - here convert to orbital time using torb - ignore tf if orbit is frozen */
      if(!(tagfrozenLISA)) {
        tforb = tf + torb;
      } else {
        tforb = torb;
      }
      //clock_t tbegGAB = clock();
      EvaluateGABmode(variant, &g12mode, &g21mode, &g23mode, &g32mode, &g31mode, &g13mode, f, tforb, Yfactorplus, Yfactorcross, 0, responseapprox); /* does not include the R-delay term */
      //clock_t tendGAB = clock();
      //timingcumulativeGABmode += (double) (tendGAB-tbegGAB) /CLOCKS_PER_SEC;
      /**/
      EvaluateTDIfactor3Chan(variant, &factor1, &factor2, &factor3, g12mode, g21mode, g23mode, g32mode, g31mode, g13mode, f, tditag, responseapprox);
      double complex amphtilde = gsl_vector_get(amp_real_resample, j) + I * gsl_vector_get(amp_imag_resample, j);
      camp1 = factor1 * amphtilde;
      camp2 = factor2 * amphtilde;
      camp3 = factor3 * amphtilde;
      /* Phase term due to the R-delay, including correction to first order */
      double phase=variant->OrbitOmega*tforb + variant->OrbitPhi0 - lambda;
      double OrbitRoC=variant->OrbitR/C_SI;
      //double phaseRdelay = -2*PI*R_SI/C_SI*f*cos(beta)*cos(Omega_SI*tf - lambda) * (1 + R_SI/C_SI*cos(beta)*Omega_SI*sin(Omega_SI*tf - lambda));
      double phaseRdelay;
      if(tagtRefatLISA==0){//delay so that tinj=torb refers to arrival time at SSB
        phaseRdelay = -2*PI*OrbitRoC*f*cos(beta)*cos(phase) * (1 + cos(beta)*OrbitRoC*variant->OrbitOmega*sin(phase));
      } else {
        //In this version we delay so that tinj=torb is relative to LISAcenter arrival time, so there is no orbital delay when tf = tinj
        //If the original delay realized td=t+d(t), now we want td=t+d(t)-d(t0), that we we change d(t) -> d(t) + d(t0)
        //Then with the approximation d(td)=d(t)(1-ddot(t)),
        double phase0 = variant->OrbitOmega*torb + variant->OrbitPhi0 - lambda;
        phaseRdelay = -2*PI*OrbitRoC*f*cos(beta)*( cos(phase)-cos(phase0) ) * (1 + cos(beta)*OrbitRoC*variant->OrbitOmega*sin(phase));
      }
      if(responseapprox==lowf) { /* In the full low-f approximation, ignore this delay term */
        phaseRdelay = 0.;
      }
      double phasewithRdelay = gsl_vector_get(phase_resample, j) + phaseRdelay;

      /**/
      gsl_vector_set(amp_real1, j, creal(camp1));
      gsl_vector_set(amp_imag1, j, cimag(camp1));
      gsl_vector_set(amp_real2, j, creal(camp2));
      gsl_vector_set(amp_imag2, j, cimag(camp2));
      gsl_vector_set(amp_real3, j, creal(camp3));
      gsl_vector_set(amp_imag3, j, cimag(camp3));
      gsl_vector_set(phase1, j, phasewithRdelay);
      gsl_vector_set(phase2, j, phasewithRdelay);
      gsl_vector_set(phase3, j, phasewithRdelay);
    }
    //clock_t tendconstellation = clock();
    //printf("Set constellation time: %g s\n", (double)(tendconstellation - tbegconstellation) / CLOCKS_PER_SEC);
    //printf("GAB cumulated time: %g s\n", timingcumulativeGABmode);

    /* Copying the vectors of frequencies - we have to allow for the case where it has been shortened */
    gsl_vector_view freq_resample_subview = gsl_vector_subvector(freq_resample, 0, len_resample);
    gsl_vector_memcpy(freq1, &freq_resample_subview.vector);
    gsl_vector_memcpy(freq2, &freq_resample_subview.vector);
    gsl_vector_memcpy(freq3, &freq_resample_subview.vector);

    /* Append the modes to the ouput list-of-modes structures */
    *listTDI1 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*listTDI1, modefreqseries1, l, m);
    *listTDI2 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*listTDI2, modefreqseries2, l, m);
    *listTDI3 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*listTDI3, modefreqseries3, l, m);

    /* Going to the next mode in the list */
    listelement = listelement->next;

    /* Clean up */
    gsl_spline_free(spline_phi);
    gsl_interp_accel_free(accel_phi);
    gsl_vector_free(freqrhigh);
    gsl_vector_free(freqr);
    CAmpPhaseFrequencySeries_Cleanup(freqseriesr);
    // /* If we used resampling, then we need to free the additional resources that were allocated */
    // if(resampled) {
    //   gsl_vector_free(freq_resample);
    //   gsl_vector_free(amp_real_resample);
    //   gsl_vector_free(amp_imag_resample);
    //   gsl_vector_free(phase_resample);
    // }
  }

  return SUCCESS;
}


/*
 *  emu_noh.c
 *
 *
 *  Created by Earl Lawrence on 9/17/09.
 *  Update 11/30/2012
 *
 *  This program was prepared by Los Alamos National Security, LLC at Los Alamos National Laboratory (LANL)
 *  under contract No. DE-AC52-06NA25396 with the U.S. Department of Energy (DOE). All rights in the program
 *  are reserved by the DOE and Los Alamos National Security, LLC.  Permission is granted to the public to
 *  copy and use this software without charge, provided that this Notice and any statement of authorship are
 *  reproduced on all copies.  Neither the U.S. Government nor LANS makes any warranty, express or implied,
 *  or assumes any liability or responsibility for the use of this software.
 *
 *
 */

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>

#include "pcbasis_noh.h"
#include "design_noh.h"
#include "pcweights_noh.h"
#include "corrlengths_noh.h"
#include "precisions_noh.h"
#include "meansd.h"
#include "kemu.h"
#include "ksim.h"

#ifndef M_PI
#define M_PI           3.14159265358979323846
#endif

// Sizes of stuff and number of redshifts
static int m=37, neta=5500, p=5, peta=6, rs=11, nsim=582;
// Kriging basis computed by emuInit, sizes should be peta and m
static double KrigBasis[6][37];

// Initialization function that computes the Kriging basis
void emuInit_noh() {
    int i,j,k,l;
    double cov;
    gsl_matrix *SigmaSim = gsl_matrix_alloc(m,m);
    gsl_vector *b = gsl_vector_alloc(m);

    // Do these one principal component at a time
    for(i=0; i<peta; i++) {
        // Fill in the covariance matrix for the principals components
        // Also make a gsl_vector with the weights.
        for(j=0; j<m; j++) {
            // Diagonal
            gsl_matrix_set(SigmaSim, j, j, (1.0/lamz[i]) + (1.0/lamws[i]));
            // Off-diagonals
            for(k=0; k<j; k++) {
                // Compute the covariance
                cov = 0.0;
                for(l=0; l<p; l++) {
                    cov -= beta[i][l]*pow(x[j][l]-x[k][l], 2.0);
                }
                cov = exp(cov)/lamz[i];
                gsl_matrix_set(SigmaSim, j, k, cov);
                gsl_matrix_set(SigmaSim, k, j, cov);
            } // for(k=0; k<j; k++)
            gsl_vector_set(b, j, w[i][j]);
        } // for(j=0; j<m; j++)

        // Cholesky and solve
        gsl_linalg_cholesky_decomp(SigmaSim);
        gsl_linalg_cholesky_svx(SigmaSim, b);

        // Copy into the Kriging Basis
        for(j=0; j<m; j++) {
            KrigBasis[i][j] = gsl_vector_get(b, j);
        }
    } // for(i=0; i<peta; i++)
    gsl_matrix_free(SigmaSim);
    gsl_vector_free(b);
}

// The actual emulation
// Cosmological parameters, placeholder for the output, type of output
void emu_noh(double *xstar, double *ystar, int *outtype) {
    static int inited=0;
    int i, j, k;
    double wstar[peta], Sigmastar[peta][m], ystaremu[neta], ystar_allz[rs*nsim], logc;
    double xstarstd[p];
    double zemu[rs], ybyz[rs];
    FILE *fp;
    // Interpolation stuff for k and then z
    gsl_spline *lininterp_k = gsl_spline_alloc(gsl_interp_linear, neta/rs);
    gsl_spline *lininterp_z = gsl_spline_alloc(gsl_interp_linear, rs);
    gsl_interp_accel *accel = gsl_interp_accel_alloc();

    // Iinitialize if necessary
    if(inited==0) {
        emuInit_noh();
        inited=1;
    }

    // Check the inputs to make sure we're interpolating.
    for(i=0; i<p; i++) {
        if((xstar[i] < xmin[i]) || (xstar[i] > xmin[i]+xrange[i])) {
            //printf("The inputs are outside the domain of the emulator.\n");
            switch(i) {
                case 0:
                    printf("omega_b must be between %f and %f.\n", xmin[i], xmin[i]+xrange[i]);
                    break;
                case 1:
                    printf("omega_m must be between %f and %f.\n", xmin[i], xmin[i]+xrange[i]);
                    break;
                case 2:
                    printf("n_s must be between %f and %f.\n", xmin[i], xmin[i]+xrange[i]);
                    break;
                case 3:
                    printf("w must be between %f and %f.\n", xmin[i], xmin[i]+xrange[i]);
                    break;
                case 4:
                    printf("sigma_8 must be between %f and %f.\n", xmin[i], xmin[i]+xrange[i]);
                    break;
            }
            exit(1);
        }
    } // for(i=0; i<p; i++)

    // Check redshift to make sure we're interpolating
    if((xstar[p] < 0) || (xstar[p] > 4)) {
        //printf("The inputs are outside the domain of the emulator.\n");
        printf("z must between 0 and 4\n");
        exit(1);
    }

    // Standardize the inputs
    for(i=0; i<p; i++) {
        xstarstd[i] = (xstar[i] - xmin[i]) / xrange[i];
    }

    // Compute the covariances between the new input and sims for all PCs
    for(i=0; i<peta; i++) {
        for(j=0; j<m; j++) {
            logc = 0.0;
            for(k=0; k<p; k++) {
                logc -= beta[i][k]*pow(x[j][k]-xstarstd[k], 2.0);
            }
            Sigmastar[i][j] = exp(logc)/lamz[i];
        }
    }

    // Compute wstar, the predicted PC weights for the new input
    for(i=0; i<peta; i++) {
        wstar[i]=0.0;
        for(j=0; j<m; j++) {
            wstar[i] += Sigmastar[i][j] * KrigBasis[i][j];
        }
    }

    // Compute ystar, the new output
    for(i=0; i<neta; i++) {
        ystaremu[i] = 0.0;
        for(j=0; j<peta; j++) {
            ystaremu[i] += K[i][j]*wstar[j];
        }
        ystaremu[i] = ystaremu[i]*sd + mean[i];
    }

    //printf("Emulated\n");

    // Interpolate the emulated output onto the original domain.
    for(i=0; i<rs; i++) {
        gsl_spline_init(lininterp_k, kemu, &ystaremu[i*neta/rs], neta/rs);
        for(j=0; j<nsim; j++) {
            ystar_allz[i*nsim+j] = gsl_spline_eval(lininterp_k, ksim[j], accel);
        }
        gsl_interp_accel_reset(accel);
    }

    //printf("k interped\n");

    // Fill in the k values for the final output
    for(i=0; i<nsim; i++) {
        ystar[i] = ksim[i];
    }

    // Interpolate on to the desired redshift
    // The order needs to be reversed here.
    for(i=0; i<rs; i++) {
        zemu[i] = (1/aemu[rs-i-1]) - 1.0;
    }
    for(i=0; i<nsim; i++) {
        // Build an array with the values of y for a given value of z
        // Reverse the order
        for(j=0; j<rs; j++) {
            ybyz[rs-j-1] = ystar_allz[j*nsim+i];
        }
        gsl_spline_init(lininterp_z, zemu, ybyz, rs);
        ystar[nsim+i] = gsl_spline_eval(lininterp_z, xstar[p], accel);
        gsl_interp_accel_reset(accel);
    }

    //printf("z interped\n");

    switch(outtype[0]) {
        default:
	   for(j=0; j<nsim; j++) {
	     ystar[nsim+j] = pow(10.0,ystar[nsim+j]);
	   }

            break;
        case 1:
            // Transform to Delta^2
            for(j=0; j<nsim; j++) {
                ystar[nsim+j] = ystar[nsim+j] + 1.5*log10(ksim[j]);
                ystar[nsim+j] = pow(10.0,ystar[nsim+j]);
            }
            break;
        case 2:
            // Transform to P(k)
            for(j=0; j<nsim; j++) {
                ystar[nsim+j] = ystar[nsim+j] - 1.5*log10(ksim[j]);
                ystar[nsim+j] = pow(10.0,ystar[nsim+j])*2.0*M_PI*M_PI;
            }
            break;
    }

    // Free some stuff.  I always forget this.
    // Thanks to Tim Eifler for discovering it (apparently the hard way).
    gsl_spline_free(lininterp_z);
    gsl_spline_free(lininterp_k);
    gsl_interp_accel_free(accel);
}

// Linker function for use with Fortran
void emu_noh_(double *x, double *y, int *outtype) {
    void emu();
    emu(x,y,outtype);
}


/* specfunc/debye.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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.
 */

/* Author:  G. Jungman */
/* augmented to n=5 and 6 2005-11-08 by R. J. Mathar, http://www.strw.leidenuniv.nl/~mathar */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_debye.h>

#include "error.h"
#include "check.h"

#include "chebyshev.h"
#include "cheb_eval.c"

static double adeb1_data[17] = {
   2.4006597190381410194,
   0.1937213042189360089,
  -0.62329124554895770e-02,
   0.3511174770206480e-03,
  -0.228222466701231e-04,
   0.15805467875030e-05,
  -0.1135378197072e-06,
   0.83583361188e-08,
  -0.6264424787e-09,
   0.476033489e-10,
  -0.36574154e-11,
   0.2835431e-12,
  -0.221473e-13,
   0.17409e-14,
  -0.1376e-15,
   0.109e-16,
  -0.9e-18
};
static cheb_series adeb1_cs = {
  adeb1_data,
  16,
  -1.0, 1.0,
  9
};

static double adeb2_data[18] = {
   2.5943810232570770282,
   0.2863357204530719834,
  -0.102062656158046713e-01,
   0.6049109775346844e-03,
  -0.405257658950210e-04,
   0.28633826328811e-05,
  -0.2086394303065e-06,
   0.155237875826e-07,
  -0.11731280087e-08,
   0.897358589e-10,
  -0.69317614e-11,
   0.5398057e-12,
  -0.423241e-13,
   0.33378e-14,
  -0.2645e-15,
   0.211e-16,
  -0.17e-17,
   0.1e-18
};
static cheb_series adeb2_cs = {
  adeb2_data,
  17,
  -1.0, 1.0,
  10
};

static double adeb3_data[17] = {
   2.707737068327440945,
   0.340068135211091751,
  -0.12945150184440869e-01,
   0.7963755380173816e-03,
  -0.546360009590824e-04,
   0.39243019598805e-05,
  -0.2894032823539e-06,
   0.217317613962e-07,
  -0.16542099950e-08,
   0.1272796189e-09,
  -0.987963460e-11,
   0.7725074e-12,
  -0.607797e-13,
   0.48076e-14,
  -0.3820e-15,
   0.305e-16,
  -0.24e-17
};
static cheb_series adeb3_cs = {
  adeb3_data,
  16,
  -1.0, 1.0,
  10
};

static double adeb4_data[17] = {
   2.781869415020523460,
   0.374976783526892863,
  -0.14940907399031583e-01,
   0.945679811437042e-03,
  -0.66132916138933e-04,
   0.4815632982144e-05,
  -0.3588083958759e-06,
   0.271601187416e-07,
  -0.20807099122e-08,
   0.1609383869e-09,
  -0.125470979e-10,
   0.9847265e-12,
  -0.777237e-13,
   0.61648e-14,
  -0.4911e-15,
   0.393e-16,
  -0.32e-17
};
static cheb_series adeb4_cs = {
  adeb4_data,
  16,
  -1.0, 1.0,
  10
};

static double adeb5_data[17] = {
   2.8340269546834530149,
   0.3994098857106266445,
  -0.164566764773099646e-1,
   0.10652138340664541e-2,
  -0.756730374875418e-4,
   0.55745985240273e-5,
  -0.4190692330918e-6,
   0.319456143678e-7,
  -0.24613318171e-8,
   0.1912801633e-9,
  -0.149720049e-10,
   0.11790312e-11,
  -0.933329e-13,
   0.74218e-14,
  -0.5925e-15,
   0.475e-16,
  -0.39e-17
};
static cheb_series adeb5_cs = {
  adeb5_data,
  16,
  -1.0, 1.0,
  10
};

static double adeb6_data[17] = {
  2.8726727134130122113,
  0.4174375352339027746,
 -0.176453849354067873e-1,
  0.11629852733494556e-2,
 -0.837118027357117e-4,
  0.62283611596189e-5,
 -0.4718644465636e-6,
  0.361950397806e-7,
 -0.28030368010e-8,
  0.2187681983e-9,
 -0.171857387e-10,
  0.13575809e-11,
 -0.1077580e-12,
  0.85893e-14,
 -0.6872e-15,
  0.552e-16,
 -0.44e-17
};
static cheb_series adeb6_cs = {
  adeb6_data,
  16,
  -1.0, 1.0,
  10
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_debye_1_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 1.64493406684822644;
  const double xcut = -GSL_LOG_DBL_MIN;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0 - 0.25*x + x*x/36.0;
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double t = x*x/8.0 - 1.0;
    gsl_sf_result c;
    cheb_eval_e(&adeb1_cs, t, &c);
    result->val = c.val - 0.25 * x;
    result->err = c.err + 0.25 * x * GSL_DBL_EPSILON;
    return GSL_SUCCESS;
  }
  else if(x < -(M_LN2 + GSL_LOG_DBL_EPSILON)) {
    const int nexp = floor(xcut/x);
    const double ex  = exp(-x);
    double sum = 0.0;
    double xk  = nexp * x;
    double rk  = nexp;
    int i;
    for(i=nexp; i>=1; i--) {
      sum *= ex;
      sum += (1.0 + 1.0/xk)/rk;
      rk -= 1.0;
      xk -= x;
    }
    result->val = val_infinity/x - sum*ex;
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < xcut) {
    result->val = (val_infinity - exp(-x)*(x+1.0)) / x;
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = val_infinity/x;
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_debye_2_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 4.80822761263837714;
  const double xcut = -GSL_LOG_DBL_MIN;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*M_SQRT2*GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0 - x/3.0 + x*x/24.0;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double t = x*x/8.0 - 1.0;
    gsl_sf_result c;
    cheb_eval_e(&adeb2_cs, t, &c);
    result->val = c.val - x/3.0;
    result->err = c.err + GSL_DBL_EPSILON * x/3.0;
    return GSL_SUCCESS;
  }
  else if(x < -(M_LN2 + GSL_LOG_DBL_EPSILON)) {
    const int nexp = floor(xcut/x);
    const double ex  = exp(-x);
    double xk  = nexp * x;
    double rk  = nexp;
    double sum = 0.0;
    int i;
    for(i=nexp; i>=1; i--) {
      sum *= ex;
      sum += (1.0 + 2.0/xk + 2.0/(xk*xk)) / rk;
      rk -= 1.0;
      xk -= x;
    }
    result->val = val_infinity/(x*x) - 2.0 * sum * ex;
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < xcut) {
    const double x2  = x*x;
    const double sum = 2.0 + 2.0*x + x2;
    result->val = (val_infinity - 2.0 * sum * exp(-x)) / x2;
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = (val_infinity/x)/x;
    result->err = GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

int gsl_sf_debye_3_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 19.4818182068004875;
  const double xcut = -GSL_LOG_DBL_MIN;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*M_SQRT2*GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0 - 3.0*x/8.0 + x*x/20.0;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double t = x*x/8.0 - 1.0;
    gsl_sf_result c;
    cheb_eval_e(&adeb3_cs, t, &c);
    result->val = c.val - 0.375*x;
    result->err = c.err + GSL_DBL_EPSILON * 0.375*x;
    return GSL_SUCCESS;
  }
  else if(x < -(M_LN2 + GSL_LOG_DBL_EPSILON)) {
    const int nexp = floor(xcut/x);
    const double ex  = exp(-x);
    double xk  = nexp * x;
    double rk  = nexp;
    double sum = 0.0;
    int i;
    for(i=nexp; i>=1; i--) {
      double xk_inv = 1.0/xk;
      sum *= ex;
      sum += (((6.0*xk_inv + 6.0)*xk_inv + 3.0)*xk_inv + 1.0) / rk;
      rk -= 1.0;
      xk -= x;
    }
    result->val = val_infinity/(x*x*x) - 3.0 * sum * ex;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x < xcut) {
    const double x3 = x*x*x;
    const double sum = 6.0 + 6.0*x + 3.0*x*x + x3;
    result->val = (val_infinity - 3.0 * sum * exp(-x)) / x3;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else {
    result->val = ((val_infinity/x)/x)/x;
    result->err = GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

int gsl_sf_debye_4_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 99.5450644937635129;
  const double xcut = -GSL_LOG_DBL_MIN;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*M_SQRT2*GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0 - 2.0*x/5.0 + x*x/18.0;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double t = x*x/8.0 - 1.0;
    gsl_sf_result c;
    cheb_eval_e(&adeb4_cs, t, &c);
    result->val = c.val - 2.0*x/5.0;
    result->err = c.err + GSL_DBL_EPSILON * 2.0*x/5.0;
    return GSL_SUCCESS;
  }
  else if(x < -(M_LN2 + GSL_LOG_DBL_EPSILON)) {
    const int nexp = floor(xcut/x);
    const double ex  = exp(-x);
    double xk  = nexp * x;
    double rk  = nexp;
    double sum = 0.0;
    int i;
    for(i=nexp; i>=1; i--) {
      double xk_inv = 1.0/xk;
      sum *= ex;
      sum += ((((24.0*xk_inv + 24.0)*xk_inv + 12.0)*xk_inv + 4.0)*xk_inv + 1.0) / rk;
      rk -= 1.0;
      xk -= x;
    }
    result->val = val_infinity/(x*x*x*x) - 4.0 * sum * ex;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x < xcut) {
    const double x2 = x*x;
    const double x4 = x2*x2;
    const double sum = 24.0 + 24.0*x + 12.0*x2 + 4.0*x2*x + x4;
    result->val = (val_infinity - 4.0 * sum * exp(-x)) / x4;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else {
    result->val = (((val_infinity/x)/x)/x)/x;
    result->err = GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

int gsl_sf_debye_5_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 610.405837190669483828710757875 ;
  const double xcut = -GSL_LOG_DBL_MIN;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*M_SQRT2*GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0 - 5.0*x/12.0 + 5.0*x*x/84.0;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double t = x*x/8.0 - 1.0;
    gsl_sf_result c;
    cheb_eval_e(&adeb5_cs, t, &c);
    result->val = c.val - 5.0*x/12.0;
    result->err = c.err + GSL_DBL_EPSILON * 5.0*x/12.0;
    return GSL_SUCCESS;
  }
  else if(x < -(M_LN2 + GSL_LOG_DBL_EPSILON)) {
    const int nexp = floor(xcut/x);
    const double ex  = exp(-x);
    double xk  = nexp * x;
    double rk  = nexp;
    double sum = 0.0;
    int i;
    for(i=nexp; i>=1; i--) {
      double xk_inv = 1.0/xk;
      sum *= ex;
      sum += (((((120.0*xk_inv + 120.0)*xk_inv + 60.0)*xk_inv + 20.0)*xk_inv + 5.0)*xk_inv+ 1.0) / rk;
      rk -= 1.0;
      xk -= x;
    }
    result->val = val_infinity/(x*x*x*x*x) - 5.0 * sum * ex;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x < xcut) {
    const double x2 = x*x;
    const double x4 = x2*x2;
    const double x5 = x4*x;
    const double sum = 120.0 + 120.0*x + 60.0*x2 + 20.0*x2*x + 5.0*x4 + x5;
    result->val = (val_infinity - 5.0 * sum * exp(-x)) / x5;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else {
    result->val = ((((val_infinity/x)/x)/x)/x)/x;
    result->err = GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

int gsl_sf_debye_6_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 4356.06887828990661194792541535 ;
  const double xcut = -GSL_LOG_DBL_MIN;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*M_SQRT2*GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0 - 3.0*x/7.0 + x*x/16.0;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double t = x*x/8.0 - 1.0;
    gsl_sf_result c;
    cheb_eval_e(&adeb6_cs, t, &c);
    result->val = c.val - 3.0*x/7.0;
    result->err = c.err + GSL_DBL_EPSILON * 3.0*x/7.0;
    return GSL_SUCCESS;
  }
  else if(x < -(M_LN2 + GSL_LOG_DBL_EPSILON)) {
    const int nexp = floor(xcut/x);
    const double ex  = exp(-x);
    double xk  = nexp * x;
    double rk  = nexp;
    double sum = 0.0;
    int i;
    for(i=nexp; i>=1; i--) {
      double xk_inv = 1.0/xk;
      sum *= ex;
      sum += ((((((720.0*xk_inv + 720.0)*xk_inv + 360.0)*xk_inv + 120.0)*xk_inv + 30.0)*xk_inv+ 6.0)*xk_inv+ 1.0) / rk;
      rk -= 1.0;
      xk -= x;
    }
    result->val = val_infinity/(x*x*x*x*x*x) - 6.0 * sum * ex;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else if(x < xcut) {
    const double x2 = x*x;
    const double x4 = x2*x2;
    const double x6 = x4*x2;
    const double sum = 720.0 + 720.0*x + 360.0*x2 + 120.0*x2*x + 30.0*x4 + 6.0*x4*x +x6 ;
    result->val = (val_infinity - 6.0 * sum * exp(-x)) / x6;
    result->err = GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else {
    result->val = (((((val_infinity/x)/x)/x)/x)/x)/x ;
    result->err = GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_debye_1(const double x)
{
  EVAL_RESULT(gsl_sf_debye_1_e(x, &result));
}

double gsl_sf_debye_2(const double x)
{
  EVAL_RESULT(gsl_sf_debye_2_e(x, &result));
}

double gsl_sf_debye_3(const double x)
{
  EVAL_RESULT(gsl_sf_debye_3_e(x, &result));
}

double gsl_sf_debye_4(const double x)
{
  EVAL_RESULT(gsl_sf_debye_4_e(x, &result));
}

double gsl_sf_debye_5(const double x)
{
  EVAL_RESULT(gsl_sf_debye_5_e(x, &result));
}

double gsl_sf_debye_6(const double x)
{
  EVAL_RESULT(gsl_sf_debye_6_e(x, &result));
}