/*
 * particle_filters.c
 *
 *  Created on: 18 Sep 2020
 *      Author: heine
 */
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_eigen.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <Accelerate/Accelerate.h>

#include "particle_filters.h"

void MLbootstrapfilter(double* y, double sig_std, double obs_std, int data_length, double x0,
                       int* sample_sizes, double *worst_case_sign_ratio, double* x_hats, double *filtering_time, int* mesh_sizes,  int kl, int ku, double* ab0, double* ab1, int ldab, int* ipiv0, int* ipiv1, int ldb0, int ldb1, int info, double w, double d, double E, double I, double h0, double h1, double L , double* meass, int N_meas) {

  int make_correction = 1; // If one ML telescoping correction is made, if 0 not.
  int N_levels = 2; // hard coded for now
  int N = 0;

  const char prog_bar[51] = "-------------------------------------------------";
  int* minds0 = (int*)malloc(2 * N_meas * sizeof(int));
  int* minds1 = minds0 + N_meas;
  double* obss = (double*)malloc(N_meas*sizeof(double));
  double lower_bound = 0.25, upper_bound = L - 0.25;

  clock_t start = clock();
  int bar_segment_count = 0;

  for (short i = 0; i < N_levels; i++) {
    N += sample_sizes[i];
    printf("N%i = %i, ", i, sample_sizes[i]);
  }
  printf("N = %i, L0 = %i, L1 = %i\n", N, mesh_sizes[0],mesh_sizes[1]);

  double *X = (double*) malloc(2 * N * sizeof(double));
  double W1_tmp;
  double* B0 = (double*) malloc(sample_sizes[0]*mesh_sizes[0]*sizeof(double));
  double* B1 = (double*) malloc(sample_sizes[1]*mesh_sizes[1]*sizeof(double));
  double* pred_meass0 = (double*) malloc(2 * sample_sizes[1] * N_meas * sizeof(double));
  double* pred_meass1 = pred_meass0 + sample_sizes[1] * N_meas;
  double* W = (double*) malloc(2 * N * sizeof(double));
  double* absW = W + N;
  short* signs = (short*) malloc(2 * N * sizeof(short));
  short* signs_res = signs + N;
  int* ind = (int*) malloc(2 * N * sizeof(int));
  int* permutation = ind + N; // needed for uniform resampling

  double x_hat = 0;
  double p_hat = 0;

  double* X0 = X;
  double* X1 = X + sample_sizes[0];
  double* W0 = W;
  double* W1 = W + sample_sizes[0];
  double* X_res = X + N;

  printf(
         "%s\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
         prog_bar);
  fflush(stdout);

  /* Make copies of the band matrices as DGBSV destroys them */
  double* a0_copy = (double*) malloc(ldab * mesh_sizes[0] * sizeof(double));
  double* a1_copy = (double*) malloc(ldab * mesh_sizes[1] * sizeof(double));

  /*
   * Initial sample and level indicator
   */
  gsl_rng * rng = gsl_rng_alloc(gsl_rng_taus);
  gsl_rng_set(rng, clock());
  for (int i = 0; i < N; i++) {
    X[i] = gsl_ran_gaussian(rng, sig_std) + x0; // init sample
    W[i] = (double) 1.0 / (double) N; // init weights
    signs_res[i] = 1;
  }

  double normaliser = 0;
  double abs_normaliser = 0;
  //  double diff;
  double ESS, ESSABS;
  //  double tmp_x_hat;
  double positive_mass, negative_mass;

  //  FILE* bpf_out = fopen("mlbpf_out.txt", "w");

  clock_t iteration_timer;
  clock_t resampling_timer;
  clock_t level_timer_start;
  double level_0_particle_cost = 0;
  double level_1_particle_cost = 0;
  double rsamp_time_per_prcl = 0;
  worst_case_sign_ratio[0] = 100;
  double sign_balance;
  double obs_std_scale = 0.8;
  double intercepts[2];
  double slopes[2];

  int res_pos, res_neg;
  int dodb = 0;

  // Mesh point indices for the measurement locations
  for(int i = 0; i < N_meas; i++) {
    minds0[i] = (int)(meass[i] / L * (double) mesh_sizes[0]);
    minds1[i] = (int)(meass[i] / L * (double) mesh_sizes[1]);
  }

  /*
   * FILTER MAIN LOOP
   */
  for (int n = 0; n < data_length; n++) {

    // Restore the destroyed band matrix
    array_copy(ab0, a0_copy, ldab * mesh_sizes[0]);
    array_copy(ab1, a1_copy, ldab * mesh_sizes[1]);

    iteration_timer = clock(); // start the stopwatch
    level_timer_start = clock();

    // Extract the observations for the current time step
    for(int i = 0; i < N_meas; i++)
      obss[i] = y[n + i * data_length];

    /*
     * Level 1 weight calculation
     */

    /* Calculate Level 1 first, so we can estimate the bias of the observation mapping */

    /* Solve the beam for level 0 mesh and sample N1 */
    ebb_solve(sample_sizes[1], X1, mesh_sizes[0], B1, w, d, h0, E, I, L, kl, ku, a0_copy, ldab, ipiv0, ldb0, info, N_meas, minds0, pred_meass0);

    /* Solve the beam for level 1 mesh and sample N1 */
    ebb_solve(sample_sizes[1], X1, mesh_sizes[1], B1, w, d, h1, E, I, L, kl, ku, a1_copy, ldab, ipiv1, ldb1, info, N_meas, minds1, pred_meass1);

    /* Fit a first order LS model for the error between level 0 and level 1 */
    if(sample_sizes[1] > 0){ // do only if N1 > 0
      ls_fit(N_meas, sample_sizes[1], X1, pred_meass0, pred_meass1, intercepts, slopes);
    }

    /* Now that we know the bias, we can evaluate the weights for level 1 */
    double diff;
    for (int j = 0; j < sample_sizes[1]; j++) {
      W1[j] = 1;
      W1_tmp = 1;
      for(int k = 0; k < N_meas; k++){
        /* Level 0 */
        diff = obss[k] - pred_meass0[sample_sizes[1] * k + j] - intercepts[k] - slopes[k] * X1[j];
        W1_tmp *= exp( - diff * diff / (double) 2.0 / (obs_std * obs_std_scale) / (obs_std * obs_std_scale));
        /* Level 1 */
        diff = obss[k] - pred_meass1[sample_sizes[1] * k + j];
        W1[j] *= exp( - diff * diff / (double) 2.0 / obs_std / obs_std );
      }

      W1[j] -= W1_tmp; // take the difference and...
      W1[j] /= (double) sample_sizes[1]; // ... normalise by sample size N1
//      W1[j] = 0; /* uncomment to disable level 1 correction*/
    }

    /*
     Level 0 weight calculation
     */

    array_copy(ab0, a0_copy, ldab * mesh_sizes[0]);

    likelihood(X0, sample_sizes[0], obss, obs_std_scale*obs_std, mesh_sizes[0], kl, ku, a0_copy, ldab, ipiv0, ldb0, info, B0, w, d, h0, E, I, W0, L, minds0, N_meas, dodb, intercepts, slopes);

    // normalise by the sample size (N0)
    for (int i = 0; i < sample_sizes[0]; i++)
      W[i] /= (double)sample_sizes[0];

    if(!make_correction) {
      for(int i = 0; i < sample_sizes[1]; i++) {
        W1[i] = 0;
      }
    }

    /* Joint normalisation of the weights */
    normaliser = 0;
    abs_normaliser = 0;
    for (long i = 0; i < N; i++) {

      W[i] *= (double) signs_res[i]; // Take the sign into account

      absW[i] = fabs(W[i]);
      signs[i] = W[i] > 0 ? 1 : -1;
      normaliser += W[i];
      abs_normaliser += absW[i];
    }

    /* Normalise */
    positive_mass = 0;
    negative_mass = 0;
    for (long i = 0; i < N; i++) {
      W[i] /= normaliser;
      if (W[i] > 0) {
        positive_mass += W[i];
      } else {
        negative_mass += fabs(W[i]);
      }
      absW[i] /= abs_normaliser;
    }
    sign_balance = positive_mass / negative_mass;
    if (sign_balance < worst_case_sign_ratio[0]) {
      worst_case_sign_ratio[0] = sign_balance;
    }

    ESS = 0;
    ESSABS = 0;
    for (long i = 0; i < N; i++) {
      ESS += W[i] * W[i];
      ESSABS += absW[i] * absW[i];
    }
    ESS = (double) 1.0 / ESS;
    ESSABS = (double) 1.0 / ESSABS;

    /* Resample */
    resampling_timer = clock();
    resample(N, absW, ind, rng);
    random_permuter(permutation, N, rng);
    rsamp_time_per_prcl += (double) (clock() - resampling_timer) / CLOCKS_PER_SEC / (double) N;

    normaliser = 0;
    res_pos = 0;
    res_neg = 0;
    for (long i = 0; i < N; i++) {
      X_res[permutation[i]] = X[ind[i]];
      signs_res[permutation[i]] = signs[ind[i]];
      normaliser += signs[ind[i]];
      if(signs[ind[i]]>0) {
        res_pos++;
      }else{
        res_neg++;
      }
    }
    for (long i = 0; i < N; i++) {
      W[i] = (double) signs_res[i] / normaliser;
    }

    /* Calculate the output: posterior mean */
    if(x_hats!=NULL){
      x_hat = 0;
      for (int i = 0; i < N; i++) {
        x_hat += X_res[i] * W[i];
        if(isnan(x_hat)){
          printf("NaN detected!\n X_res[i] = %e, W[i] = %e, i = %i, normaliser = %e + = %e, - = %e\nESS=%e, res_+ =%i, res_- = %i\n",
                 X_res[i], W[i], i,normaliser,positive_mass, negative_mass,ESS,res_pos,res_neg);
          fflush(stdout);
          getchar();
        }
      }
      x_hats[n] = x_hat;

      /* Calculate the output: posterior variance */
      p_hat = 0;
      for (long i = 0; i < N; i++) {
        diff = X_res[i] - x_hat;
        p_hat += diff * diff * W[i];
      }
    }

    /* Mutate */
    for (long i = 0; i < N; i++) {
      X[i] = X_res[i] + gsl_ran_gaussian(rng, sig_std);

      if(X[i] < lower_bound)
        X[i] = lower_bound + lower_bound-X[i];
      if(X[i] > upper_bound)
        X[i] = upper_bound - (X[i]-upper_bound);
    }

    if (floor(n * (double) 50 / data_length) > bar_segment_count) {
      printf("█");
      fflush(stdout);
      bar_segment_count++;
    }

  }

  filtering_time[0] = (double) (clock() - start) / CLOCKS_PER_SEC / (double) data_length;
  filtering_time[1] = level_0_particle_cost / (double) data_length / (double) sample_sizes[0];
  filtering_time[2] = level_1_particle_cost / (double) data_length / (double) sample_sizes[1];
  filtering_time[3] = rsamp_time_per_prcl / (double) data_length / (double) N;
  printf(" %5.5f sec\n", filtering_time[0]);

  free(X);
  free(W);
  free(signs);
  free(ind);
  free(a0_copy);
  free(a1_copy);
  free(B0);
  free(B1);
  free(minds0);
  free(obss);
  free(pred_meass0);
  gsl_rng_free(rng);

}

void bootstrapfilter(double* y, double sig_std, double obs_std, int data_length, double x0,
                     int N, double* x_hats, double* p_hats, double *filtering_time, int mesh_size,	int kl, int ku, double* ab, int ldab, int* ipiv, int ldb, int info, double w, double d, double E, double I, double h , double L, double *meass, int N_meas) {

  const char prog_bar[51] = "-------------------------------------------------";
  clock_t start;
  int bar_segment_count = 0;

  printf("Basic BPF with N = %i\n", N);

  double *X = (double*) malloc(2 * N * sizeof(double));
  double *X_res = X + N;
  double *W = (double*) malloc(N * sizeof(double));
  int *ind = (int*) malloc(N * sizeof(int));
  double* payloads = (double*) malloc(mesh_size * N * sizeof(double));
  // Because DGBSV destroys the AB matrix, we need to make a copy of it
  double* ab_copy = (double*) malloc(ldab * mesh_size * sizeof(double));
  double x_hat = 0;
  double p_hat = 0;
  double lower_bound = 0.25, upper_bound = L - 0.25;
  double intercepts[2] = {0,0};
  double slopes[2] = {0,0};

  printf(
         "%s\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
         prog_bar);
  fflush(stdout);

  start = clock();

  /*
   * Initial sample and level indicator
   */
  gsl_rng * rng = gsl_rng_alloc(gsl_rng_taus);
  gsl_rng_set(rng, clock());
  for (int i = 0; i < N; i++) {
    X[i] = gsl_ran_gaussian(rng, sig_std) + x0;
    W[i] = (double) 1.0 / (double) N;
  }

  double normaliser = 0;
  double diff;
  double ESS;
  double cost_per_particle = 0;

  int* minds = (int*)malloc(N_meas * sizeof(int));
  double* obss = (double*)malloc(N_meas * sizeof(double));
  for(int i = 0; i < N_meas; i++){
    minds[i] = (int)(meass[i] / L * (double) mesh_size);
  }

  /* Classic BPF main loop */
  /* --------------------- */
  for (int n = 0; n < data_length; n++) {

    /* Weight calculation */
    normaliser = 0;

    array_copy(ab, ab_copy, ldab * mesh_size);

    /*
     * Weight calculation
     */
    clock_t stopwatch_start = clock();
    for(int i = 0; i < N_meas; i++)
      obss[i] = y[n + i*data_length];

    likelihood(X, N, obss, obs_std, mesh_size, kl, ku, ab_copy, ldab, ipiv, ldb, info, payloads, w, d, h, E, I, W, L, minds, N_meas, 0, intercepts, slopes);
    clock_t stopwatch_stop = clock();

    cost_per_particle += (double)(stopwatch_stop-stopwatch_start)/(double) CLOCKS_PER_SEC;

    // Normalise the weights
    normaliser = 0;
    for (int i = 0; i < N; i++)
      normaliser += W[i];
    for (int i = 0; i < N; i++)
      W[i] /= normaliser;

    ESS = 0;
    for (int i = 0; i < N; i++) {
      ESS += W[i] * W[i];
    }
    ESS = (double) 1.0 / ESS;

    /* Resample */
    resample(N, W, ind, rng);
    for (int i = 0; i < N; i++) {
      X_res[i] = X[ind[i]];
    }

    /* Calculate the output: posterior mean */
    if(x_hats != NULL) {
      x_hat = 0;
      for (int i = 0; i < N; i++) {
        x_hat += X_res[i] / (double) N;
      }
      x_hats[n] = x_hat;
      /* Calculate the output: posterior variance */
      p_hat = 0;
      for (int i = 0; i < N; i++) {
        diff = X_res[i] - x_hat;
        p_hat += diff * diff / (double) (N-1);
      }
      if(p_hats!=NULL)
        p_hats[n] = p_hat;
    }
    /* Mutate */
    for (int i = 0; i < N; i++) {
      X[i] = X_res[i] + gsl_ran_gaussian(rng, sig_std);

      if(X[i] < lower_bound)
        X[i] = lower_bound + lower_bound-X[i];
      if(X[i] > upper_bound)
        X[i] = upper_bound - (X[i]-upper_bound);
    }

    if (floor(n * (double) 50 / data_length) > bar_segment_count) {
      printf("█");
      fflush(stdout);
      bar_segment_count++;
    }
  }
  filtering_time[0] = (double) (clock() - start) / CLOCKS_PER_SEC / (double) data_length;
  filtering_time[1] = cost_per_particle / (double) data_length;
  filtering_time[2] = 0;
  filtering_time[3] = 0;
  printf(" %5.5f sec\n", filtering_time[0]);

  free(X);
  free(W);
  free(ind);
  free(ab_copy);
  free(payloads);
  free(minds);
  free(obss);
  gsl_rng_free(rng);
}

/* Computes a first order LS fit for error between level 0 and level 1 */
void ls_fit(int N_meas, int N1, double* X1, double* pred_meass0, double* pred_meass1, double* intercepts, double* slopes) {

  double a,b,c,d,e,f;

  for(int j = 0; j < N_meas; j++) {

    /* First order polynomial fit to the bias */
    a = 0;
    b = 0;
    d = 0;
    e = 0;
    f = 0;
    for(int i = 0; i < N1; i++) {
      a++;
      b += X1[i];
      d += X1[i] * X1[i];
      e += pred_meass1[j * N1 + i] - pred_meass0[j * N1 + i];
      f += X1[i] * (pred_meass1[j * N1 + i] - pred_meass0[j * N1 + i]);
    }
    c = b;
    intercepts[j] = (e * d - b * f) / (a * d - b * c);
    slopes[j] = (-c * e  + a * f) / (a * d - b * c);
  }
}

void likelihood(double* x, int N, double* y, double std, int mesh_size, int kl, int ku, double* ab, int ldab, int* ipiv, int ldb, int info, double* b, double w, double d, double h, double E, double I, double* likes, double L, int* minds, int N_meas, int dodb, double* intercepts, double* slopes) {

  // Create the instantaneous payload
  create_payload(N, mesh_size, b, w, d, x, h, E, I, L);

  // Solve the Euler-Bernoulli beam for each particle
  dgbsv_(&mesh_size, &kl, &ku, &N, ab, &ldab, ipiv, b, &ldb, &info);

  double diff;
  for (int j = 0; j < N; j++) {
    diff = 1;
    likes[j] = 1;
    for(int k = 0; k < N_meas; k++){
      diff = y[k] - b[j * mesh_size + minds[k]] - x[j]*slopes[k] - intercepts[k];
      likes[j] *= exp( - diff * diff / (double) 2.0 / std / std) ;
    }
  }
}

void random_permuter(int *permutation, int N, gsl_rng *r) {

  for (int i = 0; i < N; i++)
    permutation[i] = i;

  int j;
  int tmp;
  for (int i = N - 1; i > 0; i--) {
    j = (int)gsl_rng_uniform_int(r, i + 1);
    tmp = permutation[j];
    permutation[j] = permutation[i];
    permutation[i] = tmp;
  }

}

void resample(int size, double *w, int *ind, gsl_rng *r) {

  /* Generate the exponentials */
  double *e = (double*) malloc((size + 1) * sizeof(double));
  double g = 0;
  for (int i = 0; i <= size; i++) {
    e[i] = gsl_ran_exponential(r, 1.0);
    g += e[i];
  }
  /* Generate the uniform order statistics */
  double *u = (double *) malloc((size + 1) * sizeof(double));
  u[0] = 0;
  for (int i = 1; i <= size; i++)
    u[i] = u[i - 1] + e[i - 1] / g;

  /* Do the actual sampling with inverse cdf */
  double cdf = w[0];
  int j = 0;
  for (int i = 0; i < size; i++) {
    while (cdf < u[i + 1]) {
      j++;
      cdf += w[j];
    }
    ind[i] = j;
  }

  free(e);
  free(u);
}

void create_payload(int N, int n, double *b, double w, double d, double* x, double h, double E, double I, double L) {

  double g = -9.81; // Gravity acceleration
  double scaler = h * h * h * h / (E * I);
  double slab_thickness = 0.20; // meters
  double density = 7874; // kg/ m^3
  double slab_width = 0.72; // m
  int location_index_centre = 0, location_index_right = 0, location_index_left = 0;
  double deviation = slab_thickness / (double) 8;
  double diff;

  for (long j = 0; j < N;j++) { // iterate over particles

    for(int i = 0; i < n; i++)
      b[j * n + i] = g * w * d * (double)480;

    // Find the index corresponding to the location x
    location_index_centre = (int)(x[j] / L * (double) n);

    // Left end of the slab
    location_index_left = location_index_centre -  ceil(slab_thickness / h / (double) 2);
    // make sure index is non-negative
    location_index_left = location_index_left < 0 ? 0 : location_index_left;

    // Right end of the slab
    location_index_right = location_index_centre +  ceil(slab_thickness / h / (double) 2);
    // make sure index is less than mesh size
    location_index_right = location_index_right > n - 1 ? n - 1 : location_index_right;

    if(location_index_centre < 0) {
      printf("NEGATIVE INDEX!\n");
      fflush(stdout);
    }

    // Add weight to the location
    for(int i = location_index_left; i <= location_index_right ; i++){
      diff = x[j] - i*h;

      b[j * n + i] -=  slab_width * slab_width * density * 9.81 * exp(- diff * diff / (double) 2 / deviation / deviation);
    }
    // Scale appropriately (there is some physical justification to this)
    for(int i = 0; i < n;i++) {
      b[j * n + i] *= scaler;
    }
  }
}

void ebb_solve(int N, double* x, int mesh_size, double* b, double w, double d, double h, double E, double I, double L, int kl, int ku, double *ab, int ldab, int *ipiv, int ldb, int info, int N_meas, int* minds, double* pred_measurements) {

  create_payload(N, mesh_size, b, w, d, x, h, E, I, L);
  dgbsv_(&mesh_size, &kl, &ku, &N, ab, &ldab, ipiv, b, &ldb, &info);

  for (int j = 0; j < N; j++) {
      for(int k = 0; k < N_meas; k++){
        pred_measurements[N * k + j] = b[j * mesh_size + minds[k]];
      }
    }
}

void array_copy(double *src, double* dst, int length) {
  for(int i = 0; i < length; i++)
    dst[i] = src[i];
}


/* rng/transputer.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 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 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 <gsl/gsl_rng.h>

/* This is the INMOS Transputer Development System generator. The sequence is,

   x_{n+1} = (a x_n) mod m

   with a = 1664525 and m = 2^32. The seed specifies the initial
   value, x_1.

   The theoretical value of x_{10001} is 1244127297.

   The period of this generator is 2^30. */

static inline unsigned long int transputer_get (void *vstate);
static double transputer_get_double (void *vstate);
static void transputer_set (void *state, unsigned long int s);

typedef struct
  {
    unsigned long int x;
  }
transputer_state_t;

static unsigned long int
transputer_get (void *vstate)
{
  transputer_state_t *state = (transputer_state_t *) vstate;

  state->x = (1664525 * state->x) & 0xffffffffUL;

  return state->x;
}

static double
transputer_get_double (void *vstate)
{
  return transputer_get (vstate) / 4294967296.0 ;
}

static void
transputer_set (void *vstate, unsigned long int s)
{
  transputer_state_t *state = (transputer_state_t *) vstate;

  if (s == 0)
    s = 1 ;   /* default seed is 1. */

  state->x = s;

  return;
}

static const gsl_rng_type transputer_type =
{"transputer",                          /* name */
 0xffffffffUL,                  /* RAND_MAX */
 1,                             /* RAND_MIN */
 sizeof (transputer_state_t),
 &transputer_set,
 &transputer_get,
 &transputer_get_double};

const gsl_rng_type *gsl_rng_transputer = &transputer_type;


#ifndef ALM_L_KATYUSHA_H
#define ALM_L_KATYUSHA_H

#include "L_Katyusha.h"

#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <sstream>
//#include "cmd_line.h"

//This class implements the method IPALM_L_Katyusha

/*
The optimization problem to solve is:

min f_0(x)+\sum_{i=1}^m h_i(f_i(x))+P(x)

x is a d-dimensional vector.

// Each subproblem solves problem of the form
f_0(x)+ \sum_{i=1}^m h_{\beta_s}^i(f_i(x);\lambda_s^i) +g(x)+ \beta_s/2\|x-x_s\|^2
 by L_Katyuhsa
*/

template<typename L, typename D>
class ALM_L_Katyusha: public L_Katyusha<L, D>
{
private:

protected:

  std::vector<D> x_s;

  std::vector<D> old_x_s;

  std::vector<D> lambda_s;

  std::vector<D> old_lambda_s;

  std::vector<D> x_tmp;

  std::vector<D> g_tmp;

  std::vector<D> g_tmp_x;
  std::vector<D> g_tmp_w;
  std::vector<D> valuefi;

  std::vector<D> is_feasiblity_constraint;

  D beta_s;

  D epsilon_s;

  D m_s;

  D m_0;

  L m;

  L d;

  D eta;

  D rho;

  D tau_s;

  std::vector<D> L_phi;

  D function_value;

  D infeasibility;

  L print_every_N_ALM;

  D running_time_ALM;

  L nb_outer_iters;

  ofstream samp_ALM;

public:

  D mu_g;

  D lambda1;

  D lambda2;

  virtual inline void set_Li_Lf(){}

  virtual inline D value_of_P(vector<D> &){return D(NULL);}
  virtual inline void prox_of_P(D, vector<D> &, vector<D> &){} //prox_of_P(L,x,y) computes y=argmin{P(u)+L/2||u-x||^2}
  virtual inline D prox_of_h_star_j(D,D, L){return D(NULL);} //prox_of_h_star_j(x,beta,j) computes argmin{h*(y)+beta/2(y-x)^2}
  virtual inline D value_of_h_star_j(D, L){return D(NULL);}
  virtual inline void compute_gradient_f0(vector<D> &,vector<D> &){} //compute_gradient_f0(x,g) computes g=nabla f0(x)
  virtual inline D compute_gradient_and_value_f_i(vector<D> &,vector<D> &, L){return D(NULL);}//compute_gradient_and_value_f_i(i,x,g) computes g=nabla f_i(x) and return f_i(x)

  virtual inline D value_of_h_j(D, L){return D(NULL);}
  virtual inline D distance_to_domain_of_h_j(D,L){return D(NULL);}

  virtual inline D value_of_f0(vector<D> &){return D(NULL);} //f0(x)
  virtual inline D value_of_f_i(vector<D> &,L){return D(NULL);} // f_i(x)
  virtual inline void set_dimension(){} // set the value of m and d
  virtual inline void set_is_feasibility_constraint(){}

  ALM_L_Katyusha()
  : L_Katyusha<L,D>()
  {

  }

  D value_of_phi_i(L i){
    D res= 0;
    if (i==0)
    res=value_of_f0(this->x);
    else
    {
      D fi=value_of_f_i(this->x,i);
      i=i-1;
      D lambdasi=lambda_s[i];
      D tmp= prox_of_h_star_j(fi/beta_s+ lambdasi,beta_s,i);
      res=fi*tmp- value_of_h_star_j(tmp,i)- beta_s/2*(tmp- lambdasi)*(tmp- lambdasi);
    }
    return this->nsamples*res;
  }

  void compute_full_gradient(vector<D> & x, vector<D> & gx){
    compute_gradient_f0(x,gx);
    for(L i=0;i<m;i++){
          D fi=compute_gradient_and_value_f_i(x,g_tmp,i+1);
          D tmp= prox_of_h_star_j(fi/beta_s+ lambda_s[i],beta_s,i);
          for(L j=0;j<d;j++)
            gx[j]+=g_tmp[j]*tmp;
    }
  }

  void compute_batch_delta_gradient(){
    for (L j=0; j< this->batch_size; j++){
      L s= this->batch_i[j];
      if(s==0){
        compute_gradient_f0(this->x,g_tmp_x);
        compute_gradient_f0(this->w,g_tmp_w);
        add_up(1.,1.,this->theta_S[s]);
      }
      else{
        L i=s-1;
        D fi_x=compute_gradient_and_value_f_i(this->x,g_tmp_x,s);
        D tmp_x= prox_of_h_star_j(fi_x/beta_s+ lambda_s[i],beta_s,i);
        D fi_w=compute_gradient_and_value_f_i(this->w,g_tmp_w,s);
        D tmp_w= prox_of_h_star_j(fi_w/beta_s+ lambda_s[i],beta_s,i);
        add_up(tmp_x,tmp_w,this->theta_S[s]);
      }
    }
  }

  void  add_up(D fx, D fw, D c){
      for(L i=0;i<d;i++)
        this->batch_delta_gradient[i]+=(fx*g_tmp_x[i]-fw*g_tmp_w[i])*c;
    }

  inline D value_of_g(){
    D res= value_of_P(this->x);
    for(L i=0;i<d;i++)
      res+= 0.5*beta_s*(this->x[i]- x_s[i])*(this->x[i]- x_s[i]);
    return res;
  }

  void prox_of_g(D stepsz, vector<D> & x, vector<D> & g, vector<D> & nextx){
    D tmp0=beta_s+stepsz;
    for(L i=0;i<d;i++)
     x_tmp[i]=(beta_s*x_s[i]+stepsz*x[i]-g[i])/tmp0;
    prox_of_P(tmp0, x_tmp, nextx);
  }

  void compute_x(){
    for (L i=0; i< d; i++){
      x_s[i]= this->x[i];
    }
    for (L i= 0; i< m; i++){
      valuefi[i]=value_of_f_i(x_s,i+1);
    }
  }

  void update_lambda(){
    for(L j=0;j<m;j++)
    {
      D fi= valuefi[j];
      lambda_s[j]= prox_of_h_star_j(fi/beta_s+ lambda_s[j],beta_s,j);
    }
  }

  void compute_function_value(){
    function_value=value_of_f0(x_s);
    D res= 0;
    for (L i= 0; i< m; i++){
      D fi=value_of_f_i(x_s,i+1);
      if(is_feasiblity_constraint[i]==1){
        D tmp=distance_to_domain_of_h_j(fi,i);
        res+=tmp*tmp;
      }
      else
       function_value+=value_of_h_j(fi,i);
    }
    function_value+=value_of_P(x_s);
    infeasibility=sqrt(res);
  }

inline void compute_m0(D beta0, D val_eta, D val_rho, L val_tau){
  cout<<"beta0="<<beta0<<"val_eta="<<val_eta<<"; val_rho="<<val_rho<<"; val_tau="<<val_tau<<endl;
  m_s= this->nsamples/val_tau*1e+6;

  //D tmp7=1-val_tau/this->n*sqrt(beta0/(max_Lf_s+max_M_s/beta0+beta0));
  //cout<<"tmp7: "<<tmp7<<"; "<<val_tau/(this->n+0.)*sqrt(beta0/(max_Lf_s+max_M_s/beta0+beta0))<<endl;
  //m_0=(2*log(val_rho)+log(val_eta)+log(2))/log(tmp7);

  cout<<" m_s="<<m_s<<endl;
}

void Initialize(D beta_0, D epsilon_0,  D val_eta, D val_rho,L val_tau, vector<D> & x0,vector<D> & y0){
  cout<<"start initializing ALM"<<endl;
  set_dimension();
  this->nsamples=m+1;
  this->nfeatures=d;
  this->tau=val_tau;

  beta_s=beta_0;
  tau_s= 1;
  epsilon_s=epsilon_0;
  compute_m0(beta_s,val_eta,val_rho,val_tau);

  eta=val_eta;
  rho=val_rho;

  x_s.resize(d,0);
  old_x_s.resize(d,0);
  lambda_s.resize(m,0);
  old_lambda_s.resize(m,0);
  for(L i=0;i<d;i++){
    x_s[i]=x0[i];
    old_x_s[i]= x0[i];
  }
  for(L j=0;j<m;j++){
    lambda_s[j]=y0[j];
    old_lambda_s[j]= y0[j];
  }
  g_tmp.clear();
  g_tmp.resize(d,0);
  g_tmp_x.clear();
  g_tmp_x.resize(d,0);
  g_tmp_w.clear();
  g_tmp_w.resize(d,0);
  valuefi.clear();
  valuefi.resize(m,0);
  x_tmp.clear();
  x_tmp.resize(d,0);
  is_feasiblity_constraint.resize(m,0);

  set_is_feasibility_constraint();
  cout<<"Initialization ALM finished!"<<endl;

}

void reset_everything(){
  epsilon_s*=rho;
  beta_s*=eta;
}

void update_m_s(L val_tau){
  D tmp= 0;
  D tmpx= 0;
  D tmpy= 0;
  D tmpy2= 0;
  for(L j=0;j<m;j++)
  {
    D tmp_p= prox_of_h_star_j(valuefi[j]/beta_s/eta+ lambda_s[j], beta_s/eta, j);
    tmpy+=(tmp_p- lambda_s[j])*(tmp_p- lambda_s[j]);
    tmp+= (lambda_s[j]- old_lambda_s[j])*(lambda_s[j]- old_lambda_s[j]);
    tmpy2+= (beta_s*eta*lambda_s[j]- beta_s*old_lambda_s[j])*(beta_s*eta*lambda_s[j]- beta_s*old_lambda_s[j]);
  }
  for (L i= 0; i< d;i++){
    tmpx= (x_s[i]- old_x_s[i])*(x_s[i]- old_x_s[i]);
  }
  D tmp4= 2*epsilon_s+ beta_s*tmp;
  D tmp5= (1- eta)*beta_s/2*tmpy;
  D tmp6= 0;
  D tmp7= sqrt(tmp)*sqrt(tmpy2);
  D tmp1= log(tmp4+ tmp5+ tmp6+ tmp7+ 2/(2*eta- 1)*tau_s*beta_s/2*tmpx);
  D tmp2= log(epsilon_s)+ log(rho)- log(2);
  D tmp3= sqrt(beta_s/this->sumLi);
  m_s= ceil((tmp1- tmp2)/log(2)*4*max(1.0+this->nsamples,1/tmp3)/val_tau);
  cout<<"here here="<<tmp4<<" tmp5="<<tmp5<<" tmp6="<<tmp6<<" tmp7="<<tmp7<<" tmp1="<<tmp1<<" tmp2="<<tmp2<<" tmp3="<<tmp3<<endl;
}

inline void compute_and_record_res(){
  if(nb_outer_iters%print_every_N_ALM==0){
    compute_function_value();
    cout<<setprecision(9)<<"Iteration: "<<nb_outer_iters<<"; time="<<running_time_ALM<< "; function value="<<function_value<<"; infeasibility= "<< infeasibility<<endl;
    samp_ALM<<setprecision(9)<<nb_outer_iters<<" "<<running_time_ALM<<" "<< function_value<<" "<< infeasibility<<endl;
  }
}

void ALM_solve_with_L_Katyusha(D beta_0, D epsilon_0,  D eta, D rho,vector<D> & x0,vector<D> & y0, L val_tau, L max_nb_outer, L p_N_1, L p_N_2, string filename1, string filename2, D time){
  Initialize(beta_0, epsilon_0, eta, rho,val_tau, x0, y0);
  nb_outer_iters=0;
  filename1= "results/ALM_"+filename1;
  samp_ALM.open(filename1.c_str());
  running_time_ALM=0;
  print_every_N_ALM=p_N_1;
  this->set_print_every_N(p_N_2);
  compute_and_record_res();

 while(nb_outer_iters<max_nb_outer){
  D start= std::clock();
  cout<<"m_s= "<< ceil(m_s/this->nsamples*val_tau)<<"; beta_s="<<beta_s<<"; epsilon_s="<<epsilon_s<<endl;
  this->loopless2(x_s, filename2, mu_g+ tau_s*beta_s,  ceil(m_s/this->nsamples*val_tau), epsilon_s, val_tau, 1, 1, 0, 1);
  for(L i=0;i<d;i++){
    old_x_s[i]=x_s[i];
  }
  for(L i=0;i<m;i++){
    old_lambda_s[i]=lambda_s[i];
  }
  compute_x();
  update_lambda();
  update_m_s(val_tau);
  nb_outer_iters++;
  running_time_ALM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  compute_and_record_res();
  start = std::clock();
  reset_everything();
  running_time_ALM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    if (running_time_ALM> time){
      break;
    }
  }
}

};

#endif /* MIN_SMOOTH_CONVEX_H */


/* linalg/choleskyc.c
 *
 * Copyright (C) 2007 Patrick Alken
 * Copyright (C) 2010 Huan Wu (gsl_linalg_complex_cholesky_invert)
 *
 * 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_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>

/*
 * This module contains routines related to the Cholesky decomposition
 * of a complex Hermitian positive definite matrix.
 */

static void cholesky_complex_conj_vector(gsl_vector_complex *v);

/*
gsl_linalg_complex_cholesky_decomp()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix. See Golub & Van Loan, "Matrix Computations" (3rd ed),
algorithm 4.2.2.

Inputs: A - (input/output) complex postive definite matrix

Return: success or error

The lower triangle of A is overwritten with the Cholesky decomposition
*/

int
gsl_linalg_complex_cholesky_decomp(gsl_matrix_complex *A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i, j;
      gsl_complex z;
      double ajj;

      for (j = 0; j < N; ++j)
        {
          z = gsl_matrix_complex_get(A, j, j);
          ajj = GSL_REAL(z);

          if (j > 0)
            {
              gsl_vector_complex_const_view aj =
                gsl_matrix_complex_const_subrow(A, j, 0, j);

              gsl_blas_zdotc(&aj.vector, &aj.vector, &z);
              ajj -= GSL_REAL(z);
            }

          if (ajj <= 0.0)
            {
              GSL_ERROR("matrix is not positive definite", GSL_EDOM);
            }

          ajj = sqrt(ajj);
          GSL_SET_COMPLEX(&z, ajj, 0.0);
          gsl_matrix_complex_set(A, j, j, z);

          if (j < N - 1)
            {
              gsl_vector_complex_view av =
                gsl_matrix_complex_subcolumn(A, j, j + 1, N - j - 1);

              if (j > 0)
                {
                  gsl_vector_complex_view aj =
                    gsl_matrix_complex_subrow(A, j, 0, j);
                  gsl_matrix_complex_view am =
                    gsl_matrix_complex_submatrix(A, j + 1, 0, N - j - 1, j);

                  cholesky_complex_conj_vector(&aj.vector);

                  gsl_blas_zgemv(CblasNoTrans,
                                 GSL_COMPLEX_NEGONE,
                                 &am.matrix,
                                 &aj.vector,
                                 GSL_COMPLEX_ONE,
                                 &av.vector);

                  cholesky_complex_conj_vector(&aj.vector);
                }

              gsl_blas_zdscal(1.0 / ajj, &av.vector);
            }
        }

      /* Now store L^H in upper triangle */
      for (i = 1; i < N; ++i)
        {
          for (j = 0; j < i; ++j)
            {
              z = gsl_matrix_complex_get(A, i, j);
              gsl_matrix_complex_set(A, j, i, gsl_complex_conjugate(z));
            }
        }

      return GSL_SUCCESS;
    }
} /* gsl_linalg_complex_cholesky_decomp() */

/*
gsl_linalg_complex_cholesky_solve()
  Solve A x = b where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_solve (const gsl_matrix_complex * cholesky,
                                   const gsl_vector_complex * b,
                                   gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_complex_memcpy (x, b);

      /* solve for y using forward-substitution, L y = b */

      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasNonUnit, cholesky, x);

      /* perform back-substitution, L^H x = y */

      gsl_blas_ztrsv (CblasLower, CblasConjTrans, CblasNonUnit, cholesky, x);

      return GSL_SUCCESS;
    }
} /* gsl_linalg_complex_cholesky_solve() */

/*
gsl_linalg_complex_cholesky_svx()
  Solve A x = b in place where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_svx (const gsl_matrix_complex * cholesky,
                                 gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* solve for y using forward-substitution, L y = b */

      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasNonUnit, cholesky, x);

      /* perform back-substitution, L^H x = y */

      gsl_blas_ztrsv (CblasLower, CblasConjTrans, CblasNonUnit, cholesky, x);

      return GSL_SUCCESS;
    }
} /* gsl_linalg_complex_cholesky_svx() */

/******************************************************************************

gsl_linalg_complex_cholesky_invert()
  Compute the inverse of an Hermitian positive definite matrix in
  Cholesky form.

Inputs: LLT - matrix in cholesky form on input
              A^{-1} = L^{-H} L^{-1} on output

Return: success or error
******************************************************************************/

int
gsl_linalg_complex_cholesky_invert(gsl_matrix_complex * LLT)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      size_t N = LLT->size1;
      size_t i, j;
      gsl_vector_complex_view v1;

      /* invert the lower triangle of LLT */
      for (i = 0; i < N; ++i)
        {
          double ajj;
          gsl_complex z;

          j = N - i - 1;

          {
            gsl_complex z0 = gsl_matrix_complex_get(LLT, j, j);
            ajj = 1.0 / GSL_REAL(z0);
          }

          GSL_SET_COMPLEX(&z, ajj, 0.0);
          gsl_matrix_complex_set(LLT, j, j, z);

          {
            gsl_complex z1 = gsl_matrix_complex_get(LLT, j, j);
            ajj = -GSL_REAL(z1);
          }

          if (j < N - 1)
            {
              gsl_matrix_complex_view m;

              m = gsl_matrix_complex_submatrix(LLT, j + 1, j + 1,
                                       N - j - 1, N - j - 1);
              v1 = gsl_matrix_complex_subcolumn(LLT, j, j + 1, N - j - 1);

              gsl_blas_ztrmv(CblasLower, CblasNoTrans, CblasNonUnit,
                             &m.matrix, &v1.vector);

              gsl_blas_zdscal(ajj, &v1.vector);
            }
        } /* for (i = 0; i < N; ++i) */

      /*
       * The lower triangle of LLT now contains L^{-1}. Now compute
       * A^{-1} = L^{-H} L^{-1}
       *
       * The (ij) element of A^{-1} is column i of conj(L^{-1}) dotted into
       * column j of L^{-1}
       */

      for (i = 0; i < N; ++i)
        {
          gsl_complex sum;
          for (j = i + 1; j < N; ++j)
            {
              gsl_vector_complex_view v2;
              v1 = gsl_matrix_complex_subcolumn(LLT, i, j, N - j);
              v2 = gsl_matrix_complex_subcolumn(LLT, j, j, N - j);

              /* compute Ainv[i,j] = sum_k{conj(Linv[k,i]) * Linv[k,j]} */
              gsl_blas_zdotc(&v1.vector, &v2.vector, &sum);

              /* store in upper triangle */
              gsl_matrix_complex_set(LLT, i, j, sum);
            }

          /* now compute the diagonal element */
          v1 = gsl_matrix_complex_subcolumn(LLT, i, i, N - i);
          gsl_blas_zdotc(&v1.vector, &v1.vector, &sum);
          gsl_matrix_complex_set(LLT, i, i, sum);
        }

      /* copy the Hermitian upper triangle to the lower triangle */

      for (j = 1; j < N; j++)
        {
          for (i = 0; i < j; i++)
            {
              gsl_complex z = gsl_matrix_complex_get(LLT, i, j);
              gsl_matrix_complex_set(LLT, j, i, gsl_complex_conjugate(z));
            }
        }

      return GSL_SUCCESS;
    }
} /* gsl_linalg_complex_cholesky_invert() */

/********************************************
 *           INTERNAL ROUTINES              *
 ********************************************/

static void
cholesky_complex_conj_vector(gsl_vector_complex *v)
{
  size_t i;

  for (i = 0; i < v->size; ++i)
    {
      gsl_complex z = gsl_vector_complex_get(v, i);
      gsl_vector_complex_set(v, i, gsl_complex_conjugate(z));
    }
} /* cholesky_complex_conj_vector() */


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include <math.h>
#include <time.h>
#include <stdbool.h>
#include "bb_smc.h"

/* ==========================================================
Last update: 26.4.20 -- v1.1

ABC-SMC (parallel version)
Author: Marco Esposito
=========================================================== */

#define n_param 1
#define size_data 10

#define a0 0.5
#define b0 0.5
#define std_ker 0.4
#define striding_size 2
#define n 10

// ARGUMENT NUMBER 1: time at which the simulation starts
// ARGUMENT NUMBER 2: seed
// ARGUMENT NUMBER 3: number of particles to consider for the simulation (for shorter runs)

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

	/* ==================== VARIABLES DECLARATION ======================== */
	FILE *w_theta;
	char weights_theta[500];

	int time = atoi(argv[1]);
	int seed = atoi(argv[2]);
	int n_particles = atoi(argv[3]);

	double weight;
	int status;
	double distance;

	int k;
	double** theta;
	theta = malloc((n_param) * sizeof(double*));
	for (k=0; k<n_param; k++) {
		theta[k] = calloc(n_particles, sizeof(double));
	}

	double* weights;
	weights = malloc((n_particles) * sizeof(double));

	double* theta_star;
	theta_star = malloc((n_param) * sizeof(double));

	double* std_kernel;
	std_kernel = malloc((n_param) * sizeof(double));

	std_kernel[0] = std_ker;

	double r2, aux;
	int i, l, i_th, l_temp;
	int sample_accepted;
	/* =============================================== */

	/* If the number of arguments is 4, then argv[3] is the number of particles to consider.
	This is useful when a shorter run is needed (necessary to specify as weights have to
	be normalized accordingly!) */
	if (argc==4) {sprintf(weights_theta, "w_theta_time_%d_part_%d.txt", time, n_particles);}
	else {
		printf("argc = %d\n", argc);
		printf("Usage: %s\n", argv[0]);
		printf("      %s [parameters] \n", argv[0]);
		exit(1);
	}

	// int n_param = 1;

	/* ============== IMPORT THE FILE WITH THE DATA =========== */
	int y[size_data];
	status = import_data_BB(y, size_data);
	if (status == 1) {
		exit(1);
	}
	double mean_y = mean_int(y, size_data);
	/* ========================================================= */

	/* ==================== READ THE DISTANCE FROM A FILE ===================== */
	double eps_t;
	status = read_epsilon(eps_t, time);
	if (status == 1) {
		exit(1);
	}
	/* ========================================================================= */

	/* ================== set up GSL RNG =================== */
	gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);
	gsl_rng_set (r, seed+11); // set seed
	/* ===================================================== */

	sample_accepted = 1;

	/* =============================================================================== */
	/* =============================== START THE ABC ROUND =========================== */
	/* =============================================================================== */
	while (sample_accepted==0) {

		i_th = -1;

		aux = 0.;
		w_theta = fopen(weights_theta, "r");
		if (w_theta == NULL) {printf("File %s not found!!!\n", weights_theta);}
		fscanf(w_theta, "%*[^\n]");
		r2 = RND;
		for (i=0; i<n_particles; i++) {
			fscanf(w_theta, "%lf %lf", &weights[i], &theta[0][i]);
			aux += weights[i];
			if (r2<aux) {
				if (i_th<0) {
					i_th = i;
				}
			}
		}
		fclose(w_theta);

		double* a;
		double* b;

		a = malloc((n_param) * sizeof(double));
		b = malloc((n_param) * sizeof(double));

		a[0] = a0; b[0] = b0;

		/* YOU CAN SIMPLY PERTURB THE PARTICLE WITH A KERNEL AT WILL WITHOUT DOING
		THE LOOP OVER PARAMETERS AS IT'S JUST ONE IN THIS CASE. POSSIBLY IMPLEMENT
		THIS IN A FUNCTION AND PUT IT IN THE .h FILE */
		l_temp = 0;
		//Perturb the i_th particles via a gaussian kernel with std = std_kernel
		for (l=0; l<n_param; l++) {
			theta_star[l] = gsl_ran_gaussian(r, std_kernel[l]) + theta[l][i_th];
			//If the prior on theta_star[l] is zero when calculated in theta_star[l],
			//the new particle is rejected and we start over
			if (prior_BB(l, theta_star[l], a0, b0) == 0.) {
				break;
			}
			l_temp = l;
		}
		if (l_temp < n_param-1) {continue;}

		/* HERE YOU CAN JUST IMPLEMENT THIS WITH A BINARY FUNCTION sample_accepted = verify_sample(distance, ..) */
		// -- If the distance function is too large, start over
		// -- If all the conditions are fulfilled, the sample is accepted
		int g[size_data];
		for (d=0; d<size_data; d++) {g[d] = gsl_ran_binomial(r, theta_star[0], n);}

		if (mean_int(g, size_data) - mean_y == 0) {
			sample_accepted = 1;
		}
		else {continue;} // Start over if distance function not equal to zero

		/* ================ CALCULATE THE WEIGHTS ===================== */
		double tot_prior_pdf = 1.;
		double kernel_value_pdf[n_particles];
		for (i=0; i<n_particles; i++) {kernel_value_pdf[i] = 1.;}

		for (l=0; l<n_param; l++) {
			tot_prior_pdf *= prior_BB(l, theta_star[l], a0, b0);
		}

		double denominator = 0.;
		for (i=0; i<n_particles; i++) {
			for (l=0; l<n_param; l++) {
				kernel_value_pdf[i] *= gsl_ran_gaussian_pdf(theta_star[l] - theta[l][i], std_kernel[l]);
			}
			denominator += weights[i] * kernel_value_pdf[i];
		}
		/* =============================================================== */

		weight = tot_prior_pdf/(float)(denominator);

		/* ============== CREATE THE FILE WITH WEIGHTS AND PARAMETERS ============= */
		write_particles_BB(theta_star, time, seed, striding_size, weight);
		/* ===================================================================== */

		free(a);
		free(b);

	} //END OF THE WHILE LOOP -- PARTICLE WAS ACCEPTED

	/* ====================== CREATE THE FILE WITH THE DISTANCE ============== */
	write_epsilon(distance, time, seed, striding_size);
	/* ======================================================================= */

	for (k=0; k<n_param; k++) {
		free(theta[k]);
	}
	free(theta);
	free(weights);
	free(theta_star);
	free(std_kernel);

	return 0;
} //End of the program