#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);
}


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


#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


/**
* Output routines for stamped images and overlaid traces.
*/
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_vector.h>

#include "spce_output.h"
#include "aXe_grism.h"
#include "aXe_utils.h"
#include "spce_pathlength.h"
#include "spc_driz.h"
#include "crossdisp_utils.h"

#define MIN(x,y) (((x)<(y))?(x):(y))
#define MAX(x,y) (((x)>(y))?(x):(y))
#define SQR(x) ((x)*(x))

/* transformation from xi to the index into the trace pixel table */
#define XI_QUANT(xi) ((int)rint(((xi)-minxi)/xi_bin_width))

/**
 * Function: stamp_img
 * Produce a gsl array containing a stamp image.
 * Returns NULL is input object is NULL.
 *
 * Parameters:
 * @param ap_p    - a pointer to a -1 terminated ap_pixel array
 * @param width   - extraction width used
 * @param stp_min - xy-coords of the lower left corner on image
 *
 * Returns:
 * @return res  - a pointer to a newly allocated gsl_matrix containing the
 *                spectrum
 */
gsl_matrix * stamp_img (const ap_pixel * const ap_p, float width, d_point *stp_min)
{
  const ap_pixel *cur_p;
  double min_px = 1e32;
  double max_px = -1e32;
  double min_py = 1e32;
  double max_py = -1e32;
  gsl_matrix *res;
  long xsize,ysize,i,j,n=0;

  if (ap_p==NULL) {
    /* Create a dummy stamp image */
    res = gsl_matrix_alloc(10,10);
    /* Fill stamp with 0.0 values */
    gsl_matrix_set_all(res, 0.0);

    return res;
  }

  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      if (fabs(cur_p->dist)>width+.5) continue; /* Skip this pixel if it was not actually used */
      min_px = MIN (min_px, cur_p->p_x);
      max_px = MAX (max_px, cur_p->p_x);
      min_py = MIN (min_py, cur_p->p_y);
      max_py = MAX (max_py, cur_p->p_y);
      n++;
    }

  xsize = max_px-min_px+2;
  ysize = max_py-min_py+2;

  stp_min->x = min_px;
  stp_min->y = min_py;

  if (n>0)
    {
      res = gsl_matrix_alloc(xsize,ysize);
    } else {
      /* Create a dummy stamp image */
      res = gsl_matrix_alloc(10,10);
      /* Fill stamp with 0.0 values */
      gsl_matrix_set_all(res, 0.0);
      return res;
    }

  /* Fill stamp with NaN values */
  gsl_matrix_set_all(res, 0.0);
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      if (fabs(cur_p->dist)>width+.5) continue; /* Skip this pixel if it was not actually used */
      i = (long) floor(cur_p->p_x - min_px+.5);
      j = (long) floor(cur_p->p_y - min_py+.5);
      if ((i<0) || (i>=xsize)) continue;
      if ((j<0) || (j>=ysize)) continue;

      //fprintf(stderr,"i: %d j: %d\n",i,j);
      gsl_matrix_set(res,i,j,cur_p->count);
    }

  return res;
}

/*
 * Function: get_minxy_from_PET
 * The function determines the minimum in
 * x and y of the pixels in a PET
 *
 * Parameters:
 * @param ap_p - the PET vector
 *
 * Returns:
 * @return res - the minima in x, y
 */
d_point
get_minxy_from_PET(const ap_pixel * const ap_p)
{
  const ap_pixel *cur_p;
  double min_px = 1e32;
  //double max_px = -1e32;
  double min_py = 1e32;
  //double max_py = -1e32;
  d_point res;

  // return -1 in case of an empty PET
  if (ap_p==NULL) {
    res.x = -1;
    res.y = -1;
    return res;
  }

  // go over each pixel, determine the maximum
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      min_px = MIN (min_px, cur_p->p_x);
      min_py = MIN (min_py, cur_p->p_y);
    }

  // fill the return
  res.x = min_px;
  res.y = min_py;

  // return the result
  return res;
}

/*
 * Function: get_maxxy_from_PET
 * The function determines the maximum in
 * x and y of the pixels in a PET
 *
 * Parameters:
 * @param ap_p - the PET vector
 *
 * Returns:
 * @return res - the maximum in x, y
 */
d_point
get_maxxy_from_PET(const ap_pixel * const ap_p)
{
  const ap_pixel *cur_p;
  //double min_px = 1e32;
  double max_px = -1e32;
  //double min_py = 1e32;
  double max_py = -1e32;
  d_point res;

  // return -1 in case of an empty PET
  if (ap_p==NULL) {
    res.x = -1;
    res.y = -1;
    return res;
  }

  // go over each pixel, determine the maximum
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      max_px = MAX (max_px, cur_p->p_x);
      max_py = MAX (max_py, cur_p->p_y);
    }

  // fill the return
  res.x = max_px;
  res.y = max_py;

  // return the result
  return res;
}

/**
 * Function: stamp_img_drzprep
 * Creates all drizzle prepare stamp images from a PET
 * vector. In case of an empty PET, dummy stamps are returned.
 *
 * Parameters:
 * @param opt_extr   - flagg to indicate optimal extraction
 * @param ap_p       - a pointer to a -1 terminated ap_pixel array
 * @param width      - extraction width used
 * @param nullval    - the default pixel value
 * @param quant_cont - flag to indicate quantitative cont.
 * @param dimension  - size information
 * @param drzcoeffs  - the drizzle coefficients
 * @param exptime    - the exposure time
 * @param sky_cps    - the sky background
 *
 * Returns:
 * @return drzprep_stamps - a pointer to the new struct with the
 *                          drzprep stamp images
 */
drzprep *
stamp_img_drzprep (const int opt_extr, const ap_pixel *const ap_p, const ap_pixel *const se_p, float width,
		   float nullval, int quant_cont, drzstamp_dim dimension,
		   gsl_matrix *drzcoeffs, double exptime, double sky_cps,
		   double rdnoise, const int bckmode)
{
  drzprep *drzprep_stamps;
  const ap_pixel *cur_p;
  const ap_pixel *xxx_p;

  int i, j;

  double var;
  double corr;
  double sqr_expt;
  double sqr_rdns;

  // allocate space for the output structure
  drzprep_stamps = (drzprep *)malloc(sizeof(drzprep));

  if (ap_p==NULL) {

    // create a dummy count image and set it to default
    drzprep_stamps->counts = gsl_matrix_alloc(10,10);
    gsl_matrix_set_all(drzprep_stamps->counts, nullval);

    // create a dummy error image and set it to default
    drzprep_stamps->error = gsl_matrix_alloc(10,10);
    gsl_matrix_set_all(drzprep_stamps->error, nullval);

    // create a dummy contamination image and set it to default
    drzprep_stamps->cont = gsl_matrix_alloc(10,10);
    gsl_matrix_set_all(drzprep_stamps->cont, nullval);

    // create a dummy model image and set it to default
    drzprep_stamps->model = gsl_matrix_alloc(10,10);
    gsl_matrix_set_all(drzprep_stamps->model, nullval);

    // create a dummy model image and set it to default
    drzprep_stamps->vari = gsl_matrix_alloc(10,10);
    gsl_matrix_set_all(drzprep_stamps->vari, nullval);

    return drzprep_stamps;
  }

  // allocate the count matrix and fill it with default value
  drzprep_stamps->counts  = gsl_matrix_alloc(dimension.xsize, dimension.ysize);
  gsl_matrix_set_all(drzprep_stamps->counts, nullval);

  // allocate the error matrix and fill it with default value
  drzprep_stamps->error  = gsl_matrix_alloc(dimension.xsize, dimension.ysize);
  gsl_matrix_set_all(drzprep_stamps->error, 0.0);

  // allocate the contamination matrix and fill it with default value
  drzprep_stamps->cont  = gsl_matrix_alloc(dimension.xsize, dimension.ysize);
  gsl_matrix_set_all(drzprep_stamps->cont, 0.0);

  // set the two extentions
  // which are only somethimes
  // used to NULL
  drzprep_stamps->model  = NULL;
  drzprep_stamps->vari  = NULL;

  if (opt_extr)
    {
      // allocate the mode; matrix and fill it with default value
      drzprep_stamps->model  = gsl_matrix_alloc(dimension.xsize, dimension.ysize);
      gsl_matrix_set_all(drzprep_stamps->model, 0.0);

      // allocate the mode; matrix and fill it with default value
      drzprep_stamps->vari  = gsl_matrix_alloc(dimension.xsize, dimension.ysize);
      gsl_matrix_set_all(drzprep_stamps->vari, 0.0);
    }

  // square the exposure time for speed
  sqr_expt = exptime*exptime;
  sqr_rdns = rdnoise*rdnoise;

  // go over each PET pixel
  if (opt_extr && bckmode)
    xxx_p = se_p;
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {

      // compute the coordinates in the stamp images
      // if (fabs(cur_p->dist)>width+.5) continue; /* Skip this pixel if it was not actually used */
      i = (int)(cur_p->p_x - dimension.xstart);
      j = (int)(cur_p->p_y - dimension.ystart);

      // check whether the coordinates are inside
      if ((i<0) || (i>=dimension.xsize) || (j<0) || (j>=dimension.ysize))
	{
	  if (opt_extr && bckmode)
	    xxx_p++;
	continue;
	}

      // comute the jacobian of the drizzle coefficients
      corr = fabs(get_det_jacobian(i+1, j+1, drzcoeffs, (int)dimension.xsize,(int)dimension.ysize));

      // compute the count value
      gsl_matrix_set(drzprep_stamps->counts,i,j,(cur_p->count)/corr);

      // MODIFIED in Dec. 2010 in order
      // to get error propagation
      // compute the true error value in electrons
      gsl_matrix_set(drzprep_stamps->error,i,j,(cur_p->error*cur_p->error*sqr_expt)/corr/corr);

      // compute the contamination value
      if (quant_cont)
	gsl_matrix_set(drzprep_stamps->cont,i,j,(cur_p->contam)/corr);
      else
	gsl_matrix_set(drzprep_stamps->cont,i,j,(cur_p->contam));

      if (opt_extr)
	{

	  if (bckmode)
	    {
	      // check that the foreground and background
	      // PET element describe the same pixel
	      if (xxx_p->x != cur_p->x || xxx_p->y != cur_p->y)
		aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
			     "aXe_DRZPREP: Background PET and Object PET "
			     "have different pixel orders in PET's.\n");

	      // compute the inverse variance value for the object PET
	      // derived from the noise characteristics
	      var = ((xxx_p->model+xxx_p->contam+sky_cps+cur_p->count)*exptime + sqr_rdns)/sqr_expt;
	      //	      var = cur_p->count*exptime / sqr_expt;

	      // compute the model value
	      gsl_matrix_set(drzprep_stamps->model,i,j,(xxx_p->model/corr));
	    }
	  else
	    {
	      // compute the inverse variance value for the object PET
	      // derived from the noise characteristics
	      var = ((cur_p->model+cur_p->contam+sky_cps)*exptime + sqr_rdns)/sqr_expt;

	      // compute the model value
	      gsl_matrix_set(drzprep_stamps->model,i,j,(cur_p->model/corr));
	    }

	  if (var != 0.0)
	    gsl_matrix_set(drzprep_stamps->vari,i,j,1.0/var);

	}

      if (opt_extr && bckmode)
	xxx_p++;
    }

  // return the result structure
  return drzprep_stamps;
}

/**
 * Function: rectified_stamp_img
 * Produce a gsl array containing a 'rectified aperture'. That is,
 * an image of the spectum showing count on an xi,dist grid instead
 * of the traditional image grid x,y. Returns NULL is input object is NULL.
 *
 * Parameters:
 * @param ap_p  - a pointer to a -1 terminated ap_pixel array
 * @param width - extraction width used
 *
 * Returns:
 * @return res - a pointer to a newly allocated gsl_matrix containing the
 * rectified spectrum
 */
gsl_matrix *
rectified_stamp_img (const ap_pixel * const ap_p, float width, d_point *stp_min)
{
  const ap_pixel *cur_p;
  double min_xi = 1e32;
  double max_xi = -1e32;
  double min_dist = 1e32;
  double max_dist = -1e32;
  gsl_matrix *res;
  long xsize,ysize,i,j,n=0;

  if (ap_p==NULL) {
    /* Create a dummy stamp image */
    res = gsl_matrix_alloc(10,10);
    /* Fill stamp with 0.0 values */
    gsl_matrix_set_all(res, 0.0);
    return res;
  }

  i=0;
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      if (fabs(cur_p->dist)>width+.5) continue; /* Skip this pixel if it was not actually used */
      i++;
      min_xi = MIN (min_xi, cur_p->xi);
      max_xi = MAX (max_xi, cur_p->xi);
      min_dist = MIN (min_dist, cur_p->dist);
      max_dist = MAX (max_dist, cur_p->dist);
      n++;
    }

  if (i==0) {
    /* Create a dummy stamp image */
    res = gsl_matrix_alloc(10,10);
    /* Fill stamp with 0.0 values */
    gsl_matrix_set_all(res, 0.0);
    return res;
  }

  min_dist = floor(min_dist);
  max_dist = floor(max_dist);
  min_xi = floor(min_xi);
  max_xi = floor(max_xi);

  // compute the array dimensions
  xsize = max_xi-min_xi+2;
  ysize = max_dist-min_dist+2;

  // store the minima in both coords
  stp_min->x = min_xi;
  stp_min->y = min_dist;

  if (n>0)
    {
      res = gsl_matrix_alloc(xsize,ysize);
    } else {
      /* Create a dummy stamp image */
      res = gsl_matrix_alloc(10,10);
      /* Fill stamp with 0.0 values */
      gsl_matrix_set_all(res, 0.0);
      return res;
    }

  res = gsl_matrix_alloc(xsize,ysize);
  /* Fill stamp with NaN values */
  gsl_matrix_set_all(res, GSL_NAN);

  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      if (fabs(cur_p->dist)>width+.5) continue; /* Skip this pixel if it was not actually used */
      i = (long) floor(cur_p->xi - min_xi+.5);
      j = (long) floor(cur_p->dist - min_dist+.5);
      //fprintf(stderr,"i: %d j: %d\n",i,j);
      gsl_matrix_set(res,i,j,cur_p->count);
    }

  return res;
}

/**
 * Function: drizzled_stamp_img
 * Produce a structure containing a drizzled stamp image plux ther
 * associated weight image. The drizzled stamp image has constant
 * dispersion along the x-axis. No drizzling is applied
 * in cross-dispersion direction. The dimensions of the drizzled
 * stampimage were setr previously, and the information
 * is in an input structure. In case that there are no valid pixels,
 * a dummy structure with 10x10 empty pixels is returned.
 *
 * @param ap_p      - pointer to the list of PET pixels
 * @param width     - extraction width used
 * @param dimension - dimensional information on the drizzled stamp
 *
 * @return res      - structure with counts and weights of the drizzled stamp
*/
drzstamp *
drizzled_stamp_img (const  ap_pixel * const ap_p, double width,
		     double orient, const drzstamp_dim dimension)
{
  const ap_pixel *cur_p;

  quadrangle quad;

  drzstamp *res;
  gsl_matrix *counts;
  gsl_matrix *weight;

  int icen, ilow, iupp;
  int jcen, jlow, jupp;

  double xi, jacob=0;
  double value, allweig, weig;

  double maxarr=0.0;
  int iim, jjm;

  //double totweigth;
  //double cos_phi=0.0;

  int ii,jj;
  double arr;
  int stpi,stpj;
  double stpc;

  // allocate space for the result
  res = (drzstamp *) malloc(sizeof(drzstamp));

  // handle an empty PET
  if (ap_p==NULL || !dimension.resolution) {
    /* Create a dummy stamp image */
    counts = gsl_matrix_alloc(10,10);
    weight = gsl_matrix_alloc(10,10);
    /* Fill stamp with 0.0 values */
    gsl_matrix_set_all(counts, 0.0);
    gsl_matrix_set_all(weight, 0.0);

    // fill the output
    res->counts = counts;
    res->weight = weight;

    // return the output
    return res;
  }

  /* Allocate the stamp matrix*/
  /* Fill stamp with NaN values */
  counts = gsl_matrix_alloc(dimension.xsize,dimension.ysize);
  gsl_matrix_set_all(counts, GSL_NAN);

  /* Allocate the weight matrix*/
  /* Fill weight with 0.0 values */
  weight = gsl_matrix_alloc(dimension.xsize, dimension.ysize);
  gsl_matrix_set_all(weight, 0.0);

  // go over each pixel
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {

      // Skip this pixel if it was not actually used
      if (fabs(cur_p->dist)>width+1.5)
	continue;

      // create the quadrangle for the current pixel
      //      quad = get_quad_from_pixel2(cur_p, dimension);
      quad = get_quad_from_pixel(cur_p, orient, dimension);

      // get the jacobian (well, easy here)
      // the term "cos(cur_p->dxs)" must be there
      // to correct the enlargement necessary
      // to cover the whole lambda-cross dispersion area!
      // NOT COMPLETELY understood
      jacob = dimension.resolution/cur_p->dlambda*cos(cur_p->dxs);

      // get the central pixel (icen, jcen) of the current PET-pixel
      xi = cur_p->lambda/dimension.resolution;
      icen = (int) floor(xi - dimension.xstart+.5);
      jcen = (int) floor(cur_p->dist - dimension.ystart+.5);

      // get the uper and lower extend of the quadrangle in x
      iupp = (int)floor(quad.xmax - (double)icen + 0.5)+1;
      ilow = (int)floor(quad.xmin - (double)icen + 0.5);

      // get the uper and lower extend of the quadrangle in x
      jupp = (int)floor(quad.ymax - (double)jcen + 0.5)+1;
      jlow = (int)floor(quad.ymin - (double)jcen + 0.5);

      maxarr=0.0;
      //      totweigth = 0.0;
      // go over the extend in x
      for (ii=ilow;ii<iupp;ii++) {
      	// go over the extend in x
      	for (jj=jlow;jj<jupp;jj++) {

      	  // get the coordinates of the current output pixel
      	  stpi = icen+ii;
      	  stpj = jcen+jj;

      	  // check whether the current output pixel is within
      	  // the stamp image; continue if not
      	  if ( (stpi>=dimension.xsize)||(stpi<0)||(stpj>=dimension.ysize)||(stpj<0) )
      	    continue;

      	  // get the area which falls onto the current output pixel
      	  arr = boxer(stpi,stpj,quad.x,quad.y);
      	  if (arr > maxarr)
      	    {
      	      maxarr=arr;
      	      iim = ii;
      	      jjm = jj;
      	    }
      	  // get the already existing counts and weights
      	  stpc = gsl_matrix_get(counts,stpi,stpj);
      	  weig = gsl_matrix_get(weight,stpi,stpj);

      	  // initialize the counts, if necessary
      	  if (isnan(stpc) && (arr!=0.0))
      	    stpc = 0.0;

      	  // compute the new, total weight of the current output pixel
      	  //	  allweig = weig + jacob*arr;
      	  allweig = weig + arr;

      	  // do a weighted sum of the count value at the current output pixel
      	  value = (stpc*weig + arr*cur_p->count*jacob) / (allweig);

      	  // store the new count value and the new weight
      	  gsl_matrix_set(counts,stpi,stpj,value);
      	  gsl_matrix_set(weight,stpi,stpj,allweig);
      	  //	  totweigth = totweigth + arr;
      	}
      }
    }

  // fill the output structure
  res->counts = counts;
  res->weight = weight;

  // return the output
  return res;
}

/**
 * Function: get_drzprep_dim
 * The function computes the dimensional properties
 * of the stamp images created in the drizzle prepare task.
 * Those properties fix the image size
 * and the offsets in both coordinates.
 *
 * Parameter:
 * @param ap_p        - the list of beam pixels to find the diemnsions for
 * @param width       - the extraction width of the beam
 * @param boxwidth    - the boxsize
 * @param boxheight   - the boxwidth
 *
 * Returns:
 * @return dimensions - the filled quadrangle sturcture
 */
drzstamp_dim
get_drzprep_dim(const ap_pixel *const ap_p, float width,
		int boxwidth, int boxheight)
{

  drzstamp_dim dimensions;

  const ap_pixel *cur_p;

  double min_px = 1e32;
  double max_px = -1e32;
  double min_py = 1e32;
  double max_py = -1e32;

  // return a dummy if the PET is NULL
  if (ap_p==NULL)
    {
      dimensions.xstart=0.0;
      dimensions.ystart=0.0;

      dimensions.xsize=0;
      dimensions.ysize=0;

      return dimensions;
    }

  // go over each pixel, find minimum and maximum in x,y
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      // if (fabs(cur_p->dist)>width+.5) continue; /* Skip this pixel if it was not actually used */
      min_px = MIN (min_px, cur_p->p_x);
      max_px = MAX (max_px, cur_p->p_x);
      min_py = MIN (min_py, cur_p->p_y);
      max_py = MAX (max_py, cur_p->p_y);
    }

  // compute the dimension of the stamps
  dimensions.xsize = max_px-min_px+1;
  dimensions.ysize = max_py-min_py+1;

  // correct the dimesions if necessary
  if (boxwidth > dimensions.xsize){
    dimensions.xsize = (long)boxwidth;
  }
  if (boxheight > dimensions.ysize){
    dimensions.ysize = boxheight;
  }

  // fix the start coordinates
  dimensions.xstart= min_px;
  dimensions.ystart= min_py;

  // return the structure
  return dimensions;
}

/**
 * Function: get_stamp_dim
 * The function computes the dimensional properties
 * of the drizzled stamp image associated to a beam.
 * Those properties fix the image size, dispersion
 * and the offsets in both coordinates
 *
 * Parameters:
 * @param ap_p        - the list of beam pixels to find the diemnsions for
 * @param width       - the extraction width of the beam
 * @param conf        - the configuration structure
 * @param beamID      - the the beam ID
 *
 * Returns:
 * @return dimensions - the filled quadrangle sturcture
 */
drzstamp_dim
get_stamp_dim(const ap_pixel *const ap_p, float width,
	      aperture_conf *conf, const int beamID, d_point *stp_min)
{
  drzstamp_dim dimensions;

  const ap_pixel *cur_p;

  double min_lam  = 1e32;
  double max_lam  = -1e32;
  double min_xi   = 1e32;
  double max_xi   = -1e32;
  double min_dist = 1e32;
  double max_dist = -1e32;
  double min_dlam = 1e32;
  double max_dlam = -1e32;

  //double resampwidth = 0.0;

  //long xsize,ysize;
  long npixel=0;
  //long j;

  // check whether there are entries;
  // if not, return a dummy structure
  if (ap_p==NULL)
    {
      dimensions.resolution=0.0;

      dimensions.xstart=0.0;
      dimensions.ystart=0.0;

      dimensions.xsize=0;
      dimensions.ysize=0;

      return dimensions;
    }

  // reset the pixel counter
  npixel=0;

  // go over each pixel
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {

      // Skip the pixel if it was not actually used
      if (fabs(cur_p->dist)>width+.5)
	continue;

      // search for minimum/maximum in wavelength
      min_lam = MIN (min_lam, cur_p->lambda);
      max_lam = MAX (max_lam, cur_p->lambda);

      // search for minimum/maximum in crossdispersion direction
      min_dist = MIN (min_dist, cur_p->dist);
      max_dist = MAX (max_dist, cur_p->dist);

      // search for minimum/maximum in dispersion
      min_dlam = MIN (min_dlam, cur_p->dlambda);
      max_dlam = MAX (max_dlam, cur_p->dlambda);

      // enhance the pixel counter
      npixel++;
    }

  // in case there were no valid pixels,
  // return a dummy structure
  if (!npixel)
    {
      dimensions.resolution=0.0;

      dimensions.xstart=0.0;
      dimensions.ystart=0.0;

      dimensions.xsize=0;
      dimensions.ysize=0;

      return dimensions;
    }

  // fill the resolution value
  // if the drizzle resolution is defined in the
  // configuration file and it is a first order beam
  if (conf->drz_resol && !beamID)
    // take the drizzle value from the config file
    dimensions.resolution = (double)conf->drz_resol;
  // otherwise
  else
    // take the average resoution
    dimensions.resolution = (max_dlam+min_dlam)/2.0;

  // round up and off in crossdispersion direction
  min_dist = floor(min_dist);
  max_dist = ceil(max_dist);

  // round up and off in dispersion direction
  min_xi = floor(min_lam/dimensions.resolution);
  max_xi = ceil(max_lam/dimensions.resolution);

  // derive and store the stamp image dimensions
  dimensions.xsize = max_xi-min_xi+2;
  dimensions.ysize = max_dist-min_dist+2;

  // store the start values
  dimensions.xstart = min_xi;
  dimensions.ystart = min_dist;

  stp_min->x = min_xi * dimensions.resolution;
  stp_min->y = min_dist;

  // return the structure
  return dimensions;
}

/**
 * Function: get_quad_from_pixel
 * The subroutine creates a quadrangle for a PET pixel
 * in the coordinate system of a drizzled stam image.
 * The quadrangle later serves as an input to the boxer
 * routine to resample the PET pixel.
 * Form the math point of view, the generation
 * of the quadrange corresponds to executing an affine
 * transformation of the pixel (x,y) corners into
 * the coordinate system spanned by the wavelength
 * and the crossdispersion direction.
 *
 * Parameters:
 * @param cur_p     - the PET pixel to construct the quadrangle for
 * @param dimension - the dimensions of the stamp image
 *
 * Returns:
 * @return quad - the filled quadrangle sturcture
 */
quadrangle
get_quad_from_pixel(const ap_pixel *cur_p, const double orient,
		    const drzstamp_dim dimension)
{
  quadrangle quad;

  double dxi, ddist;
  double x, y;

  double phi_1, phi_2;
  double cos_phi_1, sin_phi_1, tan_phi_2;
  double term_1, term_2;

  // nomenclature: see contamination.dvi
  phi_1 = orient - 1.5707963267948966;  // crossdispersion direction corresponds to beta; numeric=90deg in rad
  phi_2 = cur_p->dxs - orient + 1.5707963267948966; // traceangle corresponds to alpha

  // nomenclature: see contamination.dvi
  cos_phi_1 = cos(phi_1);
  sin_phi_1 = sin(phi_1);
  tan_phi_2 = tan(phi_2);

  // nomenclature: see contamination.dvi
  term_1 = cos_phi_1*tan_phi_2 + sin_phi_1;
  term_2 = cos_phi_1           - sin_phi_1*tan_phi_2;

  // Bottom left corner (-.5, -.5):
  // get the coos of that corner in the stamp image coo system
  x = -0.5;
  y = -0.5;
  dxi   = +x*cos_phi_1 + y*sin_phi_1;
  ddist = -x*term_1    + y*term_2;
  /*** IMPORTANT:
   * the division by "cos(cur_p->dxs)" in the equations below is
   * is necessary to cover the whole lambda-crossdisp plane.
   * However then the counts are not anymore preserved.
   * This must be taken into account in the calling routine!!
   **/
  quad.x[0] = (cur_p->lambda + cur_p->dlambda*dxi/cos(cur_p->dxs))/dimension.resolution - dimension.xstart;
  quad.y[0] = cur_p->dist + ddist - dimension.ystart;
  //  fprintf(stdout, "%f %f\n", quad.x[0], quad.y[0]);

  // Top left corner (-.5, +.5):
  // get the coos of that corner in the stamp image coo system
  x = -0.5;
  y = +0.5;
  dxi   = +x*cos_phi_1 + y*sin_phi_1;
  ddist = -x*term_1    + y*term_2;
  //  quad.x[1] = cur_p->xi + dxi/cos(cur_p->dxs);
  quad.x[1] = (cur_p->lambda + cur_p->dlambda*dxi/cos(cur_p->dxs))/dimension.resolution - dimension.xstart;
  quad.y[1] = cur_p->dist + ddist - dimension.ystart;

  // Top right corner (+.5, +.5):
  // get the coos of that corner in the stamp image coo system
  x = +0.5;
  y = +0.5;
  dxi   = +x*cos_phi_1 + y*sin_phi_1;
  ddist = -x*term_1    + y*term_2;
  quad.x[2] = (cur_p->lambda + cur_p->dlambda*dxi/cos(cur_p->dxs))/dimension.resolution - dimension.xstart;
  quad.y[2] = cur_p->dist + ddist - dimension.ystart;

  // Bottom right corner (+.5, -.5):
  // get the coos of that corner in the stamp image coo system
  x = +0.5;
  y = -0.5;
  dxi   = +x*cos_phi_1 + y*sin_phi_1;
  ddist = -x*term_1    + y*term_2;
  quad.x[3] = (cur_p->lambda + cur_p->dlambda*dxi/cos(cur_p->dxs))/dimension.resolution - dimension.xstart;
  quad.y[3] = cur_p->dist + ddist - dimension.ystart;

  // get the maximum and minimum of the quadrangle
  // in each dimension
  quad.xmax = MAX(quad.x[3],MAX(quad.x[2],MAX(quad.x[1],quad.x[0])));
  quad.xmin = MIN(quad.x[3],MIN(quad.x[2],MIN(quad.x[1],quad.x[0])));
  quad.ymax = MAX(quad.y[3],MAX(quad.y[2],MAX(quad.y[1],quad.y[0])));
  quad.ymin = MIN(quad.y[3],MIN(quad.y[2],MIN(quad.y[1],quad.y[0])));

  return quad;
}

/**
    Produce a gsl array containing a 'drizzled aperture'. That is,
    an image of the spectum showing count on an xi,dist grid instead
    of the traditional image grid x,y. Returns NULL is input object is NULL.
	[modifies ap_p!!]
    @param ap_p a pointer to a -1 terminated ap_pixel array
    @param width extraction width used
	@param resampwidth A per pixel of output drizzled spectra
    @return a pointer to a newly allocated gsl_matrix containing the
    rectified spectrum
*/
gsl_matrix *
drizzled_stamp_img_orig (const  ap_pixel * const ap_p, float width, aperture_conf *conf)
{
  const ap_pixel *cur_p;

  double min_xi   = 1e32;
  double max_xi   = -1e32;
  double min_dist = 1e32;
  double max_dist = -1e32;
  double min_dlam = 1e32;
  double max_dlam = -1e32;

  double resampwidth = 0.0;

  gsl_matrix *res;

  long xsize,ysize,i,j,n=0;

  double xi;

  if (ap_p==NULL) {
    /* Create a dummy stamp image */
    res = gsl_matrix_alloc(10,10);
    /* Fill stamp with 0.0 values */
    gsl_matrix_set_all(res, 0.0);
    return res;
  }

  i=0;
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      if (fabs(cur_p->dist)>width+.5) continue; /* Skip this pixel if it was not actually used */
      i++;

      xi = cur_p->lambda;

      min_xi = MIN (min_xi, xi);
      max_xi = MAX (max_xi, xi);

      min_dist = MIN (min_dist, cur_p->dist);
      max_dist = MAX (max_dist, cur_p->dist);

      min_dlam = MIN (min_dlam, cur_p->dlambda);
      max_dlam = MAX (max_dlam, cur_p->dlambda);

      n++;
    }

  if (conf->drz_resol)
    {
      resampwidth = (double)conf->drz_resol;
    }
  else
    {
      resampwidth = (max_dlam+min_dlam)/2.0;
    }

  fprintf(stdout, "resamplewidth: %f\n", resampwidth);

  if (i==0) {
    /* Create a dummy stamp image */
    res = gsl_matrix_alloc(10,10);
    /* Fill stamp with 0.0 values */
    gsl_matrix_set_all(res, 0.0);
    return res;
  }

  min_dist = floor(min_dist);
  max_dist = floor(max_dist);
  min_xi = floor(min_xi/resampwidth);
  max_xi = floor(max_xi/resampwidth);

  xsize = max_xi-min_xi+2;
  ysize = max_dist-min_dist+2;

  if (n>0)
    {
      res = gsl_matrix_alloc(xsize,ysize);
    } else {
      /* Create a dummy stamp image */
      res = gsl_matrix_alloc(10,10);
      /* Fill stamp with 0.0 values */
      gsl_matrix_set_all(res, 0.0);
      return res;
    }

  res = gsl_matrix_alloc(xsize,ysize);
  /* Fill stamp with NaN values */
  gsl_matrix_set_all(res, GSL_NAN);

  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      int ii,jj;
      double x,y,xp,yp,arr;
      double xx[4],yy[4];
      int stpi,stpj;
      double stpc;

      /* Skip this pixel if it was not actually used */
      if (fabs(cur_p->dist)>width+.5)
	continue;

      //cur_p->dxs=0.;
      //cur_p->xi = 100.5 + min_xi;
      //cur_p->dist = 10. + min_dist;
      xi = cur_p->lambda/resampwidth;

      i = (long) floor(xi - min_xi+.5);
      j = (long) floor(cur_p->dist - min_dist+.5);

      /* The center of this PET entry should fall in pixel (i.j) */
      /* Bottom left corner (-.5, -.5) */
      x = -0.5;
      y = -0.5;
      xp =  x*cos(cur_p->dxs) + y*sin(cur_p->dxs);
      yp = -x*sin(cur_p->dxs) + y*cos(cur_p->dxs);
      xx[0] = xp+xi - min_xi;
      yy[0] = yp+cur_p->dist - min_dist;

      /* Top left corner (-.5, +.5) */
      x = -0.5;
      y = +0.5;
      xp =  x*cos(cur_p->dxs) + y*sin(cur_p->dxs);
      yp = -x*sin(cur_p->dxs) + y*cos(cur_p->dxs);
      xx[1] = xp+ xi - min_xi;
      yy[1] = yp+cur_p->dist - min_dist;

      /* Top right corner (+.5, +.5) */
      x = +0.5;
      y = +0.5;
      xp =  x*cos(cur_p->dxs) + y*sin(cur_p->dxs);
      yp = -x*sin(cur_p->dxs) + y*cos(cur_p->dxs);
      xx[2] = xp+ xi - min_xi;
      yy[2] = yp+cur_p->dist - min_dist;

      /* Bottom right corner (+.5, -.5) */
      x = +0.5;
      y = -0.5;
      xp =  x*cos(cur_p->dxs) + y*sin(cur_p->dxs);
      yp = -x*sin(cur_p->dxs) + y*cos(cur_p->dxs);
      // fprintf(stderr,"1 %f %f\n",cur_p->xi - min_xi,cur_p->dist - min_dist);
      xx[3] = xp+ xi - min_xi;
      yy[3] = yp+cur_p->dist - min_dist;

      for (ii=-1;ii<=1;ii++) {
	for (jj=-1;jj<=1;jj++) {
	  //fprintf(stderr,"PET: XI: %f DIST: %f DXS: %f cos:%f\n",cur_p->xi,cur_p->dist,cur_p->dxs,cos(cur_p->dxs));
	  arr = boxer((int) floor(xi - min_xi)+ii,(int) floor(cur_p->dist - min_dist)+jj,xx,yy);

	  //fprintf(stderr,"STP: i:%d j:%d arr: %f\n",(int) floor(cur_p->xi - min_xi)+ii,(int) floor(cur_p->dist - min_dist)+jj,arr);
	  stpi = (int) floor(xi - min_xi)+ii;
	  stpj = (int) floor(cur_p->dist - min_dist)+jj;
	  if ( (stpi>=xsize)||(stpi<0)||(stpj>=ysize)||(stpj<0) ) continue;
	  stpc = gsl_matrix_get(res,stpi,stpj);
	  if (isnan(stpc) && (arr!=0.0)) stpc = 0.0;
	  gsl_matrix_set(res,stpi,stpj,stpc+arr*cur_p->count);
	}
      }

    }

  return res;
}

/**
 * Function: interpolate_over_NaN
 * Function to interpolate over and replace NaN values in a rectified
 * stamp image. This is done by interpolating in the vertical direction.
 * The original gsl_matrix is free'd and is replaced by a new gsl_matrix pointer.
 * Does nothing if input data is NULL.
 *
 * Parameters:
 * @param data - the gsl_matrix where NaN values are to be interpolated over
 */
void interpolate_over_NaN (gsl_matrix *data)
{
  double *x,*y;
  int i,j,n;
  gsl_interp_accel *acc;
  gsl_spline *spline ;

  if (data==NULL) return;

  x = malloc(sizeof(double)*data->size2*data->size1);
  y = malloc(sizeof(double)*data->size2*data->size1);

  for (i=0;i<(int)data->size1;i++) {
    n = 0;
    for (j=0;j<(int)data->size2;j++) {
      if ( !isnan(gsl_matrix_get(data,i,j)) )  {
	       x[n] = j;
	       y[n] = gsl_matrix_get(data,i,j);
	       n++;
      }
    }
    //if (n<data->size2) fprintf(stderr,"%d elements non-NaN found out of %d.\n",n,data->size2);
    if (n<3) continue;

    acc = gsl_interp_accel_alloc ();
    spline = gsl_spline_alloc (gsl_interp_cspline, n);

    /* Interpolating */
    gsl_spline_init (spline, x, y, n);

    for (j=0;j<(int)data->size2;j++) {
      double xi = j;
      //double yi = gsl_spline_eval(spline, xi, acc);
      // revert to older extrapolation allowing functionality
      double yi = spline->interp->type->eval(spline->interp->state, spline->x, spline->y, spline->interp->size, x[j], acc, &y[j]);
      //if (n<data->size2)  fprintf(stderr,"%d %g => %d %g\n",j,gsl_matrix_get(data,i,j),j,yi);
      gsl_matrix_set(data, i, j, yi);
    }
    gsl_spline_free (spline);
    gsl_interp_accel_free(acc);
  }

  free(x);
  free(y);
}

/*
 * Function: free_drzprep
 * The function releases the memory allocated
 * to the various components of a drzprep structure
 *
 * Parameters:
 * @param: drzprep_stamps - the stamp image to free
 */
void
free_drzprep(drzprep *drzprep_stamps)
{
  // release the individual matrixes
  gsl_matrix_free(drzprep_stamps->counts);
  gsl_matrix_free(drzprep_stamps->error);
  gsl_matrix_free(drzprep_stamps->cont);
  if (drzprep_stamps->model)
    gsl_matrix_free(drzprep_stamps->model);
  if (drzprep_stamps->vari)
    gsl_matrix_free(drzprep_stamps->vari);

  // release the struct
  free(drzprep_stamps);
}

/**
 * Function: free_stamp_img
 * Function to free the memeory allocated by a stamp image
 *
 * Parameters:
 * @param rstamp - the stamp image
 */
void free_stamp_img(gsl_matrix *rstamp)
{
  // simply free it and "basta"!
  gsl_matrix_free(rstamp);
}

/**
 * Function: free_drzstamp
 * Function to free the memeory allocated by a
 * drizzled stamp image.
 *
 * Parameters:
 * @param rstamp  - the structure for the drizzled stamp image
 */
void free_drzstamp(drzstamp *rstamp)
{
  // release each component
  gsl_matrix_free(rstamp->counts);
  gsl_matrix_free(rstamp->weight);

  // release the rest
  free(rstamp);
}

/**
  Find the indices of pixels corresponding to the trace (i.e. the
  one with the smallest dist in each column).  If we get
  traces with a slope of significantly more than one, a similar
  function would have to be written operating on rows.

  @param ap_p the aperture pixel table
  @return a gsl_matrix with the indices of the trace pixels in ap_p
*/
gsl_vector_int *
get_trace_inds (const ap_pixel * const ap_p)
{
  const ap_pixel *cur_p;
  int trace_resolution = 10000;	/* Number of xi bins in which to follow the
				   trace */
  gsl_vector_int *trace_inds = gsl_vector_int_alloc (trace_resolution);
  gsl_vector_int *trace_inds2;
  int n, i;
  double distmin;

  gsl_vector_int_set_all (trace_inds, -1);

  /* Find the minimum pixel-to-trace distance in this aperture */
  distmin = 1e32;
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      distmin = MIN (distmin, fabs(cur_p->dist));
    }

  n = 0;
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {
      if (fabs(cur_p->dist) < (distmin + .25))
	{
	  gsl_vector_int_set (trace_inds, n, cur_p - ap_p);
	  n++;
	}
    }
  trace_inds2 = gsl_vector_int_alloc (n);
  n = 0;
  for (i = 0; i < (int)trace_inds->size; i++)
    {
      if (gsl_vector_int_get (trace_inds, i) != -1)
	{
	  gsl_vector_int_set (trace_inds2, n,
			      gsl_vector_int_get (trace_inds, i));
	  n++;
	}
    }

  return trace_inds2;
}