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

#include <gsl/gsl_integration.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "ccl.h"

/*
 * We use the remapping procedure in https://arxiv.org/pdf/1601.07230.pdf
 * to map w0-wa models onto wa == 0 models.
 * The functions below implement this procedure.
*/

// I snuck a private function from background.c into here. :P
// The default routines create a full spline but we just need one value.
// This may seem like over optimizing, but in testing initializing halofit
// is an order of magnitude or more slower if we don't do this.
void compute_chi(double a, ccl_cosmology *cosmo, double * chi, int * stat);

static double zdrag_eh(ccl_parameters *params) {
  // eqn 4 of Eisenstein & Hu 1998
  double OMh2 = (params->Omega_c + params->Omega_b) * params->h * params->h;
  double OBh2 = params->Omega_b * params->h * params->h;
  double b1 = 0.313 * pow(OMh2, -0.419) * (1 + 0.607*pow(OMh2, 0.674));
  double b2 = 0.238 * pow(OMh2, 0.223);
  return 1291 * pow(OMh2, 0.251) * (1 + b1*pow(OBh2, b2)) / (1 + 0.659*pow(OMh2, 0.828));
}

struct hf_model_match_data {
  double chi_drag;
  double a;
  ccl_cosmology *cosmo;
  int *status;
};

static ccl_cosmology *create_w0eff_cosmo(double w0eff, ccl_cosmology *cosmo, int *status) {
  // create a cosmology with the same parameters as the input except w0-wa. Instead
  // the cosmology is created with w0 = w0eff.
  ccl_parameters params_w0eff;
  double norm_pk;
  double mnu[3];
  int i;

  for(i=0; i<3; ++i)
    mnu[i] = 0;
  for(i=0; i<cosmo->params.N_nu_mass; ++i)
    mnu[i] = cosmo->params.m_nu[i];

  if (isnan(cosmo->params.A_s))
    norm_pk = cosmo->params.sigma8;
  else
    norm_pk = cosmo->params.A_s;

  params_w0eff = ccl_parameters_create(
    cosmo->params.Omega_c, cosmo->params.Omega_b, cosmo->params.Omega_k,
    cosmo->params.Neff, mnu, cosmo->params.N_nu_mass,
    w0eff, 0, cosmo->params.h, norm_pk,
    cosmo->params.n_s, cosmo->params.bcm_log10Mc, cosmo->params.bcm_etab,
    cosmo->params.bcm_ks, cosmo->params.mu_0, cosmo->params.sigma_0,
    cosmo->params.c1_mg, cosmo->params.c2_mg, cosmo->params.lambda_mg,
    cosmo->params.nz_mgrowth,
    cosmo->params.z_mgrowth, cosmo->params.df_mgrowth, status);

    if(*status != 0)
      return NULL;

    return ccl_cosmology_create(params_w0eff, cosmo->config);
}

static double w0eff_func(double w0eff, void *p) {
  // function used to compare the distance to the CMB in a test cosmology to
  // to the value in the original cosmology
  // returns chi_eff - chi
  struct hf_model_match_data *hfd = (struct hf_model_match_data*)p;
  ccl_cosmology *cosmo_w0eff = NULL;
  double chi_drag_w0eff, tmp, zdrag_w0eff;

  // make the equivalent cosmology
  cosmo_w0eff = create_w0eff_cosmo(w0eff, hfd->cosmo, hfd->status);
  if (cosmo_w0eff == NULL) {
    *(hfd->status) = CCL_ERROR_MEMORY;
    return NAN;
  }

  // get the comoving distance to zdrag
  zdrag_w0eff = zdrag_eh(&(cosmo_w0eff->params));
  compute_chi(1.0 / (1.0 + zdrag_w0eff), cosmo_w0eff, &tmp, hfd->status);
  chi_drag_w0eff = tmp;
  compute_chi(hfd->a, cosmo_w0eff, &tmp, hfd->status);
  chi_drag_w0eff -= tmp;
  if (*(hfd->status) != 0) {
    return NAN;
  }

  ccl_parameters_free(&(cosmo_w0eff->params));
  ccl_cosmology_free(cosmo_w0eff);
  return chi_drag_w0eff - hfd->chi_drag;
}

static double get_w0eff(double a, struct hf_model_match_data data) {
  // For a given input w0-wa cosmology, this function solves for the value of
  // w0eff such that the comoving distance from a to the CMB in a cosmology
  // with the same parameters, but with w0, wa = w0eff, 0, is the same as the
  // original cosmology.
  double w0eff, w0eff_low = -2.0, w0eff_high = -0.35;
  double flow, fhigh;
  int itr, max_itr = 1000, gsl_status;
  const gsl_root_fsolver_type *T;
  gsl_root_fsolver *s;
  gsl_function F;

  data.a = a;
  data.chi_drag = ccl_comoving_radial_distance(data.cosmo, 1.0 / (1.0 + zdrag_eh(&(data.cosmo->params))), data.status);
  data.chi_drag -= ccl_comoving_radial_distance(data.cosmo, a, data.status);
  if(*(data.status) != 0) {
    ccl_cosmology_set_status_message(
      data.cosmo,
      "ccl_halofit.c: get_w0eff(): "
      "could not compute chi_drag for cosmology\n");
    return NAN;
  }

  F.function = &w0eff_func;
  F.params = &data;

  // we have to bound the root, otherwise return -1
  // we will fiil in any -1's in the calling routine
  flow = w0eff_func(w0eff_low, &data);
  fhigh = w0eff_func(w0eff_high, &data);
  if (flow * fhigh > 0) {
    return -1;
  }

  T = gsl_root_fsolver_brent;
  s = gsl_root_fsolver_alloc(T);
  if (s == NULL) {
    *(data.status) = CCL_ERROR_MEMORY;
  }
  else {
    gsl_root_fsolver_set(s, &F, w0eff_low, w0eff_high);

    itr = 0;
    do {
      itr++;
      gsl_status = gsl_root_fsolver_iterate(s);
      if (gsl_status == GSL_EBADFUNC)
        break;

      w0eff = gsl_root_fsolver_root(s);
      w0eff_low = gsl_root_fsolver_x_lower(s);
      w0eff_high = gsl_root_fsolver_x_upper(s);

      gsl_status = gsl_root_test_interval(
        w0eff_low, w0eff_high,
        1e-6,
        1e-6);
    } while (gsl_status == GSL_CONTINUE && itr < max_itr);

    gsl_root_fsolver_free(s);

    if (gsl_status != GSL_SUCCESS || itr >= max_itr) {
      ccl_raise_gsl_warning(
        gsl_status, "ccl_halofit.c: get_w0eff(): error in root finding for the halofit matching cosmology\n");
      *(data.status) |= gsl_status;
    }
  }

  return w0eff;
}

/* helper data and functions for integrals

 the integral is

 \int dlnk \Delta^{2}(k) \exp(-k^{2}R^{2})

 for halofit, we also need the first and second derivatives of this
 integral
*/
struct hf_int_data {
  double r;
  double r2;
  double a;
  ccl_cosmology *cosmo;
  ccl_f2d_t *plin;
  int *status;
  gsl_integration_cquad_workspace *workspace;
};

static double gauss_norm_int_func(double lnk, void *p) {
  struct hf_int_data *hfd = (struct hf_int_data*)p;
  double k = exp(lnk);
  double k2 = k*k;

  return (
    ccl_f2d_t_eval(hfd->plin, lnk, hfd->a, hfd->cosmo, hfd->status) *
    k*k2/2.0/M_PI/M_PI *
    exp(-k2 * (hfd->r2)));
}

static double onederiv_gauss_norm_int_func(double lnk, void *p) {
  struct hf_int_data *hfd = (struct hf_int_data*)p;
  double k = exp(lnk);
  double k2 = k*k;

  return (
    ccl_f2d_t_eval(hfd->plin, lnk, hfd->a, hfd->cosmo, hfd->status) *
    k*k2/2.0/M_PI/M_PI *
    exp(-k2 * (hfd->r2)) *
    (-k2 * 2.0 * (hfd->r)));
}

static double twoderiv_gauss_norm_int_func(double lnk, void *p) {
  struct hf_int_data *hfd = (struct hf_int_data*)p;
  double k = exp(lnk);
  double k2 = k*k;

  return (
    ccl_f2d_t_eval(hfd->plin, lnk, hfd->a, hfd->cosmo, hfd->status) *
    k*k2/2.0/M_PI/M_PI *
    exp(-k2 * (hfd->r2)) *
    (-2.0*k2 + 4.0*k2*k2 * (hfd->r2)));
}

// function whose root is \sigma^2{rsigma, a} = 1
static double rsigma_func(double rsigma, void *p) {
  struct hf_int_data *hfd = (struct hf_int_data*)p;
  double result, lnkmin, lnkmax;
  gsl_function F;
  int gsl_status;

  lnkmin = hfd->plin->lkmin;
  lnkmax = hfd->plin->lkmax;
  hfd->r = rsigma;
  hfd->r2 = rsigma * rsigma;
  F.function = &gauss_norm_int_func;
  F.params = (void *)hfd;
  gsl_status = gsl_integration_cquad(
    &F, lnkmin, lnkmax,
    0.0, hfd->cosmo->gsl_params.INTEGRATION_SIGMAR_EPSREL,
    hfd->workspace, &result, NULL, NULL);

  if (gsl_status != GSL_SUCCESS) {
    ccl_raise_gsl_warning(
      gsl_status,
      "ccl_halofit.c: rsigma_func(): error in integration "
      "for finding the halofit non-linear scale\n");
    *(hfd->status) |= gsl_status;
  }

  return result - 1.0;
}

static double get_rsigma(double a, struct hf_int_data data) {
  double rsigma, rlow = 1e-2, rhigh = 1e2;
  double flow, fhigh;
  int itr, max_itr = 1000, gsl_status;
  const gsl_root_fsolver_type *T;
  gsl_root_fsolver *s;
  gsl_function F;

  data.a = a;
  F.function = &rsigma_func;
  F.params = &data;

  // we have to bound the root, otherwise return -1
  // we will fiil in any -1's in the calling routine
  flow = rsigma_func(rlow, &data);
  fhigh = rsigma_func(rhigh, &data);
  if (flow * fhigh > 0) {
    return -1;
  }

  T = gsl_root_fsolver_brent;
  s = gsl_root_fsolver_alloc(T);
  if (s == NULL) {
    *(data.status) = CCL_ERROR_MEMORY;
  }
  else {
    gsl_root_fsolver_set(s, &F, rlow, rhigh);

    itr = 0;
    do {
      itr++;
      gsl_status = gsl_root_fsolver_iterate(s);
      if (gsl_status == GSL_EBADFUNC)
        break;

      rsigma = gsl_root_fsolver_root(s);
      rlow = gsl_root_fsolver_x_lower(s);
      rhigh = gsl_root_fsolver_x_upper(s);

      gsl_status = gsl_root_test_interval(
        rlow, rhigh,
        data.cosmo->gsl_params.INTEGRATION_SIGMAR_EPSREL,
        data.cosmo->gsl_params.INTEGRATION_SIGMAR_EPSREL);
    } while (gsl_status == GSL_CONTINUE && itr < max_itr);

    gsl_root_fsolver_free(s);

    if (gsl_status != GSL_SUCCESS || itr >= max_itr) {
      ccl_raise_gsl_warning(
        gsl_status, "ccl_halofit.c: get_rsigma(): error in root finding for the halofit non-linear scale\n");
      *(data.status) |= gsl_status;
    }
  }

  return rsigma;
}

/*
 * Allocate a new struct for storing halofit data
 * @param cosmo Cosmological data
 * @param int, status of computations
 */
halofit_struct* ccl_halofit_struct_new(ccl_cosmology *cosmo,
                                       ccl_f2d_t *plin, int *status) {
  size_t n_a;
  int i, gsl_status;
  double lnkmin, lnkmax;
  double *a_vec = NULL;
  double *vals = NULL;
  double *vals_om = NULL;
  double *vals_de = NULL;
  halofit_struct *hf = NULL;
  struct hf_int_data data;
  gsl_function F;
  gsl_integration_cquad_workspace *workspace = NULL;
  double result;
  double sigma2, rsigma, dsigma2drsigma;
  struct hf_model_match_data data_w0eff;
  ccl_cosmology *cosmo_w0eff = NULL;

  // compute spline point locations and integral bounds
  // note that the spline point locations in `a` determine a radius by
  // solving sigma2(R, a) = 1
  // it is this radius that is needed for the subsequent splines of
  // the derivatives.
  lnkmin = plin->lkmin;
  lnkmax = plin->lkmax;
  if(plin->fa != NULL) {
    n_a = plin->fa->size;
    a_vec = plin->fa->x;
  }
  else if(plin->fka != NULL) {
    n_a = plin->fka->interp_object.ysize;
    a_vec = plin->fka->yarr;
  }
  else {
    *status = CCL_ERROR_SPLINE;
    ccl_cosmology_set_status_message(cosmo,
           "ccl_halofit.c: ccl_halofit_struct_new(): "
           "input pk2d has no splines.\n");
  }

  if(*status == 0) {
    ///////////////////////////////////////////////////////
    // memory allocation
    hf = (halofit_struct*)malloc(sizeof(halofit_struct));
    if (hf == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
                                       cosmo,
                                       "ccl_halofit.c: ccl_halofit_struct_new(): "
                                       "memory could not be allocated for halofit_struct\n");
    } else {
      hf->rsigma = NULL;
      hf->sigma2 = NULL;
      hf->n_eff = NULL;
      hf->C = NULL;
      hf->weff = NULL;
      hf->omeff = NULL;
      hf->deeff = NULL;
    }
  }

  if (*status == 0) {
    workspace = gsl_integration_cquad_workspace_alloc(cosmo->gsl_params.N_ITERATION);
    if (workspace == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for cquad workspace\n");
    }
  }

  if (*status == 0) {
    vals = (double*)malloc(sizeof(double) * n_a);
    if (vals == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for results vector\n");
    }
  }

  if (*status == 0) {
    vals_om = (double*)malloc(sizeof(double) * n_a);
    if (vals_om == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for OmegaM results vector\n");
    }
  }

  if (*status == 0) {
    vals_de = (double*)malloc(sizeof(double) * n_a);
    if (vals_de == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for OmegaDE results vector\n");
    }
  }

  ////////////////////////////////////////////////////////
  // if wa != 0, then we need to find an equivalent
  // cosmology with wa = 0
  data_w0eff.cosmo = cosmo;
  data_w0eff.status = status;

  if (*status == 0) {
    if (cosmo->params.wa != 0) {
      for(i=0; i<n_a; ++i) {
        vals[i] = get_w0eff(a_vec[i], data_w0eff);
        if (*status != 0) {
          *status = CCL_ERROR_ROOT;
          ccl_cosmology_set_status_message(
            cosmo,
            "ccl_halofit.c: ccl_halofit_struct_new(): "
            "could not solve for effective value of w0 for w0-wa cosmology\n");
          break;
        }

        // now get omeff and deff
        cosmo_w0eff = create_w0eff_cosmo(vals[i], cosmo, status);
        if (cosmo_w0eff == NULL) {
          *status = CCL_ERROR_MEMORY;
          ccl_cosmology_set_status_message(
            cosmo,
            "ccl_halofit.c: ccl_halofit_struct_new(): "
            "could not allocat memory for effective w0 for w0-wa cosmology\n");
          break;
        }

        vals_om[i] = ccl_omega_x(cosmo_w0eff, a_vec[i], ccl_species_m_label, status) +
          ccl_omega_x(cosmo_w0eff, a_vec[i], ccl_species_nu_label, status);
        vals_de[i] = ccl_omega_x(cosmo_w0eff, a_vec[i], ccl_species_l_label, status);

        ccl_parameters_free(&(cosmo_w0eff->params));
        ccl_cosmology_free(cosmo_w0eff);
        cosmo_w0eff = NULL;

        if (*status != 0) {
          ccl_cosmology_set_status_message(
            cosmo,
            "ccl_halofit.c: ccl_halofit_struct_new(): "
            "could not compute OmegaM and OmegaDE for cosmology\n");
          break;
        }
      }
    } else {
      for(i=0; i<n_a; ++i) {
        vals[i] = cosmo->params.w0;
        vals_om[i] = ccl_omega_x(cosmo, a_vec[i], ccl_species_m_label, status) +
          ccl_omega_x(cosmo, a_vec[i], ccl_species_nu_label, status);
        vals_de[i] = ccl_omega_x(cosmo, a_vec[i], ccl_species_l_label, status);
        if (*status != 0) {
          ccl_cosmology_set_status_message(
            cosmo,
            "ccl_halofit.c: ccl_halofit_struct_new(): "
            "could not compute OmegaM and OmegaDE for cosmology\n");
          break;
        }
      }
    }
  }

  // spline the weff values
  if (*status == 0) {
    hf->weff = gsl_spline_alloc(gsl_interp_akima, n_a);
    if (hf->weff == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for weff spline\n");
    }
  }

  if (*status == 0) {
    gsl_status = gsl_spline_init(hf->weff, a_vec, vals, n_a);
    if (gsl_status != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): could not build weff spline\n");
    }
  }

  // spline the omeff values
  if (*status == 0) {
    hf->omeff = gsl_spline_alloc(gsl_interp_akima, n_a);
    if (hf->omeff == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for omeff spline\n");
    }
  }

  if (*status == 0) {
    gsl_status = gsl_spline_init(hf->omeff, a_vec, vals_om, n_a);
    if (gsl_status != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): could not build omeff spline\n");
    }
  }

  // spline the deeff values
  if (*status == 0) {
    hf->deeff = gsl_spline_alloc(gsl_interp_akima, n_a);
    if (hf->deeff == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for deeff spline\n");
    }
  }

  if (*status == 0) {
    gsl_status = gsl_spline_init(hf->deeff, a_vec, vals_de, n_a);
    if (gsl_status != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): could not build deeff spline\n");
    }
  }

  ///////////////////////////////////////////////////////
  // find the nonlinear scale at each scale factor
  if (*status == 0) {
    // setup for integrations
    data.status = status;
    data.cosmo = cosmo;
    data.plin = plin;
    data.workspace = workspace;

    for (i=0; i<n_a; ++i) {
      vals[i] = get_rsigma(a_vec[i], data);
      if (*status != 0) {
        *status = CCL_ERROR_ROOT;
        ccl_cosmology_set_status_message(
          cosmo,
          "ccl_halofit.c: ccl_halofit_struct_new(): "
          "could not solve for non-linear scale for halofit at scale factor %f\n", a_vec[i]);
        break;
      }
    }

    // now go backwards and fill any -1's
    // one must work, so set an error if not
    if (vals[n_a-1] == -1) {
      *status = CCL_ERROR_ROOT;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "could not solve for non-linear scale for halofit at least once\n");
    }
    if (*status == 0) {
      // linearly set rsigma to a very small number at high-z
      double min_a = -1, max_a = -1;
      double max_val = -1;
      double w;

      // first non -1 value and scale factor of last -1 value
      for (i=1; i<n_a; ++i) {
        if (vals[i] != -1) {
          max_a = a_vec[i];
          max_val = vals[i];
          break;
        }
      }
      // scale factor of first -1 value
      for (i=0; i<n_a; ++i) {
        if (vals[i] == -1) {
          min_a = a_vec[i];
          break;
        }
      }

      if (min_a != -1) {
        // at least one value is -1 so set the zeroth value
        vals[0] = 1e-6;

        // interp any values that remain -1
        for(i=1; i<n_a-1; ++i) {
          if (vals[i] == -1) {
            w = (a_vec[i] - min_a) / (max_a - min_a);
            vals[i] = w * max_val + (1.0 - w) * vals[0];
          }
        }
      }
    }
  }

  // spline the non-linear scales
  if (*status == 0) {
    hf->rsigma = gsl_spline_alloc(gsl_interp_akima, n_a);
    if (hf->rsigma == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for Rsigma spline\n");
    }
  }

  if (*status == 0) {
    gsl_status = gsl_spline_init(hf->rsigma, a_vec, vals, n_a);
    if (gsl_status != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): could not build Rsigma spline\n");
    }
  }

  ///////////////////////////////////////////////////////
  // now compute sigma2(R) at each spline point
  // and spline that
  // this should be close to 1 OFC, but better to use the exact value
  if (*status == 0) {
    for (i=0; i<n_a; ++i) {
      data.a = a_vec[i];
      vals[i] = rsigma_func(gsl_spline_eval(hf->rsigma, a_vec[i], NULL), &data) + 1;
      if (*status != 0) {
        ccl_cosmology_set_status_message(
          cosmo,
          "ccl_halofit.c: ccl_halofit_struct_new(): could not eval "
          "points for sigma2(R) spline\n");
        break;
      }
    }
  }

  if (*status == 0) {
    hf->sigma2 = gsl_spline_alloc(gsl_interp_akima, n_a);
    if (hf->sigma2 == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for sigma2(R) spline\n");
    }
  }

  if (*status == 0) {
    gsl_status = gsl_spline_init(hf->sigma2, a_vec, vals, n_a);
    if (gsl_status != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): could not build sigma2(R) spline\n");
    }
  }

  ///////////////////////////////////////////////////////
  // now compute the effective spectral index
  if (*status == 0) {
    F.function = &onederiv_gauss_norm_int_func;
    F.params = &data;

    for (i=0; i<n_a; ++i) {
      rsigma = gsl_spline_eval(hf->rsigma, a_vec[i], NULL);
      sigma2 = gsl_spline_eval(hf->sigma2, a_vec[i], NULL);

      data.a = a_vec[i];
      data.r = rsigma;
      data.r2 = rsigma * rsigma;

      gsl_status = gsl_integration_cquad(
        &F,
        lnkmin, lnkmax,
        0.0, cosmo->gsl_params.INTEGRATION_SIGMAR_EPSREL,
        workspace, &result, NULL, NULL);

      if (gsl_status != GSL_SUCCESS) {
        *status = CCL_ERROR_INTEG;
        ccl_raise_gsl_warning(
          gsl_status,
          "ccl_power.c: ccl_halofit_struct_new(): could not eval "
          "points for n_eff spline\n");
        ccl_cosmology_set_status_message(
          cosmo,
          "ccl_halofit.c: ccl_halofit_struct_new(): could not eval "
          "points for n_eff spline\n");
        break;
      }

      // this is n_eff but expressed in terms of linear derivs
      // see eqn A5 of Takahashi et al.
      vals[i] = -rsigma / sigma2 * result - 3.0;
    }
  }

  if (*status == 0) {
    hf->n_eff = gsl_spline_alloc(gsl_interp_akima, n_a);
    if (hf->n_eff == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for n_eff spline\n");
    }
  }

  if (*status == 0) {
    gsl_status = gsl_spline_init(hf->n_eff, a_vec, vals, n_a);
    if (gsl_status != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): could not build n_eff spline\n");
    }
  }

  ///////////////////////////////////////////////////////
  // now compute the curvature C
  if (*status == 0) {
    F.function = &twoderiv_gauss_norm_int_func;
    F.params = &data;

    for (i=0; i<n_a; ++i) {
      rsigma = gsl_spline_eval(hf->rsigma, a_vec[i], NULL);
      sigma2 = gsl_spline_eval(hf->sigma2, a_vec[i], NULL);

      // we need to solve for the deriv we need from n_eff here
      dsigma2drsigma = gsl_spline_eval(hf->n_eff, a_vec[i], NULL);
      dsigma2drsigma = (dsigma2drsigma + 3.0) / (-rsigma / sigma2);

      data.a = a_vec[i];
      data.r = rsigma;
      data.r2 = rsigma * rsigma;

      gsl_status = gsl_integration_cquad(
        &F,
        lnkmin, lnkmax,
        0.0, cosmo->gsl_params.INTEGRATION_SIGMAR_EPSREL,
        workspace, &result, NULL, NULL);

      if (gsl_status != GSL_SUCCESS) {
        *status = CCL_ERROR_INTEG;
        ccl_raise_gsl_warning(
          gsl_status,
          "ccl_power.c: ccl_halofit_struct_new(): could not eval "
          "points for C spline\n");
        ccl_cosmology_set_status_message(
          cosmo,
          "ccl_halofit.c: ccl_halofit_struct_new(): could not eval "
          "points for C spline\n");
        break;
      }

      // this is C but expressed in terms of linear derivs
      // see eqn A5 of Takahashi et al.
      vals[i] = (
        -1.0 * (
          result * rsigma * rsigma / sigma2 +
          dsigma2drsigma * rsigma / sigma2 -
          dsigma2drsigma * dsigma2drsigma * rsigma * rsigma / sigma2 / sigma2));
    }
  }

  if (*status == 0) {
    hf->C = gsl_spline_alloc(gsl_interp_akima, n_a);
    if (hf->C == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): "
        "memory could not be allocated for C spline\n");
    }
  }

  if (*status == 0) {
    gsl_status = gsl_spline_init(hf->C, a_vec, vals, n_a);
    if (gsl_status != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halofit.c: ccl_halofit_struct_new(): could not build C spline\n");
    }
  }

  // free stuff on the way out
  // if the status is non-zero, then we should free any data already
  // accumulated
  if (*status != 0) {
    ccl_halofit_struct_free(hf);
    hf = NULL;
  }
  gsl_integration_cquad_workspace_free(workspace);
  free(vals);
  free(vals_om);
  free(vals_de);
  if (cosmo_w0eff != NULL) {
    ccl_parameters_free(&(cosmo_w0eff->params));
    ccl_cosmology_free(cosmo_w0eff);
  }

  return hf;
}

/*
 * Free a halofit struct
 * @param hf, pointer to halofit struct to free
 */
void ccl_halofit_struct_free(halofit_struct *hf) {
  if (hf != NULL) {
    if (hf->rsigma != NULL)
      gsl_spline_free(hf->rsigma);

    if (hf->sigma2 != NULL)
      gsl_spline_free(hf->sigma2);

    if (hf->n_eff != NULL)
      gsl_spline_free(hf->n_eff);

    if (hf->C != NULL)
      gsl_spline_free(hf->C);

    if (hf->weff != NULL)
      gsl_spline_free(hf->weff);

    if (hf->omeff != NULL)
      gsl_spline_free(hf->omeff);

    if (hf->deeff != NULL)
      gsl_spline_free(hf->deeff);

    free(hf);
  }
}

/**
 * Computes the halofit non-linear power spectrum
 * @param cosmo: cosmology object containing parameters
 * @param lk: natural logarithm of wavenumber in units of Mpc^{-1}
 * @param a: scale factor normalised to a=1 today
 * @param status: Status flag: 0 if there are no errors, non-zero otherwise
 * @param hf: halofit splines for evaluating the power spectrum
 * @return halofit_matter_power: halofit power spectrum, P(k), units of Mpc^{3}
 */
double ccl_halofit_power(ccl_cosmology *cosmo, ccl_f2d_t *plin,
                         double lk, double a, halofit_struct *hf, int *status) {
  double rsigma, neff, C;
  double ksigma, weffa, omegaMz, omegaDEwz, kh;
  double PkL, PkNL, f1, f2, f3, an, bn, cn, gamman, alphan, betan, nun, mun, y, fy;
  double DeltakL, DeltakL_tilde, DeltakQ, DeltakH, DeltakHprime, DeltakNL;
  double Qnu, fnu;
  double f1a, f2a, f3a, f1b, f2b, f3b, fb_frac;
  double neff2, neff3, neff4;
  double kh2, y2;
  double delta2_norm, om_nu;
  double k=exp(lk);

  // all eqns are from Takahashi et al. unless stated otherwise
  // eqns A4 - A5
  rsigma = gsl_spline_eval(hf->rsigma, a, NULL);
  neff = gsl_spline_eval(hf->n_eff, a, NULL);
  C = gsl_spline_eval(hf->C, a, NULL);

  weffa = cosmo->params.w0;
  omegaMz = ccl_omega_x(cosmo, a, ccl_species_m_label, status);
  omegaDEwz = ccl_omega_x(cosmo, a, ccl_species_l_label, status);

  // not using these to match CLASS better - might be a bug in CLASS
  // weffa = gsl_spline_eval(hf->weff, a, NULL);
  // omegaMz = gsl_spline_eval(hf->omeff, a, NULL);
  // omegaDEwz = gsl_spline_eval(hf->deeff, a, NULL);

  ksigma = 1.0 / rsigma;
  neff2 = neff * neff;
  neff3 = neff2 * neff;
  neff4 = neff3 * neff;

  delta2_norm = k*k*k/2.0/M_PI/M_PI;

  // compute the present day neutrino massive neutrino fraction
  // uses all neutrinos even if they are moving fast
  om_nu = cosmo->params.sum_nu_masses / 93.14 / cosmo->params.h / cosmo->params.h;
  fnu = om_nu / (cosmo->params.Omega_m);

  // eqns A6 - A13 of Takahashi et al.
  an = pow(
    10.0,
    1.5222 + 2.8553*neff + 2.3706*neff2 + 0.9903*neff3 +
    0.2250*neff4 - 0.6038*C + 0.1749*omegaDEwz*(1.0 + weffa));
  bn = pow(10.0, -0.5642 + 0.5864*neff + 0.5716*neff2 - 1.5474*C + 0.2279*omegaDEwz*(1.0 + weffa));
  cn = pow(10.0, 0.3698 + 2.0404*neff + 0.8161*neff2 + 0.5869*C);
  gamman = 0.1971 - 0.0843*neff + 0.8460*C;
  alphan = fabs(6.0835 + 1.3373*neff - 0.1959*neff2 - 5.5274*C);
  betan = 2.0379 - 0.7354*neff + 0.3157*neff2 + 1.2490*neff3 + 0.3980*neff4 - 0.1682*C;
  mun = 0.0;
  nun = pow(10.0, 5.2105 + 3.6902*neff);

  // eqns C17 and C18 for Smith et al.
  if (fabs(1.0 - omegaMz) > 0.01) {
    f1a = pow(omegaMz, -0.0732);
    f2a = pow(omegaMz, -0.1423);
    f3a = pow(omegaMz, 0.0725);
    f1b = pow(omegaMz, -0.0307);
    f2b = pow(omegaMz, -0.0585);
    f3b = pow(omegaMz, 0.0743);
    fb_frac = omegaDEwz / (1.0 - omegaMz);
    f1 = fb_frac * f1b + (1.0 - fb_frac) * f1a;
    f2 = fb_frac * f2b + (1.0 - fb_frac) * f2a;
    f3 = fb_frac * f3b + (1.0 - fb_frac) * f3a;
  } else {
    f1 = 1.0;
    f2 = 1.0;
    f3 = 1.0;
  }

  // correction to betan from Bird et al., eqn A10
  betan += (fnu * (1.081 + 0.395*neff2));

  // eqns A1 - A3
  PkL = ccl_f2d_t_eval(plin, lk, a, cosmo, status);
  y = k/ksigma;
  y2 = y * y;
  fy = y/4.0 + y2/8.0;
  DeltakL = PkL * delta2_norm;

  // correction to DeltakL from Bird et al., eqn A9
  kh = k / cosmo->params.h;
  kh2 = kh * kh;
  DeltakL_tilde = DeltakL * (1.0 + fnu * (47.48 * kh2) / (1.0 + 1.5 * kh2));
  DeltakQ = DeltakL * pow(1.0 + DeltakL_tilde, betan) / (1.0 + alphan*DeltakL_tilde) * exp(-fy);

  DeltakHprime = an * pow(y, 3.0*f1) / (1.0 + bn*pow(y, f2) + pow(cn*f3*y, 3.0 - gamman));
  DeltakH = DeltakHprime / (1.0 + mun/y + nun/y2);

  // correction to DeltakH from Bird et al., eqn A6-A7
  Qnu = fnu * (0.977 - 18.015 * (cosmo->params.Omega_m - 0.3));
  DeltakH *= (1.0 + Qnu);

  DeltakNL = DeltakQ + DeltakH;
  PkNL = DeltakNL / delta2_norm;

  // we check the status once
  if(*status != 0)
    return NAN;
  else
    return PkNL;
}


/**
 *
 * @file testing_ssyev.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Azzam Haidar
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:45:18 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_smain.h"

#undef COMPLEX
#define REAL

static int check_orthogonality(int, int, float*, int, float);
static int check_reduction(int, int, int, float*, float*, int, float*, float);
static int check_solution(int, float*, float*, float);

int testing_ssyev(int argc, char **argv)
{
    /* Check for number of arguments*/
    if (argc != 2) {
        USAGE("HEEV", "N LDA",
              "   - N    : size of the matrix A\n"
              "   - LDA  : leading dimension of the matrix A\n");
        return -1;
    }

    int    N     = atoi(argv[0]);
    int    LDA   = atoi(argv[1]);
    int    LDQ   = LDA;
    int    mode  = 4;
    float eps   = LAPACKE_slamch_work('e');
    float dmax  = 1.0;
    float rcond = 1.0e6;
    int INFO=-1;

    PLASMA_enum uplo = PlasmaUpper;
    PLASMA_enum vec  = PlasmaVec;
    int info_ortho     = 0;
    int info_solution  = 0;
    int info_reduction = 0;
    int LDAxN = LDA*N;
    int LDQxN = LDQ*N;

    float *A1   = NULL;
    float *A2   = (float *)malloc(LDAxN*sizeof(float));
    float *Q    = NULL;
    float             *W1   = (float *)malloc(N*sizeof(float));
    float             *W2   = (float *)malloc(N*sizeof(float));
    float *work = (float *)malloc(3*N* sizeof(float));
    PLASMA_desc *T;

    /* Check if unable to allocate memory */
    if ( (!A2) || (!W1) || (!W2) ){
        printf("Out of Memory \n ");
        return -2;
    }

    PLASMA_Disable(PLASMA_AUTOTUNING);
    PLASMA_Set(PLASMA_TILE_SIZE, 120);
    PLASMA_Set(PLASMA_INNER_BLOCK_SIZE, 20);

    PLASMA_Enable(PLASMA_WARNINGS);
    PLASMA_Enable(PLASMA_ERRORS);
    PLASMA_Alloc_Workspace_ssyev(N, N, &T);

    /*----------------------------------------------------------
    *  TESTING SSYEV
    */
    /* Initialize A1 */
    LAPACKE_slatms_work( LAPACK_COL_MAJOR, N, N,
                         lapack_const(PlasmaDistSymmetric), ISEED,
                         lapack_const(PlasmaHermGeev), W1, mode, rcond,
                         dmax, N, N,
                         lapack_const(PlasmaNoPacking), A2, LDA, work );

    /*
     * Sort the eigenvalue because when computing the tridiag
     * and then the eigenvalue of the DSTQR are sorted.
     * So to avoid testing fail when having good results W1 should be sorted
    */
    LAPACKE_slasrt_work( 'I', N, W1 );

    if ( vec == PlasmaVec ) {
        Q  = (float *)malloc(LDQxN*sizeof(float));
        A1 = (float *)malloc(LDAxN*sizeof(float));

        /* Copy A2 into A1 */
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR, 'A', N, N, A2, LDA, A1, LDA);
    }
    /*
     * PLASMA SSYEV
     */
    INFO = PLASMA_ssyev(vec, uplo, N, A2, LDA, W2, T, Q, LDQ);
    if(INFO!=0){
            printf(" ERROR OCCURED INFO %d\n",INFO);
            goto fin;
    }

    printf("\n");
    printf("------ TESTS FOR PLASMA SSYEV ROUTINE -------  \n");
    printf("        Size of the Matrix %d by %d\n", N, N);
    printf("\n");
    printf(" The matrix A is randomly generated for each test.\n");
    printf("============\n");
    printf(" The relative machine precision (eps) is to be %e \n",eps);
    printf(" Computational tests pass if scaled residuals are less than 60.\n");

    /* Check the orthogonality, reduction and the eigen solutions */
    if (vec == PlasmaVec) {
        info_ortho = check_orthogonality(N, N, Q, LDQ, eps);
        info_reduction = check_reduction(uplo, N, 1, A1, W2, LDA, Q, eps);
    }
    info_solution = check_solution(N, W1, W2, eps);

    if ( (info_solution == 0) & (info_ortho == 0) & (info_reduction == 0) ) {
        printf("***************************************************\n");
        printf(" ---- TESTING SSYEV ...................... PASSED !\n");
        printf("***************************************************\n");
    }
    else {
        printf("************************************************\n");
        printf(" - TESTING SSYEV ... FAILED !\n");
        printf("************************************************\n");
    }

fin:
    PLASMA_Dealloc_Handle_Tile(&T);
    free(A2);
    free(W1);
    free(W2);
    free(work);
    if (Q  != NULL) free(Q);
    if (A1 != NULL) free(A1);

    return 0;
}

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */
static int check_orthogonality(int M, int N, float *Q, int LDQ, float eps)
{
    float  done  =  1.0;
    float  mdone = -1.0;
    float  normQ, result;
    int     info_ortho;
    int     minMN = min(M, N);
    float *work = (float *)malloc(minMN*sizeof(float));

    /* Build the idendity matrix */
    float *Id = (float *) malloc(minMN*minMN*sizeof(float));
    LAPACKE_slaset_work(LAPACK_COL_MAJOR, 'A', minMN, minMN, 0., 1., Id, minMN);

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_ssyrk(CblasColMajor, CblasUpper, CblasTrans, N, M, done, Q, LDQ, mdone, Id, N);
    else
        cblas_ssyrk(CblasColMajor, CblasUpper, CblasNoTrans,   M, N, done, Q, LDQ, mdone, Id, M);

    normQ = LAPACKE_slansy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), 'U', minMN, Id, minMN, work);

    result = normQ / (minMN * eps);
    printf(" ======================================================\n");
    printf(" ||Id-Q'*Q||_oo / (N*eps)          : %15.3E \n",  result );
    printf(" ======================================================\n");

    if ( isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }
    free(work); free(Id);
    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the reduction
 */
static int check_reduction(int uplo, int N, int bw, float *A, float *D, int LDA, float *Q, float eps )
{
    float zone  =  1.0;
    float mzone = -1.0;
    float *TEMP     = (float *)malloc(N*N*sizeof(float));
    float *Residual = (float *)malloc(N*N*sizeof(float));
    float *work = (float *)malloc(N*sizeof(float));
    float Anorm, Rnorm, result;
    int info_reduction;
    int i;

    /* Compute TEMP =  Q * LAMBDA */
    LAPACKE_slacpy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaUpperLower), N, N, Q, LDA, TEMP, N);
    for (i = 0; i < N; i++){
            cblas_sscal(N, D[i], &(TEMP[i*N]),1);
    }
    /* Compute Residual = A - Q * LAMBDA * Q^H */
    /* A is Hermetian but both upper and lower
     * are assumed valable here for checking
     * otherwise it need to be symetrized before
     * checking.
     */
    LAPACKE_slacpy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaUpperLower), N, N, A, LDA, Residual, N);
    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasTrans, N, N, N, (mzone), TEMP, N,  Q, LDA, (zone), Residual,     N);

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaOneNorm), N, N, Residual, N,   work);
    Anorm = LAPACKE_slansy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaOneNorm), lapack_const(uplo), N, A,       LDA, work);

    result = Rnorm / ( Anorm * N * eps);
    if ( uplo == PlasmaLower ){
        printf(" ======================================================\n");
        printf(" ||A-Q*LAMBDA*Q'||_oo/(||A||_oo.N.eps) : %15.3E \n",  result );
        printf(" ======================================================\n");
    }else{
        printf(" ======================================================\n");
        printf(" ||A-Q'*LAMBDA*Q||_oo/(||A||_oo.N.eps) : %15.3E \n",  result );
        printf(" ======================================================\n");
    }

    if ( isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- Reduction is suspicious ! \n");
        info_reduction = 1;
    }
    else {
        printf("-- Reduction is CORRECT ! \n");
        info_reduction = 0;
    }

    free(TEMP); free(Residual);
    free(work);

    return info_reduction;
}
/*------------------------------------------------------------
 *  Check the eigenvalues
 */
static int check_solution(int N, float *E1, float *E2, float eps)
{
    int info_solution, i;
    float resid;
    float maxtmp;
    float maxel = fabs( fabs(E1[0]) - fabs(E2[0]) );
    float maxeig = max( fabs(E1[0]), fabs(E2[0]) );
    for (i = 1; i < N; i++){
        resid   = fabs(fabs(E1[i])-fabs(E2[i]));
        maxtmp  = max(fabs(E1[i]), fabs(E2[i]));

        /* Update */
        maxeig = max(maxtmp, maxeig);
        maxel  = max(resid,  maxel );
    }

    maxel = maxel / (maxeig * N * eps);
    printf(" ======================================================\n");
    printf(" | D - eigcomputed | / (|D| * N * eps) : %15.3E \n",  maxel );
    printf(" ======================================================\n");

    if ( isnan(maxel) || isinf(maxel) || (maxel > 100) ) {
        printf("-- The eigenvalues are suspicious ! \n");
        info_solution = 1;
    }
    else{
        printf("-- The eigenvalues are CORRECT ! \n");
        info_solution = 0;
    }
    return info_solution;
}


// -*- C++ -*-
//
// michael a.g. aïvázis <michael.aivazis@para-sim.com>
//
// (c) 2013-2020 parasim inc
// (c) 2010-2020 california institute of technology
// all rights reserved
//

// code guard
#if !defined(altar_bayesian_CoolingStep_h)
#define altar_bayesian_CoolingStep_h

// externals
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

// place everything in the local namespace
namespace altar {
    namespace bayesian {

        // forward declarations
        class CoolingStep;

    } // of namespace bayesian
} // of namespace altar

// declaration
class altar::bayesian::CoolingStep
{
    // types
public:
    typedef gsl_vector vector_t;
    typedef gsl_matrix matrix_t;

    // interface
public:
    inline auto samples() const;
    inline auto parameters() const;

    inline auto beta() const;
    inline void beta(double value);

    inline auto theta() const;
    inline auto prior() const;
    inline auto data() const;
    inline auto posterior() const;
    inline auto sigma() const;

    // meta-methods
public:
    virtual ~CoolingStep();
    inline CoolingStep(size_t samples, size_t parameters, double beta=0.0);

    // data
private:
    const size_t _samples; // the number of samples
    const size_t _parameters; // the number of model parameters

    double _beta; // the current temperature
    matrix_t * const _theta; // the sample set, a (samples x parameters) matrix

    vector_t * const _prior; // the log likelihoods of the samples in the model prior
    vector_t * const _data; // the log likelihoods of the samples given the data
    vector_t * const _posterior; // the posterior log likelihood

    matrix_t * const _sigma; // the parameter covariance

    // disallow
private:
    CoolingStep(const CoolingStep &) = delete;
    const CoolingStep & operator=(const CoolingStep &) = delete;
};

// get the inline definitions
#define altar_bayesian_CoolingStep_icc
#include "CoolingStep.icc"
#undef altar_bayesian_CoolingStep_icc

# endif
// end of file


#ifndef SCANMATCHERPROCESSOR_H
#define SCANMATCHERPROCESSOR_H

#include <gmapping/log/sensorlog.h>
#include <gmapping/sensor/sensor_range/rangesensor.h>
#include <gmapping/sensor/sensor_range/rangereading.h>
//#include <gsl/gsl_eigen.h>
#include <gmapping/scanmatcher/scanmatcher.h>

namespace GMapping {

class ScanMatcherProcessor
{
	public:
  ScanMatcherProcessor(const ScanMatcherMap& m);
  ScanMatcherProcessor (double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta);
		virtual ~ScanMatcherProcessor ();
		virtual void processScan(const RangeReading & reading);
		void setSensorMap(const SensorMap& smap, std::string sensorName="FLASER");
		void init();
		void setMatchingParameters
			(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance=false);
		void setRegistrationParameters(double regScore, double critScore);
		OrientedPoint getPose() const;
		inline const ScanMatcherMap& getMap() const {return m_map;}
		inline ScanMatcher& matcher() {return m_matcher;}
		inline void setmaxMove(double mmove){m_maxMove=mmove;}
		bool useICP;
	protected:
		ScanMatcher m_matcher;
		bool m_computeCovariance;
		bool m_first;
		SensorMap m_sensorMap;
		double m_regScore, m_critScore;
		unsigned int m_beams;
		double m_maxMove;
		//state
		ScanMatcherMap m_map;
		OrientedPoint m_pose;
		OrientedPoint m_odoPose;
		int  m_count;
		//gsl_eigen_symmv_workspace * m_eigenspace;
};

};

#endif


//
// ePDF
//

#pragma once

#include <gsl/gsl_integration.h>

namespace ePDF
{
  struct int_params
  {
    double z;
    int nl;
  };

  /**
   * @brief The "NumericIntegrals" class
   */
  class NumericIntegrals
  {
  public:
    /**
     * @brief The "NumericIntegrals" constructor.
     * @param flv: 0 = singlet, 1 = photon, 2 = nonsinglet
     * @param nl: number of leptons (needed for singlet)
     */
    NumericIntegrals(int const& iflv, int const& nl);

    /**
     * @brief The "NumericIntegrals" destructor.
     */
    ~NumericIntegrals();

    /**
     * @brief Perform the integration
     * @param z: momentum fraction
     */
    double integrate(double const& z) const;

  private:
    int    const _iflv;
    int    const _nl;
    double const _epsabs;
    double const _epsrel;
    gsl_integration_workspace *_w;
  };
}


/******************************************************************************
 *                                                                            *
 * ELECTRONS.C                                                                *
 *                                                                            *
 * ELECTRON THERMODYNAMICS                                                    *
 *                                                                            *
 ******************************************************************************/

#include "decs.h"
#include <gsl/gsl_sf_bessel.h>

/* TODO: Encapsulate electron equation of state in EOS framework.
 *       For now, I'v ebroken encapsulation but made the code
 *	 complain/break if EOS_TYPE is not EOS_TYPE_GAMMA
 *	 when ELECTRONS are active.
 *
 *       The best strategy is probably to make a two-temperature
 *       electron EOS one of the available EOS's.
 *
 *       Until then, I'm worried that we might hit a maintainability
 *       problem becasue if the implementation in ELECTRONS changes
 *       the implementation in EOS_TYPE_GAMMA needs to change too.
 *       ~JMM
 */

#if ELECTRONS
#if EOS == EOS_TYPE_GAMMA
void heat_electrons_zone(int i, int j, int k, double Pi[NVAR], double Ps[NVAR],
    double Pf[NVAR], double Dt);
void fixup_electrons_1zone(double P[NVAR]);

void init_electrons() {
  double uel;

  ZSLOOP(-NG, N1 + NG - 1, -NG, NG + N2 - 1, -NG, NG + N3 - 1) {
    // Set electron internal energy to constant fraction of internal energy
    uel = fel0 * P[i][j][k][UU];

    // Initialize entropies
    P[i][j][k][KTOT] = (gam - 1.) * P[i][j][k][UU] * pow(P[i][j][k][RHO], -gam);
    P[i][j][k][KEL]  = (game - 1.) * uel * pow(P[i][j][k][RHO], -game);
  }

  bound_prim(P);
}

void heat_electrons(
    grid_prim_type Pi, grid_prim_type Ps, grid_prim_type Pf, double Dt) {
  timer_start(TIMER_ELECTRON);
#pragma omp parallel for collapse(3) schedule(dynamic)
  ZLOOP {
    heat_electrons_zone(i, j, k, Pi[i][j][k], Ps[i][j][k], Pf[i][j][k], Dt);
  }
  timer_stop(TIMER_ELECTRON);
}

void heat_electrons_zone(int i, int j, int k, double Pi[NVAR], double Ps[NVAR],
    double Pf[NVAR], double Dt) {
  double ktotharm, ktotadv, fel;

  // Calculated and advected entropy at final time
  ktotharm = (gam - 1.) * Pf[UU] / pow(Pf[RHO], gam);
  ktotadv  = Pf[KTOT];

  // Electron heating fraction
  fel = get_fel(i, j, k, Ps);

  // Update final electron entropy according to Ressler+ 2015 Eqn. 27:
  Pf[KEL] += (game - 1.) / (gam - 1.) * pow(Ps[RHO], gam - game) * fel *
             (ktotharm - ktotadv);

  // Diagnostics
  struct of_geom *geom = &ggeom[i][j][CENT];
  struct of_state q;
  get_state(Ps, geom, &q);
  double uadv = ktotadv / (gam - 1.) * pow(Pf[RHO], gam);
  double Qud  = q.ucon[0] * q.ucov[0] * (Pf[UU] - uadv) *
               pow(Ps[RHO] / Pf[RHO], gam) / Dt;
  Qvisc[i][j][k] = fel * Qud;

  // Reset total entropy
  Pf[KTOT] = ktotharm;
}

double get_fel(int i, int j, int k, double P[NVAR]) {
#if BETA_HEAT == 0
  return fel0;
#endif

  struct of_geom geom;
  double         beta, fel, c1, Trat, Tpr, mbeta, qrat;
  double         pres, bsq;
  double         c2, c3, c22, c32;

  c1 = 0.92;

  Tpr        = (gam - 1.) * P[UU] / P[RHO];
  double uel = 1. / (game - 1.) * P[KEL] * pow(P[RHO], game);
  double Tel = (game - 1.) * uel / P[RHO];
  if (Tel <= 0.)
    Tel = SMALL;
  if (Tpr <= 0.)
    Tpr = SMALL;
  Trat = fabs(Tpr / Tel);

  pres = P[RHO] * Tpr; // Proton pressure

  geom = *get_geometry(i, j, k, CENT);
  bsq  = bsq_calc(P, &geom);

  beta = pres / bsq * 2.;
  if (beta > 1.e20)
    beta = 1.e20;
  mbeta = 2. - 0.2 * log10(Trat);

  if (Trat <= 1.) {
    c2 = 1.6 / Trat;
    c3 = 18. + 5. * log10(Trat);
  } else {
    c2 = 1.2 / Trat;
    c3 = 18.;
  }
  c22 = pow(c2, 2.);
  c32 = pow(c3, 2.);

  qrat = c1 * (c22 + pow(beta, mbeta)) / (c32 + pow(beta, mbeta)) *
         exp(-1. / beta) * pow(MP / ME * Trat, .5);
  fel = 1. / (1. + qrat);

  return fel;
}

// Modified Bessel function of second kind with safe inputs
double safe_Kn(int n, double x) {
  if (x > 100.) {
    return exp(-x) * sqrt(M_PI / (2. * x));
  } else {
    return gsl_sf_bessel_Kn(n, x);
  }
}

#if RADIATION
void coulomb(
    grid_prim_type Pi, grid_prim_type Ps, grid_prim_type Pf, double Dt) {
  timer_start(TIMER_ELECTRON);

#pragma omp parallel for collapse(3)
  ZLOOP {
    double          rho     = Ps[i][j][k][RHO];
    double          thetae  = MP / ME * Ps[i][j][k][KEL] * pow(rho, game - 1.);
    double          ue      = Ps[i][j][k][KEL] * pow(rho, game) / (game - 1.);
    double          up      = Ps[i][j][k][UU] - ue;
    double          n       = rho * Ne_unit;
    double          Ti      = up * U_unit * (gamp - 1.) / (n * KBOL);
    double          thetai  = KBOL * Ti / (MP * CL * CL);
    double          thetam  = 1. / (1. / thetae + 1. / thetai);
    double          logCoul = COULOMB_LOG;
    double          Te      = thetae * ME * CL * CL / KBOL;
    struct of_geom *geom;
    struct of_state q;

    // Sanity checks, although electron fixup routine should catch these
    if (!isnan(Te) && !isnan(Ti) && Te > 0. && Ti > 0.) {
      double Qc, term1, term2;

      // Get Coulomb heating rate.
      // Need to handle cases where Thetai < 1e-2, Thetae < 1e-2, and both
      // Thetae and Thetai < 1e-2 separately due to Bessel functions exploding
      double prefac =
          3. / 2. * ME / MP * n * n * logCoul * CL * KBOL * THOMSON * (Ti - Te);
      double thetaCrit = 1.e-2;
      if (thetae < thetaCrit && thetai < thetaCrit) {
        term1 = sqrt(thetam / (M_PI * thetae * thetai / 2.));
        term2 = sqrt(thetam / (M_PI * thetae * thetai / 2.));
      } else if (thetae < thetaCrit) {
        term1 =
            exp(-1. / thetai) / safe_Kn(2, 1. / thetai) * sqrt(thetam / thetae);
        term2 =
            exp(-1. / thetai) / safe_Kn(2, 1. / thetai) * sqrt(thetam / thetae);
      } else if (thetai < thetaCrit) {
        term1 =
            exp(-1. / thetae) / safe_Kn(2, 1. / thetae) * sqrt(thetam / thetai);
        term2 =
            exp(-1. / thetae) / safe_Kn(2, 1. / thetae) * sqrt(thetam / thetai);
      } else {
        term1 = safe_Kn(1, 1. / thetam) /
                (safe_Kn(2, 1. / thetae) * safe_Kn(2, 1. / thetai));
        term2 = safe_Kn(0, 1. / thetam) /
                (safe_Kn(2, 1. / thetae) * safe_Kn(2, 1. / thetai));
      }
      term1 *= (2. * pow(thetae + thetai, 2) + 1.) / (thetae + thetai);
      term2 *= 2.;
      Qc = prefac * (term1 + term2);

      // Convert to code units
      Qc *= T_unit / U_unit;

      // Update electron internal energy
      geom = &ggeom[i][j][CENT];
      get_state(Ps[i][j][k], geom, &q);

      double ue_f =
          Pf[i][j][k][KEL] * pow(Pf[i][j][k][RHO], game) / (game - 1.);
      ue_f += Qc * Dt / q.ucon[0];

      // Record diagnostic
      Qcoul[i][j][k] = q.ucov[0] * Qc;

      // Update electron entropy
      Pf[i][j][k][KEL] = (game - 1.) * ue_f * pow(Pf[i][j][k][RHO], -game);
    }
  } // ZLOOP

  timer_stop(TIMER_ELECTRON);
}

void apply_rad_force_e(grid_prim_type Prh, grid_prim_type Pr,
    grid_fourvector_type radG, double Dt) {
// Apply only to active zones for this proc -- ghost zone four-force
// depositions already communicated over MPI
#pragma omp parallel for collapse(3) schedule(dynamic)
  ZLOOP {
    struct of_geom *geom = &ggeom[i][j][CENT];
    struct of_state q;
    double          Uel, Urho;
    double          C = 0.;

    // Get fluid state at n + 1/2 where radiation four-force is centered
    get_state(Prh[i][j][k], geom, &q);

    for (int mu = 0; mu < NDIM; mu++) {
      C += -q.ucon[mu] * radG[i][j][k][mu];
    }

    // Get fluid state at n+1 for full update
    get_state(Pr[i][j][k], geom, &q);

    // Remove \sqrt{-g} from radG
    C = C / geom->g;

    Urho = Pr[i][j][k][RHO] * q.ucon[0];
    Uel  = Pr[i][j][k][KEL] * Urho;

    Uel += Dt * (C * (game - 1.) * pow(Prh[i][j][k][RHO], 1. - game));

    // Supercooling diagnostics
    if (Uel < 0.) {
      double          U_1[NVAR], prim_2[NVAR], U_2[NVAR];
      struct of_state q_1, q_2;
      Nsuper[i][j][k]++;

      // (1) Record total energy density after cooling
      get_state(Pr[i][j][k], &ggeom[i][j][CENT], &q_1);
      primtoflux(Pr[i][j][k], &q_1, 0, 0, &ggeom[i][j][CENT], U_1);

      // (2) Calculate total energy density with zero electron energy density
      PLOOP  prim_2[ip] = psupersave[i][j][k][ip];
      double ue         = prim_2[KEL] * pow(prim_2[RHO], game) / (game - 1.);
      prim_2[UU] -= ue;
      get_state(prim_2, &ggeom[i][j][CENT], &q_2);
      primtoflux(prim_2, &q_2, 0, 0, &ggeom[i][j][CENT], U_2);

      // Subtract (2) from (1); integrated over volume, this is fake energy
      Esuper[i][j][k] += fabs((U_1[UU] - U_2[UU]) * dx[1] * dx[2] * dx[3]);
    } // Uel < 0

    Pr[i][j][k][KEL] = Uel / Urho;

    // Reset total entropy
    Pr[i][j][k][KTOT] =
        (gam - 1.) * Pr[i][j][k][UU] * pow(Pr[i][j][k][RHO], -gam);
  } // ZSLOOP
}
#endif // RADIATION

void fixup_electrons(grid_prim_type P) {
  timer_start(TIMER_ELECTRON);
#pragma omp parallel for collapse(3) schedule(dynamic)
  ZLOOP { fixup_electrons_1zone(P[i][j][k]); }
  timer_stop(TIMER_ELECTRON);
}

void fixup_electrons_1zone(double P[NVAR]) {
  double kelmax =
      P[KTOT] * pow(P[RHO], gam - game) / (tptemin + (gam - 1.) / (game - 1.));
  double kelmin =
      P[KTOT] * pow(P[RHO], gam - game) / (tptemax + (gam - 1.) / (game - 1.));

  // Replace NANs with cold electrons
  if (isnan(P[KEL]))
    P[KEL] = kelmin;

  // Enforce maximum Tp/Te
  P[KEL] = MY_MAX(P[KEL], kelmin);

  // Enforce minimum Tp/Te
  P[KEL] = MY_MIN(P[KEL], kelmax);
}
#endif // EOS == EOS_TYPE_GAMMA
#endif // ELECTRONS


#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include <math.h>
#include <libgen.h>

#include <options/options.h>
#include "../csm/csm_all.h"

struct ld_noise_params {
	int seed;
	double discretization;
	double sigma;
	const char* file_input;
	const char* file_output;
	int lambertian;
};

int main(int argc, const char * argv[]) {
	sm_set_program_name(argv[0]);

	options_banner("ld_noise: Adds noise to readings in a scan");

	struct ld_noise_params p;

	struct option* ops = options_allocate(20);
	options_double(ops, "discretization", &p.discretization, 0.0,
		"Size of discretization (disabled if 0)");
	options_double(ops, "sigma", &p.sigma, 0.0,
		"Std deviation of gaussian noise (disabled if 0)");
	options_int(ops, "lambertian", &p.lambertian, 0,
		"Use lambertian model cov = sigma^2 / cos(beta^2) where beta is the incidence. Need have alpha or true_alpha.");
	options_int(ops, "seed", &p.seed, 0,
		"Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
	options_string(ops, "in", &p.file_input, "stdin", "Input file ");
	options_string(ops, "out", &p.file_output, "stdout", "Output file ");

	if(!options_parse_args(ops, argc, argv)) {
		fprintf(stderr, "A simple program for adding noise to sensor scans.\n\nUsage:\n");
		options_print_help(ops, stderr);
		return -1;
	}

	FILE * in = open_file_for_reading(p.file_input);
	if(!in) return -3;

	FILE * out = open_file_for_writing(p.file_output);
	if(!out) return -2;

	gsl_rng_env_setup();
	gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
	if(p.seed != 0)
	gsl_rng_set(rng, (unsigned int) p.seed);

	LDP ld; int count = 0;
	while( (ld = ld_from_json_stream(in))) {
		if(!ld_valid_fields(ld))  {
			sm_error("Invalid laser data (#%d in file)\n", count);
			continue;
		}

		int i;
		for(i=0;i<ld->nrays;i++) {
			if(!ld->valid[i]) continue;

			double * reading = ld->readings + i;
			if(p.sigma > 0) {
				double add_sigma = p.sigma;

				if(p.lambertian) {

					int have_alpha = 0;
					double alpha = 0;
					if(!is_nan(ld->true_alpha[i])) {
						alpha = ld->true_alpha[i];
						have_alpha = 1;
					} else if(ld->alpha_valid[i]) {
						alpha = ld->alpha[i];;
						have_alpha = 1;
					} else have_alpha = 0;

					if(have_alpha) {
						/* Recall that alpha points outside the surface */
						double beta = (alpha+M_PI) - ld->theta[i];
					    add_sigma = p.sigma / cos(beta);
					} else {
						sm_error("Because lambertian is active, I need either true_alpha[] or alpha[]");
						ld_write_as_json(ld, stderr);
						return -1;
					}

				}

			   *reading += gsl_ran_gaussian(rng, add_sigma);

				if(is_nan(ld->readings_sigma[i])) {
					ld->readings_sigma[i] = add_sigma;
				} else {
					ld->readings_sigma[i] = sqrt(square(add_sigma) + square(ld->readings_sigma[i]));
				}
			}
			if(p.discretization > 0)
				*reading -= fmod(*reading , p.discretization);
		}

		ld_write_as_json(ld, out);
		ld_free(ld);
	}

	return 0;
}


#ifndef _GLOBAL_SETTING_
#define _GLOBAL_SETTING_

#define MATRIX_WIDTH 128
#define MATRIX_HEIGHT 128
#define MATRIX_CHANNEL 64
#define MATRIX_DEPTH 1

#define KERNEL_WIDTH 3
#define KERNEL_HEIGHT 3
#define KERNEL_CHANNEL MATRIX_CHANNEL
#define KERNEL_DEPTH 128

#define FEATURE_WIDTH (MATRIX_WIDTH - KERNEL_WIDTH + 1)
#define FEATURE_HEIGHT (MATRIX_HEIGHT - KERNEL_HEIGHT + 1)
#define FEATURE_CHANNEL 1
#define FEATURE_DEPTH 128

#ifdef CBLAS
    #include <cblas.h>
#endif  // #ifdef CBLAS

#ifdef OMP
	#include <omp.h>
#endif	// #ifdef OMP

#ifdef OPENCL
	#ifdef __APPLE__
		#include <OpenCL/opencl.h>
	#elif linux
		#include <CL/cl.h>
	#elif __WIN32__
		#include <cl.h>
	#else
		#include <CL/cl.h>
	#endif	//	#ifdef __APPLE__
#endif	//	#ifdef OPENCL

#endif	//	_GLOBAL_SETTING_


//////////////////////////////////////////// Spin glass phase transition ////////////////////////////////////////

/*

Phase space of the SK model.

Please cite the following paper when you use this code.

[Ezaki T, Fonseca dos Reis E, Watanabe T, Sakaki M, Masuda N. Closer to critical resting-state neural dynamics in individuals with higher fluid intelligence. Commun Biol 3:1 (2020).](https://www.nature.com/articles/s42003-020-0774-y)

If you find a bug in this code, please contact the authors.

Author: Elohim Fonseca dos Reis

Date: July 8, 2020

Parameters:
----------
   This program maps the SK model phase space using the following parameters that have to be set by the user before compiling:
       * Total number of spins (N);
       * Number of time steps to calculate the averages given a fixed configuration of $J_{ij}$ (tdim);
       * Number of $J_{ij}$ configurations for the configurational average (conf_num);
       * Number of sweeps in the transient period (thermal);
       * Array with the mean values of $J_{ij}$ (mu). The user has to set the minimum value (mu_min), maximum value (mu_max) and the step (mu_step). The size of the array with the mean values of $J_{ij}$ is given by 1+(mu_max-mu_min)/mu_step.
       * Array with the standard deviation values of $J_{ij}$ (sd). The user has to set the minimum value (sd_min), the maximum value (sd_max) and the step (sd_step). The size of the array with the standard deviation values of $J_{ij}$ is given by 1+(max_sd-min_sd)/sd_step.

Outputs:
-------
The program outputs the following measures:
 * spin glass susceptibility (Xsg);
 * uniform susceptibility (Xuni);
 * spin glass order parameter (q);
 * magnetization (m);
 * specific heat (c).

The outputs of the program are multiple .txt files, one for each pair of (mu, sd), and each .txt file contains one line with seven values in this
order: mu, sd, Xsg, Xuni, q, m, c. For example, if the user sets *n* values for the mu array and *m* values for the sd array, the program outputs *nm* .txt files. The files are named as: file_0_0.txt, file_0_1.txt, ..., file_1_0.txt, file_1_1.txt... The reason for generating multiple .txt files is for parallelizing the program.

The program has a built-in naive parallelization for the mu and sd arrays, i.e., after compiling the code, if the user runs the same program on 10 different instances,
for example, the total time will decrease 10 times. So the user might be interested in running an array of jobs, each job running the same program for a given set of parameters.

*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <unistd.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>
#include "mt19937ar.h"

// Parameters
#define N 264          // Total number of spins
#define tdim 10000     // Thermal average dimension size
#define conf_num 1000  // Number of interaction configurations
#define thermal 10000  // Equilibration sweeps

#define mu_min -0.002   // Minimum Average Interaction
#define mu_max 0.01     // Maximum Average Interaction
#define mu_step 0.0005  // Average interaction step
#define sd_min 0        // Minimum interaction standard deviation
#define sd_max 0.15     // Maximum interaction standard deviation
#define sd_step 0.0075  // Interaction sd step

// Matrix Serializations
#define SSArr(SS,i,j) SS[ tdim*i + j ] // Spin series array
#define JArr(J,i,j) J[ N*i + j ] // Interactions array
#define CovArr(var,i,j) var[N*i + j] // Covariance array

// Global Variables
int *s; // Spins
int *SS; // Spin series array (N x T)
double *J; // Interactions (Jij) drawn from a gaussian distribution

// Functions
double randnum(); // Real random number [0,1)
void init_randnum(); // Seed of the random number generator
void sweep(); // Sweep of the system with N metropolis steps
double cov(int *A, int k, int l); // Covariance cov[A(k),A(l)]
void elapsed_time();

int main()
{

///////////////////////////// Summary /////////////////////////////
	printf("\n\nSummary:\n\n");
	printf("Number of spins: %d\n", N);
	printf("Thermal average dimension: %d\n",tdim);
	printf("Number of configurations: %d\n",conf_num);
	printf("Equilibration sweeps: %d\n",thermal);
	printf("Number of parameter values: %d mean(J),  %d std(J)", (int)round(((mu_max - mu_min) / mu_step + 1)), (int)round(((sd_max - sd_min) / sd_step + 1)));
	printf("\n\n");

///////////////////////////// Model Parameters /////////////////////////////

	/* Average interaction array (main function variable) */
	int mu_size = (int)round( (mu_max - mu_min) / mu_step + 1); //printf("\n\nmu size: %d\n\n", mu_size);
	double *mu=malloc(mu_size*sizeof(double));
	if(!mu){
		printf("Out of memory.1\n");
		exit(1);
	}
	for(int i=0; i<mu_size; i++){
		mu[i] = (mu_min + i*mu_step); //printf("m[%d]=%f\n\n",i,mu[i]);
	}

	/* Interaction standard deviation array (main function variable) */
	int sd_size = (int)round( (sd_max - sd_min) / sd_step + 1); //printf("\n\nsd size: %d\n\n", sd_size);
	double *sd=malloc(sd_size*sizeof(double));
	if(!sd){
		printf("Out of memory.2\n");
		exit(1);
	}
	for(int i=0; i<sd_size; i++){
		sd[i] = (sd_min + i*sd_step); //printf("sd[%d]=%f\n\n",i,sd[i]);
	}

//////////////////////////////// Variables ///////////////////////////////

	/* Spin array (global) */
	s=malloc(N*sizeof(int));
	if(!s){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Spin series array (global) */
	SS=malloc(N*tdim*sizeof(int));
	if(!SS){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Interaction matrix (global) */
	J=malloc(N*N*sizeof(double));
	if(!J){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Covariance matrix configuration average sum */
	double *SumC=calloc(N*N,sizeof(double));
	if(!SumC){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Covariance matrix configuration average product sum */
	double *ProdC=calloc(N*N,sizeof(double));
	if(!ProdC){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Magnetization */
	double *m=malloc(mu_size*sd_size*sizeof(double));
	if(!m){
		printf("Out of memory.\n");
		exit(1);
	}

	/* SG order parameter */
	double *q=malloc(mu_size*sd_size*sizeof(double));
	if(!q){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Uniform Susceptibiliy */
	double *Xuni=calloc(mu_size*sd_size,sizeof(double));
	if(!Xuni){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Spin Glass Susceptibiliy */
	double *Xsg=calloc(mu_size*sd_size,sizeof(double));
	if(!Xsg){
		printf("Out of memory.\n");
		exit(1);
	}

	/* Specific Heat */
	double *c=calloc(mu_size*sd_size,sizeof(double));
	if(!c){
		printf("Out of memory.\n");
		exit(1);
	}

/////////////////////////////// Program ///////////////////////////////

	/* Initialize mt rng */
	init_randnum();

	/* Interaction array variables */
	const gsl_rng_type * Q;
	gsl_rng * r;
	Q = gsl_rng_mt19937;
	r = gsl_rng_alloc (Q);

	//////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////// AVERAGE LOOP (A) /////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////////
	for(int A=0; A<mu_size; A++){

	//////////////////////////////////////////////////////////////////////////////////
	///////////////////////// STANDARD DEVIATION LOOP (B) ////////////////////////////
	//////////////////////////////////////////////////////////////////////////////////
	for(int B=0; B<sd_size; B++){

		// Output file
		FILE *file;
		char *fname=malloc(100*sizeof(char));
		snprintf(fname,100,"file_%d_%d.txt",A,B);

		// Check if file exists
		if( access( fname, F_OK ) != -1 ) {
		    // file exists
		    continue;
		}

		// file doesn't exist: perform calculations

		// Save blank file
		if((file = fopen(fname,"w"))==NULL){
			printf("Cannot open file.\n");
		        exit(1);
		}
		fclose(file);
		printf("\nfile_%d_%d.txt  mu=%f  sd=%f",A,B, mu[A], sd[B]);

		//~~~~~~~~~ Beginning of calculations

		//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		// Initial condition
		for(int i=0; i<N; i++) s[i]=1;

		//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		// Reset convariance array
		memset(SumC, 0, N*N*sizeof(*SumC));
		memset(ProdC, 0, N*N*sizeof(*ProdC));

		//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		// Reset specific heat variables
		double ProdSumE = 0;
		double SumProdE = 0;

		//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		// Configuration Average
		for(int D=0; D<conf_num; D++){

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~;
			// Interaction array
			for(int i=0; i<N; i++){
			for(int j=i; j<N; j++){
				JArr(J,i,j) = mu[A] + gsl_ran_gaussian_ziggurat(r,sd[B]);
				JArr(J,j,i) = JArr(J,i,j);
				JArr(J,i,i) = 0;
			}}

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Thermalization
			for(int i=0; i<thermal; i++){
				sweep();
			}

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Sampling
			for (int j=0; j<tdim; j++){
				sweep();
				for (int i=0; i<N; i++){
					SSArr(SS,i,j)=s[i];
				}
			}

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Magnetization and SG order parameter
			double mag=0;
			double sgop=0;
			double aux=0;
			for(int i=0; i<N; i++){
				aux=0;
				for(int t=0; t<tdim; t++){
					aux += SSArr(SS,i,t);
				}
				mag += aux;
				sgop += aux*aux;
			}
			m[sd_size*A + B] += mag;
			q[sd_size*A + B] += sgop;

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Covariance Matrix: configuration average sum (SumC) for Xuni and product (ProdC) for Xsg
			double cov_ij;
			for(int i=0; i<N; i++){
			for(int j=i; j<N; j++){
				cov_ij = cov(SS,i,j);
				CovArr(SumC,i,j) += cov_ij;
				CovArr(SumC,j,i) = CovArr(SumC,i,j);
				CovArr(ProdC,i,j) += cov_ij*cov_ij;
				CovArr(ProdC,j,i) = CovArr(ProdC,i,j);
			}}

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Specific heat:
			double sumE=0, sumE2=0, E=0;
			for(int t=1; t<tdim; t++){
				E=0;
				for(int i=0; i<N; i++){
				for(int j=i; j<N; j++){
					E -= JArr(J,i,j) * SSArr(SS,i,t) *SSArr(SS,j,t);
				}}
				sumE += E;
				sumE2 += E*E;
			}
			ProdSumE += sumE*sumE;
			SumProdE += sumE2;

		//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
		} // End of Configuration Average

		m[sd_size*A+B] = fabs(m[sd_size*A+B]/(1.0*N*tdim*conf_num));
		q[sd_size*A+B] = q[sd_size*A+B]/(1.0*N*tdim*tdim*conf_num);

		int NN=N*N;
		for(int i=0; i<NN; i++){
			Xuni[sd_size*A+B] += SumC[i];
			Xsg[sd_size*A+B] += ProdC[i];
		}
		Xuni[sd_size*A+B] = Xuni[sd_size*A+B]/(1.0*N*conf_num);
		Xsg[sd_size*A+B] = Xsg[sd_size*A+B]/(1.0*N*conf_num);

		c[sd_size*A+B] = (SumProdE/(1.0*tdim) - ProdSumE/(1.0*tdim*tdim))/(1.0*N*conf_num);

		// Save file with results
		if((file = fopen(fname,"w"))==NULL){
			printf("Cannot open file.\n");
        		exit(1);
        	}

		fprintf(file,"%f\t%f\t%f\t%f\t%f\t%f\t%f", mu[A],
							   sd[B],
							   Xsg[A*sd_size+B],
							   Xuni[A*sd_size+B],
							   q[A*sd_size+B],
							   m[A*sd_size+B],
							   c[A*sd_size+B]);

        	fclose(file);
		free(fname); fname = NULL;

	}/////////////////////////////////////////////////////////////////////////////////
	///////////////////////// END OF STANDARD DEVIATION LOOP (B) /////////////////////
	//////////////////////////////////////////////////////////////////////////////////

	}/////////////////////////////////////////////////////////////////////////////////
	////////////////////////////// END OF  AVERAGE LOOP (A) //////////////////////////
	//////////////////////////////////////////////////////////////////////////////////

// !!!! FREE POINTERS !!!!
	free(mu); mu=NULL;
	free(sd); sd=NULL;
	free(s); s=NULL;
	free(SS); SS=NULL;
	free(J); J=NULL;
	free(m); m=NULL;
	free(q); q=NULL;
	free(SumC); SumC=NULL;
	free(ProdC); ProdC=NULL;
	free(Xuni); Xuni=NULL;
	free(Xsg); Xsg=NULL;
	free(c); c=NULL;
	gsl_rng_free (r);

//-------- END OF THE PROGRAM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	elapsed_time();
	return 0;

}

// 	FUNCTIONS	//

/*********************************************************************************************
		                      Metropolis algorithm
**********************************************************************************************/

void sweep()
{
	int k;
	double Heff, delta;

	for(int i=0; i<N; i++)
	{
		/* Choose a site at random */
		k = N*randnum();

		/* Calculate the effective field */
		Heff=0;
		for(int j=0; j<N; j++) Heff += JArr(J,k,j)*s[j];

		/* Calculate the change in energy */
		delta = s[k]*(Heff);

		/* Decide whether to flip the spin */
		if (delta <= 0){
			s[k] = -s[k];
		}
		else if ( randnum() < exp(-2*delta) ){
			s[k] = -s[k];
		}
	}
}

/*********************************************************************************************
                      Covariance: function to calculate correlations
**********************************************************************************************/

double cov(int *A, int k, int l)
{
	long long int P=0, Qk=0, Ql=0;
	double R=0;

	for(int t=0; t<tdim; t++){
		P += SSArr(A,k,t) * SSArr(A,l,t);
		Qk += SSArr(A,k,t);
		Ql += SSArr(A,l,t);
	}

	R = P/(1.0*tdim) - Qk * Ql/(1.0*tdim*tdim);

	return R;
}

/*********************************************************************************************
                 Mersenne Twister random number generator: real number in [0,1)
**********************************************************************************************/

/*Seeding the MT random number generator*/
void init_randnum()
{
	//init_genrand(time(NULL));
	init_genrand(0);
}
/*Generates a real number "rand()" between [0,1)*/
double randnum()
{
	double num;
	num=genrand_real2();
	return num;
}

/*********************************************************************************************
                                       Execution time
**********************************************************************************************/

void elapsed_time(void)
{
	printf("\n.\n.\n.\nElapsed time: %f min\n\n", clock()/(60.0*CLOCKS_PER_SEC));
}