/*
 * 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 (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


/*
 * 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;
}


/*

cp ../../../../native_ref/<target binary> libnetlib.so

gcc-mp-4.8 -O3 dgemmtest.c common.c -o dgemmtest -L. -lnetlib -I../../../../netlib/CBLAS
./dgemmtest  > ../../../results/mac_os_x-x86_64-dgemm-CBLAS.csv

gcc-mp-4.8 -O3 dgemmtest.c common.c -o dgemmtest -I/System/Library/Frameworks/vecLib.framework/Headers -framework veclib
./dgemmtest  > ../../../results/mac_os_x-x86_64-dgemm-veclib.csv

gcc-mp-4.8 -O3 dgemmtest.c common.c -o dgemmtest -I/opt/local/include /opt/local/lib/libatlas.a /opt/local/lib/libcblas.a /opt/local/lib/liblapack.a /opt/local/lib/libf77blas.a -lgfortran
./dgemmtest  > ../../../results/mac_os_x-x86_64-dgemm-atlas.csv

gcc-mp-4.8 -O3 dgemmtest.c common.c -o dgemmtest -I../../../../netlib/CBLAS -L/opt/intel/composerxe/mkl/lib -lmkl_rt
export DYLD_LIBRARY_PATH=/opt/intel/composerxe/mkl/lib:/opt/intel/composerxe/lib/
./dgemmtest  > ../../../results/mac_os_x-x86_64-dgemm-mkl.csv

gcc-mp-4.8 -O3 dgemmtest.c cudawrapper.c common.c -o dgemmtest -I../../../../netlib/CBLAS -I/usr/local/cuda/include/ -L/usr/local/cuda/lib -lcublas
export DYLD_LIBRARY_PATH=/usr/local/cuda/lib
./dgemmtest  > ../../../results/mac_os_x-x86_64-dgemm-cuda.csv

CLB=/Users/samuel/Documents/Projects/clBLAS/src
gcc-mp-4.8 -O3 dgemmtest.c clwrapper.c common.c -o dgemmtest -I../../../../netlib/CBLAS -I$CLB -I$CLB/include -L. -lclBLAS  -framework OpenCL
./dgemmtest  > ../../../results/mac_os_x-x86_64-dgemm-clblas.csv

*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <cblas.h>
#include "common.h"

// does AB == C ? If not, complain on stderr
void test(int m, double* a, double *b, double *c) {
	int i, j, k, exact = 0, wrong = 0;
	double diff;
	double* d = calloc(m * m, sizeof(double));
	for (i = 0 ; i < m ; i++) {
		for (j = 0 ; j < m ; j++) {
			for (k = 0 ; k < m ; k++) {
				d[i + j * m] += a[i + k * m] * b[j * m + k];
			}
		}
	}
	for (i = 0 ; i < m ; i++) {
		for (j = 0 ; j < m ; j++) {
			diff = c[i * m + j] - d[i * m + j];
			if (diff != 0.0) {
				exact++;
			}
			if (abs(diff) > 0.000001) {
				wrong++;
			}
		}
	}
	free(d);
	if (wrong > 0) {
		fprintf(stderr, "not exact = %d, wrong = %d\n", exact, wrong);
	}
}

long benchmark(int size) {
    int m = sqrt(size);
	long requestStart, requestEnd;

    double* a = random_array(m * m);
    double* b = random_array(m * m);
    double* c = calloc(m * m, sizeof(double));

	requestStart = currentTimeNanos();

    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, m, m, m, 1, a, m, b, m, 0, c, m);

	requestEnd = currentTimeNanos();

#ifdef __TEST__
	test(m, a, b, c);
#endif

    free(a);
    free(b);
    free(c);

    return (requestEnd - requestStart);
  }

main()
{
	srand(time(NULL));

    double factor = 6.0 / 100.0;
    int i, j;
    for (i = 0 ; i < 10 ; i++) {
        for (j = 1 ; j <= 100 ; j++) {
            int size = (int) pow(10.0, factor * j);
            if (size < 10) continue;
            long took = benchmark(size);
            printf("\"%d\",\"%lu\"\n", size, took);
			fflush(stdout);
        }
    }
}


/*
 * author: Achim Gaedke
 * created: January 2002
 * file: pygsl/src/multiminmodule.c
 * $Id: multimin.c,v 1.6 2008/10/25 20:45:04 schnizer Exp $
 */
#include <pygsl/solver.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_multimin.h>
#include "multiminmodule_doc.h"

const char  * filename = __FILE__;
PyObject *module = NULL;

/*
 * I have two different types to minimize and I am too lazy to implement the same
 * twice. Therefore I use unions and add a flag to the minimizer for the type
 */

/* The Callbacks */
double
PyGSL_multimin_function_f(const gsl_vector* x, void* params)
{
     double result;
     int flag;
     int i;

     FUNC_MESS_BEGIN();
     PyGSL_solver *min_o;
     min_o = (PyGSL_solver *) params;
     assert(PyGSL_solver_check(min_o));
     for(i = 0; i<x->size; i++){
	  DEBUG_MESS(2, "Got a x[%d] of %f", i, gsl_vector_get(x, i));
     }
     assert(min_o->mstatic->n_cbs >= 1);
     flag = PyGSL_function_wrap_On_O(x, min_o->cbs[0], min_o->args, &result,
				     NULL, x->size, __FUNCTION__);
     if (flag!= GSL_SUCCESS){
	  result = gsl_nan();
	  if(min_o->isset == 1) longjmp(min_o->buffer,flag);
     }
     DEBUG_MESS(2, "Got a result of %f", result);
     FUNC_MESS_END();
     return result;
}

void
PyGSL_multimin_function_df(const gsl_vector* x, void* params, gsl_vector *df)
{
     int flag, i;
     PyGSL_solver *min_o;
     min_o = (PyGSL_solver *) params;

     FUNC_MESS_BEGIN();
     assert(PyGSL_solver_check(min_o));
     for(i = 0; i<x->size; i++){
	  DEBUG_MESS(2, "Got a x[%d] of %f", i, gsl_vector_get(x, i));
     }
     assert(min_o->mstatic->n_cbs >= 2);
     flag = PyGSL_function_wrap_Op_On(x, df, min_o->cbs[1], min_o->args,
				      x->size, x->size, __FUNCTION__);
     for(i = 0; i<df->size; i++){
	  DEBUG_MESS(2, "Got df x[%d] of %f", i, gsl_vector_get(df, i));
     }
     if(flag!=GSL_SUCCESS){
	  if(min_o->isset == 1) longjmp(min_o->buffer,flag);
     }
     FUNC_MESS_END();
}

void
PyGSL_multimin_function_fdf(const gsl_vector* x, void* params, double *f, gsl_vector *df)
{
     int flag, i;
     PyGSL_solver *min_o;

     FUNC_MESS_BEGIN();
     min_o = (PyGSL_solver *) params;
     assert(PyGSL_solver_check(min_o));
     for(i = 0; i<x->size; i++){
	  DEBUG_MESS(2, "Got a x[%d] of %f", i, gsl_vector_get(x, i));
     }
     assert(min_o->mstatic->n_cbs >= 3);
     flag = PyGSL_function_wrap_On_O(x, min_o->cbs[2], min_o->args, f,
				     df, x->size, __FUNCTION__);
     DEBUG_MESS(2, "Got a result of %f", *f);
     for(i = 0; i<df->size; i++){
	  DEBUG_MESS(2, "Got df x[%d] of %f", i, gsl_vector_get(df, i));
     }
     if (flag!= GSL_SUCCESS){
	  *f = gsl_nan();
	  if(min_o->isset == 1) longjmp(min_o->buffer,flag);
     }
     FUNC_MESS_END();
     return;
}

static PyObject*
PyGSL_multimin_set_f(PyGSL_solver *self, PyObject *pyargs, PyObject *kw)
{

     PyObject* func = NULL, * args = Py_None, * x = NULL, * steps = NULL;
     PyArrayObject * xa = NULL, * stepsa = NULL;
     int n=0, flag=GSL_EFAILED;
     PyGSL_array_index_t stride_recalc;
     gsl_vector_view gsl_x;
     gsl_vector_view gsl_steps;
     gsl_multimin_function * c_sys = NULL;
     static const char *kwlist[] = {"f", "x0", "args", "steps", NULL};

     FUNC_MESS_BEGIN();
     assert(PyGSL_solver_check(self));
     if (self->solver == NULL) {
	  pygsl_error("Got a NULL Pointer of min.f",  filename, __LINE__ - 3, GSL_EFAULT);
	  return NULL;
     }

     assert(pyargs);
     /* arguments PyFunction, Parameters, start Vector, step Vector */
     if (0==PyArg_ParseTupleAndKeywords(pyargs,kw,"OOOO", (char **)kwlist, &func,
					&x,&args,&steps))
	  return NULL;

     if(!PyCallable_Check(func)){
	  pygsl_error("First argument must be callable",  filename, __LINE__ - 3, GSL_EBADFUNC);
	  return NULL;
     }
     n=self->problem_dimensions[0];
     xa  = PyGSL_vector_check(x, n, PyGSL_DARRAY_INPUT(4), &stride_recalc, NULL);
     if (xa == NULL){
	  PyGSL_add_traceback(module, filename, __FUNCTION__, __LINE__ - 1);
	  goto fail;
     }

     gsl_x = gsl_vector_view_array_with_stride((double *)(xa->data), stride_recalc, xa->dimensions[0]);
     stepsa =  PyGSL_vector_check(steps, n, PyGSL_DARRAY_INPUT(5), &stride_recalc, NULL);
     if (stepsa == NULL){
	  PyGSL_add_traceback(module, filename, __FUNCTION__, __LINE__ - 1);
	  goto fail;
     }
     gsl_steps = gsl_vector_view_array_with_stride((double *)(stepsa->data), stride_recalc, stepsa->dimensions[0]);

     if (self->c_sys != NULL) {
	  /* free the previous function and args */
	  c_sys = self->c_sys;
     } else {
	  /* allocate function space */
	  c_sys=calloc(1, sizeof(gsl_multimin_function));
	  if (c_sys == NULL) {
	       pygsl_error("Could not allocate the object for the minimizer function",
			 filename, __LINE__ - 3, GSL_ENOMEM);
	       goto fail;
	  }
     }
     DEBUG_MESS(3, "Everything allocated args = %p", args);
     /* add new function  and parameters */

     if(PyGSL_solver_func_set(self, args, func, NULL, NULL) != GSL_SUCCESS)
	  goto fail;

     /* initialize the function struct */
     c_sys->n=n;
     c_sys->f=PyGSL_multimin_function_f;
     c_sys->params=(void*)self;

     DEBUG_MESS(3, "Setting jmp buffer isset = % d", self->isset);
     if((flag = setjmp(self->buffer)) == 0){
	  self->isset = 1;
	  flag = gsl_multimin_fminimizer_set(self->solver,c_sys, &gsl_x.vector, &gsl_steps.vector);
	  if(PyGSL_ERROR_FLAG(flag) != GSL_SUCCESS){
	       goto fail;
	  }
     } else {
	  goto fail;
     }
     DEBUG_MESS(4, "Set evaluated. flag = %d", flag);
     self->c_sys = c_sys;
     Py_DECREF(xa);
     Py_DECREF(stepsa);
     self->set_called = 1;

     Py_INCREF(Py_None);
     self->isset = 0;
     FUNC_MESS_END();
     return Py_None;

 fail:
     FUNC_MESS("Fail");
     PyGSL_ERROR_FLAG(flag);
     self->isset = 0;
     Py_XDECREF(xa);
     Py_XDECREF(stepsa);
     return NULL;

}

static PyObject*
PyGSL_multimin_set_fdf(PyGSL_solver *self, PyObject *pyargs, PyObject *kw)
{

     PyObject * f = NULL, * df = NULL, * fdf = NULL, * args = Py_None,
	      * x = NULL;
     PyArrayObject * xa = NULL;
     int n=0, flag=GSL_EFAILED;
     PyGSL_array_index_t stride_recalc=-1;
     double step=0.01, tol=1e-4;
     gsl_vector_view gsl_x;
     gsl_multimin_function_fdf * c_sys;
     static const char *kwlist[] = {"f", "df", "fdf",  "x0", "args", "step", "tol", NULL};

     FUNC_MESS_BEGIN();
     assert(PyGSL_solver_check(self));
     if (self->solver == NULL) {
	  pygsl_error("Got a NULL Pointer of min.fdf", filename, __LINE__ - 3, GSL_EFAULT);
	       return NULL;
	  }

     /* arguments PyFunction, Parameters, start Vector, step Vector */
     if (0==PyArg_ParseTupleAndKeywords(pyargs, kw, "OOOO|Odd", (char **)kwlist,
					&f, &df, &fdf, &x, &args, &step, &tol))
	  return NULL;

     n=self->problem_dimensions[0];
     xa  = PyGSL_vector_check(x, n, PyGSL_DARRAY_INPUT(4), &stride_recalc, NULL);
     if (xa == NULL){
	  PyGSL_add_traceback(module, filename, __FUNCTION__, __LINE__ - 1);
	  goto fail;
     }
     gsl_x = gsl_vector_view_array_with_stride((double *)(xa->data), stride_recalc, xa->dimensions[0]);

     if (self->c_sys != NULL) {
	  /* free the previous function and args */
	  c_sys = self->c_sys;
     } else {
	  /* allocate function space */
	  c_sys=malloc(sizeof(gsl_multimin_function_fdf));
	  if (c_sys==NULL) {
	       pygsl_error("Could not allocate the object for the minimizer function",
			 filename, __LINE__ - 3, GSL_ENOMEM);
	       goto fail;
	  }
     }
     if(PyGSL_solver_func_set(self, args, f, df, fdf) != GSL_SUCCESS)
	  goto fail;

     /* initialize the function struct */
     c_sys->n=n;
     c_sys->f  =PyGSL_multimin_function_f;
     c_sys->df =PyGSL_multimin_function_df;
     c_sys->fdf=PyGSL_multimin_function_fdf;
     c_sys->params=(void*)self;

     if((flag = setjmp(self->buffer)) == 0){
	  self->isset = 1;
	  flag = gsl_multimin_fdfminimizer_set(self->solver, c_sys, &gsl_x.vector, step, tol);
	  if(PyGSL_ERROR_FLAG(flag) != GSL_SUCCESS){
	       goto fail;
	  }
     }else{
	  goto fail;
     }
     self->c_sys = c_sys;
     self->isset = 0;
     Py_DECREF(xa);
     self->set_called = 1;

     Py_INCREF(Py_None);
     FUNC_MESS_END();
     return Py_None;

 fail:
     PyGSL_ERROR_FLAG(flag);
     self->isset = 0;
     Py_XDECREF(xa);
     return NULL;

}

static PyObject*
PyGSL_multimin_dx(PyGSL_solver *self, PyObject *args)
{
     return PyGSL_solver_ret_vec(self, args, (ret_vec)gsl_multimin_fdfminimizer_dx);
}

static PyObject*
PyGSL_multimin_gradient(PyGSL_solver *self, PyObject *args)
{
     return PyGSL_solver_ret_vec(self, args, (ret_vec)gsl_multimin_fdfminimizer_gradient);
}

static PyObject*
PyGSL_multimin_f_x(PyGSL_solver *self, PyObject *args)
{
     return PyGSL_solver_ret_vec(self, args, (ret_vec)gsl_multimin_fminimizer_x);
}

static PyObject*
PyGSL_multimin_fdf_x(PyGSL_solver *self, PyObject *args)
{
     return PyGSL_solver_ret_vec(self, args, (ret_vec)gsl_multimin_fdfminimizer_x);
}

static PyObject*
PyGSL_multimin_f_minimum(PyGSL_solver *self, PyObject *args)
{
     return PyGSL_solver_ret_double(self, args, (double_m_t) gsl_multimin_fminimizer_minimum);
}

static PyObject*
PyGSL_multimin_fdf_minimum(PyGSL_solver *self, PyObject *args)
{
     return PyGSL_solver_ret_double(self, args, (double_m_t) gsl_multimin_fdfminimizer_minimum);
}

static PyObject*
PyGSL_multimin_f_size(PyGSL_solver *self, PyObject *args)
{
     return PyGSL_solver_ret_double(self, args, (double_m_t) gsl_multimin_fminimizer_size);
}

static PyObject*
PyGSL_multimin_test_size_method(PyGSL_solver *self, PyObject *args)
{
     int flag=GSL_EFAILED;
     double epsabs;

     FUNC_MESS_BEGIN();

     if (0==PyArg_ParseTuple(args,"d", &epsabs))
	  return NULL;
     flag = gsl_multimin_test_size(gsl_multimin_fminimizer_size(self->solver), epsabs);
     FUNC_MESS_END();
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}

static PyObject*
PyGSL_multimin_test_gradient_method(PyGSL_solver * self, PyObject *args)
{

     double epsabs;
     int flag;
     FUNC_MESS_BEGIN();

     assert(PyGSL_solver_check(self));
     if (0==PyArg_ParseTuple(args,"d", &epsabs))
	  return NULL;

     flag = gsl_multimin_test_gradient(gsl_multimin_fdfminimizer_gradient(self->solver), epsabs);
     FUNC_MESS_END();
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);

}

static PyMethodDef PyGSL_multimin_fmethods[] = {
     {"x",           (PyCFunction)PyGSL_multimin_f_x,      METH_NOARGS,(char *)multimin_x_doc,      },
     {"minimum",     (PyCFunction)PyGSL_multimin_f_minimum,METH_NOARGS,(char *)multimin_minimum_doc,},
     {"set",      (PyCFunction)PyGSL_multimin_set_f,           METH_VARARGS|METH_KEYWORDS, (char *)multimin_set_f_doc    },
     {"size",     (PyCFunction)PyGSL_multimin_f_size,            METH_NOARGS,  (char *)multimin_size_doc     },
     {"test_size",(PyCFunction)PyGSL_multimin_test_size_method,METH_VARARGS, (char *)multimin_test_size_doc},
     {NULL, NULL, 0, NULL}           /* sentinel */
};

static PyMethodDef PyGSL_multimin_fdfmethods[] = {
     {"x",           (PyCFunction)PyGSL_multimin_fdf_x,      METH_NOARGS,(char *)multimin_x_doc,      },
     {"minimum",     (PyCFunction)PyGSL_multimin_fdf_minimum,METH_NOARGS,(char *)multimin_minimum_doc,},
     {"set",      (PyCFunction)PyGSL_multimin_set_fdf,                 METH_VARARGS|METH_KEYWORDS, (char *)multimin_set_fdf_doc      },
     {"dx",       (PyCFunction)PyGSL_multimin_dx,                      METH_NOARGS,  (char *)multimin_dx_doc           },
     {"gradient", (PyCFunction)PyGSL_multimin_gradient,                METH_NOARGS,  (char *)multimin_gradient_doc     },
     {"test_gradient",(PyCFunction)PyGSL_multimin_test_gradient_method,METH_VARARGS, (char *)multimin_test_gradient_doc},
     {NULL, NULL, 0, NULL}           /* sentinel */
};

const struct _GSLMethods
multimin_f   = { (void_m_t) gsl_multimin_fminimizer_free,
		/* gsl_multimin_fminimizer_restart */  (void_m_t) NULL,
		  (name_m_t) gsl_multimin_fminimizer_name,
		 (int_m_t) gsl_multimin_fminimizer_iterate};

const struct _GSLMethods
multimin_fdf = {(void_m_t) gsl_multimin_fdfminimizer_free,
		(void_m_t) gsl_multimin_fdfminimizer_restart,
		(name_m_t) gsl_multimin_fdfminimizer_name,
		(int_m_t)  gsl_multimin_fdfminimizer_iterate};

static const char multimin_f_type_name[] = "F-MultiMinimizer";
static const char multimin_fdf_type_name[] = "FdF-MultiMinimizer";

const struct _SolverStatic
multimin_solver_f   = {{(void_m_t) gsl_multimin_fminimizer_free,
			(void_m_t) NULL,
			(name_m_t) gsl_multimin_fminimizer_name,
			(int_m_t) gsl_multimin_fminimizer_iterate},
		       1, PyGSL_multimin_fmethods, multimin_f_type_name},
multimin_solver_fdf = {{(void_m_t) gsl_multimin_fdfminimizer_free,
			(void_m_t) gsl_multimin_fdfminimizer_restart,
			(name_m_t) gsl_multimin_fdfminimizer_name,
			(int_m_t)  gsl_multimin_fdfminimizer_iterate},
		       3, PyGSL_multimin_fdfmethods, multimin_fdf_type_name};

static PyObject*
PyGSL_multimin_f_init(PyObject *self, PyObject *args,
		      const gsl_multimin_fminimizer_type * type)
{

     PyObject *tmp=NULL;
     solver_alloc_struct s = {type, (void_an_t) gsl_multimin_fminimizer_alloc,
			      &multimin_solver_f};
     FUNC_MESS_BEGIN();
     tmp = PyGSL_solver_dn_init(self, args, &s, 1);
     FUNC_MESS_END();
     return tmp;
}

static PyObject*
PyGSL_multimin_fdf_init(PyObject *self, PyObject *args,
		      const gsl_multimin_fdfminimizer_type * type)
{

     PyObject *tmp=NULL;
     solver_alloc_struct s = {type, (void_an_t) gsl_multimin_fdfminimizer_alloc,
			      &multimin_solver_fdf};
     FUNC_MESS_BEGIN();
     tmp = PyGSL_solver_dn_init(self, args, &s, 1);
     FUNC_MESS_END();
     return tmp;
}

#define AMINIMIZER_F(name)                                                  \
static PyObject* PyGSL_multimin_init_ ## name (PyObject *self, PyObject *args)\
{                                                                             \
     PyObject *tmp = NULL;                                                    \
     FUNC_MESS_BEGIN();                                                       \
     tmp = PyGSL_multimin_f_init(self, args,  gsl_multimin_fminimizer_ ## name); \
     if (tmp == NULL){                                                        \
	  PyGSL_add_traceback(module, __FILE__, __FUNCTION__, __LINE__); \
     }                                                                        \
     FUNC_MESS_END();                                                         \
     return tmp;                                                              \
}

#define AMINIMIZER_FDF(name)                                                  \
static PyObject* PyGSL_multimin_init_ ## name (PyObject *self, PyObject *args)\
{                                                                             \
     PyObject *tmp = NULL;                                                    \
     FUNC_MESS_BEGIN();                                                       \
     tmp = PyGSL_multimin_fdf_init(self, args,  gsl_multimin_fdfminimizer_ ## name); \
     if (tmp == NULL){                                                        \
	  PyGSL_add_traceback(module, __FILE__, __FUNCTION__, __LINE__); \
     }                                                                        \
     FUNC_MESS_END();                                                         \
     return tmp;                                                              \
}

AMINIMIZER_F(nmsimplex)
AMINIMIZER_FDF(steepest_descent)
AMINIMIZER_FDF(vector_bfgs)
AMINIMIZER_FDF(conjugate_pr)
AMINIMIZER_FDF(conjugate_fr)

static PyObject*
PyGSL_multimin_test_size(PyObject * self, PyObject *args)
{
     double size, epsabs;
     int flag = GSL_EFAILED;
     FUNC_MESS_BEGIN();
     if (0==PyArg_ParseTuple(args,"dd", &size, &epsabs))
	  return NULL;
     flag = gsl_multimin_test_size(size, epsabs);
     FUNC_MESS_END();
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);

}

static PyObject*
PyGSL_multimin_test_gradient(PyObject * self, PyObject *args)
{
     return PyGSL_solver_vd_i (self, args, gsl_multimin_test_gradient);
}

static PyMethodDef mMethods[] = {
     /* solvers */
     {"nmsimplex",        PyGSL_multimin_init_nmsimplex,        METH_VARARGS, (char *)nmsimplex_doc       },
     {"steepest_descent", PyGSL_multimin_init_steepest_descent, METH_VARARGS, (char *)steepest_descent_doc},
     {"vector_bfgs",      PyGSL_multimin_init_vector_bfgs,      METH_VARARGS, (char *)vector_bfgs_doc     },
     {"conjugate_pr",     PyGSL_multimin_init_conjugate_pr,     METH_VARARGS, (char *)conjugate_pr_doc    },
     {"conjugate_fr",     PyGSL_multimin_init_conjugate_fr,     METH_VARARGS, (char *)conjugate_fr_doc    },
     /* multimin funcs */
     {"test_size",        PyGSL_multimin_test_size,             METH_VARARGS, (char *)test_size_doc       },
     {"test_gradient",    PyGSL_multimin_test_gradient,         METH_VARARGS, (char *)test_gradient_doc   },
     {NULL, NULL, 0, NULL}
};

void
initmultimin(void)
{
     PyObject* m, *dict, *item;
     FUNC_MESS_BEGIN();

     m=Py_InitModule("multimin", mMethods);
     module = m;
     assert(m);
     dict = PyModule_GetDict(m);
     if(!dict)
	  goto fail;

     init_pygsl()
     import_pygsl_solver();
     assert(PyGSL_API);

     if (!(item = PyString_FromString((char*)PyGSL_multimin_module_doc))){
	  PyErr_SetString(PyExc_ImportError,
			  "I could not generate module doc string!");
	  goto fail;
     }
     if (PyDict_SetItemString(dict, "__doc__", item) != 0){
	  PyErr_SetString(PyExc_ImportError,
			  "I could not init doc string!");
	  goto fail;
     }

     FUNC_MESS_END();

 fail:
     FUNC_MESS("FAIL");
     return;
}