/*
 * C version of Diffusive Nested Sampling (DNest4) by Brendon J. Brewer
 *
 * Yan-Rong Li, liyanrong@mail.ihep.ac.cn
 * Jun 30, 2016
 *
 */

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

#include "dnestvars.h"

double dnest(int argc, char** argv, DNestFptrSet *fptrset, int num_params,
             char *sample_dir, int max_num_saves, double ptol, const void *arg)
{
  int opt;

  dnest_arg = arg;

  dnest_check_fptrset(fptrset);

  // cope with argv

  dnest_post_temp = 1.0;
  dnest_flag_restart = 0;
  dnest_flag_postprc = 0;
  dnest_flag_sample_info = 0;
  dnest_flag_limits = 0;

  strcpy(file_save_restart, "restart_dnest.txt");
  strcpy(dnest_sample_postfix, "\0");
  strcpy(dnest_sample_tag, "\0");

  opterr = 0;
  optind = 0;
  while( (opt = getopt(argc, argv, "r:s:pt:clx:g:")) != -1)
  {
    switch(opt)
    {
      case 'r':
        dnest_flag_restart = 1;
        strcpy(file_restart, optarg);
        printf("# Dnest restarts.\n");
        break;
      case 's':
        strcpy(file_save_restart, optarg);
        //printf("# Dnest sets restart file %s.\n", file_save_restart);
        break;
      case 'p':
        dnest_flag_postprc = 1;
        dnest_post_temp = 1.0;
        printf("# Dnest does postprocess.\n");
        break;
      case 't':
        dnest_post_temp = atof(optarg);
        printf("# Dnest sets a temperature %f.\n", dnest_post_temp);
        if(dnest_post_temp == 0.0)
        {
          printf("# Dnest incorrect option -t %s.\n", optarg);
          exit(0);
        }
        if(dnest_post_temp < 1.0)
        {
          printf("# Dnest temperature should >= 1.0\n");
          exit(0);
        }
        break;
      case 'c':
        dnest_flag_sample_info = 1;
        printf("# Dnest recalculates sample information.\n");
        break;
      case 'l':
        dnest_flag_limits = 1;
        printf("# Dnest level-dependent sampling.\n");
        break;
      case 'x':
        strcpy(dnest_sample_postfix, optarg);
        printf("# Dnest sets sample postfix %s.\n", dnest_sample_postfix);
        break;
      case 'g':
        strcpy(dnest_sample_tag, optarg);
        printf("# Dnest sets sample tag %s.\n", dnest_sample_tag);
        break;
      case '?':
        printf("# Dnest incorrect option -%c %s.\n", optopt, optarg);
        exit(0);
        break;
      default:
        break;
    }
  }

  setup(argc, argv, fptrset, num_params, sample_dir, max_num_saves, ptol);

  if(dnest_flag_postprc == 1)
  {
    dnest_postprocess(dnest_post_temp, max_num_saves, ptol);
    finalise();
    return post_logz;
  }

  if(dnest_flag_sample_info == 1)
  {
    dnest_postprocess(dnest_post_temp, max_num_saves, ptol);
    finalise();
    return post_logz;
  }

  if(dnest_flag_restart==1)
    dnest_restart();

  initialize_output_file();
  dnest_run();
  close_output_file();

  dnest_postprocess(dnest_post_temp, max_num_saves, ptol);

  finalise();

  return post_logz;
}

// postprocess, calculate evidence, generate posterior sample.
void dnest_postprocess(double temperature, int max_num_saves, double ptol)
{
  options_load(max_num_saves, ptol);
  postprocess(temperature);
}

void dnest_run()
{
  int i, j, k, size_all_above_incr;
  Level *pl, *levels_orig;
  int *buf_size_above, *buf_displs;
  double *plimits;

  printf("# Start diffusive nested sampling.\n");

  while(true)
  {
    //check for termination
    if(options.max_num_saves !=0 &&
        count_saves != 0 && (count_saves%options.max_num_saves == 0))
      break;

    dnest_mcmc_run();

    count_mcmc_steps += options.thread_steps;

    if(dnest_flag_limits == 1)
    {
      // limits of smaller levels should be larger than those of higher levels
      for(j=size_levels-2; j >= 0; j--)
        for(k=0; k<particle_offset_double; k++)
        {
          limits[ j * particle_offset_double *2 + k*2 ] = fmin( limits[ j * particle_offset_double *2 + k*2 ],
                  limits[ (j+1) * particle_offset_double *2 + k*2 ] );
          limits[ j * particle_offset_double *2 + k*2 + 1] = fmax( limits[ j * particle_offset_double *2 + k*2 +1 ],
                  limits[ (j+1) * particle_offset_double *2 + k*2 + 1 ] );
        }
    }

    do_bookkeeping();

    if(count_mcmc_steps >= (count_saves + 1)*options.save_interval)
    {
      save_particle();

      // save levels, limits, sync samples when running a number of steps
      if( count_saves % num_saves == 0 )
      {
        if(size_levels <= options.max_num_levels)
        {
          save_levels();

          printf("# Save levels at N= %d.\n", count_saves);
        }
        if(dnest_flag_limits == 1)
          save_limits();
        fflush(fsample_info);
        fsync(fileno(fsample_info));
        fflush(fsample);
        fsync(fileno(fsample));
        printf("# Save limits, and sync samples at N= %d.\n", count_saves);
      }

      //if( count_saves % num_saves_restart == 0 )
      //{
      //  dnest_save_restart();
      //}
    }
  }

  //dnest_save_restart();

  //save levels
  save_levels();
  if(dnest_flag_limits == 1)
    save_limits();

  /* output state of sampler */
  FILE *fp;
  fp = fopen(options.sampler_state_file, "w");
  fprintf(fp, "%d %d\n", size_levels, count_saves);
  fclose(fp);
}

void do_bookkeeping()
{
  int i;
  //bool created_level = false;

  if(!enough_levels(levels, size_levels) && size_above >= options.new_level_interval)
  {
    // in descending order
    qsort(above, size_above, sizeof(LikelihoodType), dnest_cmp);
    int index = (int)( (1.0/compression) * size_above);

    Level level_tmp = {above[index], 0.0, 0, 0, 0, 0};
    levels[size_levels] = level_tmp;
    size_levels++;

    printf("# Creating level %d with log likelihood = %e.\n",
               size_levels-1, levels[size_levels-1].log_likelihood.value);

    // clear out the last index records
    for(i=index; i<size_above; i++)
    {
      above[i].value = 0.0;
      above[i].tiebreaker = 0.0;
    }
    size_above = index;

    if(enough_levels(levels, size_levels))
    {
      renormalise_visits();
      options.max_num_levels = size_levels;
      printf("# Done creating levles.\n");
    }
    else
    {
      kill_lagging_particles();
    }
  }
  recalculate_log_X();
}

void recalculate_log_X()
{
  int i;

  levels[0].log_X = 0.0;
  for(i=1; i<size_levels; i++)
  {
    levels[i].log_X = levels[i-1].log_X
    + log( (double)( (levels[i-1].exceeds + 1.0/compression * regularisation)
                    /(levels[i-1].visits + regularisation)  ) );
  }
}

void renormalise_visits()
{
  size_t i;

  for(i=0; i<size_levels; i++)
  {
    if(levels[i].tries >= regularisation)
    {
      levels[i].accepts = ((double)(levels[i].accepts+1) / (double)(levels[i].tries+1)) * regularisation;
      levels[i].tries = regularisation;
    }

    if(levels[i].visits >= regularisation)
    {
      levels[i].exceeds = ( (double) (levels[i].exceeds+1) / (double)(levels[i].visits + 1) ) * regularisation;
      levels[i].visits = regularisation;
    }
  }
}

void kill_lagging_particles()
{
  static unsigned int deletions = 0;

  bool *good;
  good = (bool *)malloc(options.num_particles * sizeof(bool));

  double max_log_push = -DBL_MAX;

  double kill_probability = 0.0;
  unsigned int num_bad = 0;
  size_t i;

  for(i=0; i<options.num_particles; i++)good[i] = true;

  for(i=0; i<options.num_particles; i++)
  {
    if( log_push(level_assignments[i]) > max_log_push)
      max_log_push = log_push(level_assignments[i]);

    kill_probability = pow(1.0 - 1.0/(1.0 + exp(-log_push(level_assignments[i]) - 4.0)), 3);
    if(gsl_rng_uniform(dnest_gsl_r) <= kill_probability)
    {
      good[i] = false;
      ++num_bad;
    }
  }

  if(num_bad < options.num_particles)
  {
    for(i=0; i< options.num_particles; i++)
    {
      if(!good[i])
      {
        int i_copy;
        do
        {
          i_copy = gsl_rng_uniform_int(dnest_gsl_r, options.num_particles);
        }while(!good[i_copy] || gsl_rng_uniform(dnest_gsl_r) >= exp(log_push(level_assignments[i_copy]) - max_log_push));

        memcpy(particles+i*particle_offset_size, particles + i_copy*particle_offset_size, dnest_size_of_modeltype);
        log_likelihoods[i] = log_likelihoods[i_copy];
        level_assignments[i] = level_assignments[i_copy];

        kill_action(i, i_copy);

        deletions++;

        printf("# Replacing lagging particle.\n");
        printf("# This has happened %d times.\n", deletions);
      }
    }
  }
  else
    printf("# Warning: all particles lagging!.\n");

  free(good);
}

/* save levels */
void save_levels()
{
  if(!save_to_disk)
    return;

  int i;
  FILE *fp;

  fp = fopen(options.levels_file, "w");
  fprintf(fp, "# log_X, log_likelihood, tiebreaker, accepts, tries, exceeds, visits\n");
  for(i=0; i<size_levels; i++)
  {
    fprintf(fp, "%14.12g %14.12g %f %llu %llu %llu %llu\n", levels[i].log_X, levels[i].log_likelihood.value,
      levels[i].log_likelihood.tiebreaker, levels[i].accepts,
      levels[i].tries, levels[i].exceeds, levels[i].visits);
  }
  fclose(fp);

  /* update state of sampler */
  fp = fopen(options.sampler_state_file, "w");
  fprintf(fp, "%d %d\n", size_levels, count_saves);
  fclose(fp);
}

void save_limits()
{
  int i, j;
  FILE *fp;

  fp = fopen(options.limits_file, "w");
  for(i=0; i<size_levels; i++)
  {
    fprintf(fp, "%d  ", i);
    for(j=0; j<particle_offset_double; j++)
      fprintf(fp, "%f  %f  ", limits[i*2*particle_offset_double+j*2], limits[i*2*particle_offset_double+j*2+1]);

    fprintf(fp, "\n");
  }
  fclose(fp);
}

/* save particle */
void save_particle()
{
  count_saves++;

  if(!save_to_disk)
    return;

  int whichparticle, whichtask;
  void *particle_message;

  if(count_saves%10 == 0)
    printf("#[%.1f%%] Saving sample N= %d.\n", 100.0*count_saves/options.max_num_saves, count_saves);

  whichparticle =  gsl_rng_uniform_int(dnest_gsl_r,options.num_particles);

  print_particle(fsample, particles + whichparticle * particle_offset_size, dnest_arg);

  fprintf(fsample_info, "%d %e %f %d\n", level_assignments[whichparticle],
        log_likelihoods[whichparticle].value,
        log_likelihoods[whichparticle].tiebreaker,
        whichparticle);
}

void dnest_mcmc_run()
{
  unsigned int which;
  unsigned int i;

  for(i = 0; i<options.thread_steps; i++)
  {

    /* randomly select out one particle to update */
    which = gsl_rng_uniform_int(dnest_gsl_r, options.num_particles);

    dnest_which_particle_update = which;

    //if(count_mcmc_steps >= 10000)printf("FFFF\n");
    //printf("%d\n", which);
    //printf("%f %f %f\n", particles[which].param[0], particles[which].param[1], particles[which].param[2]);
    //printf("level:%d\n", level_assignments[which]);
    //printf("%e\n", log_likelihoods[which].value);

    if(gsl_rng_uniform(dnest_gsl_r) <= 0.5)
    {
      update_particle(which);
      update_level_assignment(which);
    }
    else
    {
      update_level_assignment(which);
      update_particle(which);
    }

    if( !enough_levels(levels, size_levels)  && levels[size_levels-1].log_likelihood.value < log_likelihoods[which].value)
    {
      above[size_above] = log_likelihoods[which];
      size_above++;
    }
  }
}

void update_particle(unsigned int which)
{
  void *particle = particles+ which*particle_offset_size;
  LikelihoodType *logl = &(log_likelihoods[which]);

  Level *level = &(levels[level_assignments[which]]);

  void *proposal = (void *)malloc(dnest_size_of_modeltype);
  LikelihoodType logl_proposal;
  double log_H;

  memcpy(proposal, particle, dnest_size_of_modeltype);
  dnest_which_level_update = level_assignments[which];

  log_H = perturb(proposal, dnest_arg);

  logl_proposal.value = log_likelihoods_cal(proposal, dnest_arg);
  logl_proposal.tiebreaker =  (*logl).tiebreaker + gsl_rng_uniform(dnest_gsl_r);
  dnest_wrap(&logl_proposal.tiebreaker, 0.0, 1.0);

  if(log_H > 0.0)
    log_H = 0.0;

  dnest_perturb_accept[which] = 0;
  if( gsl_rng_uniform(dnest_gsl_r) <= exp(log_H) && level->log_likelihood.value < logl_proposal.value)
  {
    memcpy(particle, proposal, dnest_size_of_modeltype);
    memcpy(logl, &logl_proposal, sizeof(LikelihoodType));
    level->accepts++;

    dnest_perturb_accept[which] = 1;
    accept_action();
    account_unaccepts[which] = 0; /* reset the number of unaccepted perturb */
  }
  else
  {
    account_unaccepts[which] += 1; /* number of unaccepted perturb */
  }
  level->tries++;

  unsigned int current_level = level_assignments[which];
  for(; current_level < size_levels-1; ++current_level)
  {
    levels[current_level].visits++;
    if(levels[current_level+1].log_likelihood.value < log_likelihoods[which].value)
      levels[current_level].exceeds++;
    else
      break; // exit the loop if it does not satify higher levels
  }
  free(proposal);
}

void update_level_assignment(unsigned int which)
{
  int i;

  int proposal = level_assignments[which]
                 + (int)( pow(10.0, 2*gsl_rng_uniform(dnest_gsl_r))*gsl_ran_ugaussian(dnest_gsl_r));

  if(proposal == level_assignments[which])
    proposal =  ((gsl_rng_uniform(dnest_gsl_r) < 0.5)?(proposal-1):(proposal+1));

  proposal=mod_int(proposal, size_levels);

  double log_A = -levels[proposal].log_X + levels[level_assignments[which]].log_X;

  log_A += log_push(proposal) - log_push(level_assignments[which]);

  if(size_levels == options.max_num_levels)
    log_A += options.beta*log( (double)(levels[level_assignments[which]].tries +1)/ (levels[proposal].tries +1) );

  if(log_A > 0.0)
    log_A = 0.0;

  if( gsl_rng_uniform(dnest_gsl_r) <= exp(log_A) && levels[proposal].log_likelihood.value < log_likelihoods[which].value)
  {
    level_assignments[which] = proposal;

// update the limits of the level
    if(dnest_flag_limits == 1)
    {
      double *particle = (double *) (particles+ which*particle_offset_size);
      for(i=0; i<particle_offset_double; i++)
      {
        limits[proposal * 2 * particle_offset_double +  i*2] =
            fmin(limits[proposal * 2* particle_offset_double +  i*2], particle[i]);
        limits[proposal * 2 * particle_offset_double +  i*2+1] =
            fmax(limits[proposal * 2 * particle_offset_double +  i*2+1], particle[i]);
      }
    }

  }

}

double log_push(unsigned int which_level)
{
  if(which_level > size_levels)
  {
    printf("level overflow %d %d.\n", which_level, size_levels);
    exit(0);
  }
  if(enough_levels(levels, size_levels))
    return 0.0;

  int i = which_level - (size_levels - 1);
  return ((double)i)/options.lambda;
}

bool enough_levels(Level *l, int size_l)
{
  int i;

  if(options.max_num_levels == 0)
  {
    if(size_l >= LEVEL_NUM_MAX)
      return true;

    if(size_l < 10)
      return false;

    int num_levels_to_check = 20;
    if(size_l > 80)
      num_levels_to_check = (int)(sqrt(20) * sqrt(0.25*size_l));

    int k = size_l - 1, kc = 0;
    double tot = 0.0;
    double max = -DBL_MAX;
    double diff;

    for(i= 0; i<num_levels_to_check; i++)
    {
      diff = l[k].log_likelihood.value - l[k-1].log_likelihood.value;
      tot += diff;
      if(diff > max)
        max = diff;

      k--;
      kc++;
      if( k < 1 )
        break;
    }
    if(tot/kc < options.max_ptol && max < options.max_ptol*1.1)
      return true;
    else
      return false;
  }
  return (size_l >= options.max_num_levels);
}

void initialize_output_file()
{
  if(dnest_flag_restart !=1)
    fsample = fopen(options.sample_file, "w");
  else
    fsample = fopen(options.sample_file, "a");

  if(fsample==NULL)
  {
    fprintf(stderr, "# Cannot open file sample.txt.\n");
    exit(0);
  }
  if(dnest_flag_restart != 1)
    fprintf(fsample, "# \n");

  if(dnest_flag_restart != 1)
    fsample_info = fopen(options.sample_info_file, "w");
  else
    fsample_info = fopen(options.sample_info_file, "a");

  if(fsample_info==NULL)
  {
    fprintf(stderr, "# Cannot open file %s.\n", options.sample_info_file);
    exit(0);
  }
  if(dnest_flag_restart != 1)
    fprintf(fsample_info, "# level assignment, log likelihood, tiebreaker, ID.\n");
}

void close_output_file()
{
  fclose(fsample);
  fclose(fsample_info);
}

void setup(int argc, char** argv, DNestFptrSet *fptrset, int num_params, char *sample_dir, int max_num_saves, double ptol)
{
  int i, j;

  // root task.
  dnest_root = 0;

  // setup function pointers
  from_prior = fptrset->from_prior;
  log_likelihoods_cal = fptrset->log_likelihoods_cal;
  log_likelihoods_cal_initial = fptrset->log_likelihoods_cal_initial;
  log_likelihoods_cal_restart = fptrset->log_likelihoods_cal_restart;
  perturb = fptrset->perturb;
  print_particle = fptrset->print_particle;
  read_particle = fptrset->read_particle;
  restart_action = fptrset->restart_action;
  accept_action = fptrset->accept_action;
  kill_action = fptrset->kill_action;
  strcpy(dnest_sample_dir, sample_dir);

  // random number generator
  dnest_gsl_T = (gsl_rng_type *) gsl_rng_default;
  dnest_gsl_r = gsl_rng_alloc (dnest_gsl_T);
#ifndef Debug
  gsl_rng_set(dnest_gsl_r, time(NULL));
#else
  gsl_rng_set(dnest_gsl_r, 9999);
  printf("# debugging, dnest random seed %d\n", 9999);
#endif

  dnest_num_params = num_params;
  dnest_size_of_modeltype = dnest_num_params * sizeof(double);

  // read options
  options_load(max_num_saves, ptol);

  //dnest_post_temp = 1.0;
  compression = exp(1.0);
  regularisation = options.new_level_interval*sqrt(options.lambda);
  save_to_disk = true;

  // particles
  particle_offset_size = dnest_size_of_modeltype/sizeof(void);
  particle_offset_double = dnest_size_of_modeltype/sizeof(double);
  particles = (void *)malloc(options.num_particles*dnest_size_of_modeltype);

  // initialise sampler
  above = (LikelihoodType *)malloc(2*options.new_level_interval * sizeof(LikelihoodType));

  log_likelihoods = (LikelihoodType *)malloc(2*options.num_particles * sizeof(LikelihoodType));
  level_assignments = (unsigned int*)malloc(options.num_particles * sizeof(unsigned int));

  account_unaccepts = (unsigned int *)malloc(options.num_particles * sizeof(unsigned int));
  for(i=0; i<options.num_particles; i++)
  {
    account_unaccepts[i] = 0;
  }

  if(options.max_num_levels != 0)
  {
    levels = (Level *)malloc(options.max_num_levels * sizeof(Level));
    if(dnest_flag_limits == 1)
    {
      limits = malloc(options.max_num_levels * particle_offset_double * 2 * sizeof(double));
      for(i=0; i<options.max_num_levels; i++)
      {
        for(j=0; j<particle_offset_double; j++)
        {
          limits[i*2*particle_offset_double+ j*2] = DBL_MAX;
          limits[i*2*particle_offset_double + j*2 + 1] = -DBL_MAX;
        }
      }
    }
  }
  else
  {
    levels = (Level *)malloc(LEVEL_NUM_MAX * sizeof(Level));

    if(dnest_flag_limits == 1)
    {
      limits = malloc(LEVEL_NUM_MAX * particle_offset_double * 2 * sizeof(double));
      for(i=0; i<LEVEL_NUM_MAX; i++)
      {
        for(j=0; j<particle_offset_double; j++)
        {
          limits[i*2*particle_offset_double + j*2] = DBL_MAX;
          limits[i*2*particle_offset_double + j*2 + 1] = -DBL_MAX;
        }
      }
    }
  }

  dnest_perturb_accept = malloc(options.num_particles * sizeof(int));
  for(i=0; i<options.num_particles; i++)
  {
    dnest_perturb_accept[i] = 0;
  }

  count_mcmc_steps = 0;
  count_saves = 0;
  num_saves = (int)fmax(0.02*options.max_num_saves, 1.0);
  num_saves_restart = (int)fmax(0.2 * options.max_num_saves, 1.0);

// first level
  size_levels = 0;
  size_above = 0;
  LikelihoodType like_tmp = {-DBL_MAX, gsl_rng_uniform(dnest_gsl_r)};
  Level level_tmp = {like_tmp, 0.0, 0, 0, 0, 0};
  levels[size_levels] = level_tmp;
  size_levels++;

  for(i=0; i<options.num_particles; i++)
  {
    dnest_which_particle_update = i;
    dnest_which_level_update = 0;
    from_prior(particles+i*particle_offset_size, dnest_arg);
    log_likelihoods[i].value = log_likelihoods_cal_initial(particles+i*particle_offset_size, dnest_arg);
    log_likelihoods[i].tiebreaker = dnest_rand();
    level_assignments[i] = 0;
  }

  /*ModelType proposal;
  printf("%f %f %f \n", particles[0].param[0], particles[0].param[1], particles[0].param[2] );
  proposal = particles[0];
  printf("%f %f %f \n", proposal.param[0], proposal.param[1], proposal.param[2] );*/
}

void finalise()
{
  free(particles);
  free(above);
  free(log_likelihoods);
  free(level_assignments);
  free(levels);

  free(account_unaccepts);

  if(dnest_flag_limits == 1)
    free(limits);

  gsl_rng_free(dnest_gsl_r);

  free(dnest_perturb_accept);

  printf("# Finalizing dnest.\n");
}

void options_load(int max_num_saves, double ptol)
{
  //sscanf(buf, "%d", &options.num_particles);
  options.num_particles = 2;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%d", &options.new_level_interval);
  options.new_level_interval = options.num_particles * dnest_num_params*10;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%d", &options.save_interval);
  options.save_interval = options.new_level_interval;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%d", &options.thread_steps);
  options.thread_steps = options.new_level_interval;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%d", &options.max_num_levels);
  options.max_num_levels = 0;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%lf", &options.lambda);
  options.lambda = 10.0;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%lf", &options.beta);
  options.beta = 100.0;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%d", &options.max_num_saves);
  options.max_num_saves = max_num_saves;

  options.max_ptol = ptol;

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%s", options.sample_file);
  strcpy(options.sample_file, dnest_sample_dir);
  strcat(options.sample_file,"/sample");
  strcat(options.sample_file, dnest_sample_tag);
  strcat(options.sample_file, ".txt");
  strcat(options.sample_file, dnest_sample_postfix);

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%s", options.sample_info_file);
  strcpy(options.sample_info_file, dnest_sample_dir);
  strcat(options.sample_info_file,"/sample_info");
  strcat(options.sample_info_file, dnest_sample_tag);
  strcat(options.sample_info_file, ".txt");
  strcat(options.sample_info_file, dnest_sample_postfix);

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%s", options.levels_file);
  strcpy(options.levels_file, dnest_sample_dir);
  strcat(options.levels_file,"/levels");
  strcat(options.levels_file, dnest_sample_tag);
  strcat(options.levels_file, ".txt");
  strcat(options.levels_file, dnest_sample_postfix);

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%s", options.sampler_state_file);
  strcpy(options.sampler_state_file, dnest_sample_dir);
  strcat(options.sampler_state_file,"/sampler_state");
  strcat(options.sampler_state_file, dnest_sample_tag);
  strcat(options.sampler_state_file, ".txt");
  strcat(options.sampler_state_file, dnest_sample_postfix);

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%s", options.posterior_sample_file);
  strcpy(options.posterior_sample_file, dnest_sample_dir);
  strcat(options.posterior_sample_file,"/posterior_sample");
  strcat(options.posterior_sample_file, dnest_sample_tag);
  strcat(options.posterior_sample_file, ".txt");
  strcat(options.posterior_sample_file, dnest_sample_postfix);

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%s", options.posterior_sample_info_file);
  strcpy(options.posterior_sample_info_file, dnest_sample_dir);
  strcat(options.posterior_sample_info_file,"/posterior_sample_info");
  strcat(options.posterior_sample_info_file, dnest_sample_tag);
  strcat(options.posterior_sample_info_file, ".txt");
  strcat(options.posterior_sample_info_file, dnest_sample_postfix);

  //fgets(buf, BUF_MAX_LENGTH, fp);
  //sscanf(buf, "%s", options.limits_file);
  strcpy(options.limits_file, dnest_sample_dir);
  strcat(options.limits_file,"/limits");
  strcat(options.limits_file, dnest_sample_tag);
  strcat(options.limits_file, ".txt");
  strcat(options.limits_file, dnest_sample_postfix);

  // check options.

  if(options.new_level_interval < options.thread_steps)
  {
    printf("# incorrect options:\n");
    printf("# new level interval should be equal to or larger than");
    printf("  totaltask * thread step.\n");
    exit(0);
  }

/*  strcpy(options.sample_file, "sample.txt");
  strcpy(options.sample_info_file, "sample_info.txt");
  strcpy(options.levels_file, "levels.txt");
  strcpy(options.sampler_state_file, "sampler_state.txt");*/
}

double mod(double y, double x)
{
  if(x > 0.0)
  {
    return (y/x - floor(y/x))*x;
  }
  else if(x == 0.0)
  {
    return 0.0;
  }
  else
  {
    printf("Warning in mod(double, double) %e\n", x);
    exit(0);
  }

}

void dnest_wrap(double *x, double min, double max)
{
  *x = mod(*x - min, max - min) + min;
}

void wrap_limit(double *x, double min, double max)
{

  *x = fmax(fmin(*x, max), min);
}

int mod_int(int y, int x)
{
  if(y >= 0)
    return y - (y/x)*x;
  else
    return (x-1) - mod_int(-y-1, x);
}

double dnest_randh()
{
  return pow(10.0, 1.5 - 3.0*fabs(gsl_ran_tdist(dnest_gsl_r, 2))) * gsl_ran_ugaussian(dnest_gsl_r);
}

double dnest_rand()
{
  return gsl_rng_uniform(dnest_gsl_r);
}

int dnest_rand_int(int size)
{
  return gsl_rng_uniform_int(dnest_gsl_r, size);
}

double dnest_randn()
{
  return gsl_ran_ugaussian(dnest_gsl_r);
}

int dnest_cmp(const void *pa, const void *pb)
{
  LikelihoodType *a = (LikelihoodType *)pa;
  LikelihoodType *b = (LikelihoodType *)pb;

  // in decesending order
  if(a->value > b->value)
    return false;
  if( a->value == b->value && a->tiebreaker > b->tiebreaker)
    return false;

  return true;
}

int dnest_get_size_levels()
{
  return size_levels;
}

int dnest_get_which_level_update()
{
  return dnest_which_level_update;
}

int dnest_get_which_particle_update()
{
  return dnest_which_particle_update;
}

unsigned int dnest_get_which_num_saves()
{
  return num_saves;
}
unsigned int dnest_get_count_saves()
{
  return count_saves;
}

unsigned long long int dnest_get_count_mcmc_steps()
{
  return count_mcmc_steps;
}

void dnest_get_posterior_sample_file(char *fname)
{
  strcpy(fname, options.posterior_sample_file);
  return;
}
/*
 * version check
 *
 *  1: greater
 *  0: equal
 * -1: lower
 */
int dnest_check_version(char *version_str)
{
  int major, minor, patch;

  sscanf(version_str, "%d.%d.%d", &major, &minor, &patch);

  if(major > DNEST_MAJOR_VERSION)
    return 1;
  if(major < DNEST_MAJOR_VERSION)
    return -1;

  if(minor > DNEST_MINOR_VERSION)
    return 1;
  if(minor < DNEST_MINOR_VERSION)
    return -1;

  if(patch > DNEST_PATCH_VERSION)
    return 1;
  if(patch > DNEST_PATCH_VERSION)
    return -1;

  return 0;
}

void dnest_check_fptrset(DNestFptrSet *fptrset)
{
  if(fptrset->from_prior == NULL)
  {
    printf("\"from_prior\" function is not defined.\n");
    exit(0);
  }

  if(fptrset->print_particle == NULL)
  {
    //printf("\"print_particle\" function is not defined. \
    //  \nSet to be default function in dnest.\n");
    fptrset->print_particle = dnest_print_particle;
  }

  if(fptrset->read_particle == NULL)
  {
    //printf("\"read_particle\" function is not defined. \
    //  \nSet to be default function in dnest.\n");
    fptrset->read_particle = dnest_read_particle;
  }

  if(fptrset->log_likelihoods_cal == NULL)
  {
    printf("\"log_likelihoods_cal\" function is not defined.\n");
    exit(0);
  }

  if(fptrset->log_likelihoods_cal_initial == NULL)
  {
    //printf("\"log_likelihoods_cal_initial\" function is not defined. \
    //  \nSet to the same as \"log_likelihoods_cal\" function.\n");
    fptrset->log_likelihoods_cal_initial = fptrset->log_likelihoods_cal;
  }

  if(fptrset->log_likelihoods_cal_restart == NULL)
  {
    //printf("\"log_likelihoods_cal_restart\" function is not defined. \
    //  \nSet to the same as \"log_likelihoods_cal\" function.\n");
    fptrset->log_likelihoods_cal_restart = fptrset->log_likelihoods_cal;
  }

  if(fptrset->perturb == NULL)
  {
    printf("\"perturb\" function is not defined.\n");
    exit(0);
  }

  if(fptrset->restart_action == NULL)
  {
    //printf("\"restart_action\" function is not defined.\
    //  \nSet to the default function in dnest.\n");
    fptrset->restart_action = dnest_restart_action;
  }

  if(fptrset->accept_action == NULL)
  {
    //printf("\"accept_action\" function is not defined.\
    //  \nSet to the default function in dnest.\n");
    fptrset->accept_action = dnest_accept_action;
  }

  if(fptrset->kill_action == NULL)
  {
    //printf("\"kill_action\" function is not defined.\
    //  \nSet to the default function in dnest.\n");
    fptrset->kill_action = dnest_kill_action;
  }

  return;
}

DNestFptrSet * dnest_malloc_fptrset()
{
  DNestFptrSet * fptrset;
  fptrset = (DNestFptrSet *)malloc(sizeof(DNestFptrSet));

  fptrset->from_prior = NULL;
  fptrset->log_likelihoods_cal = NULL;
  fptrset->log_likelihoods_cal_initial = NULL;
  fptrset->log_likelihoods_cal_restart = NULL;
  fptrset->perturb = NULL;
  fptrset->print_particle = NULL;
  fptrset->read_particle = NULL;
  fptrset->restart_action = NULL;
  fptrset->accept_action = NULL;
  fptrset->kill_action = NULL;
  return fptrset;
}

void dnest_free_fptrset(DNestFptrSet * fptrset)
{
  free(fptrset);
  return;
}

/*!
 *  Save sampler state for later restart.
 */
void dnest_save_restart()
{
  FILE *fp;
  int i, j;
  void *particles_all;
  LikelihoodType *log_likelihoods_all;
  unsigned int *level_assignments_all;
  char str[200];

  sprintf(str, "%s_%d", file_save_restart, count_saves);
  fp = fopen(str, "wb");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s. \n", file_save_restart);
    exit(0);
  }

  printf("# Save restart data to file %s.\n", str);

    //fprintf(fp, "%d %d\n", count_saves, count_mcmc_steps);
    //fprintf(fp, "%d\n", size_levels_combine);

  fwrite(&count_saves, sizeof(int), 1, fp);
  fwrite(&count_mcmc_steps, sizeof(int), 1, fp);
  fwrite(&size_levels, sizeof(int), 1, fp);

  for(i=0; i<size_levels; i++)
  {
    //fprintf(fp, "%14.12g %14.12g %f %llu %llu %llu %llu\n", levels_combine[i].log_X, levels_combine[i].log_likelihood.value,
    //  levels_combine[i].log_likelihood.tiebreaker, levels_combine[i].accepts,
    //  levels_combine[i].tries, levels[i].exceeds, levels_combine[i].visits);

    fwrite(&levels[i], sizeof(Level), 1, fp);
  }

  for(i=0; i<options.num_particles; i++)
  {
    //fprintf(fp, "%d %e %f\n", level_assignments_all[j*options.num_particles + i],
    //  log_likelihoods_all[j*options.num_particles + i].value,
    //  log_likelihoods_all[j*options.num_particles + i].tiebreaker);
    fwrite(&level_assignments[j*options.num_particles + i], sizeof(int), 1, fp);
    fwrite(&log_likelihoods[j*options.num_particles + i], sizeof(LikelihoodType), 1, fp);
  }

  if(dnest_flag_limits == 1)
  {
    for(i=0; i<size_levels; i++)
    {
      //fprintf(fp, "%d  ", i);
      for(j=0; j<particle_offset_double; j++)
      {
        //fprintf(fp, "%f  %f  ", limits[i*2*particle_offset_double+j*2], limits[i*2*particle_offset_double+j*2+1]);
        fwrite(&limits[i*2*particle_offset_double+j*2], sizeof(double), 2, fp);
      }

        //fprintf(fp, "\n");
    }
  }

  for(i=0; i<options.num_particles; i++)
  {
    //print_particle(fp, particles_all + (j * options.num_particles + i) * particle_offset_size);
    fwrite(particles + (j * options.num_particles + i) * particle_offset_size, dnest_size_of_modeltype, 1, fp);
  }

  fclose(fp);

  restart_action(0);
}

void dnest_restart()
{
  FILE *fp;
  int i, j;
  void *particles_all;
  unsigned int *level_assignments_all;
  LikelihoodType *log_likelihoods_all;
  void *particle;

  fp = fopen(file_restart, "rb");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s. \n", file_restart);
    exit(0);
  }

  printf("# Reading %s\n", file_restart);

  fread(&count_saves, sizeof(int), 1, fp);
  fread(&count_mcmc_steps, sizeof(int), 1, fp);
  fread(&size_levels, sizeof(int), 1, fp);

  /* consider that the newly input max_num_levels may be different from the save one */
  if(options.max_num_levels != 0)
  {
    if(size_levels > options.max_num_levels)
    {
      printf("# input max_num_levels %d smaller than the one in restart data %d.\n", options.max_num_levels, size_levels);
      size_levels = options.max_num_levels;
    }
  }
    // read levels
  for(i=0; i<size_levels; i++)
  {
    if(i<size_levels) // not read all the levels
      fread(&levels[i], sizeof(Level), 1, fp);
    else
      fseek(fp, sizeof(Level), SEEK_CUR); /* offset the file point */
  }
    memcpy(levels, levels, size_levels * sizeof(Level));

    // read level assignment
  for(i=0; i<options.num_particles; i++)
  {
    fread(&level_assignments[j*options.num_particles + i], sizeof(int), 1, fp);
    fread(&log_likelihoods[j*options.num_particles + i], sizeof(LikelihoodType), 1, fp);

    /* reset the level assignment that exceeds the present maximum level */
    if(level_assignments[j*options.num_particles + i] > size_levels -1)
    {
      level_assignments[j*options.num_particles + i] = size_levels - 1;
    }
  }

  // read limits
  if(dnest_flag_limits == 1)
  {
    for(i=0; i<size_levels; i++)
    {
      if(i < size_levels)
      {
        for(j=0; j<particle_offset_double; j++)
        {
          fread(&limits[i*2*particle_offset_double+j*2], sizeof(double), 2, fp);
        }
      }
      else
      {
        fseek(fp, sizeof(double) * 2 * particle_offset_double, SEEK_CUR); /* offset the file point */
      }
    }
  }

  // read particles
  for(i=0; i<options.num_particles; i++)
  {
    particle = (particles + (j * options.num_particles + i) * dnest_size_of_modeltype);
    fread(particle, dnest_size_of_modeltype, 1, fp);
  }

  fclose(fp);

  if(count_saves > options.max_num_saves)
  {
    printf("# Number of samples already larger than the input number, exit!\n");
    exit(0);
  }

  num_saves = (int)fmax(0.02*(options.max_num_saves-count_saves), 1.0); /* reset num_saves */
  num_saves_restart = (int)fmax(0.2 * (options.max_num_saves-count_saves), 1.0); /* reset num_saves_restart */

  restart_action(1);

  for(i=0; i<options.num_particles; i++)
  {
    dnest_which_particle_update = i;
    dnest_which_level_update = level_assignments[i];
    //printf("%d %d %f\n", thistask, i, log_likelihoods[i].value);
    log_likelihoods[i].value = log_likelihoods_cal_restart(particles+i*particle_offset_size, dnest_arg);
    //printf("%d %d %f\n", thistask, i, log_likelihoods[i].value);
    //due to randomness, the original level assignment may be incorrect. re-asign the level
    while(log_likelihoods[i].value < levels[level_assignments[i]].log_likelihood.value)
    {
      printf("# level assignment decrease %d %f %f %d.\n", i, log_likelihoods[i].value,
        levels[level_assignments[i]].log_likelihood.value, level_assignments[i]);
      level_assignments[i]--;
    }

  }
  return;
}

void dnest_print_particle(FILE *fp, const void *model, const void *arg)
{
  int i;
  double *pm = (double *)model;

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

void dnest_read_particle(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 dnest_restart_action(int iflag)
{
  return;
}

void dnest_accept_action()
{
  return;
}

void dnest_kill_action(int i, int i_copy)
{
  return;
}


#pragma once

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

gsl_rng *RandomNumberGenerator; /* Random number generator. */
unsigned RandomSeed; /* Random seed. */

unsigned GetRandomSeed()
{
  FILE *random_file;
  unsigned seed;
  int error;

  seed = 0;
  random_file = fopen("/dev/urandom", "rb");
  if (!random_file)
    return 0;
  do
  {
    error = fread(&seed, 1, sizeof(unsigned int), random_file);
    if (error != sizeof(unsigned int))
      return 0;
  }
  while (!seed);
  fclose(random_file);
  return seed;
}

/** InitialiseRandom.

    - Initialises the random number generator. **/

int InitialiseRandom(const gsl_rng_type *random_generator)
{
  RandomSeed = GetRandomSeed();
  if (!RandomSeed)
    return EXIT_FAILURE;
  RandomNumberGenerator = gsl_rng_alloc(random_generator);
  if (!RandomNumberGenerator)
    return EXIT_FAILURE;
  gsl_rng_set(RandomNumberGenerator, RandomSeed);
  return EXIT_SUCCESS;
}

/** FinaliseRandom.

    - Finalises the random number generator. **/

int FinaliseRandom()
{
  /* Release random number generator memory. */

  if (RandomNumberGenerator)
  {
    gsl_rng_free(RandomNumberGenerator);
    return EXIT_SUCCESS; // EXIT_SUCCESS = 0
  }
  return EXIT_FAILURE; // EXIT_FAILURE = 1
}

/////////////


#include <bindings.cmacros.h>
#include <gsl/gsl_multifit_nlin.h>

BC_INLINE3VOID(GSL_MULTIFIT_FN_EVAL,gsl_multifit_function*,gsl_vector*,gsl_vector*)
BC_INLINE3(GSL_MULTIFIT_FN_EVAL_F,gsl_multifit_function_fdf*,gsl_vector*,gsl_vector*,int)
BC_INLINE3(GSL_MULTIFIT_FN_EVAL_DF,gsl_multifit_function_fdf*,gsl_vector*,gsl_matrix*,int)
BC_INLINE4(GSL_MULTIFIT_FN_EVAL_F_DF,gsl_multifit_function_fdf*,gsl_vector*,gsl_vector*,gsl_matrix*,int)


/* multifit_nlinear/fdf.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 * Copyright (C) 2015, 2016 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multifit_nlinear.h>

gsl_multifit_nlinear_workspace *
gsl_multifit_nlinear_alloc (const gsl_multifit_nlinear_type * T,
                            const gsl_multifit_nlinear_parameters * params,
                            const size_t n, const size_t p)
{
  gsl_multifit_nlinear_workspace * w;

  if (n < p)
    {
      GSL_ERROR_VAL ("insufficient data points, n < p", GSL_EINVAL, 0);
    }

  w = calloc (1, sizeof (gsl_multifit_nlinear_workspace));
  if (w == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for multifit workspace",
                     GSL_ENOMEM, 0);
    }

  w->x = gsl_vector_calloc (p);
  if (w->x == 0)
    {
      gsl_multifit_nlinear_free (w);
      GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
    }

  w->f = gsl_vector_calloc (n);
  if (w->f == 0)
    {
      gsl_multifit_nlinear_free (w);
      GSL_ERROR_VAL ("failed to allocate space for f", GSL_ENOMEM, 0);
    }

  w->dx = gsl_vector_calloc (p);
  if (w->dx == 0)
    {
      gsl_multifit_nlinear_free (w);
      GSL_ERROR_VAL ("failed to allocate space for dx", GSL_ENOMEM, 0);
    }

  w->g = gsl_vector_alloc (p);
  if (w->g == 0)
    {
      gsl_multifit_nlinear_free (w);
      GSL_ERROR_VAL ("failed to allocate space for g", GSL_ENOMEM, 0);
    }

  w->J = gsl_matrix_alloc(n, p);
  if (w->J == 0)
    {
      gsl_multifit_nlinear_free (w);
      GSL_ERROR_VAL ("failed to allocate space for Jacobian", GSL_ENOMEM, 0);
    }

  w->sqrt_wts_work = gsl_vector_calloc (n);
  if (w->sqrt_wts_work == 0)
    {
      gsl_multifit_nlinear_free (w);
      GSL_ERROR_VAL ("failed to allocate space for weights", GSL_ENOMEM, 0);
    }

  w->state = (T->alloc)(params, n, p);
  if (w->state == 0)
    {
      gsl_multifit_nlinear_free (w);
      GSL_ERROR_VAL ("failed to allocate space for multifit state", GSL_ENOMEM, 0);
    }

  w->type = T;
  w->fdf = NULL;
  w->niter = 0;
  w->params = *params;

  return w;
}

void
gsl_multifit_nlinear_free (gsl_multifit_nlinear_workspace * w)
{
  RETURN_IF_NULL (w);

  if (w->state)
    (w->type->free) (w->state);

  if (w->dx)
    gsl_vector_free (w->dx);

  if (w->x)
    gsl_vector_free (w->x);

  if (w->f)
    gsl_vector_free (w->f);

  if (w->sqrt_wts_work)
    gsl_vector_free (w->sqrt_wts_work);

  if (w->g)
    gsl_vector_free (w->g);

  if (w->J)
    gsl_matrix_free (w->J);

  free (w);
}

gsl_multifit_nlinear_parameters
gsl_multifit_nlinear_default_parameters(void)
{
  gsl_multifit_nlinear_parameters params;

  params.trs = gsl_multifit_nlinear_trs_lm;
  params.scale = gsl_multifit_nlinear_scale_more;
  params.solver = gsl_multifit_nlinear_solver_qr;
  params.fdtype = GSL_MULTIFIT_NLINEAR_FWDIFF;
  params.factor_up = 3.0;
  params.factor_down = 2.0;
  params.avmax = 0.75;
  params.h_df = GSL_SQRT_DBL_EPSILON;
  params.h_fvv = 0.02;

  return params;
}

int
gsl_multifit_nlinear_init (const gsl_vector * x,
                           gsl_multifit_nlinear_fdf * fdf,
                           gsl_multifit_nlinear_workspace * w)
{
  return gsl_multifit_nlinear_winit(x, NULL, fdf, w);
}

int
gsl_multifit_nlinear_winit (const gsl_vector * x,
                            const gsl_vector * wts,
                            gsl_multifit_nlinear_fdf * fdf,
                            gsl_multifit_nlinear_workspace * w)
{
  const size_t n = w->f->size;

  if (n != fdf->n)
    {
      GSL_ERROR ("function size does not match workspace", GSL_EBADLEN);
    }
  else if (w->x->size != x->size)
    {
      GSL_ERROR ("vector length does not match workspace", GSL_EBADLEN);
    }
  else if (wts != NULL && n != wts->size)
    {
      GSL_ERROR ("weight vector length does not match workspace", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* initialize counters for function and Jacobian evaluations */
      fdf->nevalf = 0;
      fdf->nevaldf = 0;
      fdf->nevalfvv = 0;

      w->fdf = fdf;
      gsl_vector_memcpy(w->x, x);
      w->niter = 0;

      if (wts)
        {
          w->sqrt_wts = w->sqrt_wts_work;

          for (i = 0; i < n; ++i)
            {
              double wi = gsl_vector_get(wts, i);
              gsl_vector_set(w->sqrt_wts, i, sqrt(wi));
            }
        }
      else
        {
          w->sqrt_wts = NULL;
        }

      return (w->type->init) (w->state, w->sqrt_wts, w->fdf,
                              w->x, w->f, w->J, w->g);
    }
}

int
gsl_multifit_nlinear_iterate (gsl_multifit_nlinear_workspace * w)
{
  int status =
    (w->type->iterate) (w->state, w->sqrt_wts, w->fdf,
                        w->x, w->f, w->J, w->g, w->dx);

  w->niter++;

  return status;
}

double
gsl_multifit_nlinear_avratio (const gsl_multifit_nlinear_workspace * w)
{
  return (w->type->avratio) (w->state);
}

/*
gsl_multifit_nlinear_driver()
  Iterate the nonlinear least squares solver until completion

Inputs: maxiter  - maximum iterations to allow
        xtol     - tolerance in step x
        gtol     - tolerance in gradient
        ftol     - tolerance in ||f||
        callback - callback function to call each iteration
        callback_params - parameters to pass to callback function
        info     - (output) info flag on why iteration terminated
                   1 = stopped due to small step size ||dx|
                   2 = stopped due to small gradient
                   3 = stopped due to small change in f
                   GSL_ETOLX = ||dx|| has converged to within machine
                               precision (and xtol is too small)
                   GSL_ETOLG = ||g||_inf is smaller than machine
                               precision (gtol is too small)
                   GSL_ETOLF = change in ||f|| is smaller than machine
                               precision (ftol is too small)
        w        - workspace

Return:
GSL_SUCCESS if converged
GSL_MAXITER if maxiter exceeded without converging
GSL_ENOPROG if no accepted step found on first iteration
*/

int
gsl_multifit_nlinear_driver (const size_t maxiter,
                             const double xtol,
                             const double gtol,
                             const double ftol,
                             void (*callback)(const size_t iter, void *params,
                                              const gsl_multifit_nlinear_workspace *w),
                             void *callback_params,
                             int *info,
                             gsl_multifit_nlinear_workspace * w)
{
  int status;
  size_t iter = 0;

  /* call user callback function prior to any iterations
   * with initial system state */
  if (callback)
    callback(iter, callback_params, w);

  do
    {
      status = gsl_multifit_nlinear_iterate (w);

      /*
       * If the solver reports no progress on the first iteration,
       * then it didn't find a single step to reduce the
       * cost function and more iterations won't help so return.
       *
       * If we get a no progress flag on subsequent iterations,
       * it means we did find a good step in a previous iteration,
       * so continue iterating since the solver has now reset
       * mu to its initial value.
       */
      if (status == GSL_ENOPROG && iter == 0)
        {
          *info = status;
          return GSL_EMAXITER;
        }

      ++iter;

      if (callback)
        callback(iter, callback_params, w);

      /* test for convergence */
      status = gsl_multifit_nlinear_test(xtol, gtol, ftol, info, w);
    }
  while (status == GSL_CONTINUE && iter < maxiter);

  /*
   * the following error codes mean that the solution has converged
   * to within machine precision, so record the error code in info
   * and return success
   */
  if (status == GSL_ETOLF || status == GSL_ETOLX || status == GSL_ETOLG)
    {
      *info = status;
      status = GSL_SUCCESS;
    }

  /* check if max iterations reached */
  if (iter >= maxiter && status != GSL_SUCCESS)
    status = GSL_EMAXITER;

  return status;
} /* gsl_multifit_nlinear_driver() */

gsl_matrix *
gsl_multifit_nlinear_jac (const gsl_multifit_nlinear_workspace * w)
{
  return w->J;
}

const char *
gsl_multifit_nlinear_name (const gsl_multifit_nlinear_workspace * w)
{
  return w->type->name;
}

gsl_vector *
gsl_multifit_nlinear_position (const gsl_multifit_nlinear_workspace * w)
{
  return w->x;
}

gsl_vector *
gsl_multifit_nlinear_residual (const gsl_multifit_nlinear_workspace * w)
{
  return w->f;
}

size_t
gsl_multifit_nlinear_niter (const gsl_multifit_nlinear_workspace * w)
{
  return w->niter;
}

int
gsl_multifit_nlinear_rcond (double *rcond, const gsl_multifit_nlinear_workspace * w)
{
  int status = (w->type->rcond) (rcond, w->state);
  return status;
}

const char *
gsl_multifit_nlinear_trs_name (const gsl_multifit_nlinear_workspace * w)
{
  return w->params.trs->name;
}

/*
gsl_multifit_nlinear_eval_f()
  Compute residual vector y with user callback function, and apply
weighting transform if given:

y~ = sqrt(W) y

Inputs: fdf  - callback function
        x    - model parameters
        swts - weight matrix sqrt(W) = sqrt(diag(w1,w2,...,wn))
               set to NULL for unweighted fit
        y    - (output) (weighted) residual vector
               y_i = sqrt(w_i) f_i where f_i is unweighted residual
*/

int
gsl_multifit_nlinear_eval_f(gsl_multifit_nlinear_fdf *fdf,
                            const gsl_vector *x,
                            const gsl_vector *swts,
                            gsl_vector *y)
{
  int s = ((*((fdf)->f)) (x, fdf->params, y));

  ++(fdf->nevalf);

  /* y <- sqrt(W) y */
  if (swts)
    gsl_vector_mul(y, swts);

  return s;
}

/*
gsl_multifit_nlinear_eval_df()
  Compute Jacobian matrix J with user callback function, and apply
weighting transform if given:

J~ = sqrt(W) J

Inputs: x      - model parameters
        f      - residual vector f(x)
        swts   - weight matrix W = diag(w1,w2,...,wn)
                 set to NULL for unweighted fit
        h      - finite difference step size
        fdtype - finite difference method
        fdf    - callback function
        df     - (output) (weighted) Jacobian matrix
                 df = sqrt(W) df where df is unweighted Jacobian
        work   - workspace for finite difference, size n
*/

int
gsl_multifit_nlinear_eval_df(const gsl_vector *x,
                             const gsl_vector *f,
                             const gsl_vector *swts,
                             const double h,
                             const gsl_multifit_nlinear_fdtype fdtype,
                             gsl_multifit_nlinear_fdf *fdf,
                             gsl_matrix *df,
                             gsl_vector *work)
{
  int status;

  if (fdf->df)
    {
      /* call user-supplied function */
      status = ((*((fdf)->df)) (x, fdf->params, df));
      ++(fdf->nevaldf);

      /* J <- sqrt(W) J */
      if (swts)
        {
          const size_t n = swts->size;
          size_t i;

          for (i = 0; i < n; ++i)
            {
              double swi = gsl_vector_get(swts, i);
              gsl_vector_view v = gsl_matrix_row(df, i);

              gsl_vector_scale(&v.vector, swi);
            }
        }
    }
  else
    {
      /* use finite difference Jacobian approximation */
      status = gsl_multifit_nlinear_df(h, fdtype, x, swts, fdf, f, df, work);
    }

  return status;
}

/*
gsl_multifit_nlinear_eval_fvv()
  Compute second direction derivative vector yvv with user
callback function, and apply weighting transform if given:

yvv~ = sqrt(W) yvv

Inputs: h    - step size for finite difference, if needed
        x    - model parameters, size p
        v    - unscaled geodesic velocity vector, size p
        f    - residual vector f(x), size n
        J    - Jacobian matrix J(x), n-by-p
        swts - weight matrix sqrt(W) = sqrt(diag(w1,w2,...,wn))
               set to NULL for unweighted fit
        fdf  - callback function
        yvv  - (output) (weighted) second directional derivative vector
               yvv_i = sqrt(w_i) fvv_i where f_i is unweighted
        work - workspace, size p
*/

int
gsl_multifit_nlinear_eval_fvv(const double h,
                              const gsl_vector *x,
                              const gsl_vector *v,
                              const gsl_vector *f,
                              const gsl_matrix *J,
                              const gsl_vector *swts,
                              gsl_multifit_nlinear_fdf *fdf,
                              gsl_vector *yvv, gsl_vector *work)
{
  int status;

  if (fdf->fvv != NULL)
    {
      /* call user-supplied function */
      status = ((*((fdf)->fvv)) (x, v, fdf->params, yvv));
      ++(fdf->nevalfvv);

      /* yvv <- sqrt(W) yvv */
      if (swts)
        gsl_vector_mul(yvv, swts);
    }
  else
    {
      /* use finite difference approximation */
      status = gsl_multifit_nlinear_fdfvv(h, x, v, f, J,
                                          swts, fdf, yvv, work);
    }

  return status;
}


//
// Created by pedram pakseresht on 2/18/21.
//

static char help[] = "settling in quiescent fluid";

#include <petsc.h>
#include "incompressibleFlow.h"
#include "mesh.h"
#include "particleInertial.h"
#include "particles.h"
#include "particleInitializer.h"
#include "petscviewer.h"

static PetscErrorCode uniform_u(PetscInt Dim, PetscReal time, const PetscReal *X, PetscInt Nf, PetscScalar *u, void *ctx) {
    PetscInt d;
    for (d = 0; d < Dim; ++d)
        u[d] = 0.0;
    return 0;

}

static PetscErrorCode uniform_u_t(PetscInt Dim, PetscReal time, const PetscReal *X, PetscInt Nf, PetscScalar *u, void *ctx) {
    PetscInt d;
    for (d = 0; d < Dim; ++d)
        u[d] = 0.0;
    return 0;
}

static PetscErrorCode uniform_p(PetscInt Dim, PetscReal time, const PetscReal *X, PetscInt Nf, PetscScalar *p, void *ctx) {
    p[0] = 0.0;
    return 0;
}

static PetscErrorCode uniform_T(PetscInt Dim, PetscReal time, const PetscReal *X, PetscInt Nf, PetscScalar *T, void *ctx) {
    T[0] = 0.0;
    return 0;
}
static PetscErrorCode uniform_T_t(PetscInt Dim, PetscReal time, const PetscReal *X, PetscInt Nf, PetscScalar *T, void *ctx) {
    T[0] = 0.0;
    return 0;
}

static PetscErrorCode SetInitialConditions(TS ts, Vec u) {

    PetscErrorCode (*initFuncs[3])(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar u[], void *ctx) = {uniform_u,uniform_p,uniform_T};
    // u here is the solution vector including velocity, temperature and pressure fields.
    DM dm;
    PetscReal t;
    PetscErrorCode ierr;
    PetscFunctionBegin;

    ierr = TSGetDM(ts, &dm);
    CHKERRQ(ierr);
    ierr = TSGetTime(ts, &t);

    CHKERRQ(ierr);

    ierr = DMProjectFunction(dm, 0.0, initFuncs, NULL, INSERT_ALL_VALUES, u);
    CHKERRQ(ierr);
    // get the flow to apply the completeFlowInitialization method
    ierr = IncompressibleFlow_CompleteFlowInitialization(dm, u);
    CHKERRQ(ierr);

    PetscFunctionReturn(0);
}

static PetscErrorCode MonitorFlowAndParticleError(TS ts, PetscInt step, PetscReal crtime, Vec u, void *ctx) {
    // PetscErrorCode (*exactFuncs[3])(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx);
    // void *ctxs[3];
    DM dm;
    PetscDS ds;
    Vec v;
    PetscReal ferrors[3];
    PetscInt f;
    PetscErrorCode ierr;
    PetscInt num;

    PetscFunctionBeginUser;
    ierr = TSGetDM(ts, &dm);
    CHKERRQ(ierr);
    ierr = DMGetDS(dm, &ds);
    CHKERRQ(ierr);

    // get the particle data from the context
    ParticleData particlesData = (ParticleData)ctx;
    PetscInt particleCount;
    ierr = DMSwarmGetSize(particlesData->dm, &particleCount);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // compute the average particle location
    const PetscReal *coords;
    PetscInt dims;
    PetscReal avg[3] = {0.0, 0.0, 0.0};
    ierr = DMSwarmGetField(particlesData->dm, DMSwarmPICField_coor, &dims, NULL, (void **)&coords);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    for (PetscInt p = 0; p < particleCount; p++) {
        for (PetscInt n = 0; n < dims; n++) {
            avg[n] += coords[p * dims + n] / particleCount;  // PetscReal
        }
    }
    ierr = DMSwarmRestoreField(particlesData->dm, DMSwarmPICField_coor, &dims, NULL, (void **)&coords);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = PetscPrintf(PETSC_COMM_WORLD,
                       "Timestep: %04d time = %-8.4g \t L_2 Error: [%2.3g, %2.3g, %2.3g] ParticleCount: %d\n",
                       (int)step,
                       (double)crtime,
                       (double)ferrors[0],
                       (double)ferrors[1],
                       (double)ferrors[2],
                       particleCount,
                       (double)avg[0],
                       (double)avg[1]);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "Avg Particle Location: [%2.3g, %2.3g, %2.3g]\n", (double)avg[0], (double)avg[1], (double)avg[2]);

    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = VecViewFromOptions(u, NULL, "-vec_view");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = DMSetOutputSequenceNumber(particlesData->dm, step, crtime);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    if (step == 0) {
        ierr = ParticleViewFromOptions(particlesData, "-particle_init");
        CHKERRABORT(PETSC_COMM_WORLD, ierr);
    } else {
        ierr = ParticleViewFromOptions(particlesData, "-particle_view");
        CHKERRABORT(PETSC_COMM_WORLD, ierr);
    }

    PetscFunctionReturn(0);
}

static PetscErrorCode ParticleInertialInitialize(ParticleData particles) {

    PetscFunctionBeginUser;
    PetscErrorCode  ierr;
    Vec vel,diam,dens;
    DM particleDm = particles->dm;

    // input parameters required for particles
    PetscScalar partVel = 0.0;
    PetscScalar partDiam = 0.22;
    PetscScalar partDens = 90.0;
    PetscScalar fluidDens = 1.0;
    PetscScalar fluidVisc = 1.0;
    PetscScalar gravity[3] = {0.0,1.0,0.0};

    ierr = DMSwarmCreateGlobalVectorFromField(particleDm,ParticleVelocity, &vel);CHKERRQ(ierr);
    ierr = DMSwarmCreateGlobalVectorFromField(particleDm,ParticleDiameter, &diam);CHKERRQ(ierr);
    ierr = DMSwarmCreateGlobalVectorFromField(particleDm,ParticleDensity, &dens);CHKERRQ(ierr);
    // set particle velocity, diameter and density
    ierr = VecSet(vel, partVel);CHKERRQ(ierr);
    ierr = VecSet(diam, partDiam);CHKERRQ(ierr);
    ierr = VecSet(dens, partDens);CHKERRQ(ierr);
    ierr = DMSwarmDestroyGlobalVectorFromField(particleDm,ParticleVelocity, &vel);CHKERRQ(ierr);
    ierr = DMSwarmDestroyGlobalVectorFromField(particleDm,ParticleDiameter, &diam);CHKERRQ(ierr);
    ierr = DMSwarmDestroyGlobalVectorFromField(particleDm,ParticleDensity, &dens);CHKERRQ(ierr);

    InertialParticleParameters *data;
    PetscNew(&data);
    particles->data =data;

    // set fluid parameters
    data->fluidDensity = fluidDens;
    data->fluidViscosity = fluidVisc;
    data->gravityField[0] = gravity[0];
    data->gravityField[1] = gravity[1];
    data->gravityField[2] = gravity[2];
    PetscFunctionReturn(0);

}

int main( int argc, char *argv[] )
{

    DM dm;     // domain definition
    TS ts;     // time-stepper
    PetscErrorCode ierr;

    PetscBag parameterBag; // constant flow parameters
    Vec flowField;         // flow solution vector
    FlowData flowData;

    PetscReal t;
    PetscInt Dim = 2;
    PetscReal dt = 0.05;       // dt for time stepper
    PetscInt max_steps = 10; // maximum time steps

    PetscReal Re = 0.1;       // Reynolds number
    PetscReal St = 1.0;       // Strouhal number
    PetscReal Pe = 1.0;       // Peclet number
    PetscReal Mu = 1.0;      // viscosity
    PetscReal K_input = 1.0;  // thermal conductivity
    PetscReal Cp = 1.0;    // heat capacity
    // PetscInt Np=10;

    // initialize Petsc ...
    ierr = PetscInitialize(&argc, &argv, NULL, "");CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "Settling particle in a quiescent fluid \n");CHKERRQ(ierr);

    // setup the ts
    ierr = TSCreate(PETSC_COMM_WORLD, &ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = CreateMesh(PETSC_COMM_WORLD, &dm, PETSC_TRUE, Dim);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetDM(ts, dm);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetExactFinalTime(ts, TS_EXACTFINALTIME_MATCHSTEP);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    //output the mesh
    ierr = DMViewFromOptions(dm, NULL, "-dm_view");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Setup the flow data
    ierr = FlowCreate(&flowData);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // setup problem
    ierr = IncompressibleFlow_SetupDiscretization(flowData, dm);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    IncompressibleFlowParameters flowParameters;

    // changing non-dimensional parameters manually here ...
    flowParameters.strouhal = St;
    flowParameters.reynolds = Re;
    flowParameters.peclet = Pe;
    flowParameters.mu = Mu;
    flowParameters.k = K_input;
    flowParameters.cp = Cp;

    // print out the parameters
    /*
    ierr = PetscPrintf(PETSC_COMM_WORLD, "*** non-dimensional parameters ***\n");CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "St=%g\n", flowParameters.strouhal);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "Re=%g\n", flowParameters.reynolds);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "Pe=%g\n", flowParameters.peclet);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "Mu=%g\n", flowParameters.mu);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "K=%g\n", flowParameters.k);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "Cp=%g\n", flowParameters.cp);CHKERRQ(ierr);
    */

    // Start the problem setup
    PetscScalar constants[TOTAL_INCOMPRESSIBLE_FLOW_PARAMETERS];
    ierr = IncompressibleFlow_PackParameters(&flowParameters, constants);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = IncompressibleFlow_StartProblemSetup(flowData, TOTAL_INCOMPRESSIBLE_FLOW_PARAMETERS, constants);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Override problem with source terms, boundary, and set the exact solution
    PetscDS prob;
    ierr = DMGetDS(dm, &prob);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Setup Boundary Conditions
    // Note: DM_BC_ESSENTIAL is a Dirichlet BC.
    PetscInt id;
    id = 3;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "top wall velocity", "marker", VEL, 0, NULL, (void (*)(void))uniform_u, (void (*)(void))uniform_u_t, 1, &id, NULL);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    id = 1;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "bottom wall velocity", "marker", VEL, 0, NULL, (void (*)(void))uniform_u, (void (*)(void))uniform_u_t, 1, &id, NULL);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    id = 2;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "right wall velocity", "marker", VEL, 0, NULL, (void (*)(void))uniform_u, (void (*)(void))uniform_u_t, 1, &id, NULL);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    id = 4;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "left wall velocity", "marker", VEL, 0, NULL, (void (*)(void))uniform_u, (void (*)(void))uniform_u_t, 1, &id, NULL);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    id = 3;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "top wall temp", "marker", TEMP, 0, NULL, (void (*)(void))uniform_T, (void (*)(void))uniform_T_t, 1, &id, NULL);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    id = 1;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "bottom wall temp", "marker", TEMP, 0, NULL, (void (*)(void))uniform_T, (void (*)(void))uniform_T_t, 1, &id, parameterBag);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    id = 2;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "right wall temp", "marker", TEMP, 0, NULL, (void (*)(void))uniform_T, (void (*)(void))uniform_T_t, 1, &id, parameterBag);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    id = 4;
    ierr = PetscDSAddBoundary(
        prob, DM_BC_ESSENTIAL, "left wall temp", "marker", TEMP, 0, NULL, (void (*)(void))uniform_T, (void (*)(void))uniform_T_t, 1, &id, parameterBag);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = IncompressibleFlow_CompleteProblemSetup(flowData, ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Name the flow field
    ierr = PetscObjectSetName(((PetscObject)flowData->flowField), "Numerical Solution");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = SetInitialConditions(ts, flowData->flowField);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = TSGetTime(ts, &t);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    //  *** added by Pedram for dt and number of time steps ***
    ierr = TSSetTimeStep(ts,dt);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = TSSetMaxSteps(ts,max_steps);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    // ***********************************

    ierr = DMSetOutputSequenceNumber(dm, 0, t);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // checks for convergence ...
    ierr = DMTSCheckFromOptions(ts, flowData->flowField);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ParticleData particles;

    // Setup the particle domain
    ierr = ParticleInertialCreate(&particles, Dim);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // link the flow to the particles
    ierr = ParticleInitializeFlow(particles, flowData);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // name the particle domain
    ierr = PetscObjectSetOptionsPrefix((PetscObject)(particles->dm), "particles_");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = PetscObjectSetName((PetscObject)particles->dm, "Particles");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // initialize the particles position
    ierr = ParticleInitialize(dm, particles->dm);CHKERRABORT(PETSC_COMM_WORLD, ierr);
    // initialize inertial particles velocity, diameter and density
    ierr = ParticleInertialInitialize(particles);CHKERRABORT(PETSC_COMM_WORLD, ierr);

    //ierr = ParticleInertialInitialize(particles->dm);CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // setup the flow monitor to also check particles
    ierr = TSMonitorSet(ts, MonitorFlowAndParticleError, particles, NULL);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetFromOptions(ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Setup particle position integrator
    TS particleTs;
    ierr = TSCreate(PETSC_COMM_WORLD, &particleTs);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = PetscObjectSetOptionsPrefix((PetscObject)particleTs, "particle_");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    ierr = ParticleInertialSetupIntegrator(particles, particleTs, flowData);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    // ierr = VecView(particleVelocity,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);

    // Solve the one way coupled system
    ierr = TSSolve(ts, flowData->flowField);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Cleanup
    ierr = DMDestroy(&dm);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSDestroy(&ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSDestroy(&particleTs);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = FlowDestroy(&flowData);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = ParticleInertialDestroy(&particles);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = PetscFinalize();
    exit(ierr);

}


/* multiset/multiset.c
 * based on combination/combination.c by Szymon Jaroszewicz
 * based on permutation/permutation.c by Brian Gough
 *
 * Copyright (C) 2001 Szymon Jaroszewicz
 * Copyright (C) 2009 Rhys Ulerich
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multiset.h>

size_t
gsl_multiset_n (const gsl_multiset * c)
{
  return c->n ;
}

size_t
gsl_multiset_k (const gsl_multiset * c)
{
  return c->k ;
}

size_t *
gsl_multiset_data (const gsl_multiset * c)
{
  return c->data ;
}

int
gsl_multiset_valid (gsl_multiset * c)
{
  const size_t n = c->n ;
  const size_t k = c->k ;

  size_t i, j ;

  for (i = 0; i < k; i++)
    {
      const size_t ci = c->data[i];

      if (ci >= n)
        {
          GSL_ERROR("multiset index outside range", GSL_FAILURE) ;
        }

      for (j = 0; j < i; j++)
        {
          if (c->data[j] > ci)
            {
              GSL_ERROR("multiset indices not in increasing order",
                        GSL_FAILURE) ;
            }
        }
    }

  return GSL_SUCCESS;
}

int
gsl_multiset_next (gsl_multiset * c)
{
  /* Replaces c with the next multiset (in the standard lexicographical
   * ordering).  Returns GSL_FAILURE if there is no next multiset.
   */
  const size_t n = c->n;
  const size_t k = c->k;
  size_t *data = c->data;
  size_t i;

  if(k == 0)
    {
      return GSL_FAILURE;
    }
  i = k - 1;

  while(i > 0 && data[i] == n-1)
    {
      --i;
    }

  if (i == 0 && data[0] == n-1)
    {
      return GSL_FAILURE;
    }

  ++data[i];

  while(i < k-1)
    {
      data[i+1] = data[i];
      ++i;
    }

  return GSL_SUCCESS;
}

int
gsl_multiset_prev (gsl_multiset * c)
{
  /* Replaces c with the previous multiset (in the standard
   * lexicographical ordering).  Returns GSL_FAILURE if there is no
   * previous multiset.
   */
  const size_t n = c->n;
  const size_t k = c->k;
  size_t *data = c->data;
  size_t i;

  if(k == 0)
    {
      return GSL_FAILURE;
    }
  i = k - 1;

  while(i > 0 && data[i-1] == data[i])
    {
      --i;
    }

  if(i == 0 && data[i] == 0)
    {
      return GSL_FAILURE;
    }

  data[i]--;

  if (data[i] < n-1)
    {
      while (i < k-1) {
        data[++i] = n - 1;
      }
    }

  return GSL_SUCCESS;
}

int
gsl_multiset_memcpy (gsl_multiset * dest, const gsl_multiset * src)
{
   const size_t src_n = src->n;
   const size_t src_k = src->k;
   const size_t dest_n = dest->n;
   const size_t dest_k = dest->k;

   if (src_n != dest_n || src_k != dest_k)
     {
       GSL_ERROR ("multiset lengths are not equal", GSL_EBADLEN);
     }

   {
     size_t j;

     for (j = 0; j < src_k; j++)
       {
         dest->data[j] = src->data[j];
       }
   }

   return GSL_SUCCESS;
}


/* ieee-utils/test.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 <stdlib.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_test.h>

#if defined(HAVE_IRIX_IEEE_INTERFACE)
/* don't test denormals on IRIX */
#else
#define TEST_DENORMAL 1
#endif

int
main (void)
{
  float zerof = 0.0f, minus_onef = -1.0f ;
  double zero = 0.0, minus_one = -1.0 ;

  /* Check for +ZERO (float) */

  {
    float f = 0.0f;
    const char mantissa[] = "00000000000000000000000";
    gsl_ieee_float_rep r;
    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 0, "float x = 0, sign is +");
    gsl_test_int (r.exponent, -127, "float x = 0, exponent is -127");
    gsl_test_str (r.mantissa, mantissa, "float x = 0, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_ZERO, "float x = 0, type is ZERO");
  }

  /* Check for -ZERO (float) */

  {
    float f = minus_onef;
    const char mantissa[] = "00000000000000000000000";
    gsl_ieee_float_rep r;

    while (f < 0) {
      f *= 0.1f;
    }

    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 1, "float x = -1*0, sign is -");
    gsl_test_int (r.exponent, -127, "float x = -1*0, exponent is -127");
    gsl_test_str (r.mantissa, mantissa, "float x = -1*0, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_ZERO, "float x = -1*0, type is ZERO");
  }

  /* Check for a positive NORMAL number (e.g. 2.1) (float) */

  {
    float f = 2.1f;
    const char mantissa[] = "00001100110011001100110";

    gsl_ieee_float_rep r;
    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 0, "float x = 2.1, sign is +");
    gsl_test_int (r.exponent, 1, "float x = 2.1, exponent is 1");
    gsl_test_str (r.mantissa, mantissa, "float x = 2.1, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL, "float x = 2.1, type is NORMAL");
  }

  /* Check for a negative NORMAL number (e.g. -1.3304...) (float) */

  {
    float f = -1.3303577090924210f ;
    const char mantissa[] = "01010100100100100101001";

    gsl_ieee_float_rep r;
    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 1, "float x = -1.3304..., sign is -");
    gsl_test_int (r.exponent, 0, "float x = -1.3304..., exponent is 0");
    gsl_test_str (r.mantissa, mantissa, "float x = -1.3304..., mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL,
		  "float x = -1.3304..., type is NORMAL");
  }

  /* Check for a large positive NORMAL number (e.g. 3.37e31) (float) */

  {
    float f = 3.37e31f;
    const char mantissa[] = "10101001010110101001001";
    gsl_ieee_float_rep r;
    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 0, "float x = 3.37e31, sign is +");
    gsl_test_int (r.exponent, 104, "float x = 3.37e31, exponent is 104");
    gsl_test_str (r.mantissa, mantissa, "float x = 3.37e31, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL, "float x = 3.37e31, type is NORMAL");
  }

  /* Check for a small positive NORMAL number (e.g. 3.37e-31) (float) */

  {
    float f = 3.37e-31f;
    const char mantissa[] = "10110101011100110111011";

    gsl_ieee_float_rep r;
    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 0, "float x = 3.37e-31, sign is +");
    gsl_test_int (r.exponent, -102, "float x = 3.37e-31, exponent is -102");
    gsl_test_str (r.mantissa, mantissa, "float x = 3.37e-31, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL,
		  "float x = 3.37e-31, type is NORMAL");
  }

  /* Check for FLT_MIN (smallest possible number that is not denormal) */

  {
    float f = 1.17549435e-38f;	/* FLT_MIN (float) */
    const char mantissa[] = "00000000000000000000000";
    gsl_ieee_float_rep r;
    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 0, "float x = FLT_MIN, sign is +");
    gsl_test_int (r.exponent, -126, "float x = FLT_MIN, exponent is -126");
    gsl_test_str (r.mantissa, mantissa, "float x = FLT_MIN, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL, "float x = FLT_MIN, type is NORMAL");
  }

  /* Check for FLT_MAX (largest possible number that is not Inf) */

  {
    float f = 3.40282347e+38f;	/* FLT_MAX */
    const char mantissa[] = "11111111111111111111111";

    gsl_ieee_float_rep r;
    gsl_ieee_float_to_rep (&f, &r);

    gsl_test_int (r.sign, 0, "float x = FLT_MAX, sign is +");
    gsl_test_int (r.exponent, 127, "float x = FLT_MAX, exponent is 127");
    gsl_test_str (r.mantissa, mantissa, "float x = FLT_MAX, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL, "float x = FLT_MAX, type is NORMAL");
  }

  /* Check for DENORMAL numbers (e.g. FLT_MIN/2^n) */

#ifdef TEST_DENORMAL
  {
    float f = 1.17549435e-38f;	/* FLT_MIN */
    char mantissa[] = "10000000000000000000000";

    int i;
    gsl_ieee_float_rep r;

    for (i = 0; i < 23; i++)
      {
	float x = f / (float)pow (2.0, 1 + (float) i);
	mantissa[i] = '1';
	gsl_ieee_float_to_rep (&x, &r);

	gsl_test_int (r.sign, 0, "float x = FLT_MIN/2^%d, sign is +", i + 1);
	gsl_test_int (r.exponent, -127,
		      "float x = FLT_MIN/2^%d, exponent is -127", i + 1);
	gsl_test_str (r.mantissa, mantissa,
		      "float x = FLT_MIN/2^%d, mantissa", i + 1);
	gsl_test_int (r.type, GSL_IEEE_TYPE_DENORMAL,
		      "float x = FLT_MIN/2^%d, type is DENORMAL", i + 1);
	mantissa[i] = '0';
      }
  }
#endif

  /* Check for positive INFINITY (e.g. 2*FLT_MAX) */

  {
    float f = 3.40282347e+38f;	/* FLT_MAX */
    const char mantissa[] = "00000000000000000000000";

    gsl_ieee_float_rep r;

    float x;
    x = 2 * f;
    gsl_ieee_float_to_rep (&x, &r);

    gsl_test_int (r.sign, 0, "float x = 2*FLT_MAX, sign is +");
    gsl_test_int (r.exponent, 128, "float x = 2*FLT_MAX, exponent is 128");
    gsl_test_str (r.mantissa, mantissa, "float x = 2*FLT_MAX, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_INF, "float x = -2*FLT_MAX, type is INF");
  }

  /* Check for negative INFINITY (e.g. -2*FLT_MAX) */

  {
    float f = 3.40282347e+38f;	/* FLT_MAX */
    const char mantissa[] = "00000000000000000000000";

    gsl_ieee_float_rep r;

    float x;
    x = -2 * f;
    gsl_ieee_float_to_rep (&x, &r);

    gsl_test_int (r.sign, 1, "float x = -2*FLT_MAX, sign is -");
    gsl_test_int (r.exponent, 128, "float x = -2*FLT_MAX, exponent is 128");
    gsl_test_str (r.mantissa, mantissa, "float x = -2*FLT_MAX, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_INF, "float x = -2*FLT_MAX, type is INF");
  }

  /* Check for NAN (e.g. Inf - Inf) (float) */

  {
    gsl_ieee_float_rep r;
    float x = 1.0f, y = 2.0f, z = zerof;

    x = x / z;
    y = y / z;
    z = y - x;

    gsl_ieee_float_to_rep (&z, &r);

    /* We don't check the sign and we don't check the mantissa because
       they could be anything for a NaN */

    gsl_test_int (r.exponent, 128, "float x = NaN, exponent is 128");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NAN, "float x = NaN, type is NAN");
  }

  /* Check for +ZERO */

  {
    double d = 0.0;
    const char mantissa[]
      = "0000000000000000000000000000000000000000000000000000";
    gsl_ieee_double_rep r;
    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 0, "double x = 0, sign is +");
    gsl_test_int (r.exponent, -1023, "double x = 0, exponent is -1023");
    gsl_test_str (r.mantissa, mantissa, "double x = 0, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_ZERO, "double x = 0, type is ZERO");
  }

  /* Check for -ZERO */

  {
    double d =  minus_one;
    const char mantissa[]
      = "0000000000000000000000000000000000000000000000000000";
    gsl_ieee_double_rep r;

    while (d < 0) {
      d *= 0.1;
    }

    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 1, "double x = -1*0, sign is -");
    gsl_test_int (r.exponent, -1023, "double x = -1*0, exponent is -1023");
    gsl_test_str (r.mantissa, mantissa, "double x = -1*0, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_ZERO, "double x = -1*0, type is ZERO");
  }

  /* Check for a positive NORMAL number (e.g. 2.1) */

  {
    double d = 2.1;
    const char mantissa[]
      = "0000110011001100110011001100110011001100110011001101";
    gsl_ieee_double_rep r;
    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 0, "double x = 2.1, sign is +");
    gsl_test_int (r.exponent, 1, "double x = 2.1, exponent is 1");
    gsl_test_str (r.mantissa, mantissa, "double x = 2.1, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL, "double x = 2.1, type is NORMAL");
  }

  /* Check for a negative NORMAL number (e.g. -1.3304...) */

  {
    double d = -1.3303577090924210146738460025517269968986511230468750;
    const char mantissa[]
      = "0101010010010010010100101010010010001000100011101110";
    gsl_ieee_double_rep r;
    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 1, "double x = -1.3304..., sign is -");
    gsl_test_int (r.exponent, 0, "double x = -1.3304..., exponent is 0");
    gsl_test_str (r.mantissa, mantissa, "double x = -1.3304..., mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL,
		  "double x = -1.3304..., type is NORMAL");
  }

  /* Check for a large positive NORMAL number (e.g. 3.37e297) */

  {
    double d = 3.37e297;
    const char mantissa[]
      = "0100100111001001100101111001100000100110011101000100";
    gsl_ieee_double_rep r;
    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 0, "double x = 3.37e297, sign is +");
    gsl_test_int (r.exponent, 988, "double x = 3.37e297, exponent is 998");
    gsl_test_str (r.mantissa, mantissa, "double x = 3.37e297, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL,
		  "double x = 3.37e297, type is NORMAL");
  }

  /* Check for a small positive NORMAL number (e.g. 3.37e-297) */

  {
    double d = 3.37e-297;
    const char mantissa[]
    = "0001101000011011101011100001110010100001001100110111";
    gsl_ieee_double_rep r;
    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 0, "double x = 3.37e-297, sign is +");
    gsl_test_int (r.exponent, -985, "double x = 3.37e-297, exponent is -985");
    gsl_test_str (r.mantissa, mantissa, "double x = 3.37e-297, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL,
		  "double x = 3.37e-297, type is NORMAL");
  }

  /* Check for DBL_MIN (smallest possible number that is not denormal) */

  {
    double d = 2.2250738585072014e-308;		/* DBL_MIN */
    const char mantissa[]
      = "0000000000000000000000000000000000000000000000000000";
    gsl_ieee_double_rep r;
    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 0, "double x = DBL_MIN, sign is +");
    gsl_test_int (r.exponent, -1022, "double x = DBL_MIN, exponent is -1022");
    gsl_test_str (r.mantissa, mantissa, "double x = DBL_MIN, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL,
		  "double x = DBL_MIN, type is NORMAL");
  }

  /* Check for DBL_MAX (largest possible number that is not Inf) */

  {
    double d = 1.7976931348623157e+308;		/* DBL_MAX */
    const char mantissa[]
    = "1111111111111111111111111111111111111111111111111111";
    gsl_ieee_double_rep r;
    gsl_ieee_double_to_rep (&d, &r);

    gsl_test_int (r.sign, 0, "double x = DBL_MAX, sign is +");
    gsl_test_int (r.exponent, 1023, "double x = DBL_MAX, exponent is 1023");
    gsl_test_str (r.mantissa, mantissa, "double x = DBL_MAX, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NORMAL,
		  "double x = DBL_MAX, type is NORMAL");
  }

  /* Check for DENORMAL numbers (e.g. DBL_MIN/2^n) */

#ifdef TEST_DENORMAL
  {
    double d = 2.2250738585072014e-308;		/* DBL_MIN */
    char mantissa[]
      = "1000000000000000000000000000000000000000000000000000";
    int i;
    gsl_ieee_double_rep r;

    for (i = 0; i < 52; i++)
      {
	double x = d / pow (2.0, 1 + (double) i);
	mantissa[i] = '1';
	gsl_ieee_double_to_rep (&x, &r);

	gsl_test_int (r.sign, 0, "double x = DBL_MIN/2^%d, sign is +", i + 1);
	gsl_test_int (r.exponent, -1023,
		      "double x = DBL_MIN/2^%d, exponent", i + 1);
	gsl_test_str (r.mantissa, mantissa,
		      "double x = DBL_MIN/2^%d, mantissa", i + 1);
	gsl_test_int (r.type, GSL_IEEE_TYPE_DENORMAL,
		      "double x = DBL_MIN/2^%d, type is DENORMAL", i + 1);
	mantissa[i] = '0';
      }
  }
#endif

  /* Check for positive INFINITY (e.g. 2*DBL_MAX) */

  {
    double d = 1.7976931348623157e+308;		/* DBL_MAX */
    const char mantissa[]
      = "0000000000000000000000000000000000000000000000000000";
    gsl_ieee_double_rep r;

    double x;
    x = 2.0 * d;
    gsl_ieee_double_to_rep (&x, &r);

    gsl_test_int (r.sign, 0, "double x = 2*DBL_MAX, sign is +");
    gsl_test_int (r.exponent, 1024, "double x = 2*DBL_MAX, exponent is 1024");
    gsl_test_str (r.mantissa, mantissa, "double x = 2*DBL_MAX, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_INF, "double x = 2*DBL_MAX, type is INF");
  }

  /* Check for negative INFINITY (e.g. -2*DBL_MAX) */

  {
    double d = 1.7976931348623157e+308;		/* DBL_MAX */
    const char mantissa[]
      = "0000000000000000000000000000000000000000000000000000";
    gsl_ieee_double_rep r;

    double x;
    x = -2.0 * d;
    gsl_ieee_double_to_rep (&x, &r);

    gsl_test_int (r.sign, 1, "double x = -2*DBL_MAX, sign is -");
    gsl_test_int (r.exponent, 1024, "double x = -2*DBL_MAX, exponent is 1024");
    gsl_test_str (r.mantissa, mantissa, "double x = -2*DBL_MAX, mantissa");
    gsl_test_int (r.type, GSL_IEEE_TYPE_INF,"double x = -2*DBL_MAX, type is INF");
  }

  /* Check for NAN (e.g. Inf - Inf) */

  {
    gsl_ieee_double_rep r;
    double x = 1.0, y = 2.0, z = zero;

    x = x / z;
    y = y / z;
    z = y - x;

    gsl_ieee_double_to_rep (&z, &r);

    /* We don't check the sign and we don't check the mantissa because
       they could be anything for a NaN */

    gsl_test_int (r.exponent, 1024, "double x = NaN, exponent is 1024");
    gsl_test_int (r.type, GSL_IEEE_TYPE_NAN, "double x = NaN, type is NAN");
  }

  exit (gsl_test_summary ());
}


/* rng/benchmark.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, 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 <time.h>
#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_errno.h>

void benchmark (const gsl_rng_type * T);

#define N  1000000
int isum;
double dsum;

int
main (void)
{
  benchmark(gsl_rng_ranlux);
  benchmark(gsl_rng_ranlux389);
  benchmark(gsl_rng_ranlxs0);
  benchmark(gsl_rng_ranlxs1);
  benchmark(gsl_rng_ranlxs2);
  benchmark(gsl_rng_ranlxd1);
  benchmark(gsl_rng_ranlxd2);

  benchmark(gsl_rng_slatec);
  benchmark(gsl_rng_gfsr4);
  benchmark(gsl_rng_cmrg);
  benchmark(gsl_rng_minstd);
  benchmark(gsl_rng_mrg);
  benchmark(gsl_rng_mt19937);
  benchmark(gsl_rng_r250);
  benchmark(gsl_rng_ran0);
  benchmark(gsl_rng_ran1);
  benchmark(gsl_rng_ran2);
  benchmark(gsl_rng_ran3);
  benchmark(gsl_rng_rand48);
  benchmark(gsl_rng_rand);
  benchmark(gsl_rng_random8_bsd);
  benchmark(gsl_rng_random8_glibc2);
  benchmark(gsl_rng_random8_libc5);
  benchmark(gsl_rng_random128_bsd);
  benchmark(gsl_rng_random128_glibc2);
  benchmark(gsl_rng_random128_libc5);
  benchmark(gsl_rng_random256_bsd);
  benchmark(gsl_rng_random256_glibc2);
  benchmark(gsl_rng_random256_libc5);
  benchmark(gsl_rng_random32_bsd);
  benchmark(gsl_rng_random32_glibc2);
  benchmark(gsl_rng_random32_libc5);
  benchmark(gsl_rng_random64_bsd);
  benchmark(gsl_rng_random64_glibc2);
  benchmark(gsl_rng_random64_libc5);
  benchmark(gsl_rng_random_bsd);
  benchmark(gsl_rng_random_glibc2);
  benchmark(gsl_rng_random_libc5);
  benchmark(gsl_rng_randu);
  benchmark(gsl_rng_ranf);

  benchmark(gsl_rng_ranmar);
  benchmark(gsl_rng_taus);
  benchmark(gsl_rng_transputer);
  benchmark(gsl_rng_tt800);
  benchmark(gsl_rng_uni32);
  benchmark(gsl_rng_uni);
  benchmark(gsl_rng_vax);
  benchmark(gsl_rng_zuf);
  return 0;
}

void
benchmark (const gsl_rng_type * T)
{
  int start, end;
  int i = 0, d = 0 ;
  double t1, t2;

  gsl_rng *r = gsl_rng_alloc (T);

  start = clock ();
  do
    {
      int j;
      for (j = 0; j < N; j++)
	isum += gsl_rng_get (r);

      i += N;
      end = clock ();
    }
  while (end < start + CLOCKS_PER_SEC/10);

  t1 = (end - start) / (double) CLOCKS_PER_SEC;

  start = clock ();
  do
    {
      int j;
      for (j = 0; j < N; j++)
	dsum += gsl_rng_uniform (r);

      d += N;
      end = clock ();
    }
  while (end < start + CLOCKS_PER_SEC/10);

  t2 = (end - start) / (double) CLOCKS_PER_SEC;

  printf ("%6.0f k ints/sec, %6.0f k doubles/sec, %s\n",
	  i / t1 / 1000.0, d / t2 / 1000.0, gsl_rng_name (r));

  gsl_rng_free (r);
}


#ifndef Photometry_h
#define Photometry_h
/**
 * @file
 * $Revision: 1.5 $
 * $Date: 2008/07/09 19:40:52 $
 *
 *   Unless noted otherwise, the portions of Isis written by the USGS are
 *   public domain. See individual third-party library and package descriptions
 *   for intellectual property information, user agreements, and related
 *   information.
 *
 *   Although Isis has been used by the USGS, no warranty, expressed or
 *   implied, is made by the USGS as to the accuracy and functioning of such
 *   software and related material nor shall the fact of distribution
 *   constitute any such warranty, and no responsibility is assumed by the
 *   USGS in connection therewith.
 *
 *   For additional information, launch
 *   $ISISROOT/doc//documents/Disclaimers/Disclaimers.html
 *   in a browser or see the Privacy & Disclaimers page on the Isis website,
 *   http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on
 *   http://www.usgs.gov/privacy.html.
 */

#include <string>
#include <vector>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_roots.h>

namespace Isis {
  class Pvl;
  class PhotoModel;
  class AtmosModel;
  class NormModel;
  /**
   * @author ????-??-?? Unknown
   *
   * @internal
   *  @history 2007-08-02 Steven Lambright - Fixed memory leak
   *  @history 2008-03-07 Janet Barrett - Added SetPhotomWl method to allow
   *                      the application to set the p_normWavelength variable for
   *                      use by MoonAlbedo normalization.
   *  @history 2008-06-18 Steven Koechle - Fixed Documentation Errors
   *  @history 2008-07-09 Steven Lambright - Fixed unit test
   *  @history 2011-08-19 Sharmila Prasad - Implemented brentminimizer using GSL
   *  @history 2011-09-15 Sharmila Prasad - Implemented brent's root solver using GSL
   */
  class Photometry {
    public:
      Photometry(Pvl &pvl);
      Photometry() {};
      virtual ~Photometry();

      //! Calculate the surface brightness
      void Compute(double pha, double inc, double ema, double dn,
                   double &albedo, double &mult, double &base);
      void Compute(double pha, double inc, double ema, double deminc,
                   double demema, double dn, double &albedo,
                   double &mult, double &base);

      //! Set the wavelength
      virtual void SetPhotomWl(double wl);

      //! Double precision version of bracketing algorithm ported from Python.
      //! Solution bracketing for 1-D minimization routine.
      static void minbracket(double &xa, double &xb, double &xc, double &fa,
          double &fb, double &fc, double Func(double par, void *params),
          void *params);

      //! Brent's method 1-D minimization routine using GSL's r8Brent minimization Algorithm
      static int brentminimizer(double x_lower, double x_upper, gsl_function *Func,
          double & x_minimum, double tolerance);

      //! GSL's the Brent-Dekker method (Brent's method) combines an interpolation strategy
      //! with the bisection algorithm to estimate the root of the quadratic function.
      static int brentsolver(double x_lo, double x_hi, gsl_function *Func, double tolerance, double &root);

      PhotoModel *GetPhotoModel() const {
        return p_phtPmodel;
      }

      AtmosModel *GetAtmosModel() const {
        return p_phtAmodel;
      }

      NormModel *GetNormModel() const {
        return p_phtNmodel;
      }

    protected:
      AtmosModel *p_phtAmodel;
      PhotoModel *p_phtPmodel;
      NormModel *p_phtNmodel;
  };
};

#endif


/* ******************************************************************************
 *
 *
 * This program and the accompanying materials are made available under the
 * terms of the Apache License, Version 2.0 which is available at
 * https://www.apache.org/licenses/LICENSE-2.0.
 *
 *  See the NOTICE file distributed with this work for additional
 *  information regarding copyright ownership.
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 ******************************************************************************/

//
// @author raver119@gmail.com
//

#ifndef LIBND4J_GEMM_H
#define LIBND4J_GEMM_H

#include <cblas.h>
#include <math/templatemath.h>
#include <system/op_boilerplate.h>

namespace sd {
     namespace blas {
         template <typename T>
         static void * transpose(int orderSource, int orderTarget, int rows, int cols, void *source);

         static inline int linearIndexC(int rows, int cols, int r, int c);
         static inline int linearIndexF(int rows, int cols, int r, int c);

         template <typename X, typename Y, typename Z>
         class GEMM {
         protected:
         public:
             static void op(int Order, int TransA, int TransB, int M, int N, int K, double alpha, void *A, int lda, void *B, int ldb, double beta, void *C, int ldc);
         };

         template <typename X, typename Y, typename Z>
         class GEMV : public sd::blas::GEMM<X, Y, Z>{
         public:
             static void op(int TRANS, int M, int N, double alpha, void* vA, int lda, void* vX, int incx, double beta, void* vY, int incy );
         };

         int FORCEINLINE linearIndexC(int rows, int cols, int r, int c) {
             return (r * cols + c);
         }

         int FORCEINLINE linearIndexF(int rows, int cols, int r, int c) {
             return (c * rows + r);
         }

    }
}

#endif //LIBND4J_GEMM_H


/* -----------------------------------------------------------------------------
 * Copyright 2021 Jonathan Haigh
 * SPDX-License-Identifier: MIT
 * ---------------------------------------------------------------------------*/

#ifndef SQ_INCLUDE_GUARD_core_narrow_inl_h_
#define SQ_INCLUDE_GUARD_core_narrow_inl_h_

#include "core/errors.h"

#include <fmt/format.h>
#include <fmt/ostream.h>
#include <gsl/gsl>

namespace sq {

template <typename T, typename U, typename... FormatArgs>
constexpr T narrow(U value, FormatArgs &&...format_args) {
  try {
    return gsl::narrow<T>(value);
  } catch (gsl::narrowing_error &e) {
    throw NarrowingError{T{}, value, SQ_FWD(format_args)...};
  }
}

constexpr gsl::index to_index(auto value, auto &&...format_args) {
  return narrow<gsl::index>(SQ_FWD(value), SQ_FWD(format_args)...);
}

constexpr std::size_t to_size(auto value, auto &&...format_args) {
  return narrow<std::size_t>(SQ_FWD(value), SQ_FWD(format_args)...);
}

} // namespace sq

#endif // SQ_INCLUDE_GUARD_core_narrow_inl_h_


#ifndef GBNET_GRAPHBASE
#define GBNET_GRAPHBASE

#include <iostream>
#include <string>
#include <vector>
#include <utility>
#include <map>
#include <set>
#include <stdio.h>

#include <gsl/gsl_rng.h>

#include "RVNode.h"
#include "XNode.h"
#include "HNodeORNOR.h"
#include "YDataNode.h"
#include "SNode.h"
#include "NodeDictionary.h"

namespace gbn
{
    // used accross module. might want to find a better place for these typedefs
    typedef std::vector<RVNode *> node_vector_t;
    typedef std::pair<std::string, int> trg_de_pair_t;
    typedef std::map<std::string, int> evidence_dict_t;

    typedef std::set<std::string> prior_active_tf_set_t;

    typedef std::pair<std::string, std::string> src_trg_pair_t;
    typedef std::pair<src_trg_pair_t, int> network_edge_t;
    typedef std::vector<network_edge_t> network_t;

    class GraphBase
    {
        private:
        protected:
            gsl_rng * rng;

        public:
            node_vector_t random_nodes;
            node_vector_t norand_nodes; /* Used to keep track of node pointers for later destruction */

            GraphBase (unsigned int);
            virtual ~GraphBase ();

            void sample (unsigned int);
            void burn_stats();
            void print_stats();
    };
}

#endif


/**
 * author: Jochen K"upper
 * created: Jan 2002
 * file: pygsl/src/statisticsmodule.c
 * $Id: floatmodule.c,v 1.8 2004/03/24 08:40:45 schnizer Exp $
 *
 * "
 */

#include <Python.h>
#include <gsl/gsl_statistics.h>
#include <pygsl/error_helpers.h>
#include <pygsl/block_helpers.h>

/* include real functions for default data-types (double in C) */

#define STATMOD_WEIGHTED
#define STATMOD_APPEND_PY_TYPE(X) X ## Float32
#define STATMOD_APPEND_PYC_TYPE(X) X ## FLOAT
#define STATMOD_FUNC_EXT(X, Y) X ## _float ## Y
#define STATMOD_PY_AS_C PyFloat_AsDouble
#define STATMOD_C_TYPE float
#include "functions.c"

/* initialization */

PyGSL_STATISTICS_INIT(float, "float")

/*
 * Local Variables:
 * mode: c
 * c-file-style: "Stroustrup"
 * End:
 */


#pragma once

#include <chrono>
#include <fmt/printf.h>
#include <gsl/gsl-lite.hpp>

namespace angonoka::cli {
/**
    Pretty-print values with as much info as possible.

    @var value Value to be pretty-printed
*/
template <typename T> struct verbose {
    T value;
};
template <typename T> verbose(T) -> verbose<T>;

namespace detail {
    /**
        Helper function to print verbose durations.

        @param name Duration (s, m, d, etc)

        @return Formatter function
    */
    template <typename T>
    constexpr auto verbose_duration(gsl::czstring name)
    {
        return [=](auto& total, auto& ctx, bool& needs_space) {
            const auto dur = std::chrono::floor<T>(total);
            if (dur == dur.zero()) return;
            total -= dur;
            if (needs_space) fmt::format_to(ctx.out(), " ");
            needs_space = true;
            const auto ticks = dur.count();
            fmt::format_to(ctx.out(), "{}{}", ticks, name);
        };
    }
} // namespace detail
} // namespace angonoka::cli

namespace fmt {
using angonoka::cli::verbose;

/**
    User-defined formatter for std::chrono durations.
*/
template <typename... Ts>
struct fmt::formatter<verbose<std::chrono::duration<Ts...>>> {
    using value_type = verbose<std::chrono::duration<Ts...>>;
    static constexpr auto parse(format_parse_context& ctx)
    {
        return ctx.end();
    }

    template <typename FormatContext>
    constexpr auto format(const value_type& obj, FormatContext& ctx)
    {
        using angonoka::cli::detail::verbose_duration;
        using namespace std::chrono;
        if (obj.value == obj.value.zero())
            return format_to(ctx.out(), "0s");
        bool needs_space = false;
        auto total = duration_cast<seconds>(obj.value);
        [&](auto&&... fns) {
            (fns(total, ctx, needs_space), ...);
        }(verbose_duration<months>("mo"),
          verbose_duration<days>("d"),
          verbose_duration<hours>("h"),
          verbose_duration<minutes>("m"),
          verbose_duration<seconds>("s"));
        return ctx.out();
    }
};
} // namespace fmt


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

//------------------------------------------------------------------------------
// KeypointSpatialIndex.h
//
// Allows to construction, query, and removal of Keypoint entries based on pixel position.
//------------------------------------------------------------------------------

#pragma once

#include "Utils\thread_memory.h"

#include <memory>
#include <vector>
#include <functional>
#include <opencv2\features2d\features2d.hpp>

#include <gsl/span>

namespace mage
{
    class KeypointSpatialIndex
    {
        struct Impl;

    public:
        KeypointSpatialIndex(gsl::span<const cv::KeyPoint> keypoints);

        void Query(const cv::Point2f& center, int octave, float radius, temp::vector<size_t>& results) const;
        void Remove(const cv::KeyPoint& keypoint, size_t value);

        ~KeypointSpatialIndex();

    private:
        static constexpr float octaveSpacing = 100;
        static constexpr float octaveQueryRange = 1;

        std::unique_ptr<Impl> m_impl;
    };
}


/**
 * \file Utilities.h
 */

#ifndef ATK_CORE_UTILITIES_H
#define ATK_CORE_UTILITIES_H

#include <ATK/Core/config.h>

#include <gsl/gsl>

#include <cstddef>

namespace ATK
{
  /// Class to convert arrays from different type to another type
  template<typename DataType1, typename DataType2>
  class ATK_CORE_EXPORT ConversionUtilities
  {
  public:
    /*!
     * @brief Method to convert an array to another, using double as the intermediate type
     * @param input_array
     * @param output_array
     * @param size
     * @param offset
     * @param ports
     */
    static void convert_array(const DataType1* input_array, DataType2* output_array, gsl::index size, gsl::index offset = 0, gsl::index ports = 1);
  };

  class ATK_CORE_EXPORT RuntimeError: public std::runtime_error
  {
  public:
    explicit RuntimeError(const std::string& what_arg);
    explicit RuntimeError(const char* what_arg);
  };
}

#endif