/*
 * BRAINS
 * (B)LR (R)everberation-mapping (A)nalysis (I)n AGNs with (N)ested (S)ampling
 * Yan-Rong Li, liyanrong@ihep.ac.cn
 * Thu, Aug 4, 2016
 */

/*! \file dnest_sa1d.c
 *  \brief run dnest sampling for sa and 2d rm analysis.
 */

#ifdef SpecAstro

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stddef.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_interp.h>
#include <mpi.h>

#include "brains.h"

DNestFptrSet *fptrset_sa2d;

int dnest_sa2d(int argc, char **argv)
{
  int i;
  double logz_sa2d;

  if(parset.flag_sa_par_mutual == 0) /* SA and RM have the same BLR */
  {
    set_blr_model2d();
    num_params_sa_blr_model = 0;
  }
  else if(parset.flag_sa_par_mutual == 1) /* SA and RM have different BLRs but share the same mbh and inc. */
  {
    set_blr_model2d();
    set_sa_blr_model();
  }
  else
  {
    set_blr_model2d();
    set_sa_blr_model();
  }

  /* parameter order: RM BLR, SA BLR, RM continuum */
  /* RM */
  num_params_blr = num_params_blr_model + num_params_nlr
                 + num_params_res + num_params_linecenter + 1; /* include and line sys err */
  num_params_rm = parset.n_con_recon + num_params_blr + num_params_var;

  /* SA */
  num_params_sa_blr = num_params_sa_blr_model + num_params_sa_extpar;
  num_params_sa = num_params_sa_blr;

  /* total */
  num_params_blr_tot = num_params_blr + num_params_sa_blr;
  num_params = num_params_sa + num_params_rm;
  idx_resp = num_params_blr_tot + num_params_drw + num_params_trend;
  idx_difftrend = idx_resp + num_params_resp;
  idx_linecenter = num_params_blr_model + num_params_nlr + num_params_res;

  rnd_frac = fmax(0.2, 1.0*(num_params_blr_tot+num_params_var)/num_params);

  par_fix = (int *) malloc(num_params * sizeof(int));
  par_fix_val = (double *) malloc(num_params * sizeof(double));
  par_range_model = malloc( num_params * sizeof(double *));
  par_prior_gaussian = malloc( num_params * sizeof(double *));
  for(i=0; i<num_params; i++)
  {
    par_range_model[i] = malloc(2*sizeof(double));
    par_prior_gaussian[i] = malloc(2*sizeof(double));
  }
  par_prior_model = malloc( num_params * sizeof(int));

  fptrset_sa2d = dnest_malloc_fptrset();
  /* setup functions used for dnest*/
  fptrset_sa2d->from_prior = from_prior_sa2d;
  fptrset_sa2d->print_particle = print_particle_sa2d;
  fptrset_sa2d->restart_action = restart_action_sa;
  fptrset_sa2d->accept_action = accept_action_sa2d;
  fptrset_sa2d->kill_action = kill_action_sa2d;
  fptrset_sa2d->perturb = perturb_sa2d;
  fptrset_sa2d->read_particle = read_particle_sa2d;

  fptrset_sa2d->log_likelihoods_cal_initial = log_likelihoods_cal_initial_sa2d;
  fptrset_sa2d->log_likelihoods_cal_restart = log_likelihoods_cal_restart_sa2d;
  fptrset_sa2d->log_likelihoods_cal = log_likelihoods_cal_sa2d;

  set_par_range_sa2d();
  set_par_fix_blrmodel();
  set_par_fix_sa_blrmodel();

  /* the rest parameters */
  for(i=num_params_blr_model; i<num_params_blr; i++)
  {
    par_fix[i] = 0;
    par_fix_val[i] = -DBL_MAX;
  }
  for(i=num_params_blr + num_params_sa_blr_model; i<num_params; i++)
  {
    par_fix[i] = 0;
    par_fix_val[i] = -DBL_MAX;
  }

  /* fix non-linear response */
  if(parset.flag_nonlinear !=1)
  {
    par_fix[idx_resp + 1] = 1;
    par_fix_val[idx_resp + 1] = 0.0;
  }

  /* fix systematic error of line */
  if(parset.flag_line_sys_err != 1)
  {
    par_fix[num_params_blr-1] = 1;
    par_fix_val[num_params_blr-1] = log(1.0);
  }

  /* fix mbh and inc of SA BLR if SA and RM have different BLR */
  if(parset.flag_sa_par_mutual == 1)
  {
    /* get the index for mbh and inclination parameters */
    set_idx_par_mutual();
    /* mbh */
    par_fix[num_params_blr + idx_sa_par_mutual[0]] = 1;
    par_fix_val[num_params_blr + idx_sa_par_mutual[0]] = 0.0;

    /* inc */
    par_fix[num_params_blr + idx_sa_par_mutual[1]] = 1;
    par_fix_val[num_params_blr + idx_sa_par_mutual[1]] = 0.0;
  }
  else if(parset.flag_sa_par_mutual == 2)
  {
    /* get the index for mbh and inclination parameters */
    set_idx_par_mutual();
    /* inc and opening angle */
    par_fix[num_params_blr + idx_sa_par_mutual[1]] = 1;
    par_fix_val[num_params_blr + idx_sa_par_mutual[1]] = 0.0;
    par_fix[num_params_blr + idx_sa_par_mutual[2]] = 1;
    par_fix_val[num_params_blr + idx_sa_par_mutual[2]] = 0.0;
    /* dynamical parameters from mass on */
    for(i=idx_sa_par_mutual[0]; i<num_params_sa_blr_model; i++)
    {
      par_fix[num_params_blr + i] = 1;
      par_fix_val[num_params_blr + i] = 0.0;
    }
  }

  /* fix FA */
  par_fix[num_params_blr + num_params_sa_blr_model+2] = 1;
  par_fix_val[num_params_blr + num_params_sa_blr_model+2] = log(1.0);

  /* fix systematic error of continuum */
  if(parset.flag_con_sys_err != 1)
  {
    par_fix[num_params_blr_tot] = 1;
    par_fix_val[num_params_blr_tot] = log(1.0);
  }

  /* fix continuum variation parameter sigma and tau if flag_fixvar is true */
  if(parset.flag_fixvar == 1)
  {
    par_fix[num_params_blr_tot + 1] = 1;
    par_fix_val[num_params_blr_tot + 1] = var_param[1];
    par_fix[num_params_blr_tot + 2] = 1;
    par_fix_val[num_params_blr_tot + 2] = var_param[2];
  }

  print_par_names_sa2d();

  force_update = parset.flag_force_update;
  if(parset.flag_para_name != 1)
    logz_sa2d = dnest(argc, argv, fptrset_sa2d, num_params, NULL, NULL, NULL, "data/", dnest_options_file, NULL, NULL);

  dnest_free_fptrset(fptrset_sa2d);

  return 0;
}

void set_par_range_sa2d()
{
  int i, j, i1, i2;

  /* setup parameter range, BLR parameters first */
  for(i=0; i<num_params_blr_model; i++)
  {
    par_range_model[i][0] = blr_range_model[i][0];
    par_range_model[i][1] = blr_range_model[i][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;
  }

  /* cope with narrow line */
  for(i=num_params_blr_model; i<num_params_blr_model + num_params_nlr; i++)
  {
    par_range_model[i][0] = nlr_range_model[i - num_params_blr_model][0];
    par_range_model[i][1] = nlr_range_model[i - num_params_blr_model][1];

    par_prior_model[i] = nlr_prior_model[i - num_params_blr_model];
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 1.0; /* note that for logarithm prior of flux, this value is not used, so does not matter */
  }
  /* cope with spectral broadening */
  for(i=num_params_blr_model + num_params_nlr; i<num_params_blr_model + num_params_nlr + num_params_res; i++)
  {
    par_range_model[i][0] = -10.0;
    par_range_model[i][1] =  10.0;

    par_prior_model[i] = GAUSSIAN;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 1.0;
  }
  /* cope with line center */
  for(i=num_params_blr-num_params_linecenter-1; i< num_params_blr-1; i++)
  {
    par_range_model[i][0] = -10.0;
    par_range_model[i][1] =  10.0;

    par_prior_model[i] = GAUSSIAN;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 1.0;
  }

  /* the last is systematic error */
  i = num_params_blr-1;
  par_range_model[i][0] = sys_err_line_range[0];
  par_range_model[i][1] = sys_err_line_range[1];

  par_prior_model[i] = UNIFORM;
  par_prior_gaussian[i][0] = 0.0;
  par_prior_gaussian[i][1] = 0.0;

  /* SA BLR parameters */
  for(i=num_params_blr; i<num_params_blr + num_params_sa_blr_model; i++)
  {
    par_range_model[i][0] = sa_blr_range_model[i-num_params_blr][0];
    par_range_model[i][1] = sa_blr_range_model[i-num_params_blr][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;
  }
  /* SA extra parameters */
  for(i=num_params_blr + num_params_sa_blr_model; i<num_params_blr_tot; i++)
  {
    par_range_model[i][0] = sa_extpar_range[i-(num_params_blr + num_params_sa_blr_model)][0];
    par_range_model[i][1] = sa_extpar_range[i-(num_params_blr + num_params_sa_blr_model)][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;
  }

  /* variability parameters */
   /* first systematic error */
  i = num_params_blr_tot;
  par_range_model[i][0] = var_range_model[0][0];
  par_range_model[i][1] = var_range_model[0][1];

  par_prior_model[i] = UNIFORM;
  par_prior_gaussian[i][0] = 0.0;
  par_prior_gaussian[i][1] = 0.0;

  for(i=num_params_blr_tot+1; i<num_params_drw + num_params_blr_tot; i++)
  {
    if(var_param_std[i-num_params_blr_tot] > 0.0)
    {
      par_range_model[i][0] = var_param[i-num_params_blr_tot] - 5.0 * var_param_std[i-num_params_blr_tot];
      par_range_model[i][1] = var_param[i-num_params_blr_tot] + 5.0 * var_param_std[i-num_params_blr_tot];

      /* make sure that the range lies within the initial range */
      par_range_model[i][0] = fmax(par_range_model[i][0], var_range_model[i-num_params_blr_tot][0]);
      par_range_model[i][1] = fmin(par_range_model[i][1], var_range_model[i-num_params_blr_tot][1]);

      par_prior_model[i] = GAUSSIAN;
      par_prior_gaussian[i][0] = var_param[i-num_params_blr_tot];
      par_prior_gaussian[i][1] = var_param_std[i-num_params_blr_tot];
    }
    else
    {
      par_range_model[i][0] = var_range_model[i-num_params_blr_tot][0];
      par_range_model[i][1] = var_range_model[i-num_params_blr_tot][1];

      par_prior_model[i] = UNIFORM;
      par_prior_gaussian[i][0] = 0.0;
      par_prior_gaussian[i][1] = 0.0;
    }
  }
  /* long-term trend of continuum */
  for(i=num_params_drw + num_params_blr_tot; i< num_params_drw + num_params_trend + num_params_blr_tot; i++)
  {
    par_range_model[i][0] = var_range_model[3][0];
    par_range_model[i][1] = var_range_model[3][1];

    par_prior_model[i] = GAUSSIAN;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 1.0;
  }

  /* response A and Ag */
  j = 0;
  i1 = idx_resp;
  i2 = idx_resp + num_params_resp;
  for(i=i1; i<i2; i++)
  {
    par_range_model[i][0] = resp_range[j][0];
    par_range_model[i][1] = resp_range[j][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;

    j++;
  }

  /* different trend in continuum and line */
  j = 0;
  i1 = idx_difftrend;
  i2 = idx_difftrend + num_params_difftrend;
  for(i= i1; i< i2; i++)
  {
    par_range_model[i][0] = var_range_model[4 + j][0];
    par_range_model[i][1] = var_range_model[4 + j][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;

    j++;
  }

  /* continuum ligth curve values */
  for(i=num_params_blr_tot+num_params_var; i<num_params; i++)
  {
    par_range_model[i][0] = var_range_model[4+num_params_difftrend][0];
    par_range_model[i][1] = var_range_model[4+num_params_difftrend][1];

    par_prior_model[i] = GAUSSIAN;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 1.0;
  }

  return;
}

/*!
 *  print names and prior ranges for parameters
 *
 */
void print_par_names_sa2d()
{
  if(thistask != roottask)
    return;

  int i, j;
  FILE *fp;
  char fname[BRAINS_MAX_STR_LENGTH], str_fmt[BRAINS_MAX_STR_LENGTH];

  sprintf(fname, "%s/%s", parset.file_dir, "data/para_names_sa2d.txt");
  fp = fopen(fname, "w");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", fname);
    exit(0);
  }

  strcpy(str_fmt, "%4d %-15s %10.6f %10.6f %4d %4d %15.6e\n");

  printf("# Print parameter name in %s\n", fname);

  fprintf(fp, "#*************************************************\n");
  fprint_version(fp);
  fprintf(fp, "#*************************************************\n");

  fprintf(fp, "%4s %-15s %10s %10s %4s %4s %15s\n", "#", "Par", "Min", "Max", "Prior", "Fix", "Val");

  i=-1;
  for(j=0; j<num_params_blr_model; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "BLR model", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  for(j=0; j<num_params_nlr; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "narrow line", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }
  for(j=0; j<num_params_res; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "line broaden", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }
  for(j=0; j<num_params_linecenter; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "line center", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  i++;
  fprintf(fp, str_fmt, i, "sys_err_line", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);

  for(j=0; j<num_params_sa_blr_model; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "SA BLR model", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  for(j=0; j<num_params_sa_extpar; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "SA Extra Par", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  i++;
  fprintf(fp, str_fmt, i, "sys_err_con", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  i++;
  fprintf(fp, str_fmt, i, "sigmad", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  i++;
  fprintf(fp, str_fmt, i, "taud", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);

  for(j=0; j<num_params_trend; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "trend", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  i++;
  fprintf(fp, str_fmt, i, "A", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);

  i++;
  fprintf(fp, str_fmt, i, "Ag", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);

  for(j=0; j<num_params_difftrend; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "diff trend", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  for(j=0; j<parset.n_con_recon; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "time series", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  fclose(fp);
}

/*!
 * this function generates a sample from prior.
 */
void from_prior_sa2d(void *model)
{
  int i;
  double *pm = (double *)model;

  for(i=0; i<num_params; i++)
  {
    if(par_prior_model[i] == GAUSSIAN)
    {
      pm[i] = dnest_randn()*par_prior_gaussian[i][1] + par_prior_gaussian[i][0];
      dnest_wrap(&pm[i], par_range_model[i][0], par_range_model[i][1]);
    }
    else
    {
      pm[i] = par_range_model[i][0] + dnest_rand() * (par_range_model[i][1] - par_range_model[i][0]);
    }
  }

  /* cope with fixed parameters */
  for(i=0; i<num_params; i++)
  {
    if(par_fix[i] == 1)
      pm[i] = par_fix_val[i];
  }

  which_parameter_update = -1;

  return;
}

/*!
 * this function calculates likelihood at initial step.
 */
double log_likelihoods_cal_initial_sa2d(const void *model)
{
  double logL;
  logL = prob_initial_sa2d(model);
  return logL;
}

/*!
 * this function calculates likelihood at initial step.
 */
double log_likelihoods_cal_restart_sa2d(const void *model)
{
  double logL;
  logL = prob_initial_sa2d(model);
  return logL;
}

/*!
 * this function calculates likelihood.
 */
double log_likelihoods_cal_sa2d(const void *model)
{
  double logL;
  logL = prob_sa2d(model);
  return logL;
}

/*!
 * this function prints out parameters.
 */
void print_particle_sa2d(FILE *fp, const void *model)
{
  int i;
  double *pm = (double *)model;

  for(i=0; i<num_params; i++)
  {
    fprintf(fp, "%e ", pm[i] );
  }
  fprintf(fp, "\n");
  return;
}

/*!
 * This function read the particle from the file.
 */
void read_particle_sa2d(FILE *fp, void *model)
{
  int j;
  double *psample = (double *)model;

  for(j=0; j < num_params; j++)
  {
    if(fscanf(fp, "%lf", psample+j) < 1)
    {
      printf("%f\n", *psample);
      fprintf(stderr, "#Error: Cannot read file sample_sa2d.txt.\n");
      exit(0);
    }
  }
  return;
}

/*!
 * this function perturbs parameters.
 */
double perturb_sa2d(void *model)
{
  double *pm = (double *)model;
  double logH = 0.0, limit1, limit2, width, rnd;
  int which, which_level, size_levels, count_saves;

  /*
   * fixed parameters need not to update
   * perturb important parameters more frequently
   */
  do
  {
    rnd = dnest_rand();
    if(rnd < rnd_frac)
      which = dnest_rand_int(num_params_blr_tot + num_params_var);
    else
      which = dnest_rand_int(parset.n_con_recon) + num_params_blr_tot + num_params_var;

  }while(par_fix[which] == 1);

  which_parameter_update = which;

  /* level-dependent width */
  count_saves = dnest_get_count_saves();
  which_level_update = dnest_get_which_level_update();
  size_levels = dnest_get_size_levels();
  which_level = which_level_update > (size_levels-10)?(size_levels-10):which_level_update;

  if( which_level > 0)
  {
    limit1 = limits[(which_level-1) * num_params *2 + which *2];
    limit2 = limits[(which_level-1) * num_params *2 + which *2 + 1];
    width = (limit2 - limit1);
  }
  else
  {
    limit1 = par_range_model[which][0];
    limit2 = par_range_model[which][1];
    width = (par_range_model[which][1] - par_range_model[which][0]);
  }
  width /= (2.35);

  if(par_prior_model[which] == GAUSSIAN)
  {
    logH -= (-0.5*pow((pm[which] - par_prior_gaussian[which][0])/par_prior_gaussian[which][1], 2.0) );
    pm[which] += dnest_randh() * width;
    dnest_wrap(&pm[which], par_range_model[which][0], par_range_model[which][1]);
    //dnest_wrap(&pm[which], limit1, limit2);
    logH += (-0.5*pow((pm[which] - par_prior_gaussian[which][0])/par_prior_gaussian[which][1], 2.0) );
  }
  else
  {
    pm[which] += dnest_randh() * width;
    dnest_wrap(&(pm[which]), par_range_model[which][0], par_range_model[which][1]);
    //dnest_wrap(&pm[which], limit1, limit2);
  }

  return logH;
}

/*!
 * this function calculates likelihood.
 */
double log_likelihoods_cal_sa2d_exam(const void *model)
{
  return 0.0;
}

void accept_action_sa2d()
{
  int param;
  double *ptemp;

  // the parameter previously updated
  param = which_parameter_update;

  /* continuum parameter is updated */
  if( param >= num_params_blr_tot )
  {
    /*
     *note that (response) Fline is also changed as long as Fcon is changed.
     *num_params_blr-the parameter is the systematic error of continuum.
     *the change of this parameter also changes continuum reconstruction.
     */

    ptemp = Fcon_rm_particles[which_particle_update];
    Fcon_rm_particles[which_particle_update] = Fcon_rm_particles_perturb[which_particle_update];
    Fcon_rm_particles_perturb[which_particle_update] = ptemp;
  }
  else if((
           param < num_params_blr_model ||
          (param >= num_params_blr && param < num_params_blr_tot)
          ) && force_update != 1 )
  {
    /* RM and SA BLR parameter is updated
     * Note a) that the (num_par_blr-1)-th parameter is systematic error of line.
     * when this parameter is updated, Trans2D, Fline, phase_sa, and Fline_sa are unchanged.
     *      b) Fline is always changed, except param = num_params_blr-1.
     */

    ptemp = TransTau_particles[which_particle_update];
    TransTau_particles[which_particle_update] = TransTau_particles_perturb[which_particle_update];
    TransTau_particles_perturb[which_particle_update] = ptemp;

    ptemp = Trans2D_at_veldata_particles[which_particle_update];
    Trans2D_at_veldata_particles[which_particle_update] = Trans2D_at_veldata_particles_perturb[which_particle_update];
    Trans2D_at_veldata_particles_perturb[which_particle_update] = ptemp;

    prob_sa_particles[which_particle_update] = prob_sa_particles_perturb[which_particle_update];
  }

  //Fline is always updated except param = num_params_blr - 1
  if( param != num_params_blr-1 && force_update != 1)
  {
    ptemp = Fline_at_data_particles[which_particle_update];
    Fline_at_data_particles[which_particle_update] = Fline_at_data_particles_perturb[which_particle_update];
    Fline_at_data_particles_perturb[which_particle_update] = ptemp;
  }
  return;
}

/*
 * action when particle i is killed in cdnest sampling.
 * particle i_copy's properties is copyed to particle i.
 */
void kill_action_sa2d(int i, int i_copy)
{
  memcpy(Fcon_rm_particles[i], Fcon_rm_particles[i_copy], parset.n_con_recon * sizeof(double));
  memcpy(Fline_at_data_particles[i], Fline_at_data_particles[i_copy], n_line_data * n_vel_data_ext * sizeof(double));
  memcpy(TransTau_particles[i], TransTau_particles[i_copy], parset.n_tau*sizeof(double));
  memcpy(Trans2D_at_veldata_particles[i], Trans2D_at_veldata_particles[i_copy], parset.n_tau * n_vel_data_ext * sizeof(double));

  prob_sa_particles[i] = prob_sa_particles[i_copy];

  return;
}

#endif


// Copyright 2019, Winfield Chen and Lloyd T. Elliott.

#ifndef __STATISTICS_H_
#  define __STATISTICS_H_

#include <gsl/gsl_sf.h>
#include "matrix.h"

double tcdf1m(double t, double nu);
double log_tcdf1m(double t, double nu);
void regression(t_matrix g,
                t_matrix y,
                t_matrix yt,
                t_matrix obs,
                t_matrix denom,
                t_matrix beta,
                t_matrix se,
                t_matrix tstat,
                t_matrix pval,
                t_matrix b1,
                t_matrix w1,
                t_matrix w2);
#endif


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

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_errno.h>

gsl_poly_complex_workspace *
gsl_poly_complex_workspace_alloc (size_t n)
{
  size_t nc ;

  gsl_poly_complex_workspace * w ;

  if (n == 0)
    {
      GSL_ERROR_VAL ("matrix size n must be positive integer", GSL_EDOM, 0);
    }

  w = (gsl_poly_complex_workspace *)
    malloc (sizeof(gsl_poly_complex_workspace));

  if (w == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for struct", GSL_ENOMEM, 0);
    }

  nc = n - 1;

  w->nc = nc;

  w->matrix = (double *) malloc (nc * nc * sizeof(double));

  if (w->matrix == 0)
    {
      free (w) ;       /* error in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for workspace matrix",
                        GSL_ENOMEM, 0);
    }

  return w ;
}

void
gsl_poly_complex_workspace_free (gsl_poly_complex_workspace * w)
{
  free(w->matrix) ;
  free(w);
}


/****************************************************************************/
/*                                                                          */
/* File:      typedefs.h                                                    */
/*                                                                          */
/* Purpose:   header file for all necessary inculdes and typedefs for files */
/*            associated with the NeuroMorph or CellCount projects          */
/*                                                                          */
/* Author:    Marcel Oberlaender                                            */
/*            Max-Planck-Institute for Neurobiologie                        */
/*            Am Kolpferspitz 18                                            */
/*            D-82152 Martinsried (Munich)                                  */
/*                                                                          */
/* Co-Author: Robert Egger                                                  */
/*            Max-Planck-Institute for Medical Research                     */
/*            Jahnstrasse 19                                                */
/*            D-69120 Heidelberg                                            */
/*                                                                          */
/* EMail:     regger@mpimf-heidelberg.mpg.de                                */
/*                                                                          */
/* History:   17.01.2008                                                    */
/*                                                                          */
/* Remarks:   All rights are reserved by the Max-Planck-Society             */
/*                                                                          */
/* Revised by: 	Daniel Udvary												*/
/* 				In Silico Brain Sciences									*/
/*				Max Planck Institute for Neurobiology of Behavior – caesar	*/
/*				Ludwig-Erhard-Allee 2										*/
/*				53175 Bonn, Germany											*/
/*                                                                          */
/* e-mail: 		daniel.udvary@mpinb.mpg.de									*/
/*                                                                          */
/* Date: 		21.11.2019                                                  */
/*                                                                          */
/****************************************************************************/

#ifndef TYPEDEF
#define TYPEDEF

//#define DEBUG

#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

#ifdef _OPENMP
#include <omp.h>
#endif

#define PI 3.1415926

#define _CRT_SECURE_NO_DEPRECATE
#define _SCL_SECURE_NO_DEPRECATE
#define _CRT_SECURE_NO_WARNINGS

//STL includes
#include "string"
#include <iostream>
#include <cstdio>
#include <fstream>
#include <string>
#include <list>
#include <vector>
#include <map>
#include <cmath>
#include <algorithm>
#include <complex>
#include <utility>
#include <ctime>

//GSL includes
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

//ITK includes
#include "itkImage.h"
#include "itkImageFileWriter.h"
#include "itkImageFileReader.h"
#include "itkVTKImageImport.h"
#include "itkVTKImageExport.h"
#include "itkNeighborhoodIterator.h"
#include "itkConstNeighborhoodIterator.h"
#include "itkShapedNeighborhoodIterator.h"
#include "itkImageRegionIterator.h"
#include "itkImageRegionConstIterator.h"
#include "itkImageRegionIteratorWithIndex.h"
#include "itkRescaleIntensityImageFilter.h"
#include "itkSignedDanielssonDistanceMapImageFilter.h"
#include "itkDanielssonDistanceMapImageFilter.h"
#include "itkShrinkImageFilter.h"
// // for Voronoi diagram creation & use
// #include "itkPointSet.h"
// #include "itkVoronoiDiagram2D.h"
// #include "itkVoronoiDiagram2DGenerator.h"
// #include "itkMeshSpatialObject.h"
// #include "itkSpatialObjectToImageFilter.h"

//VTK includes
//basics
#include "vtkSmartPointer.h"
#include "vtkMath.h"
#include "vtkPoints.h"
#include "vtkImageData.h"
#include "vtkPolyData.h"
#include "vtkUnstructuredGrid.h"
#include "vtkPointData.h"
#include "vtkDataArray.h"
#include "vtkFloatArray.h"
#include "vtkDoubleArray.h"
#include "vtkIdTypeArray.h"
#include "vtkTIFFWriter.h"
#include "vtkPolyDataWriter.h"
#include "vtkGenericCell.h"
#include "vtkPolygon.h"
#include "vtkTriangle.h"
#include "vtkTetra.h"
#include "vtkPlane.h"
#include "vtkLine.h"
#include "vtkCylinder.h"
#include "vtkCutter.h"
#include "vtkClipPolyData.h"
#include "vtkTransform.h"
#include "vtkParametricSpline.h"
#include "vtkKochanekSpline.h"
#include "vtkBoundingBox.h"
#include "vtkImageImport.h"
#include "vtkImageExport.h"
#include "vtkBox.h"
#include <vtkPolyDataMapper.h>
#include <vtkProperty.h>
#include <vtkActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>

//algorithms
#include "vtkContourFilter.h"
#include "vtkMarchingCubes.h"
#include "vtkSmoothPolyDataFilter.h"
#include "vtkWindowedSincPolyDataFilter.h"
#include "vtkSurfaceReconstructionFilter.h"
#include "vtkCellLocator.h"
#include "vtkSelectPolyData.h"
#include "vtkExtractPolyDataGeometry.h"
#include "vtkTransformPolyDataFilter.h"
#include "vtkAppendFilter.h"
#include "vtkAppendPolyData.h"
#include "vtkPolyDataNormals.h"
#include "vtkHull.h"
#include "vtkPointsProjectedHull.h"
#include "vtkDelaunay2D.h"
#include "vtkDelaunay3D.h"
#include "vtkDataSetSurfaceFilter.h"
#include "vtkButterflySubdivisionFilter.h"
#include "vtkLoopSubdivisionFilter.h"
#include "vtkSelectEnclosedPoints.h"

  typedef float 									CalcPixelType;
  typedef unsigned char									PixelType;
  typedef itk::Image< PixelType, 3 >							ImageType;
  typedef itk::Image< PixelType, 2 >							Image2DType;
  typedef itk::Image< CalcPixelType, 3 >						CalcImageType;
  typedef itk::Image< CalcPixelType, 2 >						CalcImage2DType;
  typedef itk::NeighborhoodIterator< ImageType >					SegNeighborhoodIteratorType;
  typedef itk::NeighborhoodIterator< CalcImageType >					CalcNeighborhoodIteratorType;
  typedef itk::ShapedNeighborhoodIterator< ImageType >					ShapedNeighborhoodIteratorType;
  typedef itk::ShapedNeighborhoodIterator< CalcImageType >				ShapedCalcNeighborhoodIteratorType;
  typedef itk::ImageRegionIterator< ImageType >						IteratorType2;
  typedef itk::ImageRegionConstIterator< ImageType >					ConstIteratorType;
  typedef itk::ImageRegionIterator< Image2DType >					Iterator2DType;
  typedef itk::ImageRegionIterator< CalcImageType >					CalcIteratorType;
  typedef itk::ImageRegionIterator< CalcImage2DType >					Calc2DIteratorType;
  typedef itk::ImageRegionConstIterator< CalcImageType >				ConstCalcIteratorType;
  typedef itk::ImageRegionIteratorWithIndex< ImageType >				IndexIteratorType;
  typedef itk::RescaleIntensityImageFilter< CalcImage2DType, Image2DType >	CalcImage2DToImage2DFilterType;
  typedef itk::SignedDanielssonDistanceMapImageFilter< ImageType, CalcImageType >	DistanceMapImageFilterType;
  typedef itk::DanielssonDistanceMapImageFilter< ImageType, CalcImageType >		DistanceMapImageFilterType2;
  typedef itk::ImageFileWriter< Image2DType >					Image2DWriterType;
  typedef itk::ImageFileReader< Image2DType > 					Image2DReaderType;
  typedef itk::VTKImageImport< ImageType >						VTK2ITKImageImportType;
  typedef itk::VTKImageExport< CalcImageType >						ITK2VTKCalcImageExportType;
  typedef std::vector< SegNeighborhoodIteratorType::OffsetType >			NeighborhoodOffsetVectorType;
  typedef std::vector< ShapedNeighborhoodIteratorType::OffsetType >			ShapedNeighborhoodOffsetVectorType;
  typedef itk::ShrinkImageFilter<Image2DType, Image2DType> 	ShrinkImageFilterType;

//   typedef itk::PointSet< PixelType, 2 >							itkPointSetType;
//   typedef itkPointSetType::PointType							itkPointType;
//   typedef itk::VoronoiDiagram2D< float >						VoronoiDiagramType;
//   typedef itk::VoronoiDiagram2DGenerator< float >					VoronoiDiagramGeneratorType;
//   typedef itk::MeshSpatialObject< VoronoiDiagramType >					VoronoiDiagramSpatialObjectType;
//   typedef itk::SpatialObjectToImageFilter< VoronoiDiagramSpatialObjectType, Image2DType >	VoronoiDiagramToImageFilterType;

  typedef vtkSmartPointer< vtkPoints >		PointsPointerType;
  typedef vtkSmartPointer< vtkDataArray >	DataArrayPointerType;
  typedef vtkSmartPointer< vtkFloatArray >	FloatArrayPointerType;
  typedef vtkSmartPointer< vtkDoubleArray >	DoubleArrayPointerType;
  typedef vtkSmartPointer< vtkIdTypeArray >	IdTypeArrayPointerType;
  typedef vtkSmartPointer< vtkPolygon >		PolygonPointerType;
  typedef vtkSmartPointer< vtkTriangle >	TrianglePointerType;
  typedef vtkSmartPointer< vtkTetra >		TetraPointerType;
  typedef vtkSmartPointer< vtkIdList >		IdListPointerType;
  typedef vtkSmartPointer< vtkPlane >		PlanePointerType;
  typedef vtkSmartPointer< vtkLine >		LinePointerType;
  typedef vtkSmartPointer< vtkCylinder >	CylinderPointerType;
  typedef vtkSmartPointer< vtkCutter >		CutterPointerType;
  typedef vtkSmartPointer< vtkClipPolyData >	ClipPolyDataPointerType;
  typedef vtkSmartPointer< vtkTransform >	TransformPointerType;
  typedef vtkSmartPointer< vtkMatrix4x4 >	HomogeneousMatrixPointerType;
  typedef vtkSmartPointer< vtkParametricSpline >	ParametricSplinePointerType;
  typedef vtkSmartPointer< vtkKochanekSpline >	KochanekSplinePointerType;
  typedef vtkSmartPointer< vtkBoundingBox >	BoundingBoxPointerType;
  typedef vtkSmartPointer< vtkImageData >	ImageDataPointerType;
  typedef vtkSmartPointer< vtkPolyData >	PolyDataPointerType;
  typedef vtkSmartPointer< vtkUnstructuredGrid >	UnstructuredGridPointerType;
  typedef vtkSmartPointer< vtkTIFFWriter >	TiffWriterPointerType;
  typedef vtkSmartPointer< vtkPolyDataWriter >	PolyDataWriterPointerType;
  typedef vtkSmartPointer< vtkGenericCell >	GenericCellPointerType;
  typedef vtkSmartPointer< vtkCell >		CellPointerType;
  typedef vtkSmartPointer< vtkImageImport >	ITK2VTKImageImportPointerType;
  typedef vtkSmartPointer< vtkImageExport >	VTK2ITKImageExportPointerType;

  typedef vtkSmartPointer< vtkMarchingCubes >			MarchingCubesPointerType;
  typedef vtkSmartPointer< vtkContourFilter >			ContourFilterPointerType;
  typedef vtkSmartPointer< vtkSmoothPolyDataFilter >		AveragePolyDataFilterType;
  typedef vtkSmartPointer< vtkWindowedSincPolyDataFilter >	LowpassPolyDataFilterType;
  typedef vtkSmartPointer< vtkSurfaceReconstructionFilter >	SurfaceReconstructionFilterType;
  typedef vtkSmartPointer< vtkCellLocator >			CellLocatorPointerType;
  typedef vtkSmartPointer< vtkSelectPolyData >			SelectPolyDataPointerType;
  typedef vtkSmartPointer< vtkExtractPolyDataGeometry >		ExtractPolyDataGeometryPointerType;
  typedef vtkSmartPointer< vtkTransformPolyDataFilter >		TransformFilterType;
  typedef vtkSmartPointer< vtkAppendFilter >			AppendFilterPointerType;
  typedef vtkSmartPointer< vtkAppendPolyData >			AppendPolyDataPointerType;
  typedef vtkSmartPointer< vtkPolyDataNormals >			PolyDataNormalsPointerType;
  typedef vtkSmartPointer< vtkHull >				ConvexHullFilterPointerType;
  typedef vtkSmartPointer< vtkPointsProjectedHull >		ConvexHull2DFilterPointerType;
  typedef vtkSmartPointer< vtkDelaunay2D >			Delaunay2DFilterPointerType;
  typedef vtkSmartPointer< vtkDelaunay3D >			Delaunay3DFilterPointerType;
  typedef vtkSmartPointer< vtkDataSetSurfaceFilter >		DataSetSurfaceFilterPointerType;
  typedef vtkSmartPointer< vtkButterflySubdivisionFilter >	MeshRefinementFilterPointerType;
  typedef vtkSmartPointer< vtkLoopSubdivisionFilter >		MeshRefinementFilter2PointerType;
  typedef vtkSmartPointer< vtkSelectEnclosedPoints >		SelectEnclosedPointsFilterType;

  typedef std::pair< unsigned int, unsigned int > ColumnCellTypePair;
  typedef std::pair< unsigned int, unsigned int > MatrixIndexType;
  typedef std::vector< unsigned int > SelectionType;

  extern float XYSAMPLING;
  extern float ZSAMPLING;
  extern float averageSomaRadius;
  extern float zScale;
  extern unsigned long BINSIZE;
  extern unsigned long BOXSIZE;

  //VoxelFeatures of the Measurement Vector Type
  #define X_COORD		0
  #define Y_COORD		1
  #define Z_COORD		2

  //celltypes for automatic dendrite detection
  #define SUPRA			1
  #define GRAN			2
  #define INFRA			3

  //Label IDs for Amira Spatial Graphs
  #define Neuron		2
  #define Dendrite		3
  #define ApicalDendrite	4
  #define BasalDendrite		5
  #define Axon			6
  #define Soma			7
  #define Landmark		8
  #define Pia			9
  #define WhiteMatter		48
  #define Vessel		10
  #define Barrel		11
  #define ZAxis			50
  #define aRow			12
  #define A1			13
  #define A2			14
  #define A3			15
  #define A4			16
  #define bRow			17
  #define B1			18
  #define B2			19
  #define B3			20
  #define B4			21
  #define cRow			22
  #define C1			23
  #define C2			24
  #define C3			25
  #define C4			26
  #define C5			27
  #define C6			28
  #define dRow			29
  #define D1			30
  #define D2			31
  #define D3			32
  #define D4			33
  #define D5			34
  #define D6			35
  #define eRow			36
  #define E1			37
  #define E2			38
  #define E3			39
  #define E4			40
  #define E5			41
  #define E6			42
  #define greekRow		43
  #define Alpha			44
  #define Beta			45
  #define Gamma			46
  #define Delta			47
  #define Septum		0

  // cell types for NeuroNet Connectome analysis
  #define L2			1
  #define L34			2
  #define L4py			3
  #define L4sp			4
  #define L4ss			5
  #define L5st			6
  #define L5tt			7
  #define L6cc			8
  #define L6ccinv		9
  #define L6ct			10
  #define VPM			11
  #define L2axon		12
  #define L34axon		13
  #define L4pyaxon		14
  #define L4spaxon		15
  #define L4ssaxon		16
  #define L5staxon		17
  #define L5ttaxon		18
  #define L6ccaxon		19
  #define L6ccinvaxon	20
  #define L6ctaxon		21
  #define SymLocal		22
  #define SymLocal1		23
  #define SymLocal2		24
  #define SymLocal3		25
  #define SymLocal4		26
  #define SymLocal5		27
  #define SymLocal6		28
  #define L1			29
  #define L23Trans		30
  #define L45Sym		31
  #define L45Peak		32
  #define L56Trans		33
  #define SymLocalaxon	34
  #define SymLocal1axon	35
  #define SymLocal2axon	36
  #define SymLocal3axon	37
  #define SymLocal4axon	38
  #define SymLocal5axon	39
  #define SymLocal6axon	40
  #define L1axon		41
  #define L23Transaxon	42
  #define L45Symaxon	43
  #define L45Peakaxon	44
  #define L56Transaxon	45
//168fines
#endif


#pragma once
#include <SDL2/SDL.h>
#include <SDL2/SDL_audio.h>
#include <chrono>
#include <cstdint>
#include <gsl/span>
#include <queue>

class CSound
{
private:
	static constexpr std::int32_t Frequency{ 44100 };

	struct SBeep
	{
		double Frequency;
		std::uint32_t SamplesLeft;
	};

	SDL_AudioDeviceID mDeviceID;
	std::queue<SBeep> mBeeps;
	double mV;

public:
	CSound();
	~CSound();

	void Beep(double frequency, std::chrono::milliseconds duration);

private:
	void GenerateSamples(gsl::span<std::int16_t> stream);

	static void SdlCallback(CSound* self, std::uint8_t* stream, std::int32_t len);
};


/* Copyright (c) 2011-2012, Jérémy Fix. All rights reserved. */

/* Redistribution and use in source and binary forms, with or without */
/* modification, are permitted provided that the following conditions are met: */

/* * Redistributions of source code must retain the above copyright notice, */
/* this list of conditions and the following disclaimer. */
/* * 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. */
/* * None of the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. */

/* THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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. */

#ifndef UKF_TYPES_H
#define UKF_TYPES_H

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include "ukf_math.h"

namespace ukf
{

    /**
      * @short The different types of implemented process noise for UKF state estimation
      */
    typedef enum
    {
        /**
          * @brief The covariance of the evolution noise is fixed to \f$\textbf{P}_{\theta\theta_i} = \alpha . \textbf{I}\f$
          */
        UKF_PROCESS_FIXED,
        /**
          * @brief The covariance of the evolution noise is defined as \f$\textbf{P}_{\theta\theta_i} = (\alpha^{-1} - 1)\textbf{P}_i\f$
          */
        UKF_PROCESS_RLS
    } ProcessNoise;

    namespace parameter
    {
        class EvolutionNoise;
        /**
          * @short Structure holding the parameters of the Unscented Kalman Filter
          *
          */
        typedef struct
        {
            /**
              * @short \f$\kappa \geq 0\f$, \f$\kappa = 0\f$ is a good choice. According to van der Merwe, its value is not critical
              */
            double kpa;

            /**
              * @short \f$0 \leq \alpha \leq 1\f$ : "Size" of sigma-point distribution. Should be small if the function is strongly non-linear
              */
            double alpha;

            /**
              * @short Non negative weights used to introduce knowledge about the higher order moments of the distribution. For gaussian distributions, \f$\beta = 2\f$ is a good choice
              */
            double beta;

            /**
              * @short \f$\lambda = \alpha^2 (n + \kappa) - n\f$
              */
            double lambda;

            /**
              * @short \f$\gamma = \sqrt{\lambda + n}\f$
              */
            double gamma;

            /**
              * @short Parameter used for the evolution noise
              */
            //double evolution_noise_parameter;

            /**
              * @short Initial value of the evolution noise
              */
            //double evolution_noise;

            /**
              * @short Evolution noise type
              */
            //EvolutionNoise evolution_noise_type;
            EvolutionNoise * evolution_noise;

            /**
              * @short Covariance of the observation noise
              */
            double observation_noise;

            /**
              * @short Prior estimate of the covariance matrix
              */
            double prior_pi;

            /**
              * @short Number of parameters to estimate
              */
            int n;

            /**
              * @short \f$nbSamples = (2 n + 1)\f$ Number of sigma-points
              */
            int nbSamples;

            /**
              * @short Dimension of the output
              */
            int no;

        } ukf_param;

        /**
          * @short Pointer to the function to approximate in the scalar case
          *
          */
        //typedef double (*ukf_function_scalar) (gsl_vector * param, gsl_vector * input);

        /**
          * @short Structure holding the matrices manipulated by the statistical linearization
          * in the scalar case
          *
          */
        typedef struct
        {
            /**
              * @short Kalman gain, a vector of size \f$n\f$
              */
            gsl_vector * Kk;

            /**
              * @short Temporary matrix for the kalman gain, a matrix of size \f$(n,1)\f$
              */
            gsl_matrix * Kk_mat;

            /**
              * @short Temporary matrix for the transpose of the kalman gain, a matrix of size \f$(1,n)\f$
              */
            gsl_matrix * Kk_mat_T;

            /**
             * @short Covariance of \f$(w, d)\f$: \f$P_{w_k d_k}\f$, of size \f$n\f$
             */
            gsl_vector * Pwdk;

            /**
              * @short Covariance of the evolution noise
              */
            gsl_matrix * Prrk;

            /**
              * @short Covariance of the observation noise
              */
            double Peek;

            /**
              * @short Covariance of the output
              */
            double Pddk;

            /**
              * @short Parameter vector, of size \f$n\f$
              */
            gsl_vector * w;

            /**
              * @short Temporary vector, holding a sigma-point
              */
            gsl_vector * wk;

            /**
              * @short Covariance matrix of the parameters, of size \f$(n,n)\f$
              */
            gsl_matrix * Pk;

            /**
              * @short Matrix holding the Cholesky decomposition of \f$P_k\f$, of size \f$(n,n)\f$
              */
            gsl_matrix * Sk;

            /**
              * @short Vector holding one column of Sk, of size \f$n\f$
              */
            gsl_vector * cSk;

            /**
              * @short Weights used to compute the mean of the sigma points' images
              * @brief \f$wm_0 = \frac{\lambda}{n + \lambda}\f$
              *        \f$wm_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wm;

            /**
              * @short Weights used to update the covariance matrices
              * @brief \f$wc_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\f$
              *        \f$wc_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wc;

            /**
              * @short Temporary vector holding the image of the sigma points, of size \f$nbSamples\f$
              */
            gsl_vector * dk;

            /**
              * @short Innovation
              */
            double ino_dk;

            /**
              * @short Variable holding the mean of the sigma points image
              */
            double d_mean;

            /**
              * @short Matrix holding the sigma points in the columns, of size \f$(n,nbSamples)\f$
              */
            gsl_matrix * sigmaPoints;

            /**
              * @short Temporary vector
              */
            gsl_vector * temp_n;

            /**
              * @short Temporary matrix
              */
            gsl_matrix * temp_n_n;

        } ukf_scalar_state;

        /**
          * @short Structure holding the matrices manipulated by the unscented kalman filter
          *  in the vectorial case, for Parameter estimation
          *
          */
        typedef struct
        {
            /**
              * @short Kalman gain, a matrix of size \f$n \times no\f$
              */
            gsl_matrix * Kk;

            /**
              * @short The tranposed Kalman gain, a vector of size \f$no \times n\f$
              */
            gsl_matrix * Kk_T;

            /**
             * @short Covariance of \f$(w, d)\f$: \f$P_{w_k d_k}\f$, of size \f$n \times no\f$
             */
            gsl_matrix * Pwdk;

            /**
              * @short Covariance of the output, a matrix of size \f$ no \times no \f$
              */
            gsl_matrix * Pddk;

            /**
              * @short Covariance of the observation noise, of size \f$ no \times no \f$
              */
            gsl_matrix * Peek;

            /**
              * @short Covariance of the evolution noise, of size \f$ n \times n \f$
              */
            gsl_matrix * Prrk;

            /**
              * @short Parameter vector, of size \f$n\f$
              */
            gsl_vector * w;

            /**
              * @short Temporary vector holding one sigma point, of size \f$n\f$
              */
            gsl_vector * wk;

            /**
              * @short Covariance matrix of the parameters, of size \f$(n,n)\f$
              */
            gsl_matrix * Pk;

            /**
              * @short Matrix holding the Cholesky decomposition of \f$P_k\f$, of size \f$(n,n)\f$
              */
            gsl_matrix * Sk;

            /**
              * @short Vector holding one column of Sk, of size \f$n\f$
              */
            gsl_vector * cSk;

            /**
              * @short Weights used to compute the mean of the sigma points' images
              * @brief \f$wm_0 = \frac{\lambda}{n + \lambda}\f$
              *        \f$wm_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wm;

            /**
              * @short Weights used to update the covariance matrices
              * @brief \f$wc_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\f$
              *        \f$wc_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wc;

            /**
              * @short Temporary matrix holding the image of the sigma points, of size \f$ no \times nbSamples\f$
              */
            gsl_matrix * dk;

            /**
              * @short Vector holding the mean of the sigma points image, of size \f$ no \f$ŝ
              */
            gsl_vector * d_mean;

            /**
              * @short Vector holding the inovation, of size \f$ no \f$ŝ
              */
            gsl_vector * ino_dk;

            /**
              * @short Matrix holding the sigma points in the columns, of size \f$(n,nbSamples)\f$
              */
            gsl_matrix * sigmaPoints;

            /**
              * @short Temporary vector of size \f$ n \f$
              */
            gsl_vector * vec_temp_n;

            /**
              * @short Temporary vector of size \f$ no \f$
              */
            gsl_vector * vec_temp_output;

            /**
              * @short Temporary matrix of size \f$ n \times 1 \f$
              */
            gsl_matrix * mat_temp_n_1;

            /**
              * @short Temporary matrix of size \f$ n \times no \f$
              */
            gsl_matrix * mat_temp_n_output;

            /**
              * @short Temporary matrix of size \f$ no \times n \f$
              */
            gsl_matrix * mat_temp_output_n;

            /**
              * @short Temporary matrix of size \f$ 1 \times no \f$
              */
            gsl_matrix * mat_temp_1_output;

            /**
              * @short Temporary matrix of size \f$ no \times 1 \f$
              */
            gsl_matrix * mat_temp_output_1;

            /**
              * @short Temporary matrix of size \f$ no \times no \f$
              */
            gsl_matrix * mat_temp_output_output;

            /**
              * @short Temporary matrix of size \f$ n \times n \f$
              */
            gsl_matrix * mat_temp_n_n;
        } ukf_state;

        /**
          * @short Mother class from which the evolution noises inherit
          */
        class EvolutionNoise
        {
        protected:
            double _initial_value;
        public:
            EvolutionNoise(double initial_value) : _initial_value(initial_value) {};
            void init(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_set_identity(s.Prrk);
                gsl_matrix_scale(s.Prrk, _initial_value);
            }
            void init(ukf_param &p, ukf_scalar_state &s)
            {
                gsl_matrix_set_identity(s.Prrk);
                gsl_matrix_scale(s.Prrk, _initial_value);
            }

            virtual void updateEvolutionNoise(ukf_param &p, ukf_state &s) = 0;
            virtual void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s) = 0;
        };

        /**
          * @short Annealing type evolution noise
          */
        class EvolutionAnneal : public EvolutionNoise
        {
            double _decay, _lower_bound;
        public:
            EvolutionAnneal(double initial_value, double decay, double lower_bound) : EvolutionNoise(initial_value),
            _decay(decay),
            _lower_bound(lower_bound)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                            gsl_matrix_set(s.Prrk, i, i, ukf::math::max(_decay * gsl_matrix_get(s.Prrk,i,i),_lower_bound));
                        else
                            gsl_matrix_set(s.Prrk, i, j, 0.0);
                    }
                }
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                            gsl_matrix_set(s.Prrk, i, i, ukf::math::max(_decay * gsl_matrix_get(s.Prrk,i,i),_lower_bound));
                        else
                            gsl_matrix_set(s.Prrk, i, j, 0.0);
                    }
                }
            }
        };

        /**
          * @short Forgetting type evolution noise
          */
        class EvolutionRLS : public EvolutionNoise
        {
            double _decay;
        public:
            EvolutionRLS(double initial_value, double decay) : EvolutionNoise(initial_value), _decay(decay)
            {
                if(ukf::math::cmp_equal(_decay, 0.0))
                    printf("Forgetting factor should not be null !!\n");
            };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_memcpy(s.Prrk, s.Pk);
                gsl_matrix_scale(s.Prrk, 1.0 / _decay - 1.0);
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                gsl_matrix_memcpy(s.Prrk, s.Pk);
                gsl_matrix_scale(s.Prrk, 1.0 / _decay - 1.0);
            }
        };

        /**
          * @short Robbins-Monro evolution noise
          */
        class EvolutionRobbinsMonro : public EvolutionNoise
        {
            double _alpha;
        public:
            EvolutionRobbinsMonro(double initial_value, double alpha) : EvolutionNoise(initial_value),
            _alpha(alpha)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                // Compute Kk * ino_yk
	      gsl_matrix_view mat_view = gsl_matrix_view_array(s.ino_dk->data, p.no, 1);
	      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.Kk, &mat_view.matrix, 0.0, s.mat_temp_n_1);
	      // Compute : Prrk = (1 - alpha) Prrk + alpha . (Kk * ino_dk) * (Kk * ino_dk)^T
	      gsl_blas_dgemm(CblasNoTrans, CblasTrans, _alpha, s.mat_temp_n_1, s.mat_temp_n_1, (1.0 - _alpha),s.Prrk);
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                // Compute  Prrk = (1 - alpha) Prrk + alpha . ino_dk^2 Kk Kk^T
	      gsl_matrix_view mat_view = gsl_matrix_view_array(s.Kk->data,p.n,1);
                gsl_blas_dgemm(CblasNoTrans, CblasTrans, gsl_pow_2(s.ino_dk) * _alpha, &mat_view.matrix, &mat_view.matrix, 1.0 - _alpha, s.Prrk);
            }
        };

    } // parameter

    namespace state
    {

        class EvolutionNoise;
        /**
          * @short Structure holding the parameters of the statistical linearization
          *
          */
        typedef struct
        {
            /**
              * @short \f$\kappa \geq 0\f$, \f$\kappa = 0\f$ is a good choice. According to van der Merwe, its value is not critical
              */
            double kpa;

            /**
              * @short \f$0 \leq \alpha \leq 1\f$ : "Size" of sigma-point distribution. Should be small if the function is strongly non-linear
              */
            double alpha;

            /**
              * @short Non negative weights used to introduce knowledge about the higher order moments of the distribution. For gaussian distributions, \f$\beta = 2\f$ is a good choice
              */
            double beta;

            /**
              * @short \f$\lambda = \alpha^2 (n + \kappa) - n\f$
              */
            double lambda;
            double lambda_aug;

            /**
              * @short \f$\gamma = \sqrt{\lambda + n}\f$
              */
            double gamma;
            double gamma_aug;

            /**
              * @short Size of the state vector
              */
            int n;

            /**
              * @short \f$nbSamples = (2 n + 1)\f$ Number of sigma-points
              */
            int nbSamples;

            /**
              * @short \f$nbSamplesMeasure = (4 n + 1)\f$ Number of sigma-points
              */
            int nbSamplesMeasure;

            /**
              * @short Dimension of the output : the measurements
              */
            int no;

            /**
              * @short Prior estimate of the covariance matrix of the state
              */
            double prior_x;

            /**
              * @short Type of process noise
              */
            EvolutionNoise * evolution_noise;

            /**
              * @short Parameter used for the evolution noise
              */
            //double process_noise;

            /**
              * @short Covariance of the observation noise
              */
            double measurement_noise;

        } ukf_param;

        /**
          * @short Structure holding the matrices manipulated by the statistical linearization
          *  in the vectorial case for state estimation
          *
          */
        typedef struct
        {
            gsl_vector * params; // The parameters of the process equation or anything you need to give to the process function

            gsl_vector * xi; // State vector
            gsl_matrix * xi_prediction; // Prediction of the states for each sigma-point
            gsl_vector * xi_mean; // Mean state vector
            gsl_matrix * Pxxi; // State covariance matrix
            gsl_matrix * cholPxxi; // Cholesky decomposition of Pxxi
            gsl_matrix * Pvvi; // Process noise covariance
            gsl_matrix * cholPvvi; // Cholesky decomposition of the process noise

            gsl_matrix * yi_prediction; // Prediction of the measurements for each sigma-point
            gsl_vector * yi_mean; // Mean measurement vector
            gsl_vector * ino_yi; // Innovations
            gsl_matrix * Pyyi; // Measurement covariance matrix
            gsl_matrix * Pnni; // Measurement noise covariance

            gsl_matrix * Pxyi; // State-Measurement covariance matrix

            gsl_vector * sigmaPoint; // Vector holding one sigma point
            gsl_matrix * sigmaPoints;  // Matrix holding all the sigma points

            gsl_vector * sigmaPointMeasure; // one sigma point for the observation equation
            gsl_matrix * sigmaPointsMeasure; // Sigma points for the observation equation

            gsl_vector * wm_j; // Weights used to compute the mean of the sigma points images
            gsl_vector * wc_j; // Weights used to update the covariance matrices

            gsl_vector * wm_aug_j; // Augmented set of Weights used to compute the mean of the sigma points images for the observations
            gsl_vector * wc_aug_j; // Augmented set of Weights used to update the covariance matrices of the observations

            gsl_matrix * Ki; // Kalman gain
            gsl_matrix * Ki_T; // Its transpose

            gsl_vector * temp_n;
            gsl_matrix * temp_n_n;
            gsl_matrix * temp_n_1;
            gsl_matrix * temp_1_n;

            gsl_matrix * temp_n_no;

            gsl_matrix * temp_no_no;
            gsl_matrix * temp_no_1;
            gsl_matrix * temp_1_no;
        } ukf_state;

        /**
          * @short Mother class from which the evolution noises inherit
          */
        class EvolutionNoise
        {
        protected:
            double _initial_value;
        public:
            EvolutionNoise(double initial_value) : _initial_value(initial_value) {};
            void init(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_set_identity(s.Pvvi);
                gsl_matrix_scale(s.Pvvi, _initial_value);
                gsl_matrix_set_identity(s.cholPvvi);
                gsl_matrix_scale(s.cholPvvi, sqrt(_initial_value));
            }

            virtual void updateEvolutionNoise(ukf_param &p, ukf_state &s) = 0;
        };

        /**
          * @short Annealing type evolution noise
          */
        class EvolutionAnneal : public EvolutionNoise
        {
            double _decay, _lower_bound;
        public:
            EvolutionAnneal(double initial_value, double decay, double lower_bound) : EvolutionNoise(initial_value),
            _decay(decay),
            _lower_bound(lower_bound)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                double value;
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                        {
                            value = ukf::math::max(_decay * gsl_matrix_get(s.Pvvi,i,i),_lower_bound);
                            gsl_matrix_set(s.Pvvi, i, i, value);
                            gsl_matrix_set(s.cholPvvi, i ,i, sqrt(value));
                        }
                        else
                        {
                            gsl_matrix_set(s.Pvvi, i, j, 0.0);
                            gsl_matrix_set(s.cholPvvi, i, j, 0.0);
                        }
                    }
                }
            }
        };

        /**
          * @short Forgetting type evolution noise
          */
        class EvolutionRLS : public EvolutionNoise
        {
            double _decay;
        public:
            EvolutionRLS(double initial_value, double decay) : EvolutionNoise(initial_value), _decay(decay)
            {
                if(ukf::math::cmp_equal(_decay, 0.0))
                    printf("Forgetting factor should not be null !!\n");
            };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_memcpy(s.Pvvi, s.Pxxi);
                gsl_matrix_scale(s.Pvvi, 1.0 / _decay - 1.0);
                gsl_matrix_memcpy(s.cholPvvi, s.Pvvi);
                gsl_linalg_cholesky_decomp(s.cholPvvi);
                // Set all the elements of cholPvvi strictly above the diagonal to zero
                for(int j = 0 ; j < p.n ; j++)
                    for(int k = j+1 ; k < p.n ; k++)
                         gsl_matrix_set(s.cholPvvi,j,k,0.0);
            }
        };

        /**
          * @short Robbins-Monro evolution noise
          */
        class EvolutionRobbinsMonro : public EvolutionNoise
        {
            double _alpha;
        public:
            EvolutionRobbinsMonro(double initial_value, double alpha) : EvolutionNoise(initial_value),
            _alpha(alpha)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                // Compute Kk * ino_yk
                gsl_blas_dgemv(CblasNoTrans, 1.0, s.Ki, s.ino_yi, 0.0, s.temp_n);
                // Compute : Prrk = (1 - alpha) Prrk + alpha . (Kk * ino_dk) * (Kk * ino_dk)^T
		gsl_matrix_view mat_view = gsl_matrix_view_array(s.temp_n->data,p.n,1);
                gsl_blas_dgemm(CblasNoTrans, CblasTrans, _alpha, &mat_view.matrix, &mat_view.matrix, (1.0 - _alpha),s.Pvvi);
            }
        };

    } // namespace for state estimation

    namespace srstate
    {

        /**
          * @short Structure holding the parameters of the statistical linearization
          *
          */
        typedef struct
        {
            /**
              * @short \f$\kappa \geq 0\f$, \f$\kappa = 0\f$ is a good choice. According to van der Merwe, its value is not critical
              */
            double kpa;

            /**
              * @short \f$0 \leq \alpha \leq 1\f$ : "Size" of sigma-point distribution. Should be small if the function is strongly non-linear
              */
            double alpha;

            /**
              * @short Non negative weights used to introduce knowledge about the higher order moments of the distribution. For gaussian distributions, \f$\beta = 2\f$ is a good choice
              */
            double beta;

            /**
              * @short \f$\lambda = \alpha^2 (n + \kappa) - n\f$
              */
            double lambda;
            double lambda_aug;

            /**
              * @short \f$\gamma = \sqrt{\lambda + n}\f$
              */
            double gamma;
            double gamma_aug;

            /**
              * @short Size of the state vector
              */
            int n;

            /**
              * @short \f$nbSamples = (2 n + 1)\f$ Number of sigma-points
              */
            int nbSamples;

            /**
              * @short \f$nbSamplesMeasure = (4 n + 1)\f$ Number of sigma-points
              */
            int nbSamplesMeasure;

            /**
              * @short Dimension of the output : the measurements
              */
            int no;

            /**
              * @short Prior estimate of the covariance matrix of the state
              */
            double prior_x;

            /**
              * @short Type of process noise
              */
            ProcessNoise process_noise_type;

            /**
              * @short Parameter used for the evolution noise
              */
            double process_noise;

            /**
              * @short Covariance of the observation noise
              */
            double measurement_noise;

        } ukf_param;

        /**
          * @short Structure holding the matrices manipulated by the statistical linearization
          *  in the vectorial case for state estimation
          *
          */
        typedef struct
        {
            gsl_vector * params; // The parameters of the process equation or anything you need to give to the process function

            gsl_vector * xi; // State vector
            gsl_matrix * xi_prediction; // Prediction of the states for each sigma-point
            gsl_vector * xi_mean; // Mean state vector
            gsl_matrix * Sxi; // Cholesky factor of variance covariance of the state
            gsl_matrix * Svi; // Cholesky factor of the process noise

            gsl_matrix * yi_prediction; // Prediction of the measurements for each sigma-point
            gsl_vector * yi_mean; // Mean measurement vector
            gsl_vector * ino_yi; // Innovations
            gsl_matrix * Syi; // Cholesky factor of the variance covariance of the output
            gsl_matrix * Sni; // Cholesky factor of the measurement noise

            gsl_matrix * Pxyi; // State-Measurement covariance matrix

            gsl_vector * sigmaPoint; // Vector holding one sigma point
            gsl_matrix * sigmaPoints;  // Matrix holding all the sigma points

            gsl_vector * sigmaPointMeasure; // one sigma point for the observation equation
            gsl_matrix * sigmaPointsMeasure; // Sigma points for the observation equation

            gsl_matrix * U;

            gsl_vector * wm_j; // Weights used to compute the mean of the sigma points images
            gsl_vector * wc_j; // Weights used to update the covariance matrices

            gsl_vector * wm_aug_j; // Augmented set of Weights used to compute the mean of the sigma points images for the observations
            gsl_vector * wc_aug_j; // Augmented set of Weights used to update the covariance matrices of the observations

            gsl_matrix * Ki; // Kalman gain
            gsl_matrix * Ki_T; // Its transpose

            gsl_vector * temp_n;
            gsl_vector * temp_no;

            gsl_matrix * temp_n_n;
            gsl_matrix * temp_n_1;
            gsl_matrix * temp_1_n;

            gsl_matrix * temp_n_no;

            gsl_matrix * temp_no_no;
            gsl_matrix * temp_no_1;
            gsl_matrix * temp_1_no;

            gsl_matrix * temp_3n_n;
            gsl_matrix * temp_2nno_no;
        } ukf_state;

    } // namespace for state estimation, square root implementation

}

#endif // UKF_TYPES_H


#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>

#include "tests.h"

void
test_spmv (void) {
const double flteps = 1e-4, dbleps = 1e-6;
  {
   int order = 101;
   int uplo = 121;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1134)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 121;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1135)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 122;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1136)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 122;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1137)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 121;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1138)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 121;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1139)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 122;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1140)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 122;
   float alpha = 0.1f;
   float beta = -0.3f;
   int N = 2;
   float A[] = { -0.174f, 0.878f, 0.478f };
   float X[] = { 0.503f, 0.313f };
   int incX = -1;
   float Y[] = { -0.565f, -0.109f };
   int incY = -1;
   float y_expected[] = { 0.221025f, 0.0714172f };
   cblas_sspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], flteps, "sspmv(case 1141)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 121;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1142)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 121;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1143)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 122;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1144)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 122;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1145)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 121;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1146)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 121;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1147)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 122;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1148)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 122;
   double alpha = -1;
   double beta = 0.1;
   int N = 2;
   double A[] = { -0.181, -0.071, -0.038 };
   double X[] = { -0.015, 0.132 };
   int incX = -1;
   double Y[] = { -0.449, -0.219 };
   int incY = -1;
   double y_expected[] = { -0.036098, 9.27e-04 };
   cblas_dspmv(order, uplo, N, alpha, A, X, incX, beta, Y, incY);
   {
     int i;
     for (i = 0; i < 2; i++) {
       gsl_test_rel(Y[i], y_expected[i], dbleps, "dspmv(case 1149)");
     }
   };
  };

}


/**
 * \file RIAAFilter.h
 * Filters for RIAA correction
 */

#ifndef ATK_EQ_RIAAFILTER_H
#define ATK_EQ_RIAAFILTER_H

#include <gsl/gsl>

#include <ATK/EQ/SecondOrderFilter.h>

namespace ATK
{
  /// RIAA coefficients used for RIAA correction
  template<typename DataType_>
  class RIAACoefficients : public SecondOrderCoreCoefficients<DataType_>
  {
  public:
    /// Simplify parent calls
    using Parent = SecondOrderCoreCoefficients<DataType_>;
    using typename Parent::AlignedScalarVector;
    using typename Parent::DataType;
    using CoeffDataType = typename TypeTraits<DataType>::Scalar;
    using Parent::input_sampling_rate;
    using Parent::output_sampling_rate;
    using Parent::setup;
  protected:
    using Parent::in_order;
    using Parent::out_order;
    using Parent::coefficients_in;
    using Parent::coefficients_out;

    void setup() override;

  public:
    /*!
     * @brief Constructor
     * @param nb_channels is the number of input and output channels
     */
    explicit RIAACoefficients(gsl::index nb_channels = 1);
  };

  /// RIAA coefficients used to create the inverse compensation (for mastering engineers)
  template<typename DataType_>
  class InverseRIAACoefficients : public SecondOrderCoreCoefficients<DataType_>
  {
  public:
    /// Simplify parent calls
    using Parent = SecondOrderCoreCoefficients<DataType_>;
    using typename Parent::AlignedScalarVector;
    using typename Parent::DataType;
    using CoeffDataType = typename TypeTraits<DataType>::Scalar;
    using Parent::input_sampling_rate;
    using Parent::output_sampling_rate;
    using Parent::setup;
  protected:
    using Parent::in_order;
    using Parent::out_order;
    using Parent::coefficients_in;
    using Parent::coefficients_out;

    void setup() override;

  public:
    /*!
     * @brief Constructor
     * @param nb_channels is the number of input and output channels
     */
    explicit InverseRIAACoefficients(gsl::index nb_channels = 1);
  };
}

#endif


/*
 * BRAINS
 * (B)LR (R)everberation-mapping (A)nalysis (I)n AGNs with (N)ested (S)ampling
 * Yan-Rong Li, liyanrong@ihep.ac.cn
 * Thu, Aug 4, 2016
 */

/*!
 *  \file dnest_con.c
 *  \brief run dnest for continuum analysis
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_interp.h>
#include <mpi.h>

#include "brains.h"

DNestFptrSet *fptrset_con;

/*!
 *  This function run dnest sampling for continuum.
 */
int dnest_con(int argc, char **argv)
{
  int i;

  num_params = parset.n_con_recon + num_params_var;

  par_range_model = malloc( num_params * sizeof(double *));
  par_prior_gaussian = malloc(num_params * sizeof(double *));
  for(i=0; i<num_params; i++)
  {
    par_range_model[i] = malloc(2*sizeof(double));
    par_prior_gaussian[i] = malloc(2*sizeof(double));
  }
  par_prior_model = malloc( num_params * sizeof(int));

  par_fix = (int *) malloc(num_params * sizeof(int));
  par_fix_val = (double *) malloc(num_params * sizeof(double));

  fptrset_con = dnest_malloc_fptrset();

  /* setup functions used for dnest*/
  fptrset_con->from_prior = from_prior_con;
  fptrset_con->perturb = perturb_con;
  fptrset_con->print_particle = print_particle_con;
  fptrset_con->restart_action = restart_action_con;
  fptrset_con->accept_action = accept_action_con;
  fptrset_con->kill_action = kill_action_con;
  fptrset_con->read_particle = read_particle_con;

  if(parset.flag_exam_prior != 1)
  {
    fptrset_con->log_likelihoods_cal = log_likelihoods_cal_con;
    fptrset_con->log_likelihoods_cal_initial = log_likelihoods_cal_initial_con;
    fptrset_con->log_likelihoods_cal_restart = log_likelihoods_cal_restart_con;
  }
  else
  {
    fptrset_con->log_likelihoods_cal = log_likelihoods_cal_con_exam;
    fptrset_con->log_likelihoods_cal_initial = log_likelihoods_cal_con_exam;
    fptrset_con->log_likelihoods_cal_restart = log_likelihoods_cal_con_exam;
  }

  set_par_range_con();

  /* setup fixed parameters */
  for(i=0; i<num_params; i++)
  {
    par_fix[i] = 0;
    par_fix_val[i] = -DBL_MAX;
  }

  /* fix systematic error of continuum */
  if(parset.flag_con_sys_err != 1)
  {
    par_fix[0] = 1;
    par_fix_val[0] = log(1.0);
  }

  print_par_names_con();

  /* if not only print parameter name */
  if(parset.flag_para_name != 1)
    logz_con = dnest(argc, argv, fptrset_con, num_params, dnest_options_file);

  dnest_free_fptrset(fptrset_con);
  return 0;
}

/*!
 * this function set the parameter range.
 */
void set_par_range_con()
{
  int i;

  /* variability parameters */
  for(i=0; i<num_params_drw; i++)
  {
    par_range_model[i][0] = var_range_model[i][0];
    par_range_model[i][1] = var_range_model[i][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;
  }

  /* parameters for long-term trend */
  for(i=num_params_drw; i<num_params_drw + num_params_trend; i++)
  {
    par_range_model[i][0] = var_range_model[3][0];
    par_range_model[i][1] = var_range_model[3][1];

    par_prior_model[i] = GAUSSIAN;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 1.0;
  }

  /* parameter for trend difference */
  for(i= num_params_drw + num_params_trend; i<num_params_var; i++)
  {
    par_range_model[i][0] = var_range_model[4 + i - (num_params_drw + num_params_trend)][0];
    par_range_model[i][1] = var_range_model[4 + i - (num_params_drw + num_params_trend)][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;
  }

  /* continuum light curve parameters */
  for(i=num_params_var; i<num_params; i++)
  {
    par_range_model[i][0] = var_range_model[4+num_params_difftrend][0];
    par_range_model[i][1] = var_range_model[4+num_params_difftrend][1];

    par_prior_model[i] = GAUSSIAN;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 1.0;
  }
  return;
}

/*!
 *  print names and prior ranges for parameters
 *
 */
void print_par_names_con()
{
  if(thistask!= roottask)
    return;

  int i, j;
  FILE *fp;
  char fname[BRAINS_MAX_STR_LENGTH], str_fmt[BRAINS_MAX_STR_LENGTH];

  sprintf(fname, "%s/%s", parset.file_dir, "data/para_names_con.txt");
  fp = fopen(fname, "w");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", fname);
    exit(0);
  }

  strcpy(str_fmt, "%4d %-15s %10.6f %10.6f %4d %4d %15.6e\n");

  printf("# Print parameter name in %s\n", fname);

  fprintf(fp, "#*************************************************\n");
  fprint_version(fp);
  fprintf(fp, "#*************************************************\n");

  fprintf(fp, "%4s %-15s %10s %10s %4s %4s %15s\n", "#", "Par", "Min", "Max", "Prior", "Fix", "Val");
  i=0;
  fprintf(fp, str_fmt, i, "sys_err_con", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  i++;
  fprintf(fp, str_fmt, i, "sigmad", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  i++;
  fprintf(fp, str_fmt, i, "taud", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);

  for(j=0; j<num_params_trend; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "trend", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  for(j=0; j<num_params_difftrend; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "diff trend", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  for(j=0; j<parset.n_con_recon; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "time series", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  fclose(fp);
}

/*!
 * This function generate a sample from the prior.
 */
void from_prior_con(void *model)
{
  int i;
  double *pm = (double *)model;

  for(i=0; i<num_params; i++)
  {
    if(par_prior_model[i] == GAUSSIAN )
    {
      pm[i] = dnest_randn() * par_prior_gaussian[i][1] + par_prior_gaussian[i][0];
      dnest_wrap(&pm[i], par_range_model[i][0], par_range_model[i][1]);
    }
    else
    {
      pm[i] = var_range_model[i][0] + dnest_rand()*(par_range_model[i][1] - par_range_model[i][0]);
    }
  }

  for(i=0; i<num_params_var; i++)
  {
    if(par_fix[i] == 1)
      pm[i] = par_fix_val[i];
  }
  /* all parameters need to update at the initial step */
  which_parameter_update = -1;
}

/*!
 * This function calculate log likelihood probability.
 */
double log_likelihoods_cal_con(const void *model)
{
  double logL;
  logL = prob_con_variability_semiseparable(model);
  return logL;
}

/*!
 * This function calculate log likelihood probability at the initial step.
 */
double log_likelihoods_cal_initial_con(const void *model)
{
  double logL;
  logL = prob_con_variability_initial_semiseparable(model);
  return logL;
}

/*!
 * This function calculate log likelihood probability at the restart step.
 */
double log_likelihoods_cal_restart_con(const void *model)
{
  double logL;
  logL = prob_con_variability_initial_semiseparable(model);
  return logL;
}
/*!
 * This function generate a new move of parameters.
 */
double perturb_con(void *model)
{
  double *pm = (double *)model;
  double logH = 0.0, limit1, limit2, width, rnd;
  int which, which_level;

  /* sample variability parameters more frequently */
  do
  {
    rnd = dnest_rand();
    if(rnd < 0.1)
      which = dnest_rand_int(num_params_var);
    else
      which = dnest_rand_int(parset.n_con_recon) + num_params_var;
  }while(par_fix[which] == 1);

  which_parameter_update = which;

  /* level-dependent width */
  which_level_update = dnest_get_which_level_update();
  which_level = which_level_update > (size_levels - 10)?(size_levels-10):which_level_update;
  if( which_level > 0)
  {
    limit1 = limits[(which_level-1) * num_params *2 + which *2];
    limit2 = limits[(which_level-1) * num_params *2 + which *2 + 1];
    width = limit2 - limit1;
  }
  else
  {
    width = ( par_range_model[which][1] - par_range_model[which][0] );
  }

  if(par_prior_model[which] == GAUSSIAN)
  {
    logH -= (-0.5*pow((pm[which] - par_prior_gaussian[which][0])/par_prior_gaussian[which][1], 2.0) );
    pm[which] += dnest_randh() * width;
    dnest_wrap(&pm[which], par_range_model[which][0], par_range_model[which][1]);
    logH += (-0.5*pow((pm[which] - par_prior_gaussian[which][0])/par_prior_gaussian[which][1], 2.0) );
  }
  else
  {
    pm[which] += dnest_randh() * width;
    dnest_wrap(&(pm[which]), par_range_model[which][0], par_range_model[which][1]);
  }
  return logH;
}

/*!
 * This function print the particle into the file.
 */
void print_particle_con(FILE *fp, const void *model)
{
  int i;
  double *pm = (double *)model;

  for(i=0; i<num_params; i++)
  {
    fprintf(fp, "%e ", pm[i] );
  }
  fprintf(fp, "\n");
}

/*!
 * This function read the particle from the file.
 */
void read_particle_con(FILE *fp, void *model)
{
  int j;
  double *psample = (double *)model;

  for(j=0; j < dnest_num_params; j++)
  {
    if(fscanf(fp, "%lf", psample+j) < 1)
    {
      printf("%f\n", *psample);
      fprintf(stderr, "#Error: Cannot read file %s.\n", options.sample_file);
      exit(0);
    }
  }
  return;
}

void restart_action_con(int iflag)
{

  return;
}

void accept_action_con()
{
  int param;

  param = which_parameter_update;
  /* only update prob when variability parameters are updated. */
  if(param < num_params_var)
  {
    prob_con_particles[which_particle_update] = prob_con_particles_perturb[which_particle_update];
  }
  return;
}

void kill_action_con(int i, int i_copy)
{
  prob_con_particles[i] = prob_con_particles[i_copy];
  return;
}

/*!
 * exam for prior
 */
double log_likelihoods_cal_con_exam(const void *model)
{
  return 0.0;
}


#include "cosmocalc.h"
#include "weaklens.h"

cosmocalcData cosmoData;
weaklensData wlData;

#include <gsl/gsl_errno.h>
void turn_off_gsl_errs(void) {
  gsl_set_error_handler_off();
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"
#include "error_cblas_l2.h"

void
cblas_sspmv (const enum CBLAS_ORDER order, const enum CBLAS_UPLO Uplo,
             const int N, const float alpha, const float *Ap, const float *X,
             const int incX, const float beta, float *Y, const int incY)
{
#define BASE float
#include "source_spmv.h"
#undef BASE
}