/**
 * File: nicback_utils.c
 * Subroutine for the NICMOS background subtraction
 *
 */
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include "aXe_grism.h"
#include "aXe_utils.h"
#include "spc_FITScards.h"
#include "spce_sect.h"
#include "spce_fitting.h"
#include "spce_is_in.h"
#include "spc_back.h"
#include "spce_pathlength.h"
#include "trfit_utils.h"
#include "nicback_utils.h"

/**
 * Function: make_nicmos_back
 * The subroutine computes and stores the background image for a specific
 * NICMOS grism image. The non-masked pixels in the grism image are
 * correlated with their corresponding values in the master background image.
 * A linear relation is fitted to the value pairs. Deviating points are
 * excluded using kappa-sigma clipping.
 * Using the slope of the final fit, the background image is computed
 * and stored as a fits image.
 *
 * Parameters:
 * @param obs         - the observation for the grism image
 * @param msk_name    - the full name of the mask file
 * @param master_bck  - the full name of the master sky background
 * @param bck_name    - the full name of the background image
 *
 * Returns:
 * @return -
 */
void
make_nicmos_back(const observation * const obs, const char *msk_name,
		 const char *master_bck, char bck_name[], char plist_name[],
		 const char corr_bck[])
{
  px_point npixels;

  double skypix_frac;

  gsl_matrix *msk_img;
  gsl_matrix *mbck_img;
  gsl_matrix *corr_img=NULL;
  gsl_matrix *grism_bck;

  gsl_vector *lfit;

  fitbck_data *fbck_data;
  //int f_status=0;

  FITScards *cards;

  char  ID[MAXCHAR];

  // fix the extension name
  sprintf (ID, "BCK");

  // load the master background image
  fprintf(stdout,"Loading DATA from: %s...", master_bck);
  mbck_img = FITSimage_to_gsl(master_bck, 1, 1);
  fprintf(stdout,". Done.\n");

  if (strlen(corr_bck) > 0)
    {
      // load the correction background image
      fprintf(stdout,"Loading DATA from: %s...", corr_bck);
      corr_img = FITSimage_to_gsl(corr_bck, 1, 1);
      fprintf(stdout,". Done.\n");
    }

  // load the mask image
  fprintf(stdout,"Loading DATA from: %s...", msk_name);
  msk_img = FITSimage_to_gsl(msk_name, 2, 1);
  fprintf(stdout,". Done.\n");

  // get the dimension of
  // the grism image
  npixels = get_npixel(obs);

  // allocate memory for the fit data
  fbck_data = alloc_fitbck_data(npixels.x * npixels.y);

  // fill the fit data into the structure
  fill_fbck_data(obs, msk_img, mbck_img, fbck_data, plist_name, corr_img);

  // make the fit, including
  // kappa-sigma iterations
  lfit = make_ksig_linfit(fbck_data);

  // report the result of the fit onto the screen
  fprintf(stdout, "\nFitting result: c0 = %f +- %f , c1 = %f +- %f , chi^2 = %f\n\n",
	  gsl_vector_get(lfit, 1), gsl_vector_get(lfit, 3),
	  gsl_vector_get(lfit, 2), gsl_vector_get(lfit, 4), gsl_vector_get(lfit, 6));

  // compute the background for the grism frame
  grism_bck = compute_nicmos_back(mbck_img, gsl_vector_get(lfit, 2),
				  gsl_vector_get(lfit, 1), corr_img);

  // write the background image to a file
  fprintf(stdout,"Writing data to: %s...", bck_name);
  gsl_to_FITSimage (grism_bck, bck_name, 1, ID);
  fprintf(stdout,". Done.\n");

  // determine the fraction of pixels
  // used in the background determination
  skypix_frac = get_skypix_frac(fbck_data);

  // create the fits keywords
  cards = nicbck_info_to_FITScards(skypix_frac, gsl_vector_get(lfit, 2),
				   gsl_vector_get(lfit, 1));

  // transfer the fits keywords
  // to the background image
  put_FITS_cards (bck_name, 2, cards);

  // release memory
  free_fitbck_data(fbck_data);
  gsl_matrix_free(mbck_img);
  if (corr_img != NULL)
    gsl_matrix_free(corr_img);
  gsl_matrix_free(msk_img);
  gsl_matrix_free(grism_bck);
  gsl_vector_free(lfit);
  free_FITScards(cards);
}

/**
 * Function: alloc_fitbck_data
 */
fitbck_data *
alloc_fitbck_data(const int n_data)
{
  fitbck_data *fbck_data;

  fbck_data = (fitbck_data *)malloc(sizeof(fitbck_data));

  fbck_data->x_values = (double *)malloc(n_data * sizeof(double));
  fbck_data->y_values = (double *)malloc(n_data * sizeof(double));
  fbck_data->e_values = (double *)malloc(n_data * sizeof(double));
  fbck_data->x_pos = (int *)malloc(n_data * sizeof(int));
  fbck_data->y_pos = (int *)malloc(n_data * sizeof(int));

  fbck_data->n_data = n_data;

  return fbck_data;
}

/**
 * Function: get_skypix_frac
 * The subroutine computes the fraction of image pixels which were used for
 * determining the scaling factor of the master background and the computation
 * of the bacground image. Pixels not used were masked out, since they are part
 * of an object beam or rejected in the kappa-sigma clipping.
 *
 * Parameters:
 * @param fbck_data - the data used for the fit
 *
 * Returns:
 * @return skypix_frac - fraction of pixels used for background determination
 */
double
get_skypix_frac(const fitbck_data *fbck_data)
{
  int index;
  int nvalues=0;

  double skypix_frac=0.0;

  // go over all data values
  for (index=0; index < fbck_data->n_data; index++)
    {
      // check whether the weight is not NULL
      if (fbck_data->e_values[index])
	// enhance the number of used pixels
	nvalues++;
    }

  // compute the fration of used pixels
  skypix_frac = (double)nvalues / (double)fbck_data->n_data;

  // return the fraction
  return skypix_frac;
}

/**
 * Function: compute_nicmos_back
 * The subroutine applies a scaling factor to a master background
 * in order to get the background image for a specific grism image.
 *
 * Parameters:
 * @param mbck_img - the master background pixel
 * @param factor   - the scaling factor
 *
 * Returns:
 * @return grism_bck - the background image as gsl-matrix
 */
gsl_matrix *
compute_nicmos_back(const gsl_matrix *mbck_img, const double factor,
		    const double offset, const gsl_matrix *corr_img)
{
  gsl_matrix *grism_bck;

  int ii, jj;

  // allocate the space for the background image
  grism_bck = gsl_matrix_alloc(mbck_img->size1, mbck_img->size2);

  // go over each row
  for (ii=0; ii < (int)mbck_img->size1; ii++)
    {
      // go over each column
      for (jj=0; jj < (int)mbck_img->size2; jj++)
	{
	  // compute and set the value in the background image
	  if (corr_img != NULL)
	    // using the pedestal image
	    gsl_matrix_set(grism_bck, ii, jj,
	        (gsl_matrix_get(mbck_img, ii, jj) + gsl_matrix_get(corr_img, ii, jj)) * factor);
	  else
	    // using the fit values only
	    gsl_matrix_set(grism_bck, ii, jj, gsl_matrix_get(mbck_img, ii, jj) * factor + offset);
	}
    }

  // return the background image
  return grism_bck;
}

/**
 * Function: compute_nicmos_back
 * The subroutine iteratively fits a linear relation to the pairs of
 * grism image - master background pixel values. After each fit,
 * pairs with large deviations are determined and excluded.
 * The result of the final fit is returned.
 *
 * Parameters:
 * @param fbck_data - the data used for the fits
 *
 * Returns:
 * @return lfit - the results of the final fit
 */
gsl_vector *
make_ksig_linfit(fitbck_data *fbck_data)
{
  gsl_vector *lfit=NULL;

  int index=0;

  int clipped=1;

  // check whether iterations still
  // must be done and whether
  // the data was changed from clipping
  while (index < N_KSIGBCK_ITER && clipped)
    {
      if (check_fbck_data(fbck_data))
	{
	  // make a non-weighted linear fit
	  lfit = det_vector_linear(fbck_data->x_values, fbck_data->y_values, fbck_data->e_values,
				   fbck_data->n_data, 0);

	  // make a clipping iteration
	  clipped = clipp_fbck_data(fbck_data, lfit, N_KSIGBCK_KAPPA);

	  // inhance the clipping counter
	  index++;
	}
      else
	{
	  // release the space
	  // for the old vector
	  if (lfit)
	    gsl_vector_free(lfit);

	  // get a new dummy vector
	  lfit = get_fbck_defaults();

	  break;
	}
    }

  // check whether the scale is within the boundaries
  if (gsl_vector_get(lfit, 2) < BCK_SCALE_MIN || gsl_vector_get(lfit, 2) > BCK_SCALE_MAX)
    {

      fprintf(stdout, "aXe_NICBACK: Background scale is out of bounds. It is re-adjusted.\n");
      // release the space
      // for the old vector
      gsl_vector_free(lfit);

      // get a new dummy vector
      lfit = get_fbck_defaults();
    }

  // return the fit result
  return lfit;
}

int
check_fbck_data(fitbck_data *fbck_data)
{
  int check=1;
  int ix_min=60000;
  int ix_max=0;
  int iy_min=60000;
  int iy_max=0;
  int index;
  double frac;
  double x_ext, y_ext;

  // get the fraction of background pixel
  frac =  get_skypix_frac(fbck_data);

  if (frac < FRAC_BPIX_MIN)
    {
      check = 0;
      fprintf(stdout, "aXe_NICBACK: Not enough background pixels. Fraction: %e\n", frac);
    }
  else
    {
      // go over all data values
      for (index=0; index < fbck_data->n_data; index++)
	{
	  // check whether the weight is not NULL
	  if (fbck_data->e_values[index])
	    {
	      if (fbck_data->x_pos[index] > ix_max)
		ix_max = fbck_data->x_pos[index];
	      if (fbck_data->x_pos[index] < ix_min)
		ix_min = fbck_data->x_pos[index];
	      if (fbck_data->y_pos[index] > iy_max)
		iy_max = fbck_data->y_pos[index];
	      if (fbck_data->y_pos[index] < iy_min)
		iy_min = fbck_data->y_pos[index];
	    }
	}
      x_ext = (float)(ix_max-ix_min) / NPIX_DIM1;
      y_ext = (float)(iy_max-iy_min) / NPIX_DIM2;

      if (x_ext < AREA_EXTEND_MIN || y_ext < AREA_EXTEND_MIN)
	{
	  check = 0;
	  fprintf(stdout, "aXe_NICBACK: Not enough coverage. xcover,ycover: %e,%e\n", x_ext, y_ext);
	}
    }

  return check;
}

gsl_vector*
get_fbck_defaults()
{
  gsl_vector *ret;

  ret = gsl_vector_alloc(10);

  /*
  gsl_vector_set(ret, 0, xean);
  gsl_vector_set(ret, 1, c0);
  gsl_vector_set(ret, 2, c1);
  gsl_vector_set(ret, 3, cov00);
  gsl_vector_set(ret, 4, cov01);
  gsl_vector_set(ret, 5, cov11);
  gsl_vector_set(ret, 6, chisq);
  */

  gsl_vector_set(ret, 0, 0.0);
  gsl_vector_set(ret, 1, 0.0);
  gsl_vector_set(ret, 2, 1.0);
  gsl_vector_set(ret, 3, 1.0);
  gsl_vector_set(ret, 4, 0.0);
  gsl_vector_set(ret, 5, 1.0);
  gsl_vector_set(ret, 6, 0.0);

  return ret;
}

/**
 * Function: clipp_fbck_data
 * The subroutine applies kappa-sigma clipping to a set of data points.
 * The difference between the measured values and the expected values
 * according to linear regression are determined.
 * Data which is outside of the accepted interval is excluded by
 * setting the weight to '0.0'. If at least one value was clipped,
 * '1' is returned, otherwise '0' (if all values are within the
 * accepted interval.
 *
 * Parameters:
 * @param fbck_data - the data used for the fits
 * @param lfit      - the result of the fit
 * @param kappa     - the kappa value
 *
 * Returns:
 * @return clipped - boolean to indicate clipped values
 */
int
clipp_fbck_data(fitbck_data *fbck_data, const gsl_vector *lfit, const float kappa)
{
  int clipped=0;
  int nclipps=0;

  int ndata=0;
  int index;

  double dy_mean=0.0;
  double stdev=0.0;
  double absdev;
  double dy_act;

  double *y_diff;

  // allocate memory for the tmp-vector
  y_diff = (double *) malloc(fbck_data->n_data * sizeof(double));

  // go over all data values
  for (index=0; index < fbck_data->n_data; index++)
    {
      // check whether the weight is not NULL
      if (fbck_data->e_values[index])
	{
	  // compute the difference from the data point
	  // to the prediction from the fit
	  dy_act = (fbck_data->x_values[index] - gsl_vector_get(lfit, 0)) * gsl_vector_get(lfit, 2)
	    + gsl_vector_get(lfit, 1)-fbck_data->y_values[index];

	  // fill the tmp vector
	  y_diff[ndata] = dy_act;

	  // pre-comute the mean
	  dy_mean += dy_act;

	  // enhance the counter
	  // of non-NULL values
	  ndata++;
	}
    }

  // compute the mean offset
  dy_mean = dy_mean / (double)ndata;

  // compute the standard deviation
  stdev = comp_stdev_from_array(y_diff, ndata, dy_mean);

  // get the maximum allowed
  // deviation
  absdev = kappa*stdev;

  // go over the data
  for (index=0; index < fbck_data->n_data; index++)
    {
      // check whether the data has weigth
      if (fbck_data->e_values[index])
	{
	  // compute the absolute difference from the data point
	  // to the prediction from the fit
	  dy_act = fabs((fbck_data->x_values[index] - gsl_vector_get(lfit, 0)) * gsl_vector_get(lfit, 2)
			+ gsl_vector_get(lfit, 1)-fbck_data->y_values[index]);

	  // check whether the difference
	  // is larger than allowed
	  if (dy_act > absdev)
	    {
	      // make the weight NULL
	      fbck_data->e_values[index] = 0.0;

	      // set the clipped flagg
	      clipped=1;
	      nclipps++;
	    }
	}
    }

  fprintf(stdout, "Number of clipped pixels: %i\n", nclipps);

  // release memory
  free(y_diff);

  // return the integer
  // indicating clipped values
  return clipped;
}

/**
 * Function: comp_stdev_from_array
 * The subroutine computes the standard deviation
 * of a data vector, using the known mean value.
 *
 * Parameters:
 * @param data  - the data vector
 * @param ndata - number of data points to use
 * @param mean  - mean value of the data points
 *
 * Returns:
 * @return clipped - boolean to indicate clipped values
 */
double
comp_stdev_from_array(const double *data, const int ndata, const double mean)
{
  int index=0;

  double stdev=0.0;

  // go over the data array
  for (index=0; index < ndata; index++)
    {
      // compute and add the square difference to the mean
      stdev += (data[index] - mean) * (data[index] - mean);
    }

  // compute the average square difference
  if (ndata > 1)
    stdev /= ((double)ndata-1);

  // compute the final
  // standard deviation
  stdev = sqrt(stdev);

  // return the stdev
  return stdev;
}

/**
 * Function: make_nicmos_back
 * The subroutine creates and fills a structure with pairs of correpsonding
 * from a grism image and a master sky background. Dq-masked pixels values
 * and pixels which are part of an object beam are used, but marked with
 * a zero weight.
 *
 * Parameters:
 * @param obs       - the observation for the grism image
 * @param msk_image - the maks image values
 * @param mbck_img  - the master background values
 * @param fbck_data    - structure with the data pairs
 *
 * Returns:
 * @return -
 */
void
fill_fbck_data(const observation * const obs, const gsl_matrix *msk_img,
	       const gsl_matrix *mbck_img, fitbck_data *fbck_data, char plist_name[],
	       const gsl_matrix *corr_img)
{
  int ix=0;
  int iy=0;
  int index=0;
  char Buffer[10240];
  FILE *fout;

  int px_min=20;
  int px_max=180;
  int py_min=50;
  int py_max=245;
  //int px_min=0;
  //int px_max=255;
  //int py_min=0;
  //int py_max=255;
  //10:240,15:245[10:180,20:245][20:180,50:245]

  // intitialize the index for
  // the fit_data structure
  index = 0;

  double diffval;

  fout = fopen(plist_name, "w");

  // go over all pixels
  // in the grism image
  for (ix=0; ix < (int)obs->grism->size1; ix++)
    {
      for (iy=0; iy < (int)obs->grism->size2; iy++)
	{
	  // set the master background pixel as independent variable
	  fbck_data->x_values[index] = gsl_matrix_get(mbck_img, ix, iy);

	  if (corr_img != NULL)
	    // subtract the correction value from the background value
	    diffval = gsl_matrix_get(obs->grism, ix, iy) - gsl_matrix_get(corr_img, ix, iy);
	  else
	    // use the naked background value
	    diffval = gsl_matrix_get(obs->grism, ix, iy);

	  // set the grism image pixel as dependent variable
	  //fbck_data->y_values[index] = gsl_matrix_get(obs->grism, ix, iy);
	  // set the difference as dependent variable
	  fbck_data->y_values[index] = diffval;

	  // set the x- and y-positions
	  fbck_data->x_pos[index] = ix;
	  fbck_data->y_pos[index] = iy;

	  // check whether the pixel was masked out
	  // or is part of an object
	  if (isnan(gsl_matrix_get(obs->grism, ix, iy))
		    || gsl_matrix_get(msk_img, ix, iy) < -9.0e+05)
	    // mask the pixel in the array
	    fbck_data->e_values[index] = 0.0;
	  else
	    {
	      // give the pixel full weight
	      //x,y,value_back,value_grism_imag
	      fbck_data->e_values[index] = 1.0;
	      sprintf (Buffer, "%i %i %e %e\n",ix, iy, gsl_matrix_get(obs->grism, ix, iy), gsl_matrix_get(mbck_img, ix, iy));
	      fputs (Buffer, fout);
	    }

	  // check whether the pixel is in the
	  // allowed are
	  if (ix < px_min || ix > px_max
	      || iy < py_min || iy > py_max)
	    // mask the pixel in the array
	    fbck_data->e_values[index] = 0.0;
	  // enhance the counter
	  index++;
	}
    }

  fclose (fout);
}

/*
 * Function: free_fitbck_data
 * The function releases the memory allocated in
 * a fit-data structure
 *
 * Parameters:
 * @param fbck_data - the allocated structure
 *
 * Returns:
 * @return -
 */
void
free_fitbck_data(fitbck_data *fbck_data)
{
  free(fbck_data->x_values);
  free(fbck_data->y_values);
  free(fbck_data->e_values);
  free(fbck_data->x_pos);
  free(fbck_data->y_pos);

  free(fbck_data);

  fbck_data = NULL;
}


/** @file */
#ifndef __CCL_CORE_H_INCLUDED__
#define __CCL_CORE_H_INCLUDED__

#include <stdbool.h>
#include <stdio.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

CCL_BEGIN_DECLS

/**
 * Struct containing the parameters defining a cosmology
 */
typedef struct ccl_parameters {

  // Densities: CDM, baryons, total matter, neutrinos, curvature
  double Omega_c; /**< Density of CDM relative to the critical density*/
  double Omega_b; /**< Density of baryons relative to the critical density*/
  double Omega_m; /**< Density of all matter relative to the critical density*/
  double Omega_k; /**< Density of curvature relative to the critical density*/
  double sqrtk; /**< Square root of the magnitude of curvature, k */ //TODO check
  int k_sign; /**<Sign of the curvature k */

  // Dark Energy
  double w0;
  double wa;

  // Hubble parameters
  double H0;
  double h;

  // Neutrino properties

  double Neff; // Effective number of relativistic neutrino species in the early universe.
  int N_nu_mass; // Number of species of neutrinos which are nonrelativistic today
  double N_nu_rel;  // Number of species of neutrinos which are relativistic  today
  double *mnu;  // total mass of massive neutrinos (This is a pointer so that it can hold multiple masses.)
  double sum_nu_masses; // sum of the neutrino masses.
  double Omega_n_mass; // Omega_nu for MASSIVE neutrinos
  double Omega_n_rel; // Omega_nu for MASSLESS neutrinos

  //double Neff_partial[CCL_MAX_NU_SPECIES];
  //double mnu[CCL_MAX_NU_SPECIES];

  // Primordial power spectra
  double A_s;
  double n_s;

  // Radiation parameters
  double Omega_g;
  double T_CMB;

  // BCM baryonic model parameters
  double bcm_log10Mc;
  double bcm_etab;
  double bcm_ks;

  // Derived parameters
  double sigma8;
  double Omega_l;
  double z_star;

  //Modified growth rate
  bool has_mgrowth;
  int nz_mgrowth;
  double *z_mgrowth;
  double *df_mgrowth;
} ccl_parameters;

/**
 * Struct containing references to gsl splines for distance and acceleration calculations
 */
typedef struct ccl_data{
  // These are all functions of the scale factor a.

  // Distances are defined in Mpc
  double growth0;
  gsl_spline * chi;
  gsl_spline * growth;
  gsl_spline * fgrowth;
  gsl_spline * E;
  gsl_spline * achi;

  // All these splines use the same accelerator so that
  // if one calls them successively with the same a value
  // they will be much faster.
  gsl_interp_accel *accelerator;
  gsl_interp_accel *accelerator_achi;
  gsl_interp_accel *accelerator_m;
  gsl_interp_accel *accelerator_d;
  //TODO: it seems like we're not really using this accelerator, and we should
  gsl_interp_accel *accelerator_k;

  // Function of Halo mass M

  gsl_spline * logsigma;
  gsl_spline * dlnsigma_dlogm;

  // splines for halo mass function
  gsl_spline * alphahmf;
  gsl_spline * betahmf;
  gsl_spline * gammahmf;
  gsl_spline * phihmf;
  gsl_spline * etahmf;

  // These are all functions of the wavenumber k and the scale factor a.
  gsl_spline2d * p_lin;
  gsl_spline2d * p_nl;
  double k_min_lin; //k_min  [1/Mpc] <- minimum wavenumber that the power spectrum has been computed to
  double k_min_nl;
  double k_max_lin;
  double k_max_nl;
} ccl_data;

/**
 * Sturct containing references to instances of the above structs, and boolean flags of precomputed values.
 */
typedef struct ccl_cosmology
{
  ccl_parameters    params;
  ccl_configuration config;
  ccl_data          data;

  bool computed_distances;
  bool computed_growth;
  bool computed_power;
  bool computed_sigma;
  bool computed_hmfparams;

  int status;
  //this is optional - less tedious than tracking all numerical values for status in error handler function
  char status_message[500];

  // other flags?
} ccl_cosmology;

// Label for whether you are passing a pointer to a sum of neutrino masses or a pointer to a list of 3 masses.
typedef enum ccl_mnu_convention {
  ccl_mnu_list=0,   // you pass a list of three neutrino masses
  ccl_mnu_sum =1,  // sum, defaults to splitting with normal hierarchy
  ccl_mnu_sum_inverted = 2, //sum, split with inverted hierarchy
  ccl_mnu_sum_equal = 3, //sum, split into equal masses
  // More options could be added here
} ccl_mnu_convention;

// Initialization and life cycle of objects
void ccl_cosmology_read_config(void);
ccl_cosmology * ccl_cosmology_create(ccl_parameters params, ccl_configuration config);

/* Internal function to set the status message safely. */
void ccl_cosmology_set_status_message(ccl_cosmology * cosmo, const char * status_message, ...);

// User-facing creation routines
/**
 * Create a cosmology
 * @param Omega_c Omega_c
 * @param Omega_b Omega_b
 * @param Omega_k Omega_k
 * @param Neff Number of relativistic neutrino species in the early universe
 * @param mnu neutrino mass, either sum or list of length 3
 * @param mnu_type determines neutrino mass convention (ccl_mnu_list, ccl_mnu_sum, ccl_mnu_sum_inverted, ccl_mnu_sum_equal)
 * @param w0 Dark energy EoS parameter
 * @param wa Dark energy EoS parameter
 * @param h Hubble constant in units of 100 km/s/Mpc
 * @param norm_pk the normalization of the power spectrum, either A_s or sigma8
 * @param n_s the power-law index of the power spectrum
 * @param bcm_log10Mc log10 cluster mass, one of the parameters of the BCM model
 * @param bcm_etab ejection radius parameter, one of the parameters of the BCM model
 * @param bcm_ks wavenumber for the stellar profile, one of the parameters of the BCM model
 * @param nz_mgrowth the number of redshifts where the modified growth is provided
 * @param zarr_mgrowth the array of redshifts where the modified growth is provided
 * @param dfarr_mgrowth the modified growth function vector provided
 * @param status Status flag. 0 if there are no errors, nonzero otherwise.
 * For specific cases see documentation for ccl_error.c
 * @return void
 */
ccl_parameters ccl_parameters_create(double Omega_c, double Omega_b, double Omega_k,
                     double Neff, double* mnu, ccl_mnu_convention mnu_type,
                     double w0, double wa, double h, double norm_pk,
                     double n_s, double bcm_log10Mc, double bcm_etab, double bcm_ks,
                     int nz_mgrowth,double *zarr_mgrowth,
                     double *dfarr_mgrowth, int *status);

/* ------- ROUTINE: ccl_parameters_create_flat_lcdm --------
INPUT: some cosmological parameters needed to create a flat LCDM model
TASK: call ccl_parameters_create to produce an LCDM model
*/
ccl_parameters ccl_parameters_create_flat_lcdm(double Omega_c, double Omega_b, double h,
					       double norm_pk, double n_s, int *status);

/**
 * Free a parameters struct
 * @param params ccl_parameters struct
 * @return void
 */
void ccl_parameters_free(ccl_parameters * params);

/**
 * Write a cosmology parameters object to a file in yaml format, .
 * @param params Cosmological parameters
 * @param filename Name of file to create and write
 * @param status Status flag. 0 if there are no errors, nonzero otherwise.
 * @return void
 */
void ccl_parameters_write_yaml(ccl_parameters * params, const char * filename, int * status);

/**
 * Read a cosmology parameters object from a file in yaml format, .
 * @param filename Name of existing file to read from
 * @param status Status flag. 0 if there are no errors, nonzero otherwise.
 * @return cosmo Cosmological parameters
 */
ccl_parameters ccl_parameters_read_yaml(const char * filename, int *status);

/**
 * Free a cosmology struct
 * @param cosmo Cosmological parameters
 * @return void
 */
void ccl_cosmology_free(ccl_cosmology * cosmo);

CCL_END_DECLS

#endif


/** \file
 * Analyze CSA output traces responding to pulses for ENC evaluation.
 */
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include <unistd.h>

#include <gsl/gsl_histogram2d.h>

#include "common.h"
#include "filters.h"
#include "hdf5rawWaveformIo.h"
typedef SCOPE_DATA_TYPE IN_WFM_BASE_TYPE;
typedef SCOPE_DATA_TYPE OUT_WFM_BASE_TYPE;

/** Parameters settable from commandline */
typedef struct param
{
    size_t diffSep;  //!< rise-time in samples for pulse finding
    double riseFrac; //!< fraction of max pulse height for pulse edge location
    double fltM;     //!< exp decay constant for Trapezoidal filter
    size_t sPre;     //!< samples before rising edge
    size_t sLen;     //!< total number of samples of a pulse
    size_t sHscale;  //!< histogram sample scaling factor
} param_t;

param_t param_default = {
    .diffSep = 150,
    .riseFrac = 0.5,
    .fltM = -1.0,
    .sPre = 1000,
    .sLen = 4000,
    .sHscale = 4
};

void print_usage(const param_t *pm)
{
    printf("Usage:\n");
    printf("      -r rise time in samples[%zd]\n", pm->diffSep);
    printf("      -f riseFrac[%g]\n", pm->riseFrac);
    printf("      -m fltM[%g]\n", pm->fltM);
    printf("      -p sPre[%zd]\n", pm->sPre);
    printf("      -l sLen[%zd]\n", pm->sLen);
    printf("      -s iStart[0] -e iStop[-1] starting and stopping(+1) eventid\n");
    printf("      inFileName(.h5) outFileName\n");
}

/** Find pulses by their rising edges.
 *
 * @param[out] npulse number of pulses found
 * @param[in] fltH input waveform should be loaded into it already
 * @param[in] diffSep basically the rise time in samples
 * @param[in] riseFrac fraction of max pulse height to set pulse edge location
 * @param[in] M exp decay constant for Trapezoidal filter
 * @return the list of rising edge positions (times)
 */
size_t *find_pulses(size_t *npulse, filters_t *fltH, size_t diffSep, double riseFrac, double M)
{

    size_t n, nc = 1024;
    size_t *pulseRiseT = NULL, *pulseRiseT1;

    size_t flt_k = 2*diffSep, flt_l = 2*flt_k;
    double flt_M = M, max, dV;
    ssize_t i, j, sep, prev;

    if((pulseRiseT = calloc(nc, sizeof(size_t))) == NULL) {
        error_printf("calloc failed for pulseRiseT in %s()\n", __FUNCTION__);
        return NULL;
    }

    filters_trapezoidal(fltH, flt_k, flt_l, flt_M);

    max = 0.0;
    for(i=0; i<fltH->wavLen; i++)
        if(fltH->outWav[i] > max) max = fltH->outWav[i];

    sep = flt_k + flt_l;
    prev = 0;
    n = 0;
    for(i=0; i<fltH->wavLen; i++) {
        j = ((i+diffSep)>=fltH->wavLen)?(fltH->wavLen-1):(i+diffSep);
        dV = fltH->inWav[j] - fltH->inWav[i];
        if(dV > max * riseFrac) {
            if(i-prev > sep) {
                pulseRiseT[n] = j; n++;
                if(n >= nc) {
                    nc *= 2;
                    if((pulseRiseT1 = realloc(pulseRiseT, nc * sizeof(size_t))) == NULL) {
                        error_printf("realloc failed for pulseRiseT in %s()\n", __FUNCTION__);
                        *npulse = n;
                        return pulseRiseT;
                    }
                    pulseRiseT = pulseRiseT1;
                }
                prev = i;
            }
        }
    }
    *npulse = n;
    return pulseRiseT;
}

int main(int argc, char **argv)
{
    int optC = 0;
    param_t pm;

    char *inFileName, *outFileName;
    struct hdf5rawWaveformIo_waveform_file *inWfmFile;
    struct waveform_attribute inWfmAttr;
    struct hdf5rawWaveformIo_waveform_event inWfmEvent;
    IN_WFM_BASE_TYPE *inWfmBuf;
    // OUT_WFM_BASE_TYPE *outWfmBuf;
    filters_t *fltHdl;

    ssize_t iStart=0, iStop=-1, i, j, k, iCh;
    size_t nEventsInFile, chGrpLen, npulse, *pulseRiseT, nSep;
    unsigned int v, c;
    size_t chGrpIdx[SCOPE_NCH] = {0};
    double frameSize, val, sep, sepMu, sepSigma;

    gsl_histogram2d *wav2H, *flt2H;

    memcpy(&pm, &param_default, sizeof(pm));
    // parse switches
    while((optC = getopt(argc, argv, "e:f:l:m:p:r:s:")) != -1) {
        switch(optC) {
        case 'e':
            iStop = atoll(optarg);
            break;
        case 'f':
            pm.riseFrac = atof(optarg);
            break;
        case 'l':
            pm.sLen = atoll(optarg);
            break;
        case 'm':
            pm.fltM = atof(optarg);
            break;
        case 'p':
            pm.sPre = atoll(optarg);
            break;
        case 'r':
            pm.diffSep = atoll(optarg);
            break;
        case 's':
            iStart = atoll(optarg);
            break;
        default:
            print_usage(&pm);
            return EXIT_FAILURE;
            break;
        }
    }

    argc -= optind;
    argv += optind;
    if(argc<2 || argc>=3) {
        print_usage(&pm);
        return EXIT_FAILURE;
    }

    inFileName = argv[0];
    outFileName = argv[1];

    inWfmFile = hdf5rawWaveformIo_open_file_for_read(inFileName);
    hdf5rawWaveformIo_read_waveform_attribute_in_file_header(inWfmFile, &inWfmAttr);

    fprintf(stderr, "waveform_attribute:\n"
            "     chMask  = 0x%04x\n"
            "     nPt     = %zd\n"
            "     nFrames = %zd\n"
            "     dt      = %g\n"
            "     t0      = %g\n"
            "     ymult   = %g %g %g %g\n"
            "     yoff    = %g %g %g %g\n"
            "     yzero   = %g %g %g %g\n",
            inWfmAttr.chMask, inWfmAttr.nPt, inWfmAttr.nFrames, inWfmAttr.dt,
            inWfmAttr.t0, inWfmAttr.ymult[0], inWfmAttr.ymult[1], inWfmAttr.ymult[2],
            inWfmAttr.ymult[3], inWfmAttr.yoff[0], inWfmAttr.yoff[1],
            inWfmAttr.yoff[2], inWfmAttr.yoff[3], inWfmAttr.yzero[0],
            inWfmAttr.yzero[1], inWfmAttr.yzero[2], inWfmAttr.yzero[3]);

    nEventsInFile = hdf5rawWaveformIo_get_number_of_events(inWfmFile);
    fprintf(stderr, "Number of events in file: %zd\n", nEventsInFile);
    if(iStart < 0) iStart = 0;
    if(iStart >= nEventsInFile) iStart = nEventsInFile - 1;
    if(iStop < 0) iStop = nEventsInFile;
    if(iStop <= iStart) iStop = iStart + 1;

    if(inWfmAttr.nFrames > 0) {
        frameSize = inWfmAttr.nPt / (double)inWfmAttr.nFrames;
    } else {
        frameSize = (double)inWfmAttr.nPt;
    }

    v = inWfmAttr.chMask;
    for(c=0; v; c++) v &= v - 1;
    /* Brian Kernighan's way of counting bits */
    chGrpLen = inWfmFile->nCh / c;
    i=0;
    for(v=0; v<SCOPE_NCH; v++)
        if((inWfmAttr.chMask >> v) & 0x01) {
            chGrpIdx[i] = v;
            i++;
        }
    inWfmBuf = (IN_WFM_BASE_TYPE*)malloc(inWfmFile->nPt * inWfmFile->nCh * sizeof(IN_WFM_BASE_TYPE));
    inWfmEvent.wavBuf = inWfmBuf;

    fltHdl = filters_init(NULL, inWfmFile->nPt);
    wav2H = gsl_histogram2d_alloc(pm.sLen/pm.sHscale, 128);
    gsl_histogram2d_set_ranges_uniform(wav2H, -0.5 * inWfmAttr.dt, (pm.sLen+0.5) * inWfmAttr.dt,
        inWfmAttr.yzero[chGrpIdx[0]] + (-128.5 - inWfmAttr.yoff[chGrpIdx[0]]) * inWfmAttr.ymult[chGrpIdx[0]],
        inWfmAttr.yzero[chGrpIdx[0]] + ( 127.5 - inWfmAttr.yoff[chGrpIdx[0]]) * inWfmAttr.ymult[chGrpIdx[0]]);

    flt2H = gsl_histogram2d_alloc(pm.sLen/pm.sHscale, 256);
    gsl_histogram2d_set_ranges_uniform(flt2H, -0.5 * inWfmAttr.dt, (pm.sLen+0.5) * inWfmAttr.dt, -0.01, -0.01 + 256 * inWfmAttr.ymult[chGrpIdx[0]]);

    nSep = 0; sepMu = 0.0; sepSigma = 0.0;
    for(inWfmEvent.eventId = iStart; inWfmEvent.eventId < iStop; inWfmEvent.eventId++) {
        hdf5rawWaveformIo_read_event(inWfmFile, &inWfmEvent);
        for(iCh=0; iCh < 1 /* inWfmFile->nCh */; iCh++) {
            for(i=0; i<inWfmFile->nPt; i++) {
                val = (inWfmBuf[(size_t)(iCh * inWfmFile->nPt + i)]
                       - inWfmAttr.yoff[chGrpIdx[iCh]])
                    * inWfmAttr.ymult[chGrpIdx[iCh]]
                    + inWfmAttr.yzero[chGrpIdx[iCh]];

                fltHdl->inWav[i] = val;
            }
        }
        pulseRiseT = find_pulses(&npulse, fltHdl,
                                 pm.diffSep, pm.riseFrac, pm.fltM);
        fprintf(stderr, "eventId = %zd, npulse = %zd, first at %zd\n",
               inWfmEvent.eventId, npulse, pulseRiseT[0]);
        for(i=0; i<npulse-1; i++) {
            sep = pulseRiseT[i+1] - pulseRiseT[i];
            sepMu += sep;
            sepSigma += sep * sep;
            nSep++;
        }

        for(i=1; i<npulse-1; i++) {
            for(j=0; j<pm.sLen; j++) {
                k = pulseRiseT[i]-pm.sPre + j;
                gsl_histogram2d_increment(wav2H, j*inWfmAttr.dt, fltHdl->inWav[k]);
                gsl_histogram2d_increment(flt2H, j*inWfmAttr.dt, fltHdl->outWav[k]);
            }
        }
        free(pulseRiseT);
    }
    sepMu /= (double)nSep;
    sepSigma = sqrt(1.0/(double)(nSep-1) * (sepSigma - nSep * sepMu * sepMu));

    printf("nSep = %zd, sepMu = %g, sepSigma = %g\n", nSep, sepMu, sepSigma);

    FILE *ofp;
    if((ofp = fopen(outFileName, "w"))==NULL) {
        perror(outFileName);
        goto Exit;
    }
    gsl_histogram2d_fprintf(ofp, wav2H, "%24.16e", "%g");
    fprintf(ofp, "\n\n");
    gsl_histogram2d_fprintf(ofp, flt2H, "%24.16e", "%g");
    fprintf(ofp, "\n\n");

    fprintf(ofp, "# baseline distribution");
    double yl, yu;
    for(i=0; i<gsl_histogram2d_ny(wav2H); i++) {
        gsl_histogram2d_get_yrange(wav2H, i, &yl, &yu);
        fprintf(ofp, "%24.16e, %g\n", yl, gsl_histogram2d_get(wav2H, 0, i));
    }
    fprintf(ofp, "\n\n");

    fprintf(ofp, "# filtered distribution");
    for(i=0; i<gsl_histogram2d_ny(flt2H); i++) {
        gsl_histogram2d_get_yrange(flt2H, i, &yl, &yu);
        fprintf(ofp, "%24.16e, %g\n", yl,
                gsl_histogram2d_get(flt2H, (pm.sPre+3*pm.diffSep)/pm.sHscale, i));
    }

    fclose(ofp);

Exit:
    free(inWfmBuf); inWfmBuf = NULL;
    gsl_histogram2d_free(wav2H);
    gsl_histogram2d_free(flt2H);
    filters_close(fltHdl);
    hdf5rawWaveformIo_close_file(inWfmFile);

    return EXIT_SUCCESS;
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 * =====================================================================================
 *
 *       Filename:  Reg_s0.c
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  15/03/2014 21:10:30
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:
 *
 * =====================================================================================
 */

#include <gsl/gsl_integration.h>

#include "funcs.h"

/*
   We split the interval of integration (0,+inf) into three parts:
   1) (0, O/2)
   		and we use the diffeomorphism k -> 1/k to cope with the singularity
		in k = 0 ;
   2) (O/2, 3*O/2)
   		and we calculate the Cauchy principal value around k = O ;
   3) (3*O/2, +inf)
 */

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  fu_inv
 *  Description:  To integrate on the 1) interval we make the change of variable
 *		  k -> 1/k
 * =====================================================================================
 */
double fu_inv ( double k, void* params )
{
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double temp = alpha*O*exp(-1/(k*o_c))/(1-(O*O*k*k)) ;
	double val = temp/(k*tanh(b/(k*2))) ;

	return val ;
}

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  fu_coth
 *  Description:  The function to be integrated around the singularity O, by calculating
 *		  the Cauchy principal value. We use qawc and therefore we multiply by
 *		  (k-O).
 * =====================================================================================
 */
double fu_cau ( double k, void* params )
{
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double temp = alpha*O*k*exp(-k/o_c)/(k+O) ;
	double val = temp/tanh(b*k/2) ;

	return val ;
}

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  fu_coth
 *  Description:  Function to be integrated on the tail (3*O/2 , +inf)
 * =====================================================================================
 */
double fu_coth ( double k, void* params )
{
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double temp = alpha*O*k*exp(-k/o_c)/(k*k-O*O) ;
	double val = temp/tanh(b*k/2) ;

	return val ;
}

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  re_gs0
 *  Description:  Re g_s0
 * =====================================================================================
 */
int re_gs0 ( void* params, double* val, double* error )
{
	struct f_params* pars = (struct f_params*) params ;
	double O ;
	O = pars->Omega ;

	double r, err ;
	double r1, r2, r3, err1, err2, err3 ;

	gsl_function f, F, G ;
	f.function = &fu_coth ; F.function = &fu_cau ; G.function = &fu_inv ;
	f.params = pars ; F.params = pars ; G.params = pars ;

	gsl_integration_workspace *fu_fin_ws =
		gsl_integration_workspace_alloc (WS_SZ) ;

	gsl_integration_workspace *fu_inv_ws =
		gsl_integration_workspace_alloc (WS_SZ) ;

	gsl_integration_workspace *fu_inf_ws =
		gsl_integration_workspace_alloc (WS_SZ) ;

	int status1 ;
	status1 = gsl_integration_qagiu ( &G, 2/O, 1e-9, .001, WS_SZ,
		       	fu_inv_ws, &r1,	&err1 ) ;
	int status2 ;
	status2 = gsl_integration_qawc ( &F, O/2, 3*O/2, O, 1e-9, .001, WS_SZ,
			fu_fin_ws, &r2, &err2 ) ;

	int status3 ;
	status3 = gsl_integration_qagiu ( &f, 3*O/2, 1e-9, .001, WS_SZ,
		       	fu_inf_ws, &r3,	&err3 ) ;

	r = r1 + r2 + r3 ; err = err1 + err2 + err3 ;
	*val = r ; *error = err ;

	gsl_integration_workspace_free(fu_fin_ws) ;
	gsl_integration_workspace_free(fu_inv_ws) ;
	gsl_integration_workspace_free(fu_inf_ws) ;

	int status = status1 + status2 + status3 ;
	return status ;
}


/* multilarge_nlinear/cgst.c
 *
 * Copyright (C) 2016 Patrick Alken
 *
 * 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 3 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multilarge_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

/*
 * This module contains an implementation of the Steihaug-Toint
 * conjugate gradient algorithm for nonlinear optimization problems.
 * This implementation closely follows the following works:
 *
 * [1] T. Steihaug, The conjugate gradient method and trust regions
 *     in large scale optimization, SIAM J. Num. Anal., 20(3) 1983.
 *
 * In the below algorithm, the Jacobian and gradient are scaled
 * according to:
 *
 * J~ = J D^{-1}
 * g~ = D^{-1}
 *
 * prior to any calculations which results in better numerical
 * stability when solving for the Gauss-Newton step. The resulting
 * step vector is then backtransformed as:
 *
 * dx = D^{-1} dx~
 */

typedef struct
{
  size_t n;                  /* number of observations */
  size_t p;                  /* number of parameters */
  gsl_vector *z;             /* Gauss-Newton step, size p */
  gsl_vector *r;             /* steepest descent step, size p */
  gsl_vector *d;             /* steepest descent step, size p */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *workn;         /* workspace, length n */
  double norm_g;             /* || g~ || */

  double cgtol;              /* tolerance for CG solution */
  size_t cgmaxit;            /* maximum CG iterations */
} cgst_state_t;

#include "common.c"

static void * cgst_alloc (const void * params, const size_t n, const size_t p);
static void cgst_free(void *vstate);
static int cgst_init(const void *vtrust_state, void *vstate);
static int cgst_preloop(const void * vtrust_state, void * vstate);
static int cgst_step(const void * vtrust_state, const double delta,
                     gsl_vector * dx, void * vstate);
static int cgst_preduction(const void * vtrust_state, const gsl_vector * dx,
                           double * pred, void * vstate);
static double cgst_calc_tau(const gsl_vector * p, const gsl_vector * d,
                            const double delta);

static void *
cgst_alloc (const void * params, const size_t n, const size_t p)
{
  const gsl_multilarge_nlinear_parameters *par = (const gsl_multilarge_nlinear_parameters *) params;
  cgst_state_t *state;

  state = calloc(1, sizeof(cgst_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate st state", GSL_ENOMEM);
    }

  state->z = gsl_vector_alloc(p);
  if (state->z == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for z", GSL_ENOMEM);
    }

  state->r = gsl_vector_alloc(p);
  if (state->r == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for r", GSL_ENOMEM);
    }

  state->d = gsl_vector_alloc(p);
  if (state->d == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp", GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn", GSL_ENOMEM);
    }

  state->n = n;
  state->p = p;

  state->cgmaxit = par->max_iter;
  if (state->cgmaxit == 0)
    state->cgmaxit = n;

  state->cgtol = par->tol;

  return state;
}

static void
cgst_free(void *vstate)
{
  cgst_state_t *state = (cgst_state_t *) vstate;

  if (state->z)
    gsl_vector_free(state->z);

  if (state->r)
    gsl_vector_free(state->r);

  if (state->d)
    gsl_vector_free(state->d);

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->workn)
    gsl_vector_free(state->workn);

  free(state);
}

/*
cgst_init()
  Initialize cgst solver

Inputs: vtrust_state - trust state
        vstate       - workspace

Return: success/error
*/

static int
cgst_init(const void *vtrust_state, void *vstate)
{
  /* nothing to do */

  (void)vtrust_state;
  (void)vstate;

  return GSL_SUCCESS;
}

static int
cgst_preloop(const void * vtrust_state, void * vstate)
{
  /* nothing to do */

  (void)vtrust_state;
  (void)vstate;

  return GSL_SUCCESS;
}

/*
cgst_step()
  Calculate a new step vector

Return:
GSL_SUCCESS if CG solution found
GSL_EMAXITER if no solution found
*/

static int
cgst_step(const void * vtrust_state, const double delta,
          gsl_vector * dx, void * vstate)
{
  int status;
  const gsl_multilarge_nlinear_trust_state *trust_state =
    (const gsl_multilarge_nlinear_trust_state *) vtrust_state;
  cgst_state_t *state = (cgst_state_t *) vstate;
  const gsl_vector * x = trust_state->x;
  const gsl_vector * f = trust_state->f;
  const gsl_vector * swts = trust_state->sqrt_wts;
  const gsl_vector * diag = trust_state->diag;
  const gsl_multilarge_nlinear_parameters * params = trust_state->params;
  gsl_multilarge_nlinear_fdf * fdf = trust_state->fdf;
  double alpha, beta, u;
  double norm_Jd;   /* || J D^{-1} d_i || */
  double norm_r;    /* || r_i || */
  double norm_rp1;  /* || r_{i+1} || */
  size_t i;

  /* Step 1 of [1], section 2; scale gradient as
   *
   * g~ = D^{-1} g
   *
   * for better numerical stability
   */

  for (i = 0; i < state->p; ++i)
    {
      double gi = gsl_vector_get(trust_state->g, i);
      double di = gsl_vector_get(trust_state->diag, i);

      gsl_vector_set(state->z, i, 0.0);
      gsl_vector_set(state->r, i, -gi / di);
      gsl_vector_set(state->d, i, -gi / di);
      gsl_vector_set(state->workp, i, gi / di);
    }

  /* compute || g~ || */
  state->norm_g = gsl_blas_dnrm2(state->workp);

  for (i = 0; i < state->cgmaxit; ++i)
    {
      /* workp := D^{-1} d_i */
      gsl_vector_memcpy(state->workp, state->d);
      gsl_vector_div(state->workp, trust_state->diag);

      /* workn := J D^{-1} d_i */
      status = gsl_multilarge_nlinear_eval_df(CblasNoTrans, x, f, state->workp,
                                              swts, params->h_df, params->fdtype,
                                              fdf, state->workn, NULL, NULL);
      if (status)
        return status;

      /* compute || J D^{-1} d_i || */
      norm_Jd = gsl_blas_dnrm2(state->workn);

      /* Step 2 of [1], section 2 */
      if (norm_Jd == 0.0)
        {
          double tau = cgst_calc_tau(state->z, state->d, delta);

          /* dx = z_i + tau*d_i */
          scaled_addition(1.0, state->z, tau, state->d, dx);
          gsl_vector_div(dx, diag);

          return GSL_SUCCESS;
        }

      /* Step 3 of [1], section 2 */

      norm_r = gsl_blas_dnrm2(state->r);
      u = norm_r / norm_Jd;
      alpha = u * u;

      /* workp <= z_{i+1} = z_i + alpha_i*d_i */
      scaled_addition(1.0, state->z, alpha, state->d, state->workp);

      u = gsl_blas_dnrm2(state->workp);
      if (u >= delta)
        {
          double tau = cgst_calc_tau(state->z, state->d, delta);

          /* dx = z_i + tau*d_i */
          scaled_addition(1.0, state->z, tau, state->d, dx);
          gsl_vector_div(dx, diag);

          return GSL_SUCCESS;
        }

      /* store z_{i+1} */
      gsl_vector_memcpy(state->z, state->workp);

      /* Step 4 of [1], section 2 */

      /* compute: workp := alpha B d_i = alpha D^{-1} J^T J D^{-1} d_i,
       * where J D^{-1} d_i is already stored in workn */
      status = gsl_multilarge_nlinear_eval_df(CblasTrans, x, f, state->workn,
                                              swts, params->h_df, params->fdtype,
                                              fdf, state->workp, NULL, NULL);
      if (status)
        return status;

      gsl_vector_div(state->workp, trust_state->diag);
      gsl_vector_scale(state->workp, alpha);

      /* r_{i+1} = r_i - alpha*B*d_i */
      gsl_vector_sub(state->r, state->workp);
      norm_rp1 = gsl_blas_dnrm2(state->r);

      u = norm_rp1 / state->norm_g;
      if (u < state->cgtol)
        {
          gsl_vector_memcpy(dx, state->z);
          gsl_vector_div(dx, diag);
          return GSL_SUCCESS;
        }

      /* Step 5 of [1], section 2 */

      /* compute u = ||r_{i+1}|| / || r_i|| */
      u = norm_rp1 / norm_r;
      beta = u * u;

      /* compute: d_{i+1} = rt_{i+1} + beta*d_i */
      scaled_addition(1.0, state->r, beta, state->d, state->d);
    }

  /* failed to converge, return current estimate */
  gsl_vector_memcpy(dx, state->z);
  gsl_vector_div(dx, diag);

  return GSL_EMAXITER;
}

static int
cgst_preduction(const void * vtrust_state, const gsl_vector * dx,
                double * pred, void * vstate)
{
  const gsl_multilarge_nlinear_trust_state *trust_state =
    (const gsl_multilarge_nlinear_trust_state *) vtrust_state;
  cgst_state_t *state = (cgst_state_t *) vstate;

  *pred = quadratic_preduction(trust_state, dx, state->workn);

  return GSL_SUCCESS;
}

/*
cgst_calc_tau()
  Compute tau > 0 such that:

|| p + tau*d || = delta
*/

static double
cgst_calc_tau(const gsl_vector * p, const gsl_vector * d,
              const double delta)
{
  double norm_p, norm_d, u;
  double t1, t2, tau;

  norm_p = gsl_blas_dnrm2(p);
  norm_d = gsl_blas_dnrm2(d);

  /* compute (p, d) */
  gsl_blas_ddot(p, d, &u);

  t1 = u / (norm_d * norm_d);
  t2 = t1*u + (delta + norm_p) * (delta - norm_p);
  tau = -t1 + sqrt(t2) / norm_d;

  return tau;
}

static const gsl_multilarge_nlinear_trs cgst_type =
{
  "steihaug-toint",
  cgst_alloc,
  cgst_init,
  cgst_preloop,
  cgst_step,
  cgst_preduction,
  cgst_free
};

const gsl_multilarge_nlinear_trs *gsl_multilarge_nlinear_trs_cgst = &cgst_type;


/* integration/cquad.c
 *
 * Copyright (C) 2010 Pedro Gonnet
 *
 * 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 3 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 <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>

#include "cquad_const.c"

/* Allocates a workspace for the given maximum number of intervals.
    Note that if the workspace gets filled, the intervals with the
    lowest error estimates are dropped. The maximum number of
    intervals is therefore not the maximum number of intervals
    that will be computed, but merely the size of the buffer.
    */

gsl_integration_cquad_workspace *
gsl_integration_cquad_workspace_alloc (const size_t n)
{

  gsl_integration_cquad_workspace *w;

  /* Check inputs */
  if (n < 3)
    GSL_ERROR_VAL ("workspace size n must be at least 3", GSL_EDOM, 0);

  /* Allocate first the workspace struct */
  if ((w =
       (gsl_integration_cquad_workspace *)
       malloc (sizeof (gsl_integration_cquad_workspace))) == NULL)
    GSL_ERROR_VAL ("failed to allocate space for workspace struct",
		   GSL_ENOMEM, 0);

  /* Allocate the intervals */
  if ((w->ivals =
       (gsl_integration_cquad_ival *)
       malloc (sizeof (gsl_integration_cquad_ival) * n)) == NULL)
    {
      free (w);
      GSL_ERROR_VAL ("failed to allocate space for the intervals", GSL_ENOMEM,
		     0);
    }

  /* Allocate the max-heap indices */
  if ((w->heap = (size_t *) malloc (sizeof (size_t) * n)) == NULL)
    {
      free (w->ivals);
      free (w);
      GSL_ERROR_VAL ("failed to allocate space for the heap", GSL_ENOMEM, 0);
    }

  /* Remember the size of the workspace */
  w->size = n;

  /* Return the result */
  return w;

}

/* Liberates the workspace memory.
    */

void
gsl_integration_cquad_workspace_free (gsl_integration_cquad_workspace * w)
{

  /* Nothing to be done? */
  if (w == NULL)
    return;

  /* Free the intervals first */
  if (w->ivals != NULL)
    free (w->ivals);

  /* Free the heap */
  if (w->heap != NULL)
    free (w->heap);

  /* Free the structure */
  free (w);

}

/* Compute the product of the fx with one of the inverse
    Vandermonde-like matrices. */

static void
Vinvfx (const double *fx, double *c, const int d)
{

  int i, j;

  switch (d)
    {
    case 0:
      for (i = 0; i <= 4; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 4; j++)
	    c[i] += V1inv[i * 5 + j] * fx[j * 8];
	}
      break;
    case 1:
      for (i = 0; i <= 8; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 8; j++)
	    c[i] += V2inv[i * 9 + j] * fx[j * 4];
	}
      break;
    case 2:
      for (i = 0; i <= 16; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 16; j++)
	    c[i] += V3inv[i * 17 + j] * fx[j * 2];
	}
      break;
    case 3:
      for (i = 0; i <= 32; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 32; j++)
	    c[i] += V4inv[i * 33 + j] * fx[j];
	}
      break;
    }

}

/* Downdate the interpolation given by the n coefficients c
    by removing the nodes with indices in nans. */

static void
downdate (double *c, int n, int d, int *nans, int nnans)
{

  static const int bidx[4] = { 0, 6, 16, 34 };
  double b_new[34], alpha;
  int i, j;

  for (i = 0; i <= n + 1; i++)
    b_new[i] = bee[bidx[d] + i];
  for (i = 0; i < nnans; i++)
    {
      b_new[n + 1] = b_new[n + 1] / Lalpha[n];
      b_new[n] = (b_new[n] + xi[nans[i]] * b_new[n + 1]) / Lalpha[n - 1];
      for (j = n - 1; j > 0; j--)
	b_new[j] =
	  (b_new[j] + xi[nans[i]] * b_new[j + 1] -
	   Lgamma[j + 1] * b_new[j + 2]) / Lalpha[j - 1];
      for (j = 0; j <= n; j++)
	b_new[j] = b_new[j + 1];
      alpha = c[n] / b_new[n];
      for (j = 0; j < n; j++)
	c[j] -= alpha * b_new[j];
      c[n] = 0;
      n--;
    }

}

/* The actual integration routine.
    */

int
gsl_integration_cquad (const gsl_function * f, double a, double b,
		       double epsabs, double epsrel,
		       gsl_integration_cquad_workspace * ws,
		       double *result, double *abserr, size_t * nevals)
{

  /* Some constants that we will need. */
  static const int n[4] = { 4, 8, 16, 32 };
  static const int skip[4] = { 8, 4, 2, 1 };
  static const int idx[4] = { 0, 5, 14, 31 };
  static const double w = M_SQRT2 / 2;
  static const int ndiv_max = 20;

  /* Actual variables (as opposed to constants above). */
  double m, h, temp;
  double igral, err, igral_final, err_final, err_excess;
  int nivals, neval = 0;
  int i, j, d, split, t;
  int nnans, nans[32];
  gsl_integration_cquad_ival *iv, *ivl, *ivr;
  double nc, ncdiff;

  /* Check the input arguments. */
  if (f == NULL)
    GSL_ERROR ("function pointer shouldn't be NULL", GSL_EINVAL);
  if (result == NULL)
    GSL_ERROR ("result pointer shouldn't be NULL", GSL_EINVAL);
  if (ws == NULL)
    GSL_ERROR ("workspace pointer shouldn't be NULL", GSL_EINVAL);

  /* Check for unreasonable accuracy demands */
  if (epsabs < 0.0 || epsrel < 0.0)
    GSL_ERROR ("tolerances may not be negative", GSL_EBADTOL);
  if (epsabs <= 0 && epsrel < GSL_DBL_EPSILON)
    GSL_ERROR ("unreasonable accuracy requirement", GSL_EBADTOL);

  /* Create the first interval. */
  iv = &(ws->ivals[0]);
  m = (a + b) / 2;
  h = (b - a) / 2;
  nnans = 0;
  for (i = 0; i <= n[3]; i++)
    {
      iv->fx[i] = GSL_FN_EVAL (f, m + xi[i] * h);
      neval++;
      if (!finite (iv->fx[i]))
	{
	  nans[nnans++] = i;
	  iv->fx[i] = 0.0;
	}
    }
  Vinvfx (iv->fx, &(iv->c[idx[0]]), 0);
  Vinvfx (iv->fx, &(iv->c[idx[3]]), 3);
  Vinvfx (iv->fx, &(iv->c[idx[2]]), 2);
  for (i = 0; i < nnans; i++)
    iv->fx[nans[i]] = GSL_NAN;
  iv->a = a;
  iv->b = b;
  iv->depth = 3;
  iv->rdepth = 1;
  iv->ndiv = 0;
  iv->igral = 2 * h * iv->c[idx[3]] * w;
  nc = 0.0;
  for (i = n[2] + 1; i <= n[3]; i++)
    {
      temp = iv->c[idx[3] + i];
      nc += temp * temp;
    }
  ncdiff = nc;
  for (i = 0; i <= n[2]; i++)
    {
      temp = iv->c[idx[2] + i] - iv->c[idx[3] + i];
      ncdiff += temp * temp;
      nc += iv->c[idx[3] + i] * iv->c[idx[3] + i];
    }
  ncdiff = sqrt (ncdiff);
  nc = sqrt (nc);
  iv->err = ncdiff * 2 * h;
  if (ncdiff / nc > 0.1 && iv->err < 2 * h * nc)
    iv->err = 2 * h * nc;

  /* Initialize the heaps. */
  for (i = 0; i < ws->size; i++)
    ws->heap[i] = i;

  /* Initialize some global values. */
  igral = iv->igral;
  err = iv->err;
  nivals = 1;
  igral_final = 0.0;
  err_final = 0.0;
  err_excess = 0.0;

  /* Main loop. */
  while (nivals > 0 && err > 0.0 &&
	 !(err <= fabs (igral) * epsrel || err <= epsabs)
	 && !(err_final > fabs (igral) * epsrel
	      && err - err_final < fabs (igral) * epsrel)
	 && !(err_final > epsabs && err - err_final < epsabs))
    {

      /* Put our finger on the interval with the largest error. */
      iv = &(ws->ivals[ws->heap[0]]);
      m = (iv->a + iv->b) / 2;
      h = (iv->b - iv->a) / 2;

/*      printf
        ("cquad: processing ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
         ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err, iv->depth);
*/
      /* Should we try to increase the degree? */
      if (iv->depth < 3)
	{

	  /* Keep tabs on some variables. */
	  d = ++iv->depth;

	  /* Get the new (missing) function values */
	  for (i = skip[d]; i <= 32; i += 2 * skip[d])
	    {
	      iv->fx[i] = GSL_FN_EVAL (f, m + xi[i] * h);
	      neval++;
	    }
	  nnans = 0;
	  for (i = 0; i <= 32; i += skip[d])
	    {
	      if (!finite (iv->fx[i]))
		{
		  nans[nnans++] = i;
		  iv->fx[i] = 0.0;
		}
	    }

	  /* Compute the new coefficients. */
	  Vinvfx (iv->fx, &(iv->c[idx[d]]), d);

	  /* Downdate any NaNs. */
	  if (nnans > 0)
	    {
	      downdate (&(iv->c[idx[d]]), n[d], d, nans, nnans);
	      for (i = 0; i < nnans; i++)
		iv->fx[nans[i]] = GSL_NAN;
	    }

	  /* Compute the error estimate. */
	  nc = 0.0;
	  for (i = n[d - 1] + 1; i <= n[d]; i++)
	    {
	      temp = iv->c[idx[d] + i];
	      nc += temp * temp;
	    }
	  ncdiff = nc;
	  for (i = 0; i <= n[d - 1]; i++)
	    {
	      temp = iv->c[idx[d - 1] + i] - iv->c[idx[d] + i];
	      ncdiff += temp * temp;
	      nc += iv->c[idx[d] + i] * iv->c[idx[d] + i];
	    }
	  ncdiff = sqrt (ncdiff);
	  nc = sqrt (nc);
	  iv->err = ncdiff * 2 * h;

	  /* Compute the local integral. */
	  iv->igral = 2 * h * w * iv->c[idx[d]];

	  /* Split the interval prematurely? */
	  split = (nc > 0 && ncdiff / nc > 0.1);

	}

      /* Maximum degree reached, just split. */
      else
	{
	  split = 1;
	}

      /* Should we drop this interval? */
      if ((m + h * xi[0]) >= (m + h * xi[1])
	  || (m + h * xi[31]) >= (m + h * xi[32])
	  || iv->err < fabs (iv->igral) * GSL_DBL_EPSILON * 10)
	{

/*          printf
            ("cquad: dumping ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
             ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err,
             iv->depth);
*/
	  /* Keep this interval's contribution */
	  err_final += iv->err;
	  igral_final += iv->igral;

	  /* Swap with the last element on the heap */
	  t = ws->heap[nivals - 1];
	  ws->heap[nivals - 1] = ws->heap[0];
	  ws->heap[0] = t;
	  nivals--;

	  /* Fix up the heap */
	  i = 0;
	  while (2 * i + 1 < nivals)
	    {

	      /* Get the kids */
	      j = 2 * i + 1;

	      /* If the j+1st entry exists and is larger than the jth,
	         use it instead. */
	      if (j + 1 < nivals
		  && ws->ivals[ws->heap[j + 1]].err >=
		  ws->ivals[ws->heap[j]].err)
		j++;

	      /* Do we need to move the ith entry up? */
	      if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
		break;
	      else
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	    }

	}

      /* Do we need to split this interval? */
      else if (split)
	{

	  /* Some values we will need often... */
	  d = iv->depth;

	  /* Generate the interval on the left */
	  ivl = &(ws->ivals[ws->heap[nivals++]]);
	  ivl->a = iv->a;
	  ivl->b = m;
	  ivl->depth = 0;
	  ivl->rdepth = iv->rdepth + 1;
	  ivl->fx[0] = iv->fx[0];
	  ivl->fx[32] = iv->fx[16];
	  for (i = skip[0]; i < 32; i += skip[0])
	    {
	      ivl->fx[i] =
		GSL_FN_EVAL (f, (ivl->a + ivl->b) / 2 + xi[i] * h / 2);
	      neval++;
	    }
	  nnans = 0;
	  for (i = 0; i <= 32; i += skip[0])
	    {
	      if (!finite (ivl->fx[i]))
		{
		  nans[nnans++] = i;
		  ivl->fx[i] = 0.0;
		}
	    }
	  Vinvfx (ivl->fx, ivl->c, 0);
	  if (nnans > 0)
	    {
	      downdate (ivl->c, n[0], 0, nans, nnans);
	      for (i = 0; i < nnans; i++)
		ivl->fx[nans[i]] = GSL_NAN;
	    }
	  for (i = 0; i <= n[d]; i++)
	    {
	      ivl->c[idx[d] + i] = 0.0;
	      for (j = i; j <= n[d]; j++)
		ivl->c[idx[d] + i] += Tleft[i * 33 + j] * iv->c[idx[d] + j];
	    }
	  ncdiff = 0.0;
	  for (i = 0; i <= n[0]; i++)
	    {
	      temp = ivl->c[i] - ivl->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  for (i = n[0] + 1; i <= n[d]; i++)
	    {
	      temp = ivl->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  ncdiff = sqrt (ncdiff);
	  ivl->err = ncdiff * h;

	  /* Check for divergence. */
	  ivl->ndiv = iv->ndiv + (fabs (iv->c[0]) > 0
				  && ivl->c[0] / iv->c[0] > 2);
	  if (ivl->ndiv > ndiv_max && 2 * ivl->ndiv > ivl->rdepth)
	    {
              /* need copysign(INFINITY, igral) */
	      *result = (igral >= 0) ? GSL_POSINF : GSL_NEGINF;
	      if (nevals != NULL)
		*nevals = neval;
	      return GSL_EDIVERGE;
	    }

	  /* Compute the local integral. */
	  ivl->igral = h * w * ivl->c[0];

	  /* Generate the interval on the right */
	  ivr = &(ws->ivals[ws->heap[nivals++]]);
	  ivr->a = m;
	  ivr->b = iv->b;
	  ivr->depth = 0;
	  ivr->rdepth = iv->rdepth + 1;
	  ivr->fx[0] = iv->fx[16];
	  ivr->fx[32] = iv->fx[32];
	  for (i = skip[0]; i < 32; i += skip[0])
	    {
	      ivr->fx[i] =
		GSL_FN_EVAL (f, (ivr->a + ivr->b) / 2 + xi[i] * h / 2);
	      neval++;
	    }
	  nnans = 0;
	  for (i = 0; i <= 32; i += skip[0])
	    {
	      if (!finite (ivr->fx[i]))
		{
		  nans[nnans++] = i;
		  ivr->fx[i] = 0.0;
		}
	    }
	  Vinvfx (ivr->fx, ivr->c, 0);
	  if (nnans > 0)
	    {
	      downdate (ivr->c, n[0], 0, nans, nnans);
	      for (i = 0; i < nnans; i++)
		ivr->fx[nans[i]] = GSL_NAN;
	    }
	  for (i = 0; i <= n[d]; i++)
	    {
	      ivr->c[idx[d] + i] = 0.0;
	      for (j = i; j <= n[d]; j++)
		ivr->c[idx[d] + i] += Tright[i * 33 + j] * iv->c[idx[d] + j];
	    }
	  ncdiff = 0.0;
	  for (i = 0; i <= n[0]; i++)
	    {
	      temp = ivr->c[i] - ivr->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  for (i = n[0] + 1; i <= n[d]; i++)
	    {
	      temp = ivr->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  ncdiff = sqrt (ncdiff);
	  ivr->err = ncdiff * h;

	  /* Check for divergence. */
	  ivr->ndiv = iv->ndiv + (fabs (iv->c[0]) > 0
				  && ivr->c[0] / iv->c[0] > 2);
	  if (ivr->ndiv > ndiv_max && 2 * ivr->ndiv > ivr->rdepth)
	    {
              /* need copysign(INFINITY, igral) */
	      *result = (igral >= 0) ? GSL_POSINF : GSL_NEGINF;
	      if (nevals != NULL)
		*nevals = neval;
	      return GSL_EDIVERGE;
	    }

	  /* Compute the local integral. */
	  ivr->igral = h * w * ivr->c[0];

	  /* Fix-up the heap: we now have one interval on top
	     that we don't need any more and two new, unsorted
	     ones at the bottom. */

	  /* Flip the last interval to the top of the heap and
	     sift down. */
	  t = ws->heap[nivals - 1];
	  ws->heap[nivals - 1] = ws->heap[0];
	  ws->heap[0] = t;
	  nivals--;

	  /* Sift this interval back down the heap. */
	  i = 0;
	  while (2 * i + 1 < nivals - 1)
	    {
	      j = 2 * i + 1;
	      if (j + 1 < nivals - 1
		  && ws->ivals[ws->heap[j + 1]].err >=
		  ws->ivals[ws->heap[j]].err)
		j++;
	      if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
		break;
	      else
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	    }

	  /* Now grab the last interval and sift it up the heap. */
	  i = nivals - 1;
	  while (i > 0)
	    {
	      j = (i - 1) / 2;
	      if (ws->ivals[ws->heap[j]].err < ws->ivals[ws->heap[i]].err)
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	      else
		break;
	    }

	}

      /* Otherwise, just fix-up the heap. */
      else
	{
	  i = 0;
	  while (2 * i + 1 < nivals)
	    {
	      j = 2 * i + 1;
	      if (j + 1 < nivals
		  && ws->ivals[ws->heap[j + 1]].err >=
		  ws->ivals[ws->heap[j]].err)
		j++;
	      if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
		break;
	      else
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	    }

	}

      /* If the heap is about to overflow, remove the last two
         intervals. */
      while (nivals > ws->size - 2)
	{
	  iv = &(ws->ivals[ws->heap[nivals - 1]]);

/*          printf
            ("cquad: dumping ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
             ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err,
             iv->depth);
*/
	  err_final += iv->err;
	  igral_final += iv->igral;
	  nivals--;
	}

      /* Collect the value of the integral and error. */
      igral = igral_final;
      err = err_final;
      for (i = 0; i < nivals; i++)
	{
	  igral += ws->ivals[ws->heap[i]].igral;
	  err += ws->ivals[ws->heap[i]].err;
	}

    }

  /* Dump the contents of the heap. */
/*  for (i = 0; i < nivals; i++)
    {
      iv = &(ws->ivals[ws->heap[i]]);
      printf
        ("cquad: ival %i (%i) with [%e,%e], int=%e, err=%e, depth=%i, rdepth=%i\n",
         i, ws->heap[i], iv->a, iv->b, iv->igral, iv->err, iv->depth,
         iv->rdepth);
    }
*/
  /* Clean up and present the results. */
  *result = igral;
  if (abserr != NULL)
    *abserr = err;
  if (nevals != NULL)
    *nevals = neval;

  /* All is well that ends well. */
  return GSL_SUCCESS;

}


#include <iostream>
#include <vector>
#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#include <math.h>
//#include <C:/Program Files (x86)/Intel/Composer XE/compiler/include/cilk/cilk.h>
#ifndef _HEART_H
#define _HEART_H

class heart
{
private:
	int L;  //sytem size length for now square

	std::vector<std::vector<int>> state;  //value indicates whether in excited state refactory or resting
	std::vector<std::vector<int>> new_state;  //using new_state and state to distinguish newly excited cells from previously excited
	std::vector<std::vector<int>> functional; //1 if functional 0 if disfunctional
	std::vector<std::vector<int>> bond; //1 if bond to site above current cell, 0 if no bond above site
	std::vector<std::vector<int>> last_excited;
	std::vector<std::vector<int>> last_excited_counter; // have a dummy counter so as to not interfere with APD dynamics
	std::vector<std::vector<int>> refractory;
	std::vector<std::vector<int>> total_rest; //have counter for total time cells spend in rest or refractory

	int heart_time; //internal clock
	//model parameters
	int tau; //refactory period
	int T; // pacemaker rate
	double nu, epsilom, delta; //probability vertical connection exists, probability of misfire given faulty and probability cell faulty

	//random number generator
	const gsl_rng_type*Type;
	gsl_rng*r;

	int total_bonds;
	int total_functional;
	int total_excited;
	double refractory_period;

public:

	heart()
	{

	}
	heart(int _L, int _T, int _tau, double _nu, double _epsilom, double _delta)
	{

		refractory_period =0;
		L = _L;
		nu = _nu;
		tau = _tau;
		T = _T;
		epsilom = _epsilom;
		delta = _delta;

		heart_time = T;

		state.resize(L);
		new_state.resize(L);
		functional.resize(L);
		bond.resize(L);
		last_excited.resize(L);
		last_excited_counter.resize(L);
		refractory.resize(L);
		total_rest.resize(L);
		//pacemaker action
		for (unsigned i = 0; i<L; i++)
		{
			state[i].resize(L, 0);  // all cells eligible for excitation
			new_state[i].resize(L, 0);
			functional[i].resize(L, 1);// none disfunctional at the moment
			bond[i].resize(L, 1); // all transverse bonds present at the moment
			last_excited[i].resize(L, T);
			last_excited_counter[i].resize(L, 0);
			refractory[i].resize(L, tau);
			total_rest[i].resize(L,0);
		}
		//	total_functional = L*L;
		//	total_bonds = L*L;
		total_excited = 0;//could be useful avoiding calling size of some vector

		//HAVE NOT USED gsl_rng in class before need to make sure is working correctly

		//  environment variable GSL_RNG_TYPE
		gsl_rng_env_setup();

		Type = gsl_rng_mt19937;//random number generator using the Mersenne Twister algorithm
		r = gsl_rng_alloc(Type);

		//gsl_rng_set(r, time(NULL));//random number generator seeded with time
		gsl_rng_set(r, 10);//random number generator seeded with time

		//assigning cells as having a bond above them or not and functional or not functional
		int healthy = 1;

		for (int j = 0; j<L; j++)
		{
			for (int i = 0; i<L; i++)
			{

				bond[i][j] = (gsl_rng_uniform(r)<nu);
				functional[i][j] = (gsl_rng_uniform(r)>delta);// inequality reversed
				//double x =  gsl_rng_uniform(r);
				//	std::cout <<x<< '\t' << (x<nu) << std::endl;

			}
		}

		gsl_rng_set(r, 20);//random number generator seeded with time
	}
	void operate()//one time step
	{
		total_excited = 0;
		refractory_period = 0;
		//Relax excited cells
		for (int i = 0; i < L; i++)
		{
			for (int j = 0; j<L; j++)
			{
				if (state[i][j] == 1)//searching 4 neighbours of excited cell trying to excite if resting and if don't misfire and if there is a bond
				{
					new_state[((i + 1)<L)*(i + 1)][j] += (new_state[((i + 1)<L)*(i + 1)][j]<1)*(state[((i + 1)<L)*(i + 1)][j]<1)*bond[i][j] * (1 - (1 - functional[((i + 1)<L)*(i + 1)][j])*(gsl_rng_uniform(r)<epsilom));

					new_state[(i - 1)*(i>0) + (L - 1)*(i<1)][j] += (new_state[(i - 1)*(i>0) + (L - 1)*(i<1)][j]<1)*(state[(i - 1)*(i>0) + (L - 1)*(i<1)][j]<1)*bond[(i - 1)*(i>0) + (L - 1)*(i<1)][j] * (1 - (1 - functional[(i - 1)*(i>0) + (L - 1)*(i<1)][j])*(gsl_rng_uniform(r)<epsilom));

					new_state[i][(j + 1)*(j<L - 1)] += (new_state[i][(j + 1)*(j<L - 1)]<1)*(j<L - 1)*(state[i][(j + 1)*(j<L - 1)]<1)*(1 - (1 - functional[i][(j + 1)*(j<L - 1)])*(gsl_rng_uniform(r)<epsilom));

					new_state[i][(j - 1)*(j>0)] += (new_state[i][(j - 1)*(j>0)]<1)*(j>0)*(state[i][(j - 1)*(j>0)]<1)*(1 - (1 - functional[i][(j - 1)*(j>0)])*(gsl_rng_uniform(r)<epsilom));

					refractory[i][j] = restitution(i, j);

					total_rest[i][j] += last_excited_counter[i][j];
				}
			}
		}
		//end relaxing
		//std::vector<std::pair<int,int>> next_excited_cells;

		//evolving cells that were not excited previously or now (some subtlty to those possibly excited now)
		for (int i = 0; i<L; i++)
		{
			for (int j = 0; j<L; j++)
			{
				new_state[i][j] += (state[i][j]>0);
				//if state was excited or refactory before +1 states that were resting may have been newly excited or not but this ignores them
				new_state[i][j] *= (state[i][j]<refractory[i][j]);//promote to resting after end of refactory which is now variable
				total_excited += (new_state[i][j]<2)*new_state[i][j];

				last_excited[i][j] += (state[i][j] < 1) + (state[i][j]>1);//counts if not excited
				last_excited[i][j] *= (state[i][j] < 1) + (state[i][j]>1); //cycles back to 0 if excited;  old states so don't put lastexcited to 0 before calculating new refractory
				last_excited_counter[i][j] += (state[i][j] < 1) + (state[i][j]>1);//dummy counter
				last_excited_counter[i][j] *= (state[i][j] < 1) + (state[i][j]>1); //dummy counter
				refractory_period += refractory[i][j];

			}
		}

		//SAN
		for (int i = 0; i<L; i++)
		{
			//new_state[i][0] += (state[i][0]<1)*(heart_time>T - 1)*(1 - (1 - functional[i][0])*(gsl_rng_uniform(r)<epsilom));
			new_state[i][0] += (state[i][0]<1)*(heart_time>T - 1);

		}

		heart_time++;
		heart_time *= (heart_time<T + 1);
		//end SAN

		state = new_state;

	}
	int get_state(int i, int j)//perhaps have alternative for 2d array
	{

		return state[i][j];

	}
	void set_state(int i, int j, int val)
	{

		state[i][j] = val;
	}
	int get_function(int i, int j)
	{
		return functional[i][j];
	}
	void set_function(int i, int j, int val)
	{
		functional[i][j] = val;
	}
	int get_bond(int i, int j)
	{
		return bond[i][j];
	}
	void set_bond(int i, int j, int val)
	{
		bond[i][j] = val;
	}
	void update_total_rest(int i, int j)
	{
		total_rest[i][j] += last_excited_counter[i][j];
	}
	double get_total_rest(int i,int j)
	{
		return total_rest[i][j];
	}
	void reset_total_rest(int i, int j)
	{
		total_rest[i][j] = 0;
		last_excited_counter[i][j] = 0;
	}

	int get_tau()
	{
		return tau;
	}
	int get_T()
	{
		return T;
	}

	int get_excited()
	{
		return total_excited;
	}
	int get_refractory(int i, int j)
	{
		return refractory[i][j];
	}
	int get_last_excited(int i, int j)
	{
		return last_excited[i][j];
	}
	double colour(int i, int j)
	{
		return (1 - state[i][j] / (1.0*tau))*(state[i][j]>1);
		//return state[i][j]/(1.0*T);

	}
	int restitution(int i, int j)
	{
		//some function of tau T and last_excited[i][j]
		if (last_excited[i][j] < T)
		{
			//return floor((1 - exp(-4.0*last_excited[i][j] / (1.0*T)))*tau);
			return tau;
		}
		else
		{
			return tau;
		}
		//if T since excited should be approx tau
		//if less shoud be less than tau

	}
	double get_avg_refractory()
	{

		return refractory_period/(L*L);
	}
};
#endif

standard model + apd  100% working/heart_james.h


#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <Python.h>
#include <numpy/arrayobject.h>

#include <math.h>

#ifdef __INTEL_COMPILER
#include <mkl_cblas.h>
#else
#include <cblas.h>
#define kmp_set_blocktime(k)
#endif

static inline float minf (float a, float b) { return a < b ? a : b; }
static inline float maxf (float a, float b) { return a < b ? b : a; }

PyObject * compute_overlap (float bbx1, float bby1, float bbx2, float bby2,
                            int fdimy, int fdimx, int dimy, int dimx,
                            float scale, int pady, int padx, int h, int w) {
    int im_area = h*w;
    int bbox_area = (bbx2-bbx1)*(bby2-bby1);
    int im_clip = ((double)bbox_area / (double)im_area) < 0.7;
    npy_intp dims[2] = { dimy, dimx };
    int x, y;

    PyArrayObject * pyoverlap = (PyArrayObject*)PyArray_SimpleNew ((npy_intp)2, dims, NPY_FLOAT);

    for (x = 0; x < dimx; ++x) {
        for (y = 0; y < dimy; ++y) {
            float x1 = (x - padx) * scale;
            float y1 = (y - pady) * scale;
            float x2 = x1 + fdimx*scale - 1;
            float y2 = y1 + fdimy*scale - 1;

            if (im_clip) {
                x1 = minf(maxf(x1, 0.0f), w-1);
                y1 = minf(maxf(y1, 0.0f), h-1);
                x2 = minf(maxf(x2, 0.0f), w-1);
                y2 = minf(maxf(y2, 0.0f), h-1);
            }

            float xx1 = maxf(x1, bbx1);
            float yy1 = maxf(y1, bby1);
            float xx2 = minf(x2, bbx2);
            float yy2 = minf(y2, bby2);

            float intw = xx2 - xx1 + 1;
            float inth = yy2 - yy1 + 1;

            if (intw > 0 && inth > 0) {
                float filterw = x2 - x1 + 1;
                float filterh = y2 - y1 + 1;
                float filter_area = filterw*filterh;
                float int_area = intw*inth;
                float union_area = filter_area + bbox_area - int_area;

                *(float*)PyArray_GETPTR2(pyoverlap, y, x) = int_area / union_area;
            } else {
                *(float*)PyArray_GETPTR2(pyoverlap, y, x) = 0;
            }
        }
    }

    return Py_BuildValue("N", pyoverlap);
}

#if 0
PyObject * objective_function (PyObject * pyexamples) {
    int i;
    int j;
    int k;

    for (i = 0; i < PyList_Size(pyexamples); ++i) {
        PyObject * pyexample = PyList_GetItem (pyexamples, i);
        if (!PyList_Check(pyexample)) {
            PyErr_SetString(PyExc_TypeError, "example list elements must be list.");
            return NULL;
        }

        for (j = 0; j < PyList_Size(pyexample); ++j) {

            PyObject * pyentry = PyList_GetItem(pyexample, j);
            if (!PyTuple_Check(pyentry)) {
                PyErr_SetString(PyExc_TypeError, "entries must be tuples.");
                return NULL;
            }

            if (PyTuple_Size(pyentry) != 2) {
                PyErr_SetString(PyExc_TypeError, "entry tuples must be of size 2");
                return NULL;
            }

            PyObject * pyfeatures = PyTuple_GetItem(pyentry, 0);
            if (!PyDict_Check(pyfeatures)) {
                PyErr_SetString(PyExc_TypeError, "features must be a dict");
                return NULL;
            }

            PyObject * pyfeatureslist = PyDict_Items(pyfeatures);

            float score = 0;
            for (k = 0; k < PyDict_Size(pyfeatureslist); ++k) {
                PyObject * pyfeature = PyList_GetItem(pyfeatureslist, k);
                PyObject * pyblock = PyTuple_GetItem(pyfeature, 0);

                PyObject * pyw = PyTuple_GetItem(pyfeature, 1);
                if (!PyArray_Check(pyw)) {
                    PyErr_SetString(PyExc_TypeError, "w must be a numpy array");
                    return NULL;
                }

                PyArrayObject * pywarray = (PyArrayObject*)pyw;

            }

            PyObject * pydetection = PyTuple_GetItem(pyentry, 1);
        }
    }
    return Py_BuildValue("i", 0);
}

PyObject * gradient (PyObject * examples, PyObject * gradients) {
    Py_RETURN_NONE;
}
#endif

static PyObject * ComputeOverlap(PyObject * self, PyObject * args)
{
    float x1, x2, y1, y2;
    int fdimy, fdimx, dimy, dimx;
    float scale;
    int pady, padx;
    int w, h;
    if (!PyArg_ParseTuple(args, "ffffiiiifiiii", &x1, &y1, &x2, &y2, &fdimy, &fdimx, &dimy, &dimx, &scale, &pady, &padx, &h, &w))
        return NULL;
    return compute_overlap(x1, y1, x2, y2, fdimy, fdimx, dimy, dimx, scale, pady, padx, h, w);
}

#if 0
static PyObject * ObjectiveFunction (PyObject * self, PyObject * args) {
    PyObject * pyexamples;
    if (!PyArg_ParseTuple (args, "O!", &PyList_Type, &pyexamples)) {
        return NULL;
    }

    return objective_function (pyexamples);
}

static PyObject * Gradient (PyObject * self, PyObject * args) {
    PyObject * pyexamples;
    PyObject * pygradients;
    if (!PyArg_ParseTuple (args, "O!O!", &PyList_Type, &pyexamples, &PyDict_Type, &pygradients)) {
        return NULL;
    }

    return gradient (pyexamples, pygradients);
}
#endif

#if PY_MAJOR_VERSION >= 3
static struct PyModuleDef moduledef = {
    PyModuleDef_HEAD_INIT,
    "_train",
    "Routines for training the DPM.",
    -1,
    NULL,
    NULL,
    NULL,
    NULL,
    NULL
};
#endif

#if PY_MAJOR_VERSION < 3
static PyMethodDef _train_methods[] = {
    {"compute_overlap", ComputeOverlap, METH_VARARGS, "Compute detection overlaps with bbox."},
#if 0
    {"ObjectiveFunction", ObjectiveFunction, METH_VARARGS, "WL-SSVM objective function."},
    {"Gradient", Gradient, METH_VARARGS, "WL-SSVM gradient."},
#endif
    {NULL}
};
#endif

#if PY_MAJOR_VERSION >= 3
PyMODINIT_FUNC
PyInit__train(void)
#else
PyMODINIT_FUNC
init_train(void)
#endif
{
    import_array();

#if PY_MAJOR_VERSION >= 3
    PyObject *m = PyModule_Create(&moduledef);
#else
    Py_InitModule3("_train", _train_methods, "Compute detection overlaps with bbox.");
#endif

#if PY_MAJOR_VERSION >= 3
    return m;
#endif
}