#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

void compute_MCMC(MCMC_info *MCMC) {
    char          filename_output_dir[SID_MAX_FILENAME_LENGTH];
    char          filename_chain_dir[SID_MAX_FILENAME_LENGTH];
    char          filename_results_dir[SID_MAX_FILENAME_LENGTH];
    char          filename_plots_dir[SID_MAX_FILENAME_LENGTH];
    char          filename_run[SID_MAX_FILENAME_LENGTH];
    char          filename_chain[SID_MAX_FILENAME_LENGTH];
    char          filename_chain_config[SID_MAX_FILENAME_LENGTH];
    char          filename_chain_covariance[SID_MAX_FILENAME_LENGTH];
    char          filename_stats[SID_MAX_FILENAME_LENGTH];
    char          filename_coverage[SID_MAX_FILENAME_LENGTH];
    char          filename_histograms[SID_MAX_FILENAME_LENGTH];
    char          filename_results[SID_MAX_FILENAME_LENGTH];
    char          filename_stop[SID_MAX_FILENAME_LENGTH];
    char          column_txt[SID_MAX_FILENAME_LENGTH];
    char          problem_name_test[MCMC_NAME_SIZE];
    char          P_name_test[MCMC_NAME_SIZE];
    char          format_string[64];
    char          name_test[MCMC_NAME_SIZE];
    char          array_name_test[MCMC_NAME_SIZE];
    int           n_avg_test;
    int           flag_autocor_on_test;
    int           n_P_test;
    int           n_DS_test;
    int           n_arrays_test;
    int           n_M_test;
    double        P_init_test;
    double        M_target_test;
    double        dM_target_test;
    double        array_test;
    double        P_min_test;
    double        P_max_test;
    double        ln_Pr_min;
    double        ln_Pr_max;
    double        ln_Pr_avg;
    int           i_tune;
    int           i_P, j_P, k_P;
    int           i_M, j_M;
    int           ii_P, jj_P;
    int           i_avg;
    int           i_C;
    int           i_DS, n_DS;
    int           i_iteration, j_iteration;
    int           flag_continue;
    char          flag_success;
    int           flag_stop = GBP_FALSE;
    int           i_coverage;
    int           j_coverage;
    int           bin_x;
    int           bin_y;
    int           n_avg;
    int           n_avg_burn;
    int           n_avg_integrate;
    int           n_P, n_C;
    int           n_gals;
    double *      x_P;
    double **     x_M;
    double *      M_target;
    double *      dM_target;
    double *      P_min;
    double *      P_max;
    double *      P_new;
    double *      P_last;
    double *      P_init;
    double *      P_chain;
    double *      P_avg;
    double *      dP_avg;
    double **     M_best;
    double **     M_min;
    double **     M_max;
    double **     M_new;
    double **     M_lo_68;
    double **     M_hi_68;
    double **     M_lo_95;
    double **     M_hi_95;
    size_t ***    M_histogram;
    double **     M_last;
    double **     M_avg;
    double **     dM_avg;
    double *      slopes;
    double *      dP_sub;
    double *      ln_Pr_chain;
    double *      ln_Pr_proposals;
    double *      drift;
    double **     auto_cor;
    double **     P_chain_stats;
    double **     P_prop_stats;
    size_t **     P_histogram;
    size_t **     coverage_true;
    size_t **     coverage_false;
    size_t **     coverage_keep;
    double *      V_read;
    double        L_x, L_y, L_z;
    int           n_x, n_y, n_z;
    int           n_C_x;
    FILE *        fp_run;
    FILE *        fp_chain;
    FILE *        fp_chain_config;
    FILE *        fp_chain_covariance;
    FILE *        fp_stats;
    FILE *        fp_coverage;
    FILE *        fp_histograms;
    FILE *        fp_results;
    FILE *        fp_stop;
    int           seed = 182743;
    int           i_report;
    time_t        time_start, time_stop;
    double        time_diff;
    int           i_iteration_start;
    int           i_iteration_next_report;
    int           n_accepted;
    int           n_active;
    int           n_iterations;
    int           n_iterations_burn;
    int           n_iterations_integrate;
    int           n_iterations_phase;
    int           n_thin;
    int           n_thin_burn;
    int           n_thin_integrate;
    int           i_thin;
    int           n_chains_test;
    int           flag_autocor_on;
    int           n_coverage;
    int           coverage_size;
    int           i_phase;
    int           i_array;
    int *         n_M;
    int           flag_initialized;
    int           flag_no_map_write;
    int           flag_no_map_write_test;
    int           n_used;
    double        RN;
    double ***    P_arrays;
    int *         n_P_arrays;
    double        constant;
    size_t *      histogram_index;
    size_t *      coverage_keep_index;
    size_t        P_best_index;
    size_t        M_best_index;
    double        accumulator;
    size_t        P_lo_68_index;
    size_t        P_hi_68_index;
    size_t        P_lo_95_index;
    size_t        P_hi_95_index;
    size_t        M_lo_68_index;
    size_t        M_hi_68_index;
    size_t        M_lo_95_index;
    size_t        M_hi_95_index;
    double *      P_lo_68;
    double *      P_hi_68;
    double *      P_lo_95;
    double *      P_hi_95;
    double *      P_contour_68;
    double *      P_contour_95;
    double *      P_best;
    int           my_chain;
    int           flag_init;
    int           i_column;
    int           dummy_t;
    int           n_iterations_file_total;
    int           n_iterations_file_burn;
    int           flag_restart     = GBP_FALSE;
    int           flag_minimize_IO = GBP_FALSE;
    MCMC_DS_info *current_DS;
    MCMC_DS_info *next_DS;

    size_t i_iteration_buffer;
    size_t i_P_buffer;
    size_t i_M_buffer;

    SID_log("Performing MCMC...", SID_LOG_OPEN | SID_LOG_TIMER);

    SID_log("Initializing...", SID_LOG_OPEN);

    // Initialize the covariance matrix
    set_MCMC_covariance(MCMC, NULL); // First call with NULL must be after P_init is set!

    // Initialize arrays
    init_MCMC_arrays(MCMC);

    // Grab some stuff from the MCMC structure
    n_P                    = MCMC->n_P;
    n_DS                   = MCMC->n_DS;
    n_avg                  = MCMC->n_avg;
    n_thin                 = MCMC->n_thin;
    coverage_size          = MCMC->coverage_size;
    flag_autocor_on        = MCMC->flag_autocor_on;
    my_chain               = MCMC->my_chain;
    n_iterations           = MCMC->n_iterations;
    n_iterations_burn      = MCMC->n_iterations_burn;
    n_iterations_integrate = n_iterations - n_iterations_burn;
    flag_no_map_write      = SID_CHECK_BITFIELD_SWITCH(MCMC->mode, MCMC_MODE_NO_MAP_WRITE);
    flag_minimize_IO       = SID_CHECK_BITFIELD_SWITCH(MCMC->mode, MCMC_MODE_MINIMIZE_IO);

    SID_log("temperature            = %lf", SID_LOG_COMMENT, MCMC->temperature);
    SID_log("n_avg                  = %d", SID_LOG_COMMENT, n_avg);
    SID_log("n_thin                 = %d", SID_LOG_COMMENT, n_thin);
    SID_log("coverage_size          = %d", SID_LOG_COMMENT, coverage_size);
    SID_log("n_iterations           = %d", SID_LOG_COMMENT, n_iterations);
    SID_log("n_iterations_burn      = %d", SID_LOG_COMMENT, n_iterations_burn);
    SID_log("n_iterations_integrate = %d", SID_LOG_COMMENT, n_iterations_integrate);

    // Initialize random number generator
    SID_log("random seed            = %d", SID_LOG_COMMENT, MCMC->seed);

    if(MCMC->V != NULL)
        SID_log("Covariance matrix is initialized.", SID_LOG_COMMENT);
    else
        SID_log("Covariance matrix is NOT initialized.", SID_LOG_COMMENT);
    if(flag_autocor_on)
        SID_log("Auto-correlation  is on.", SID_LOG_COMMENT);
    else
        SID_log("Auto-correlation  is off.", SID_LOG_COMMENT);
    if(flag_minimize_IO)
        SID_log("Minimize I/O      is on.", SID_LOG_COMMENT);
    else
        SID_log("Minimize I/O      is off.", SID_LOG_COMMENT);

    // Initialize chain arrays
    n_M             = MCMC->n_M;
    M_new           = MCMC->M_new;
    M_last          = MCMC->M_last;
    P_new           = MCMC->P_new;
    P_last          = MCMC->P_last;
    P_init          = MCMC->P_init;
    P_chain         = MCMC->P_chain;
    P_min           = (double *)SID_malloc(sizeof(double) * n_P);
    P_max           = (double *)SID_malloc(sizeof(double) * n_P);
    P_avg           = (double *)SID_malloc(sizeof(double) * n_P);
    dP_avg          = (double *)SID_malloc(sizeof(double) * n_P);
    ln_Pr_chain     = (double *)SID_malloc(sizeof(double) * n_avg);
    ln_Pr_proposals = (double *)SID_malloc(sizeof(double) * n_avg);
    drift           = (double *)SID_malloc(sizeof(double) * n_P);
    slopes          = (double *)SID_malloc(sizeof(double) * n_P);
    dP_sub          = (double *)SID_malloc(sizeof(double) * n_P);
    P_chain_stats   = (double **)SID_malloc(sizeof(double *) * n_P);
    P_prop_stats    = (double **)SID_malloc(sizeof(double *) * n_P);
    if(flag_autocor_on)
        auto_cor = (double **)SID_malloc(sizeof(double *) * n_P);
    else
        auto_cor = NULL;
    for(i_P = 0; i_P < n_P; i_P++) {
        drift[i_P]         = 0.;
        slopes[i_P]        = 0.;
        dP_sub[i_P]        = 0.;
        P_chain_stats[i_P] = (double *)SID_malloc(sizeof(double) * n_avg);
        P_prop_stats[i_P]  = (double *)SID_malloc(sizeof(double) * n_avg);
        if(flag_autocor_on)
            auto_cor[i_P] = (double *)SID_malloc(sizeof(double) * n_avg);
    }

    // Set directories
    sprintf(filename_output_dir, "%s/", MCMC->filename_output_dir);
    sprintf(filename_chain_dir, "%s/chains/", filename_output_dir);
    sprintf(filename_results_dir, "%s/results/", filename_output_dir);
    sprintf(filename_plots_dir, "%s/plots/", filename_output_dir);

    // Set filenames
    sprintf(filename_run, "%s/run.dat", filename_output_dir);
    sprintf(filename_chain, "%s/chain_trace_%06d.dat", filename_chain_dir, my_chain);
    sprintf(filename_chain_config, "%s/chain_config_%06d.dat", filename_chain_dir, my_chain);
    sprintf(filename_chain_covariance, "%s/chain_covariance_%06d.dat", filename_chain_dir, my_chain);
    sprintf(filename_stats, "%s/chain_stats_%06d.dat", filename_chain_dir, my_chain);
    sprintf(filename_coverage, "%s/coverage.dat", filename_results_dir);
    sprintf(filename_histograms, "%s/histograms.dat", filename_results_dir);
    sprintf(filename_stop, "%s/stop", filename_output_dir);

    SID_log("Done.", SID_LOG_CLOSE);

    // Perform integration
    SID_log("Initializing integration...", SID_LOG_OPEN);
    MCMC->flag_integrate_on = GBP_TRUE;

    // Make sure the needed directories exist
    mkdir(filename_output_dir, 02755);
    mkdir(filename_chain_dir, 02755);
    mkdir(filename_results_dir, 02755);
    mkdir(filename_plots_dir, 02755);

    // Read/Write Header file
    if(SID.I_am_Master) {
        // If run.dat already exists, this is a restart.  Check that it is consistant with the current run.
        if((fp_run = fopen(filename_run, "rb")) != NULL) {
            flag_restart = GBP_TRUE;
            SID_log("Checking the consistancy of this run with the previous run...", SID_LOG_OPEN);
            SID_fread_verify(problem_name_test, sizeof(char), MCMC_NAME_SIZE, fp_run);
            if(strcmp(problem_name_test, MCMC->problem_name))
                SID_exit_error("Problem names are inconsistant (i.e. {%s}!={%s}).", SID_ERROR_LOGIC, MCMC->problem_name,
                               problem_name_test);
            SID_fread_verify(&n_chains_test, sizeof(int), 1, fp_run);
            MCMC->n_chains = GBP_MAX(MCMC->n_chains, n_chains_test);
            SID_fread_verify(&n_avg_test, sizeof(int), 1, fp_run);
            if(n_avg_test != n_avg)
                SID_exit_error("Integration averaging intervals are inconsistant (i.e. %d!=%d).", SID_ERROR_LOGIC,
                               n_avg, n_avg_test);
            SID_fread_verify(&flag_autocor_on_test, sizeof(int), 1, fp_run);
            if(flag_autocor_on_test != flag_autocor_on)
                SID_exit_error("Autocorrelation flags are inconsistant (i.e. %d!=%d).", SID_ERROR_LOGIC,
                               flag_autocor_on, flag_autocor_on_test);
            SID_fread_verify(&flag_no_map_write_test, sizeof(int), 1, fp_run);
            if(flag_no_map_write_test != flag_no_map_write)
                SID_exit_error("Map write flags are inconsistant (i.e. %d!=%d).", SID_ERROR_LOGIC, flag_no_map_write,
                               flag_no_map_write_test);
            SID_fread_verify(&n_P_test, sizeof(int), 1, fp_run);
            if(n_P_test != n_P)
                SID_exit_error("The number of paramaters is inconsistant (i.e. %d!=%d).", SID_ERROR_LOGIC, n_P,
                               n_P_test);
            MCMC->P_name_length = 0;
            for(i_P = 0; i_P < n_P; i_P++) {
                SID_fread_verify(P_name_test, sizeof(char), MCMC_NAME_SIZE, fp_run);
                if(strcmp(P_name_test, MCMC->P_names[i_P]))
                    SID_exit_error("Parameter #%d's names are inconsistant (i.e. {%s}!={%s}).", SID_ERROR_LOGIC, i_P,
                                   MCMC->P_names[i_P], P_name_test);
                SID_fread_verify(&P_init_test, sizeof(double), 1, fp_run);
                // if(P_init_test!=MCMC->P_init[i_P])
                //   SID_exit_error("Parameter #%d's initial values are inconsistant (i.e.
                //   %le!=%le).",SID_ERROR_LOGIC,i_P,MCMC->P_init[i_P],P_init_test);
                SID_fread_verify(&P_min_test, sizeof(double), 1, fp_run);
                if(P_min_test != MCMC->P_limit_min[i_P])
                    SID_exit_error("Parameter #%d's minimum values are inconsistant (i.e. %le!=%le).", SID_ERROR_LOGIC,
                                   i_P, MCMC->P_limit_min[i_P], P_min_test);
                SID_fread_verify(&P_max_test, sizeof(double), 1, fp_run);
                if(P_max_test != MCMC->P_limit_max[i_P])
                    SID_exit_error("Parameter #%d's maximum values are inconsistant (i.e. %le!=%le).", SID_ERROR_LOGIC,
                                   i_P, MCMC->P_limit_max[i_P], P_max_test);
                MCMC->P_name_length = GBP_MAX((size_t)(MCMC->P_name_length), strlen(MCMC->P_names[i_P]));
            }
            sprintf(MCMC->P_name_format, "%%-%ds", MCMC->P_name_length);
            SID_fread_verify(&n_arrays_test, sizeof(int), 1, fp_run);
            if(n_arrays_test != MCMC->n_arrays)
                SID_exit_error("Numbers of project arrays are inconsistant (i.e. %d!=%d).", SID_ERROR_LOGIC,
                               MCMC->n_arrays, n_arrays_test);
            for(i_array = 0; i_array < MCMC->n_arrays; i_array++) {
                SID_fread_verify(&array_name_test, sizeof(char), MCMC_NAME_SIZE, fp_run);
                if(strcmp(array_name_test, MCMC->array_name[i_array]))
                    SID_exit_error("Project array names are inconsisitant (i.e. {%s}!={%s}).", SID_ERROR_LOGIC,
                                   MCMC->array_name[i_array], array_name_test);
                for(i_P = 0; i_P < n_P; i_P++) {
                    SID_fread_verify(&array_test, sizeof(double), 1, fp_run);
                    if(array_test != MCMC->array[i_array][i_P])
                        SID_exit_error("Project array #%d element #%d is inconsistant (i.e. %le!=%le).",
                                       SID_ERROR_LOGIC, i_array, i_P, MCMC->array[i_array][i_P], array_test);
                }
            }
            SID_fread_verify(&n_DS_test, sizeof(int), 1, fp_run);
            if(n_DS_test != n_DS)
                SID_exit_error("The number of datasets is inconsistant (i.e. %d!=%d).", SID_ERROR_LOGIC, n_DS,
                               n_DS_test);
            current_DS = MCMC->DS;
            i_DS       = 0;
            while(current_DS != NULL) {
                next_DS = current_DS->next;
                SID_fread_verify(name_test, sizeof(char), MCMC_NAME_SIZE, fp_run);
                if(strcmp(name_test, current_DS->name))
                    SID_exit_error("Dataset #%d's names are inconsistant (i.e. {%s}!={%s}).", SID_ERROR_LOGIC, i_DS,
                                   current_DS->name, name_test);
                SID_fread_verify(&n_M_test, sizeof(int), 1, fp_run);
                if(n_M_test != n_M[i_DS])
                    SID_exit_error("The sizes of dataset #%d are inconsistant (i.e. %d!=%d).", SID_ERROR_LOGIC, i_DS,
                                   n_M_test, n_M[i_DS]);
                for(i_M = 0; i_M < current_DS->n_M; i_M++) {
                    SID_fread_verify(&M_target_test, sizeof(double), 1, fp_run);
                    if(M_target_test != current_DS->M_target[i_M])
                        SID_exit_error("Dataset #%d, element #%d is inconsistant (i.e. %le!=%le).", SID_ERROR_LOGIC,
                                       i_DS, i_M, current_DS->M_target[i_M], M_target_test);
                }
                for(i_M = 0; i_M < current_DS->n_M; i_M++) {
                    SID_fread_verify(&dM_target_test, sizeof(double), 1, fp_run);
                    if(dM_target_test != current_DS->dM_target[i_M])
                        SID_exit_error("Dataset #%d, uncertainty element #%d is inconsistant (i.e. %le!=%le).",
                                       SID_ERROR_LOGIC, i_DS, i_M, current_DS->M_target[i_M], M_target_test);
                }
                SID_fread_verify(&n_arrays_test, sizeof(int), 1, fp_run);
                if(n_arrays_test != current_DS->n_arrays)
                    SID_exit_error("The number of arrays in dataset #%d is inconsistant (i.e. %d!=%d).",
                                   SID_ERROR_LOGIC, i_DS, n_arrays_test, current_DS->n_arrays);
                for(i_array = 0; i_array < current_DS->n_arrays; i_array++) {
                    SID_fread_verify(array_name_test, sizeof(char), MCMC_NAME_SIZE, fp_run);
                    if(strcmp(array_name_test, current_DS->array_name[i_array]))
                        SID_exit_error("Array name #%d for dataset #%d is inconsisitant (i.e. {%s}!={%s}).",
                                       SID_ERROR_LOGIC, i_array, i_DS, current_DS->array_name[i_array], array_name_test);
                    for(i_M = 0; i_M < current_DS->n_M; i_M++) {
                        SID_fread_verify(&array_test, sizeof(double), 1, fp_run);
                        if(array_test != current_DS->array[i_array][i_M])
                            SID_exit_error("Array #%d, element #%d for dataset #%d is inconsistant (i.e. %le!=%le).",
                                           SID_ERROR_LOGIC, i_array, i_M, i_DS, current_DS->array[i_array][i_M],
                                           array_test);
                    }
                }
                current_DS = next_DS;
                i_DS++;
            }
            fclose(fp_run);
            SID_log("Done.", SID_LOG_CLOSE);
        }
        // ... else if run.dat does not exist, create it
        else {
            SID_log("Write header file...", SID_LOG_OPEN);
            if((fp_run = fopen(filename_run, "wb")) == NULL)
                SID_exit_error("Could not open file for writing {%s}.", SID_ERROR_IO_OPEN, filename_run);
            // Stuff relating to this MCMC project
            fwrite(MCMC->problem_name, sizeof(char), MCMC_NAME_SIZE, fp_run);
            fwrite(&(MCMC->n_chains), sizeof(int), 1, fp_run);
            fwrite(&n_avg, sizeof(int), 1, fp_run);
            fwrite(&flag_autocor_on, sizeof(int), 1, fp_run);
            fwrite(&flag_no_map_write, sizeof(int), 1, fp_run);
            fwrite(&n_P, sizeof(int), 1, fp_run);
            for(i_P = 0; i_P < n_P; i_P++) {
                fwrite(MCMC->P_names[i_P], sizeof(char), MCMC_NAME_SIZE, fp_run);
                fwrite(&(MCMC->P_init[i_P]), sizeof(double), 1, fp_run);
                fwrite(&(MCMC->P_limit_min[i_P]), sizeof(double), 1, fp_run);
                fwrite(&(MCMC->P_limit_max[i_P]), sizeof(double), 1, fp_run);
            }
            fwrite(&(MCMC->n_arrays), sizeof(int), 1, fp_run);
            for(i_array = 0; i_array < MCMC->n_arrays; i_array++) {
                fwrite(MCMC->array_name[i_array], sizeof(char), MCMC_NAME_SIZE, fp_run);
                fwrite(MCMC->array[i_array], sizeof(double), n_P, fp_run);
            }
            // Stuff relating to the constraining datasets
            fwrite(&n_DS, sizeof(int), 1, fp_run);
            current_DS = MCMC->DS;
            while(current_DS != NULL) {
                next_DS = current_DS->next;
                fwrite(current_DS->name, sizeof(char), MCMC_NAME_SIZE, fp_run);
                fwrite(&(current_DS->n_M), sizeof(int), 1, fp_run);
                fwrite(current_DS->M_target, sizeof(double), current_DS->n_M, fp_run);
                fwrite(current_DS->dM_target, sizeof(double), current_DS->n_M, fp_run);
                fwrite(&(current_DS->n_arrays), sizeof(int), 1, fp_run);
                for(i_array = 0; i_array < current_DS->n_arrays; i_array++) {
                    fwrite(current_DS->array_name[i_array], sizeof(char), MCMC_NAME_SIZE, fp_run);
                    fwrite(current_DS->array[i_array], sizeof(double), current_DS->n_M, fp_run);
                }
                current_DS = next_DS;
            }
            fclose(fp_run);
            SID_log("Done.", SID_LOG_CLOSE);
        }
    }
    SID_Bcast(&flag_restart, 1, SID_INT, SID_MASTER_RANK, MCMC->comm);
    SID_Bcast(&(MCMC->n_chains), 1, SID_INT, SID_MASTER_RANK, MCMC->comm);
    SID_Bcast(&(MCMC->P_name_length), 1, SID_INT, SID_MASTER_RANK, MCMC->comm);
    sprintf(MCMC->P_name_format, "%%-%ds", MCMC->P_name_length);

    // If this is NOT a restart, start from scratch ...
    n_iterations_file_total = 0;
    n_iterations_file_burn  = 0;
    if(!flag_restart) {
        SID_log("Initializing state...", SID_LOG_OPEN);

        // Report the starting conditions
        if(my_chain == SID.My_rank) {
            SID_log("Initial parameters & priors:", SID_LOG_ALLRANKS | SID_LOG_OPEN);
            sprintf(format_string, "%s = %%13.6le (%%13.6le ->%%13.6le)", MCMC->P_name_format);
            for(i_P = 0; i_P < n_P; i_P++)
                SID_log(format_string,
                        SID_LOG_ALLRANKS | SID_LOG_COMMENT,
                        MCMC->P_names[i_P],
                        P_last[i_P],
                        MCMC->P_limit_min[i_P],
                        MCMC->P_limit_max[i_P]);
            SID_log("", SID_LOG_ALLRANKS | SID_LOG_NOPRINT | SID_LOG_CLOSE);
        }

        // Perform autotuning (if requested)
        if(SID_CHECK_BITFIELD_SWITCH(MCMC->mode, MCMC_MODE_AUTOTUNE))
            autotune_MCMC(MCMC);
        MCMC->flag_init_chain = GBP_TRUE;

        // Set the initial state
        SID_log("Writing chain config file...", SID_LOG_OPEN);
        if((fp_chain = fopen(filename_chain, "wb")) == NULL)
            SID_exit_error("Could not open file for writing {%s}.", SID_ERROR_IO_OPEN, filename_chain);
        if((fp_stats = fopen(filename_stats, "wb")) == NULL)
            SID_exit_error("Could not open file for writing {%s}.", SID_ERROR_IO_OPEN, filename_stats);
        if((fp_chain_config = fopen(filename_chain_config, "wb")) == NULL)
            SID_exit_error("Could not open file for writing {%s}.", SID_ERROR_IO_OPEN, filename_chain_config);
        fwrite(&n_iterations_file_total, sizeof(int), 1, fp_chain_config);
        fwrite(&n_iterations_file_burn, sizeof(int), 1, fp_chain_config);
        fwrite(&(MCMC->temperature), sizeof(double), 1, fp_chain_config);
        fwrite(MCMC->V, sizeof(double), n_P * n_P, fp_chain_config);
        fclose(fp_chain_config);
        SID_log("Done.", SID_LOG_CLOSE);

        SID_log("Done.", SID_LOG_CLOSE);
    }
    // ... else read the state we left-off from ...
    else if(my_chain == SID.My_rank) {
        SID_log("Loading previous state...", SID_LOG_OPEN);
        MCMC->flag_init_chain = GBP_FALSE;
        // ... fetch the number of intervals that have already been computed ...
        n_iterations_file_total = 0;
        if((fp_chain_config = fopen(filename_chain_config, "rb")) != NULL) {
            SID_log("Reading the existant number of iterations...", SID_LOG_OPEN);
            V_read = (double *)SID_malloc(sizeof(double) * n_P * n_P);
            SID_fread_verify(&n_iterations_file_total, sizeof(int), 1, fp_chain_config);
            SID_fread_verify(&n_iterations_file_burn, sizeof(int), 1, fp_chain_config);
            SID_fread_verify(&(MCMC->temperature), sizeof(double), 1, fp_chain_config);
            SID_fread_verify(V_read, sizeof(double), n_P * n_P, fp_chain_config);
            set_MCMC_covariance(MCMC, V_read);
            SID_free(SID_FARG V_read);
            SID_log("# burn  iterations = %d (%d requested)", SID_LOG_COMMENT, n_iterations_file_burn, n_iterations_burn);
            SID_log("# total iterations = %d (%d requested)", SID_LOG_COMMENT, n_iterations_file_total, n_iterations);
            SID_log("Temperature        = %le", SID_LOG_COMMENT, MCMC->temperature);
            fclose(fp_chain_config);
            SID_log("Done.", SID_LOG_CLOSE);
        }

        // ... scan to the end of the chain and read the last-used parameter set
        if(n_iterations_file_total > 0) {
            SID_log("Reading the last-used parameter set...", SID_LOG_OPEN);
            fp_chain = fopen(filename_chain, "rb");
            for(i_iteration = 0; i_iteration < n_iterations_file_total; i_iteration++) {
                for(i_avg = 0; i_avg < n_avg; i_avg++) {
                    SID_fread_verify(&flag_success, sizeof(char), 1, fp_chain);
                    if((i_iteration + i_avg) != 0) {
                        MCMC->ln_likelihood_last = MCMC->ln_likelihood_new;
                        memcpy(P_last, P_new, n_P * sizeof(double));
                        if(!flag_no_map_write) {
                            for(i_DS = 0; i_DS < n_DS; i_DS++)
                                memcpy(M_last[i_DS], M_new[i_DS], n_M[i_DS] * sizeof(double));
                        }
                    }
                    SID_fread_verify(&(MCMC->ln_likelihood_new), sizeof(double), 1, fp_chain);
                    SID_fread_verify(P_new, sizeof(double), n_P, fp_chain);
                    if(!flag_no_map_write) {
                        for(i_DS = 0; i_DS < n_DS; i_DS++)
                            SID_fread_verify(M_new[i_DS], sizeof(double), n_M[i_DS], fp_chain);
                    }
                    if((i_iteration + i_avg) == 0) {
                        MCMC->ln_likelihood_chain = MCMC->ln_likelihood_new;
                        MCMC->ln_likelihood_last  = MCMC->ln_likelihood_new;
                    }
                    if(flag_success) {
                        MCMC->ln_likelihood_chain = MCMC->ln_likelihood_new;
                        memcpy(P_chain, P_new, n_P * sizeof(double));
                    }
                }
            }
            fclose(fp_chain);
            fp_chain = fopen(filename_chain, "ab");
            SID_log("Done.", SID_LOG_CLOSE);

            // ... scan to the end of the chain stats and read the last-generated statistics (particularly the drift)
            SID_log("Reading the last-generated statistics...", SID_LOG_OPEN);
            fp_stats = fopen(filename_stats, "rb");
            for(i_iteration = 0; i_iteration < n_iterations_file_total; i_iteration++) {
                SID_fread_verify(P_min, sizeof(double), n_P, fp_stats); // Read proposal stats
                SID_fread_verify(P_avg, sizeof(double), n_P, fp_stats);
                SID_fread_verify(P_max, sizeof(double), n_P, fp_stats);
                SID_fread_verify(dP_avg, sizeof(double), n_P, fp_stats);
                SID_fread_verify(dP_sub, sizeof(double), n_P, fp_stats);
                SID_fread_verify(&ln_Pr_min, sizeof(double), 1, fp_stats);
                SID_fread_verify(&ln_Pr_avg, sizeof(double), 1, fp_stats);
                SID_fread_verify(&ln_Pr_max, sizeof(double), 1, fp_stats);
                if(flag_autocor_on)
                    SID_fread_verify(auto_cor, sizeof(double), n_avg - 1, fp_stats);
                SID_fread_verify(P_min, sizeof(double), n_P, fp_stats); // Read chain stats
                SID_fread_verify(P_avg, sizeof(double), n_P, fp_stats);
                SID_fread_verify(P_max, sizeof(double), n_P, fp_stats);
                SID_fread_verify(dP_avg, sizeof(double), n_P, fp_stats);
                SID_fread_verify(dP_sub, sizeof(double), n_P, fp_stats);
                SID_fread_verify(&ln_Pr_min, sizeof(double), 1, fp_stats);
                SID_fread_verify(&ln_Pr_avg, sizeof(double), 1, fp_stats);
                SID_fread_verify(&ln_Pr_max, sizeof(double), 1, fp_stats);
                if(flag_autocor_on)
                    SID_fread_verify(auto_cor, sizeof(double), n_avg - 1, fp_stats);
                SID_fread_verify(slopes, sizeof(double), n_P, fp_stats);
                SID_fread_verify(drift, sizeof(double), n_P, fp_stats);
            }
            fclose(fp_stats);
            fp_stats = fopen(filename_stats, "ab");
            SID_log("Done.", SID_LOG_CLOSE);
        } else {
            memcpy(P_last, P_init, n_P * sizeof(double));
            MCMC->flag_init_chain = GBP_TRUE;
            fp_chain              = fopen(filename_chain, "wb");
            fp_stats              = fopen(filename_stats, "wb");
        }

        // Report the starting conditions
        if(my_chain == SID.My_rank) {
            SID_log("Resume from parameters:", SID_LOG_ALLRANKS | SID_LOG_OPEN);
            sprintf(format_string, "%s = %%13.6le", MCMC->P_name_format);
            for(i_P = 0; i_P < n_P; i_P++)
                SID_log(format_string, SID_LOG_ALLRANKS | SID_LOG_COMMENT, MCMC->P_names[i_P], P_last[i_P]);
            SID_log("", SID_LOG_ALLRANKS | SID_LOG_NOPRINT | SID_LOG_CLOSE);
        }

        SID_log("Done.", SID_LOG_CLOSE);

    } // End of restart stuff
    SID_Bcast(&n_iterations_file_total, 1, SID_INT, my_chain, MCMC->comm);
    SID_Bcast(&n_iterations_file_burn, 1, SID_INT, my_chain, MCMC->comm);
    SID_Bcast(&n_iterations, 1, SID_INT, my_chain, MCMC->comm);
    SID_Bcast(&(MCMC->flag_init_chain), 1, SID_INT, my_chain, MCMC->comm);
    SID_Bcast(&(MCMC->ln_likelihood_chain), 1, SID_DOUBLE, my_chain, MCMC->comm);
    SID_Bcast(&(MCMC->ln_likelihood_new), 1, SID_DOUBLE, my_chain, MCMC->comm);

    // Remove the existant iterations from the totals we need to perform still
    if(n_iterations_file_total < n_iterations_burn) {
        i_phase     = 0;
        i_iteration = n_iterations_file_total;
    } else if(n_iterations_file_total < n_iterations) {
        i_phase     = 1;
        i_iteration = n_iterations_file_total - n_iterations_burn;
    } else {
        i_phase     = 2; // ... in other words, we're done; skip the integration.
        i_iteration = n_iterations;
    }
    i_iteration_start = i_iteration;

    // This is the end of the initialization
    SID_log("Done.", SID_LOG_CLOSE);

    // Create the chain in 2 phases: a burn-in phase and an integration phase
    SID_log("Performing integration...", SID_LOG_OPEN | SID_LOG_TIMER);
    i_iteration_buffer = 0;
    i_P_buffer         = 0;
    i_M_buffer         = 0;
    for(; i_phase < 2; i_phase++) {
        switch(i_phase) {
            case 0:
                SID_log("Performing burn-in phase...", SID_LOG_OPEN | SID_LOG_TIMER);
                n_iterations_phase = n_iterations_burn;
                break;
            case 1:
                SID_log("Performing integration phase...", SID_LOG_OPEN | SID_LOG_TIMER);
                n_iterations_phase = n_iterations - n_iterations_burn;
                break;
        }
        // Initialize progress reporting
        int n_report = 10; // This is the number of progress reports that will be creates. eg. 10 -> every 10%
        i_report     = 0;
        if(n_iterations_phase > 20)
            i_iteration_next_report = i_iteration_start + (n_iterations_phase - i_iteration_start) / n_report;
        else
            i_iteration_next_report = 20;

        // Loop until this phase (burn or integrate) is done
        flag_continue = GBP_TRUE;
        while(flag_continue) {
            // Process one averaging interval at a time

            // Start timer
            time(&time_start);

            for(i_avg = 0, i_thin = 1; i_avg < n_avg; i_thin++) {
                // Generate new proposal and determine it's chi^2
                flag_success = generate_MCMC_chain(MCMC);

                // If this rank is processing chain information ...
                if(my_chain == SID.My_rank) {
                    // ... and this proposal is kept by the thining then ...
                    if(i_thin == n_thin) {
                        // ... reformat parameter arrays for statistics generation
                        for(i_P = 0; i_P < n_P; i_P++) {
                            P_chain_stats[i_P][i_avg % n_avg] = P_chain[i_P];
                            P_prop_stats[i_P][i_avg % n_avg]  = P_new[i_P];
                        }
                        ln_Pr_chain[i_avg % n_avg]     = MCMC->ln_Pr_chain;
                        ln_Pr_proposals[i_avg % n_avg] = MCMC->ln_Pr_new;

                        // ... and write to the chain file
                        switch(flag_minimize_IO) {
                            case GBP_TRUE:
                                MCMC->flag_success_buffer[i_iteration_buffer]      = flag_success;
                                MCMC->ln_likelihood_new_buffer[i_iteration_buffer] = MCMC->ln_likelihood_new;
                                i_iteration_buffer++;
                                memcpy(&(MCMC->P_new_buffer[i_P_buffer]), P_new, (size_t)n_P * sizeof(double));
                                i_P_buffer += n_P;
                                if(!flag_no_map_write) {
                                    for(i_DS = 0; i_DS < n_DS; i_DS++) {
                                        memcpy(&(MCMC->M_new_buffer[i_M_buffer]), M_new[i_DS], (size_t)n_M[i_DS] * sizeof(double));
                                        i_M_buffer += n_M[i_DS];
                                    }
                                }
                                break;
                            default:
                                fwrite(&flag_success, sizeof(char), 1, fp_chain);
                                fwrite(&(MCMC->ln_likelihood_new), sizeof(double), 1, fp_chain);
                                fwrite(P_new, sizeof(double), n_P, fp_chain);
                                if(!flag_no_map_write) {
                                    for(i_DS = 0; i_DS < n_DS; i_DS++)
                                        fwrite(M_new[i_DS], sizeof(double), n_M[i_DS], fp_chain);
                                }
                                break;
                        }
                    }
                }

                // Change counters at the end of a thining interval
                if(i_thin == n_thin) {
                    i_thin = 0;
                    i_avg++;
                }
            } // End of averaging interval loop
            if(i_phase == 0)
                n_iterations_file_burn++;
            n_iterations_file_total++;
            i_iteration++;

            // Stop timer
            time(&time_stop);
            time_diff = difftime(time_stop, time_start);

            // Generate statistics for the averaging interval we just completed
            if(my_chain == SID.My_rank) {
                // Write the statistics to the chain stats file
                compute_MCMC_chain_stats(P_prop_stats,
                                         n_P,
                                         n_avg,
                                         P_min,
                                         P_avg,
                                         P_max,
                                         dP_avg,
                                         auto_cor,
                                         slopes,
                                         dP_sub,
                                         ln_Pr_proposals,
                                         &ln_Pr_min,
                                         &ln_Pr_avg,
                                         &ln_Pr_max);
                fwrite(P_min, sizeof(double), n_P, fp_stats);
                fwrite(P_avg, sizeof(double), n_P, fp_stats);
                fwrite(P_max, sizeof(double), n_P, fp_stats);
                fwrite(dP_avg, sizeof(double), n_P, fp_stats);
                fwrite(dP_sub, sizeof(double), n_P, fp_stats);
                fwrite(&ln_Pr_min, sizeof(double), 1, fp_stats);
                fwrite(&ln_Pr_avg, sizeof(double), 1, fp_stats);
                fwrite(&ln_Pr_max, sizeof(double), 1, fp_stats);
                if(flag_autocor_on)
                    fwrite(auto_cor, sizeof(double), n_avg - 1, fp_stats);
                compute_MCMC_chain_stats(P_chain_stats,
                                         n_P,
                                         n_avg,
                                         P_min,
                                         P_avg,
                                         P_max,
                                         dP_avg,
                                         auto_cor,
                                         slopes,
                                         dP_sub,
                                         ln_Pr_chain,
                                         &ln_Pr_min,
                                         &ln_Pr_avg,
                                         &ln_Pr_max);
                fwrite(P_min, sizeof(double), n_P, fp_stats);
                fwrite(P_avg, sizeof(double), n_P, fp_stats);
                fwrite(P_max, sizeof(double), n_P, fp_stats);
                fwrite(dP_avg, sizeof(double), n_P, fp_stats);
                fwrite(dP_sub, sizeof(double), n_P, fp_stats);
                fwrite(&ln_Pr_min, sizeof(double), 1, fp_stats);
                fwrite(&ln_Pr_avg, sizeof(double), 1, fp_stats);
                fwrite(&ln_Pr_max, sizeof(double), 1, fp_stats);
                if(flag_autocor_on)
                    fwrite(auto_cor, sizeof(double), n_avg - 1, fp_stats);
                for(i_P = 0; i_P < n_P; i_P++)
                    drift[i_P] += slopes[i_P];
                fwrite(slopes, sizeof(double), n_P, fp_stats);
                fwrite(drift, sizeof(double), n_P, fp_stats);
            }

            // Report progress
            if(i_iteration == i_iteration_next_report) {
                i_report++;
                SID_log("%3d%% complete.", SID_LOG_COMMENT | SID_LOG_TIMER, (100 / n_report) * (i_report));
                time_diff /= (double)(n_avg);
                if(time_diff > 0.1)
                    SID_log("\tMean time for single model call: %.1f", SID_LOG_COMMENT, time_diff);
                i_iteration_next_report =
                    GBP_MIN(n_iterations_phase, i_iteration_start + (n_iterations_phase - i_iteration_start) * (i_report + 1) / n_report);
            }

            // Check to see if a stop has been called to the run ...
            if(SID.I_am_Master) {
                if((fp_stop = fopen(filename_stop, "r")) != NULL) {
                    fclose(fp_stop);
                    // remove(filename_stop);
                    flag_stop = GBP_TRUE;
                }
            }
            SID_Bcast(&flag_stop, 1, SID_INT, SID_MASTER_RANK, MCMC->comm);

            // ... if so, stop all ranks and cancel the subsequent analysis stage
            if(flag_stop) {
                flag_continue          = GBP_FALSE;
                MCMC->flag_analysis_on = GBP_FALSE;
                i_phase                = 3;
                SID_log("*** Stop file found.  Terminating run. ***", SID_LOG_COMMENT);
            }

            // Check to see if this phase's iterations are complete
            if(i_iteration >= n_iterations_phase)
                flag_continue = GBP_FALSE;

        } // while flag_continue=GBP_TRUE

        // Report progress
        SID_log("Completed iterations in this phase: %d (%d requested)", SID_LOG_COMMENT, i_iteration, n_iterations_phase);

        // Report success rate
        SID_log("Proposal success: %5.3f%% (%lld of %lld)",
                SID_LOG_COMMENT,
                1e2 * ((float)(MCMC->n_success) / (float)(MCMC->n_propositions)),
                MCMC->n_success,
                MCMC->n_propositions);

        i_iteration       = 0;
        i_iteration_start = i_iteration;
        SID_log("Done.", SID_LOG_CLOSE);
    }
    if(my_chain == SID.My_rank) {
        fp_chain_config = fopen(filename_chain_config, "wb");
        fwrite(&n_iterations_file_total, sizeof(int), 1, fp_chain_config);
        fwrite(&n_iterations_file_burn, sizeof(int), 1, fp_chain_config);
        fwrite(&(MCMC->temperature), sizeof(double), 1, fp_chain_config);
        fwrite(MCMC->V, sizeof(double), n_P * n_P, fp_chain_config);
        fclose(fp_chain_config);
        fclose(fp_chain);
        fclose(fp_stats);
    }
    SID_log("Done.", SID_LOG_CLOSE);

    // Perform analysis on chain(s)
    if(MCMC->flag_analysis_on)
        analyze_MCMC(MCMC);

    // Clean-up
    SID_log("Cleaning-up...", SID_LOG_OPEN);
    SID_free(SID_FARG P_min);
    SID_free(SID_FARG P_max);
    SID_free(SID_FARG P_avg);
    SID_free(SID_FARG dP_avg);
    SID_free(SID_FARG ln_Pr_chain);
    SID_free(SID_FARG ln_Pr_proposals);
    SID_free(SID_FARG drift);
    SID_free(SID_FARG slopes);
    SID_free(SID_FARG dP_sub);
    for(i_P = 0; i_P < n_P; i_P++) {
        SID_free(SID_FARG P_chain_stats[i_P]);
        SID_free(SID_FARG P_prop_stats[i_P]);
        if(flag_autocor_on)
            SID_free(SID_FARG auto_cor[i_P]);
    }
    SID_free(SID_FARG P_chain_stats);
    SID_free(SID_FARG P_prop_stats);
    if(flag_autocor_on)
        SID_free(SID_FARG auto_cor);
    SID_log("Done.", SID_LOG_CLOSE);

    SID_log("Done.", SID_LOG_CLOSE);
}


#ifndef TAYLORSERIES_H
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <common/refcount.h>
#include "common/jexception.h"
#include <list>
#include <vector>

using namespace std;

double LogAdd(double x, double y);
double LogSub(double x, double y);
double logFactorial( unsigned int n );

#define NO_FILE_SYMBOL "NONE"
class nonamePdf {
 protected:
  vector<float>  _points; /* @note it must be positive and sorted. */
  int   _maxCoeffs;
  double **_coeffs;       /* [_points.size()][nCoeffs] */
  double **_coeffsLog;

 protected:
  void clear();

 public:
  nonamePdf();
  ~nonamePdf();
  bool loadCoeffDescFile( const String &coefDescfn );

 private:
  bool loadCoeffFiles( char *coeffn, char *logCoeffn, float a );
  int  loadCoeffFile(  char *coeffn, int preNPoints, int idx, bool logProb );
};

class gammaPdf : public nonamePdf {

 public:
  gammaPdf( int numberOfVariate = 2 );
  ~gammaPdf();
  double calcLog( double x, int N );
  double calcDerivative1( double x, int N );
  void   bi( int printLevel = 0);
  void   four( int printLevel = 0 );

  void   printCoeff();
 private:
  void allocate();
  int  indexOfCoeffArray( double x );
  bool interpolate( int printLevel );
};

typedef  refcount_ptr<nonamePdf> nonamePdfPtr;
typedef  Inherit<gammaPdf, nonamePdfPtr> gammaPdfPtr;

#endif


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

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

enum blas_order_type {
            blas_rowmajor = 101,
            blas_colmajor = 102 };

enum blas_cmach_type {
            blas_base      = 151,
            blas_t         = 152,
            blas_rnd       = 153,
            blas_ieee      = 154,
            blas_emin      = 155,
            blas_emax      = 156,
            blas_eps       = 157,
            blas_prec      = 158,
            blas_underflow = 159,
            blas_overflow  = 160,
            blas_sfmin     = 161};

enum blas_norm_type {
            blas_one_norm       = 171,
            blas_real_one_norm  = 172,
            blas_two_norm       = 173,
            blas_frobenius_norm = 174,
            blas_inf_norm       = 175,
            blas_real_inf_norm  = 176,
            blas_max_norm       = 177,
            blas_real_max_norm  = 178 };

static void
BLAS_error(char *rname, int err, int val, int x) {
  fprintf( stderr, "%s %d %d %d\n", rname, err, val, x );
  abort();
}

static
void
BLAS_sge_norm(enum blas_order_type order, enum blas_norm_type norm,
  int m, int n, const float *a, int lda, float *res) {
  int i, j; float anorm, v;
  char rname[] = "BLAS_sge_norm";

  if (order != blas_colmajor) BLAS_error( rname, -1, order, 0 );

  if (norm == blas_frobenius_norm) {
    anorm = 0.0f;
    for (j = n; j; --j) {
      for (i = m; i; --i) {
        v = a[0];
        anorm += v * v;
        a++;
      }
      a += lda - m;
    }
    anorm = sqrt( anorm );
  } else if (norm == blas_inf_norm) {
    anorm = 0.0f;
    for (i = 0; i < m; ++i) {
      v = 0.0f;
      for (j = 0; j < n; ++j) {
        v += fabsf( a[i + j * lda] );
      }
      if (v > anorm)
        anorm = v;
    }
  } else {
    BLAS_error( rname, -2, norm, 0 );
    return;
  }

  if (res) *res = anorm;
}

static
float
BLAS_spow_di(float x, int n) {
  float rv = 1.0;

  if (n < 0) {
    n = -n;
    x = 1.0 / x;
  }

  for (; n; n >>= 1, x *= x) {
    if (n & 1)
      rv *= x;
  }

  return rv;
}

static
float
BLAS_sfpinfo(enum blas_cmach_type cmach) {
  float eps = 1.0, r = 1.0, o = 1.0, b = 2.0;
  int t = 53, l = 1024, m = -1021;
  char rname[] = "BLAS_sfpinfo";

  if ((sizeof eps) == sizeof(float)) {
    t = 24;
    l = 128;
    m = -125;
  } else {
    t = 53;
    l = 1024;
    m = -1021;
  }

  /* for (i = 0; i < t; ++i) eps *= half; */
  eps = BLAS_spow_di( b, -t );
  /* for (i = 0; i >= m; --i) r *= half; */
  r = BLAS_spow_di( b, m-1 );

  o -= eps;
  /* for (i = 0; i < l; ++i) o *= b; */
  o = (o * BLAS_spow_di( b, l-1 )) * b;

  switch (cmach) {
    case blas_eps: return eps;
    case blas_sfmin: return r;
    default:
      BLAS_error( rname, -1, cmach, 0 );
      break;
  }
  return 0.0;
}

static int check_factorization(int, float*, float*, int, int , float);
static int check_solution(int, int, float*, int, float*, float*, int, float);
static int check_estimator(PLASMA_enum, int, float *, int,
                           float *, float, float,
                           float);

int testing_sposv(int argc, char **argv)
{

    /* Check for number of arguments*/
    if (argc != 4){
        USAGE("POSV", "N LDA NRHS LDB",
              "   - N    : the size of the matrix\n"
              "   - LDA  : leading dimension of the matrix A\n"
              "   - NRHS : number of RHS\n"
              "   - LDB  : leading dimension of the RHS B\n");
        return -1;
    }

    int N     = atoi(argv[0]);
    int LDA   = atoi(argv[1]);
    int NRHS  = atoi(argv[2]);
    int LDB   = atoi(argv[3]);
    float eps;
    int info_solution, info_factorization;
    int u, trans1, trans2;

    float *A1   = (float *)malloc(LDA*N*sizeof(float));
    float *A2   = (float *)malloc(LDA*N*sizeof(float));
    float *B1   = (float *)malloc(LDB*NRHS*sizeof(float));
    float *B2   = (float *)malloc(LDB*NRHS*sizeof(float));

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

    eps = BLAS_sfpinfo( blas_eps );

    for(u=0; u<2; u++) {

        trans1 = uplo[u] == PlasmaUpper ? PlasmaTrans : PlasmaNoTrans;
        trans2 = uplo[u] == PlasmaUpper ? PlasmaNoTrans : PlasmaTrans;

        /*-------------------------------------------------------------
         *  TESTING SPOSV
         */

        /* Initialize A1 and A2 for Symmetric Positif Matrix */
        PLASMA_splgsy( (float)N, N, A1, LDA, 51 );
        PLASMA_slacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

        /* Initialize B1 and B2 */
        PLASMA_splrnt( N, NRHS, B1, LDB, 371 );
        PLASMA_slacpy( PlasmaUpperLower, N, NRHS, B1, LDB, B2, LDB );

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

        /* PLASMA SPOSV */
        PLASMA_sposv(uplo[u], N, NRHS, A2, LDA, B2, LDB);

        /* Check the factorization and the solution */
        info_factorization = check_factorization( N, A1, A2, LDA, uplo[u], eps);
        info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB, eps);

        if ( (info_solution == 0) && (info_factorization == 0) ) {
            printf("***************************************************\n");
            printf(" ---- TESTING SPOSV(%s) ...................... PASSED !\n", uplostr[u]);
            printf("***************************************************\n");
        }
        else {
            printf("***************************************************\n");
            printf(" - TESTING SPOSV(%s) ... FAILED !\n", uplostr[u]);
            printf("***************************************************\n");
        }

        /*-------------------------------------------------------------
         *  TESTING SPOTRF + SPOTRS
         */

        /* Initialize A1 and A2 for Symmetric Positif Matrix */
        PLASMA_splgsy( (float)N, N, A1, LDA, 51 );
        PLASMA_slacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

        /* Initialize B1 and B2 */
        PLASMA_splrnt( N, NRHS, B1, LDB, 371 );
        PLASMA_slacpy( PlasmaUpperLower, N, NRHS, B1, LDB, B2, LDB );

        /* Plasma routines */
        PLASMA_spotrf(uplo[u], N, A2, LDA);
        PLASMA_spotrs(uplo[u], N, NRHS, A2, LDA, B2, LDB);

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

        /* Check the factorization and the solution */
        info_factorization = check_factorization( N, A1, A2, LDA, uplo[u], eps);
        info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB, eps);

        if ((info_solution == 0)&(info_factorization == 0)){
            printf("***************************************************\n");
            printf(" ---- TESTING SPOTRF + SPOTRS (%s)............ PASSED !\n", uplostr[u]);
            printf("***************************************************\n");
        }
        else{
            printf("****************************************************\n");
            printf(" - TESTING SPOTRF + SPOTRS (%s)... FAILED !\n", uplostr[u]);
            printf("****************************************************\n");
        }

        /*-------------------------------------------------------------
         *  TESTING SPOTRF + ZPTRSM + STRSM
         */

        /* Initialize A1 and A2 for Symmetric Positif Matrix */
        PLASMA_splgsy( (float)N, N, A1, LDA, 51 );
        PLASMA_slacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

        /* Initialize B1 and B2 */
        PLASMA_splrnt( N, NRHS, B1, LDB, 371 );
        PLASMA_slacpy( PlasmaUpperLower, N, NRHS, B1, LDB, B2, LDB );

        /* PLASMA routines */
        PLASMA_spotrf(uplo[u], N, A2, LDA);
        PLASMA_strsm(PlasmaLeft, uplo[u], trans1, PlasmaNonUnit,
                     N, NRHS, 1.0, A2, LDA, B2, LDB);
        PLASMA_strsm(PlasmaLeft, uplo[u], trans2, PlasmaNonUnit,
                     N, NRHS, 1.0, A2, LDA, B2, LDB);

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

        /* Check the factorization and the solution */
        info_factorization = check_factorization( N, A1, A2, LDA, uplo[u], eps);
        info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB, eps);

        if ((info_solution == 0)&(info_factorization == 0)){
            printf("***************************************************\n");
            printf(" ---- TESTING SPOTRF + STRSM + STRSM (%s)..... PASSED !\n", uplostr[u]);
            printf("***************************************************\n");
        }
        else{
            printf("***************************************************\n");
            printf(" - TESTING SPOTRF + STRSM + STRSM (%s)... FAILED !\n", uplostr[u]);
            printf("***************************************************\n");
        }

        /*-------------------------------------------------------------
         *  TESTING ZPOCON on the last call
         */
        {
            float Anorm = PLASMA_slansy( PlasmaOneNorm, uplo[u], N, A1, LDA );
            float Acond;

            info_solution = PLASMA_spocon(uplo[u], N, A2, LDA, Anorm, &Acond);
            if ( info_solution == 0 ) {
                info_solution = check_estimator(uplo[u], N, A1, LDA, A2, Anorm, Acond, eps);
            } else {
                printf(" PLASMA_spocon returned info = %d\n", info_solution );
            }
            if ((info_solution == 0)){
                printf("***************************************************\n");
                printf(" ---- TESTING SPOTRF + ZPOCON (%s) ........... PASSED !\n", uplostr[u]);
                printf("***************************************************\n");
            }
            else{
                printf("**************************************************\n");
                printf(" - TESTING SPOTRF + ZPOCON (%s) ... FAILED !\n", uplostr[u]);
                printf("**************************************************\n");
            }
        }
    }

    free(A1); free(A2); free(B1); free(B2);

    return 0;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */
static int check_factorization(int N, float *A1, float *A2, int LDA, int uplo, float eps)
{
    float Anorm, Rnorm;
    float alpha;
    int info_factorization;
    int i,j;

    float *Residual = (float *)malloc(N*N*sizeof(float));
    float *L1       = (float *)malloc(N*N*sizeof(float));
    float *L2       = (float *)malloc(N*N*sizeof(float));
    float *work              = (float *)malloc(N*sizeof(float));

    memset((void*)L1, 0, N*N*sizeof(float));
    memset((void*)L2, 0, N*N*sizeof(float));

    alpha= 1.0;

    LAPACKE_slacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_strmm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_strmm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    BLAS_sge_norm( blas_colmajor, blas_inf_norm, N, N, Residual, N, &Rnorm );
    BLAS_sge_norm( blas_colmajor, blas_inf_norm, N, N, A1, LDA, &Anorm );

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || isinf(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 60.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the accuracy of the solution of the linear system
 */

static int check_solution(int N, int NRHS, float *A1, int LDA, float *B1, float *B2, int LDB, float eps )
{
    int info_solution;
    float Rnorm, Anorm, Xnorm, Bnorm, result;
    float alpha, beta;
    float *work = (float *)malloc(N*sizeof(float));

    alpha = 1.0;
    beta  = -1.0;

    BLAS_sge_norm( blas_colmajor, blas_inf_norm, N, NRHS, B2, LDB, &Xnorm );
    BLAS_sge_norm( blas_colmajor, blas_inf_norm, N, N,    A1, LDA, &Anorm );
    BLAS_sge_norm( blas_colmajor, blas_inf_norm, N, NRHS, B1, LDB, &Bnorm );

    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, N, NRHS, N, (alpha), A1, LDA, B2, LDB, (beta), B1, LDB);
    BLAS_sge_norm( blas_colmajor, blas_inf_norm, N, NRHS, B1, LDB, &Rnorm );

    if (getenv("PLASMA_TESTING_VERBOSE"))
      printf( "||A||_oo=%f\n||X||_oo=%f\n||B||_oo=%f\n||A X - B||_oo=%e\n", Anorm, Xnorm, Bnorm, Rnorm );

    result = Rnorm / ( (Anorm*Xnorm+Bnorm)*N*eps ) ;
    printf("============\n");
    printf("Checking the Residual of the solution \n");
    printf("-- ||Ax-B||_oo/((||A||_oo||x||_oo+||B||_oo).N.eps) = %e \n", result);

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

    free(work);

    return info_solution;
}

/*------------------------------------------------------------------------
 *  Check the accuracy of the condition estimator
 */
static int check_estimator(PLASMA_enum uplo, int N, float *A1, int LDA,
                           float *A2, float Anorm, float Acond,
                           float eps)
{
    int info_solution;
    float result, Acond_lapack;
    float invcond, invcond_lapack;

    info_solution = LAPACKE_spocon(LAPACK_COL_MAJOR, lapack_const(uplo), N, A2, LDA, Anorm, &Acond_lapack);

    if ( info_solution != 0 ) {
        printf(" PLASMA_sgecon returned info = %d\n", info_solution );
        return info_solution;
    }

    invcond_lapack = 1. / ( Acond_lapack );
    invcond        = 1. / ( Acond );

    printf("============\n");
    printf("Checking the condition number \n");
    printf("-- Acond_plasma = %e, Acond_lapack = %e \n"
           "-- Ainvcond_plasma = %e, Ainvcond_lapack = %e \n",
           Acond, Acond_lapack, invcond, invcond_lapack );

    result = fabs( Acond_lapack - Acond ) / eps;
    if ( result > 60. ) {
        printf("-- The solution is suspicious ! \n");
        info_solution = 1;
     }
    else{
        printf("-- The solution is CORRECT ! \n");
        info_solution = 0;
    }
    return info_solution;
}


#ifndef STDEVSTOOL_H
#define STDEVSTOOL_H

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

#include "pdevslib.h"
#include <algorithm>    // std::binary_search, std::sort
#include <vector>

//#include "sink/ParameterReader.h"

#define STDEVS_REPRODUCIBLE_SEED 0  // Random reproducible generation. reproducibility of results
#define STDEVS_UNIQUE_SEED -1  // Random unique generation
#define STDEVS_DEFAULT_SEED STDEVS_UNIQUE_SEED  // default in case no configuration

#define SCILAB_VARNAME_REPRODUCIBLE_SIMULATION "ReproducibleSimu" // name to be used in the configuration

//* Forward declare the functions in ParameterReader.h. Including creates circular inclusion problems*/
//#include "sinks/ParameterReader.h"
void printLog(int level, const char *fmt,...);
template<typename T> T readDefaultParameterValue(std::string paramName);

/*
 * Class to generate random numbers.
 * It provides 3 singleton which can be used (it is advised to use always the first one):
 *     1) Random number generation based on the configuration (probably one of the instances below or a custom seeded)
 *     2) Reproducible random numbers (uses always the same seed)
 *     3) need unique random numbers (generates a new seed for every simulation)
 * It is advisable to use the IDistributionParameter instances, so that random number generation is based on the configuration.
 * If you don't want to read from the configuration, it is advisable to use the singleton instances available in this class. Only create new instances if you know what you are doing
 */
class STDEVS {
public:
 // Use this instance in models which need reproducible random numbers (uses always the same seed)
 static STDEVS* getReproducibleGenerationInstance();

 // Use this instance in models which need unique random numbers (generates a new seed)
 static STDEVS* getUniqueGenerationInstance();

 // Unless for specific needs, always use this instance which will point one of the previous based on the configuration or to a custom seeded.
 static STDEVS* getConfigurationInstance();

 /*
  * It is advisable to use the IDistributionParameter instances, so that random number generation is based on the configuration.
  *  If you don't want to read from the configuration, it is advisable to use the singleton instances available in this class. Only create new instances if you know what you are doing.
  *  If you are creating several instances of STDEVS in a very short time (for example when using VectorialDEVS), evaluate using the singletons available in this class to avoid generating the same seed in several instances.
  *
  */
 STDEVS(int randSeed);

  ~STDEVS() {printLog(10, "STDEVS Destroyed \n");};

	unsigned int poisson(double mu) { return gsl_ran_poisson (this->r, mu); }
	double bernoulli (double p) { return gsl_ran_bernoulli (this->r, p); }
	double exponential(double mu) { return gsl_ran_exponential (this->r, mu); }
	double uniform () { return gsl_rng_uniform (this->r); }
	double normal(double mu, double var) { return gsl_ran_gaussian(this -> r, var) + mu;}
	double pareto(double order, double minValue) {	return gsl_ran_pareto(this -> r, order, minValue); }

	double getWeightedElement(std::vector<double> elements, std::vector<double> weights);
	std::vector<double> calculateAccumulatedWeights(std::vector<double> weights);
	std::vector<int> getSortedByWeightSubset(std::vector<double> elements, std::vector<double> weights, const int length);
	std::vector<int> getWeightedSubset(std::vector<double> elements, std::vector<double> weights, const uint length);
	double modifyCumsumAvoidIndex(const int avoidIndex,
			std::vector<double>& cumsum,
			std::vector<double>& weights,
			std::vector<double>& elements);
private:
	 // Use this instance in models which need reproducible random numbers (uses always the same seed)
	 static STDEVS* ReproducibleGenerationInstance;

	 // Use this instance in models which need unique random numbers (generates a new seed)
	 static STDEVS* UniqueGenerationInstance;

	 // Unless for specific needs, always use this instance which will point one of the previous based on the configuration or to a custom seeded.
	 static STDEVS* ConfigurationInstance;
	 static STDEVS* getConfiguredSTDEVS();

	gsl_rng * r; // random seed for the instance

	struct WeightedComparator
	{
	    const std::vector<double> & value_vector;

	    WeightedComparator(const std::vector<double> & val_vec):
	        value_vector(val_vec) {}

	    bool operator()(int i1, int i2)
	    {
	        return value_vector[i1] < value_vector[i2];
	    }
	};
};

#endif


/* -*- linux-c -*- */
/* fewbody.c

   Copyright (C) 2002-2004 John M. Fregeau

   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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/times.h>
#include <unistd.h>
#include <getopt.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>
#include "fewbody.h"

int fb_debug = 0;

fb_ret_t fewbody(fb_input_t input, fb_hier_t *hier, double *t)
{
	int i, j, k, status, done=0, forceclassify=0, restart, restep;
	long clk_tck;
	double s, slast, sstop=FB_SSTOP, tout, h=FB_H, *y, texpand, tnew, R[3];
	double Ei, E, Lint[3], Li[3], L[3], DeltaL[3];
	double s2, s2prev=GSL_POSINF, s2prevprev=GSL_POSINF, s2minprev=GSL_POSINF, s2max=0.0, s2min;
	struct tms firsttimebuf, currtimebuf;
	clock_t firstclock, currclock;
	fb_hier_t phier;
	fb_ret_t retval;
	fb_nonks_params_t nonks_params;
	fb_ks_params_t ks_params;
	char string1[FB_MAX_STRING_LENGTH], string2[FB_MAX_STRING_LENGTH], logentry[FB_MAX_LOGENTRY_LENGTH];
	const gsl_odeiv_step_type *ode_type=gsl_odeiv_step_rk8pd;
	gsl_odeiv_step *ode_step;
	gsl_odeiv_control *ode_control;
	gsl_odeiv_evolve *ode_evolve;
	gsl_odeiv_system ode_sys;

	/* initialize a few things */
	fb_init_hier(hier);
	retval.iclassify = 0;
	retval.Rmin = FB_RMIN;
	retval.Rmin_i = -1;
	retval.Rmin_j = -1;
	retval.Nosc = 0;
	strncpy(logentry, input.firstlogentry, FB_MAX_LOGENTRY_LENGTH);

	/* set up the perturbation tree, initially flat */
	phier.nstarinit = hier->nstar;
	phier.nstar = hier->nstar;
	fb_malloc_hier(&phier);
	fb_init_hier(&phier);
	for (i=0; i<phier.nstar; i++) {
		fb_objcpy(&(phier.hier[phier.hi[1]+i]), &(hier->hier[hier->hi[1]+i]));
	}

	/* initialize GSL integration routine */
	if (input.ks) {
		ode_step = gsl_odeiv_step_alloc(ode_type, 8 * (hier->nstar * (hier->nstar - 1) / 2) + 1);
		ode_control = gsl_odeiv_control_y_new(input.absacc, input.relacc);
		ode_evolve = gsl_odeiv_evolve_alloc(8 * (hier->nstar * (hier->nstar - 1) / 2) + 1);
		ode_sys.function = fb_ks_func;
		ode_sys.jacobian = NULL;
		ode_sys.dimension = 8 * (hier->nstar * (hier->nstar - 1) / 2) + 1;
		ode_sys.params = &ks_params;
	} else {
		ode_step = gsl_odeiv_step_alloc(ode_type, 6 * hier->nstar);
		ode_control = gsl_odeiv_control_y_new(input.absacc, input.relacc);
		ode_evolve = gsl_odeiv_evolve_alloc(6 * hier->nstar);
		ode_sys.function = fb_nonks_func;
		ode_sys.jacobian = fb_nonks_jac;
		ode_sys.dimension = 6 * hier->nstar;
		ode_sys.params = &nonks_params;
	}

	/* set parameters for integrator */
	if (input.ks) {
		ks_params.nstar = hier->nstar;
		ks_params.kstar = ks_params.nstar*(ks_params.nstar-1)/2;
		fb_malloc_ks_params(&ks_params);
		fb_init_ks_params(&ks_params, *hier);
	} else {
		nonks_params.nstar = hier->nstar;
		fb_malloc_nonks_params(&nonks_params);
		fb_init_nonks_params(&nonks_params, *hier);
	}

	/* set the initial conditions in y_i */
	if (input.ks) {
		y = fb_malloc_vector(8*ks_params.kstar+1);
		y[0] = *t;
		fb_euclidean_to_ks(phier.obj, y, ks_params.nstar, ks_params.kstar);
		s = 0.0;
	} else {
		y = fb_malloc_vector(6*nonks_params.nstar);
		fb_euclidean_to_nonks(phier.obj, y, nonks_params.nstar);
		s = *t;
	}

	/* store the initial energy and angular momentum */
	Ei = fb_petot(&(hier->hier[hier->hi[1]]), hier->nstar) + fb_ketot(&(hier->hier[hier->hi[1]]), hier->nstar) +
		fb_einttot(&(hier->hier[hier->hi[1]]), hier->nstar);
	fb_angmom(&(hier->hier[hier->hi[1]]), hier->nstar, Li);
	fb_angmomint(&(hier->hier[hier->hi[1]]), hier->nstar, Lint);
	for (i=0; i<3; i++) {
		Li[i] += Lint[i];
	}

	/* integrate along */
	fb_dprintf("fewbody: integrating...\n");

	done = 0;
	retval.count = 0;
	tout = *t;
	texpand = 0.0;
	clk_tck = sysconf(_SC_CLK_TCK);
	firstclock = times(&firsttimebuf);
	retval.tcpu = 0.0;
	while (*t < input.tstop && retval.tcpu < input.tcpustop && !done) {
		/* take one step */
		slast = s;
		status = gsl_odeiv_evolve_apply(ode_evolve, ode_control, ode_step, &ode_sys, &s, sstop, &h, y);
		if (status != GSL_SUCCESS) {
			break;
		}

		/* set objects' positions and velocities in phier */
		if (input.ks) {
			tnew = y[0];
			fb_ks_to_euclidean(y, phier.obj, ks_params.nstar, ks_params.kstar);
		} else {
			tnew = s;
			fb_nonks_to_euclidean(y, phier.obj, nonks_params.nstar);
		}

		/* restart: do we need to restart the integrator?
		   restep: do we need to repeat the last integration step?
		   forceclassify: do we need to force a call to the classify() routine? */
		restart = 0;
		restep = 0;
		forceclassify = 0;

		/* see if we need to expand or collapse the perturbation hierarchy */
		if (fb_expand(&phier, tnew, input.tidaltol)) {
			texpand = tnew;
			s = slast;
			restart = 1;
			restep = 1;
			for (i=0; i<hier->nstar; i++) {
				for (k=0; k<3; k++) {
					phier.hier[phier.hi[1]+i].x[k] = hier->hier[hier->hi[1]+i].x[k];
					phier.hier[phier.hi[1]+i].v[k] = hier->hier[hier->hi[1]+i].v[k];
				}
			}
			fb_elkcirt(&phier, *t);
		} else if (tnew >= texpand && fb_collapse(&phier, tnew, input.tidaltol)) {
			*t = tnew;
			/* if there is only one object, then it's stable---force classify() */
			if (phier.nobj == 1) {
				forceclassify = 1;
			} else {
				restart = 1;
			}
		} else {
			*t = tnew;
		}

		/* if we're not repeating the previous integration step, then do physics */
		if (!restep) {
			/* trickle down so updated information is in hier */
			fb_trickle(&phier, *t);
			for (i=0; i<hier->nstar; i++) {
				for (k=0; k<3; k++) {
					hier->hier[hier->hi[1]+i].x[k] = phier.hier[phier.hi[1]+i].x[k];
					hier->hier[hier->hi[1]+i].v[k] = phier.hier[phier.hi[1]+i].v[k];
				}
			}

			/* update Rmin (closest approach) */
			for (i=0; i<hier->nstar-1; i++) {
				for (j=i+1; j<hier->nstar; j++) {
					for (k=0; k<3; k++) {
						R[k] = hier->hier[hier->hi[1]+i].x[k] - hier->hier[hier->hi[1]+j].x[k];
					}
					if (fb_mod(R) < retval.Rmin) {
						retval.Rmin = fb_mod(R);
						retval.Rmin_i = i;
						retval.Rmin_j = j;
					}
				}
			}

			/* do physical collisions */
			if (fb_collide(hier, input.fexp)) {
				/* initialize phier to a flat tree */
				phier.nstar = hier->nstar;
				fb_init_hier(&phier);
				for (i=0; i<phier.nstar; i++) {
					fb_objcpy(&(phier.hier[phier.hi[1]+i]), &(hier->hier[hier->hi[1]+i]));
				}

				/* if there is only one object, then it's stable---force classify() */
				if (phier.nobj == 1) {
					restart = 0;
					forceclassify = 1;
				} else {
					restart = 1;
				}
			}

			/* check for oscillations in s^2 */
			/* first calculate s^2 */
			s2 = 0.0;
			for (i=0; i<hier->nstar-1; i++) {
				for (j=i+1; j<hier->nstar; j++) {
					for (k=0; k<3; k++) {
						s2 += fb_sqr(hier->hier[hier->hi[1]+i].x[k] - hier->hier[hier->hi[1]+j].x[k]);
					}
				}
			}
			/* local min */
			if (s2 > s2prev && s2prev < s2prevprev) {
				s2min = s2prev;
				/* increment Nosc if there is a valid oscillation */
				if (s2max >= 2.0*s2minprev && s2max >= 2.0*s2min) {
					retval.Nosc++;
					fb_dprintf("fewbody: oscillation in s^2 detected: s2max/s2minprev=%g s2max/s2min=%g: Nosc=%d\n", s2max/s2minprev, s2max/s2min, retval.Nosc);
					s2minprev = s2min;
				} else {
					fb_dprintf("fewbody: oscillation in s^2 not big enough: s2max/s2minprev=%g s2max/s2min=%g\n", s2max/s2minprev, s2max/s2min);
					/* that pesky first min... */
					if (s2max == 0.0) {
						s2minprev = s2min;
					}
				}
			}
			/* local max: just set value and wait for next min */
			if (s2 < s2prev && s2prev > s2prevprev) {
				s2max = s2prev;
			}
			/* set previous values */
			s2prevprev = s2prev;
			s2prev = s2;

			/* see if we're done */
			if (retval.count % input.ncount == 0 || forceclassify) {
				status = fb_classify(hier, *t, input.tidaltol);
				retval.iclassify++;
				fb_dprintf("fewbody: current status:  t=%.6g  %s  (%s)\n",
					   *t, fb_sprint_hier(*hier, string1),
					   fb_sprint_hier_hr(*hier, string2));
				snprintf(&(logentry[strlen(logentry)]), FB_MAX_LOGENTRY_LENGTH-strlen(logentry),
					 "  current status:  t=%.6g  %s  (%s)\n", *t, fb_sprint_hier(*hier, string1),
					 fb_sprint_hier_hr(*hier, string2));
				if (status) {
					done = 1;
				}
			}

			/* print stuff if necessary */
			if (input.Dflag == 1 && (*t >= tout || done)) {
				tout = *t + input.dt;
				fb_print_story(&(hier->hier[hier->hi[1]]), hier->nstar, *t, logentry);
			}
		}

		/* restart integrator if necessary */
		if (restart) {
			fb_dprintf("fewbody: restarting integrator: nobj=%d count=%ld\n", phier.nobj, retval.count);
			fb_free_vector(y);
			if (input.ks) {
				fb_free_ks_params(ks_params);
				ks_params.nstar = phier.nobj;
				ks_params.kstar = ks_params.nstar*(ks_params.nstar-1)/2;
				fb_malloc_ks_params(&ks_params);
				fb_init_ks_params(&ks_params, phier);

				y = fb_malloc_vector(8*ks_params.kstar+1);
				y[0] = *t;
				fb_euclidean_to_ks(phier.obj, y, ks_params.nstar, ks_params.kstar);
			} else {
				fb_free_nonks_params(nonks_params);
				nonks_params.nstar = phier.nobj;
				fb_malloc_nonks_params(&nonks_params);
				fb_init_nonks_params(&nonks_params, phier);

				y = fb_malloc_vector(6*nonks_params.nstar);
				fb_euclidean_to_nonks(phier.obj, y, nonks_params.nstar);
			}

			/* and re-allocate integrator */
			gsl_odeiv_evolve_free(ode_evolve);
			gsl_odeiv_control_free(ode_control);
			gsl_odeiv_step_free(ode_step);

			/* re-initialize integrator */
			if (input.ks) {
				ode_step = gsl_odeiv_step_alloc(ode_type, 8*ks_params.kstar+1);
				ode_control = gsl_odeiv_control_y_new(input.absacc, input.relacc);
				ode_evolve = gsl_odeiv_evolve_alloc(8*ks_params.kstar+1);
				ode_sys.dimension = 8*ks_params.kstar+1;
			} else {
				ode_step = gsl_odeiv_step_alloc(ode_type, 6*nonks_params.nstar);
				ode_control = gsl_odeiv_control_y_new(input.absacc, input.relacc);
				ode_evolve = gsl_odeiv_evolve_alloc(6*nonks_params.nstar);
				ode_sys.dimension = 6*nonks_params.nstar;
			}
		}

		/* update variables that change on every integration step */
		retval.count++;
		currclock = times(&currtimebuf);
		retval.tcpu = ((double) (currtimebuf.tms_utime + currtimebuf.tms_stime - firsttimebuf.tms_utime - firsttimebuf.tms_stime))/((double) clk_tck);
	}

	/* do final classification */
	retval.retval = fb_classify(hier, *t, input.tidaltol);
	retval.iclassify++;
	fb_dprintf("fewbody: current status:  t=%.6g  %s  (%s)\n",
		   *t, fb_sprint_hier(*hier, string1),
		   fb_sprint_hier_hr(*hier, string2));
	snprintf(&(logentry[strlen(logentry)]), FB_MAX_LOGENTRY_LENGTH-strlen(logentry),
		 "  current status:  t=%.6g  %s  (%s)\n", *t, fb_sprint_hier(*hier, string1),
		 fb_sprint_hier_hr(*hier, string2));

	/* print final story */
	if (input.Dflag == 1) {
		fb_print_story(&(hier->hier[hier->hi[1]]), hier->nstar, *t, logentry);
	}

	fb_dprintf("fewbody: final: phier.nobj = %d\n", phier.nobj);

	E = fb_petot(&(hier->hier[hier->hi[1]]), hier->nstar) + fb_ketot(&(hier->hier[hier->hi[1]]), hier->nstar) +
		fb_einttot(&(hier->hier[hier->hi[1]]), hier->nstar);
	fb_angmom(&(hier->hier[hier->hi[1]]), hier->nstar, L);
	fb_angmomint(&(hier->hier[hier->hi[1]]), hier->nstar, Lint);
	for (i=0; i<3; i++) {
		L[i] += Lint[i];
		DeltaL[i] = L[i] - Li[i];
	}

	/* free GSL stuff */
	gsl_odeiv_evolve_free(ode_evolve);
	gsl_odeiv_control_free(ode_control);
	gsl_odeiv_step_free(ode_step);

	/* free our own stuff */
	fb_free_vector(y);
	fb_free_hier(phier);

	if (input.ks) {
		fb_free_ks_params(ks_params);
	} else {
		fb_free_nonks_params(nonks_params);
	}

	/* done! */
	retval.DeltaE = E-Ei;
	retval.DeltaEfrac = E/Ei-1.0;
	retval.DeltaL = fb_mod(DeltaL);
	retval.DeltaLfrac = fb_mod(DeltaL)/fb_mod(Li);
	return(retval);
}


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

#include <gsl/gsl_dft_complex.h>
#include <gsl/gsl_dft_complex_float.h>

#include "complex_internal.h"

#include "urand.c"

#define BASE_DOUBLE
#include "templates_on.h"
#include "signals_source.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "signals_source.c"
#include "templates_off.h"
#undef  BASE_FLOAT


/*
 * Copyright 2017 RIS.
 * Authored by Ron Dahlgren <ron@red83.net>
 */
#include "entities.h"
#include "utility/lists.h"
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

vector* entity_vec;

entity_info entity_template;

entity_info** working_copy;

static void no_op_renderer(double deltaT, entity_info *e) { }

static sd_result initialize() {
  gsl_matrix_view frame_view;
  const int starting_size = 10;
  working_copy = malloc(sizeof(void*) * starting_size);

  entity_vec = vector_ops.vector_alloc(starting_size);

  /* Set the desired template values */
  entity_template.shader = shaders.load_material(MAT_ID_BASIC_ALPHA);
  entity_template.renderer = no_op_renderer;
  bzero(&entity_template.animation_state, sizeof(animation_state));
  bzero(&entity_template.math_context, sizeof(math_context));
  /* First the reference frame */
  frame_view = gsl_matrix_view_array(entity_template.reference_frame, 4, 4);
  gsl_matrix_set_identity(&frame_view.matrix);
  /* Now the transform */
  frame_view = gsl_matrix_view_array(entity_template.transform, 4, 4);
  gsl_matrix_set_identity(&frame_view.matrix);

  entity_template.initialize = NULL;
  entity_template.terminate = NULL;
  entity_template.pre_update = NULL;
  entity_template.post_update = NULL;

  entity_template.data = NULL;

  return SD_OK;
}

static sd_result release_resources() {
  entity_info *cursor = NULL;
  while ((cursor = vector_ops.pop(entity_vec)) != NULL) {
    if (cursor->terminate != NULL) { cursor->terminate(cursor); }
    entities.entity_free(cursor);
  }
  return SD_OK;
}

static sd_result terminate() {
  release_resources();
  vector_ops.vector_free(&entity_vec);
  free(working_copy);

  return SD_OK;
}

void add_entity(entity_info* info) {
  vector_ops.append(entity_vec, info);
  // Resize buffer
  int size = entity_vec->size;
  free(working_copy);
  working_copy = malloc(sizeof(void*) * size);
  // initialize the entity
  if (info->initialize != NULL) { info->initialize(info); }
}

void remove_entity(entity_info* info) {
  int located_index = -1;
  unsigned int i;

  for (i = 0; i < entity_vec->size; ++i) {
    if (entity_vec->storage[i] == info) {
      located_index = i;
      break;
    }
  }
  if (located_index >= 0) {
    vector_ops.remove(entity_vec, located_index);
    // terminate the entity
    if (info->terminate != NULL) { info->terminate(info); }
  }
}

void update(double deltaT) {
  int size = entity_vec->size;
  /* TODO Safety lock here */
  memcpy(working_copy, entity_vec->storage, sizeof(void*) * size);
  /* TODO End safety lock here */

  /* Update the entities */
  entity_info *cursor = NULL;
  for (int i = 0; i < size; i++) {
    cursor = working_copy[i];
    if (cursor->pre_update) { cursor->pre_update(deltaT, cursor); }
    // UPDATE!
    // animation
    transform_animation.animate_entity(deltaT, working_copy[i]);

    gsl_matrix_view transform_view = gsl_matrix_view_array(
        cursor->transform, 4, 4);

    gsl_matrix_view frame_view = gsl_matrix_view_array(
        cursor->reference_frame, 4, 4);

    // Apply the animation if necessary
    if (cursor->animation_state.current_cycle != NULL) {
      animation_state *state = &cursor->animation_state;
      int reverse = state->playback_speed < 0;

      state->current_cycle->get_transform(
          state->current_cycle_time,
          state->playback_mode,
          reverse,
          state->current_cycle->data,
          state->animation_solution);

      gsl_matrix_view animation_view = gsl_matrix_view_array(
          state->animation_solution, 4, 4);

      // T = Frame * Anim
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &frame_view.matrix, &animation_view.matrix, 0.0, &transform_view.matrix);
    } else {
      gsl_matrix_memcpy(&transform_view.matrix, &frame_view.matrix);
    }
    // physics?

    if (cursor->post_update) { cursor->post_update(deltaT, cursor); }
  }
}

entity_info* entity_alloc() {
  entity_info* result = (entity_info*)malloc(sizeof(entity_info));
  memcpy(result, &entity_template, sizeof(entity_info));
  return result;
}
void entity_free(entity_info* info) { free(info); }

entities_struct const entities = {
  .initialize=initialize,
  .release_resources=release_resources,
  .terminate=terminate,
  .entity_alloc=entity_alloc,
  .entity_free=entity_free,
  .add_entity=add_entity,
  .remove_entity=remove_entity,
  .update=update
};


#ifndef SCALED_VARIANCE_H
#define SCALED_VARIANCE_H

#include "smash/decaymodes.h"
#include "smash/particletype.h"

#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_vector.h>

bool load_particle_types();

class ScaledVarianceCalculator {
 public:
  ScaledVarianceCalculator(const smash::ParticleTypePtrList& types_in_the_box,
                           double T, double mub, double mus, double muq,
                           bool energy_conservation,
                           bool B_conservation,
                           bool S_conservation,
                           bool Q_conservation,
                           bool quantum_statistics) :
    T_ (T),
    mub_ (mub),
    mus_ (mus),
    muq_ (muq),
    energy_conservation_ (energy_conservation),
    B_conservation_ (B_conservation),
    S_conservation_ (S_conservation),
    Q_conservation_ (Q_conservation),
    quantum_statistics_ (quantum_statistics),
    quantum_series_max_terms_(100),
    quantum_series_rel_precision_(1e-12) {
    all_types_in_the_box_.clear();
    for (const smash::ParticleTypePtr t : types_in_the_box) {
      all_types_in_the_box_.push_back(t);
    }
  }
  void set_T (double T) { T_ = T; }
  void set_mub (double mub) { mub_ = mub; }
  void set_mus (double mus) { mus_ = mus; }
  void set_muq (double muq) { muq_ = muq; }
  void set_B_conservation (bool B_cons) { B_conservation_ = B_cons; }
  void set_S_conservation (bool S_cons) { S_conservation_ = S_cons; }
  void set_Q_conservation (bool Q_cons) { Q_conservation_ = Q_cons; }
  void set_quantum_statistics (bool qs) { quantum_statistics_ = qs; }

  struct solver_params {
    smash::ParticleTypePtrList* types;
    double e;   // energy density
    double nb;  // baryon number density
    double ns;  // strangeness density
    double nq;  // charge density
  };
  static int set_eos_solver_equations(const gsl_vector* x, void* params,
                                      gsl_vector* f);
  std::string print_solver_state(size_t iter,
                                 gsl_multiroot_fsolver* solver) const;
  void setTmu_from_conserved(double Etot, double V,
                             double B, double S, double Q);

  /**
   * Assumes that there is a gas of particles species defined
   * by all_types_in_the_box_. This function computes fluctuations
   * of the total number of particles belonging to a subset of species
   * defined by type_of_interest function. Fluctuations depend on
   * the ensemble and which conservation laws it respects. This is
   * determined by variables energy_conservation_, B_conservation_,
   * S_conservation_, and Q_conservation_.
   *
   * For reference see arxiv.org/pdf/0706.3290.pdf, Eq. (29-40).
   * The approximations of this paper are only valid for large
   * enough volume.
   *
   * \param[in] type_of_interest Function true for particle species, for
   *            which the mean and the scaled variance are computed
   * \param[out] mean density and scaled variance of type_of_interest species
   */
  std::pair<double, double> scaled_variance(
    std::function<bool(const smash::ParticleTypePtr)> type_of_interest);

  void prepare_decays();

  friend std::ostream& operator<< (std::ostream&,
                                   const ScaledVarianceCalculator&);

 private:
  /// List of species included in the gas
  smash::ParticleTypePtrList all_types_in_the_box_;
  /**
   *  Structure for holding all decay final state in a form convenient
   *  for statistical caulculations:
   *  resonance ->
   *       vector of decays
   *       each decay = (branching ratio and vector (product -> count))
   *  Sum of branching ratios should be 1 for all particles.
   *  By convention stable particles decay into themselves with brancing ratio 1.
   */
  std::map<smash::ParticleTypePtr,
           std::vector<std::pair<double,
                               std::map<smash::ParticleTypePtr, int>>>>
       all_decay_final_states_;
  /// Temperature of the gas [GeV]
  double T_;
  /// Baryo-chemical potential of the gas [GeV]
  double mub_;
  /// Strangeness chemical potential of the gas [GeV]
  double mus_;
  /// Charge chemical potential of the gas [GeV]
  double muq_;
  /// If ensemble includes energy conservation
  bool energy_conservation_;
  /// If ensemble includes baryon number conservation
  bool B_conservation_;
  /// If ensemble includes strangeness conservation
  bool S_conservation_;
  /// If ensemble includes electric charge conservation
  bool Q_conservation_;
  /// If quantum statistics is included
  bool quantum_statistics_;
  /// Maximal number of terms in the series for quantum formulas
  const unsigned int quantum_series_max_terms_;
  /// Relative precision, at which quantum series summation stops
  const double quantum_series_rel_precision_;
};

#endif // SCALED_VARIANCE_H