#define ORIGIN_DATE       "2012.01.05"
#define ORIGIN_VERSION    "2.1"

#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <pthread.h>
#include <assert.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "common.h"
#include "ivector.h"
#include "species.h"
#include "specieslist.h"
#include "graph.h"
#include "utils.h"

#define MODEL_BDM_NEUTRAL      0
#define MODEL_BDM_SELECTION    1

// Parameters of the simulations.
typedef struct
{
    int m;             // Type of model.
    int communities;   // Number of communities.
    int j_per_c;       // Number of individuals per community.
    int k_gen;         // Number of generations (in thousands).
    int init_species;  // Initial number of species.
    double mu;         // Mutation rate.
    double omega;      // Weight of the links between communities.
    double s;          // Selection coefficient.
    double r;          // Radius (for random geometric graphs).
    double w;          // Width (for rectangle random geometric graphs).
    char *ofilename;   // Name of the output files.
    char *shape;       // Shape of the metacommunity.
}
Params;

// Prototype for the function used by the threads
void *sim(void *parameters);
// The function used to setup the cumulative jagged array from the graph.
double **setup_cumulative_list(const graph *g, double omega);

/////////////////////////////////////////////////////////////
// Main                                                    //
/////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    // Set default values;
    Params p;
    p.m = MODEL_BDM_SELECTION;
    p.communities = 10;
    p.j_per_c = 10000;
    p.init_species = 20;
    p.k_gen = 100;
    p.mu = 1e-4;
    p.omega = 5e-4;
    p.s = 0.15;
    p.r = 0.25;
    p.w = 0.25;
    p.ofilename = (char*)malloc(50);
    p.shape = (char*)malloc(20);

    // Number of simulations;
    int n_threads = 1;

    // Options;
    if (argc > 1 && argv[1][0] == '-' && argv[1][1] == '-')
    {
        // --help
        if (argv[1][2] == 'h')
        {
            printf("Usage: ./origin [options]\n");
            printf("Example: ./origin -x=8 -shape=circle -model=1 -s=0.10\n");
            printf("General options:\n");
            printf("  --help          How you got here...\n");
            printf("  --version       Display version.\n");
            printf("  --ref           Display reference.\n");
            printf("Simulation parameters:\n");
            printf("  -x\n");
            printf("    description:  Number of simulations to run. Each simulation\n");
            printf("                  will use a distinct POSIX thread. \n");
            printf("    values:       Any unsigned integer.\n");
            printf("    default:      1\n");
            printf("  -model\n");
            printf("    description:  Set the model used.\n");
            printf("    values:       0, 1.\n");
            printf("    details:      0 = Neutral BDM speciation.\n");
            printf("                  1 = BDM speciation with selection.\n");
            printf("    default:      1\n");
            printf("  -shape\n");
            printf("    description:  Set the shape of the metacommunity.\n");
            printf("    values:       circle, complete, random, rectangle, star.\n");
            printf("    default:      random\n");
            printf("  -r\n");
            printf("    description:  Threshold radius for random geometric\n");
            printf("                  graphs.\n");
            printf("    values:       Any double greater than 0.\n");
            printf("    default:      0.25\n");
            printf("  -w\n");
            printf("    description:  The width of a rectangle random geometric\n");
            printf("                  graph.\n");
            printf("    values:       0 < w < 1\n");
            printf("    default:      0.25\n");
            printf("  -g\n");
            printf("    description:  Number of generations in thousands.\n");
            printf("    values:       Any positive integer.\n");
            printf("    default:      100 (i.e.: 100 000 generations)\n");
            printf("  -c\n");
            printf("    description:  Number of local communities.\n");
            printf("    values:       Any integer greater than 0.\n");
            printf("    default:      10\n");
            printf("  -jpc\n");
            printf("    description:  Number of individuals per communities.\n");
            printf("    values:       Any integer greater than 0.\n");
            printf("    default:      10000\n");
            printf("  -sp\n");
            printf("    description:  Initial number of species. Must be a factor\n");
            printf("                  of the number of individuals/community.\n");
            printf("    values:       Any positive integer greater than 0.\n");
            printf("    default:      20\n");
            printf("  -s\n");
            printf("    description:  Set the selection coefficient (model 1 only).\n");
            printf("    values:       Any double in the (-1.0, 1.0) interval.\n");
            printf("    default:      0.15\n");
            printf("  -mu\n");
            printf("    description:  Set the mutation rate.\n");
            printf("    values:       Any positive double.\n");
            printf("    default:      1e-4\n");
            printf("  -omega\n");
            printf("    description:  The weight of the proper edges.\n");
            printf("    values:       Any nonnegative double.\n");
            printf("    default:      5e-4\n");
            printf("  -o\n");
            printf("    description:  Name of the output files.\n");
            printf("    values:       Any string.\n");
            return EXIT_SUCCESS;
        }
        else if (argv[1][2] == 'v') // --version
        {
            printf("origin v.%s (%s)\n", ORIGIN_VERSION, ORIGIN_DATE);
            printf("Copyright (c) 2010-2011 Philippe Desjardins-Proulx <philippe.d.proulx@gmail.com>\n");
            printf("GPLv2 license  (see LICENSE)\n");
            return EXIT_SUCCESS;
        }
        else if (argv[1][2] == 'r') // --ref
        {
            printf("P. Desjardins-Proul and D. Gravel. How likely is speciation in\n  neutral ecology? The American Naturalist 179(1).\n");
            return EXIT_SUCCESS;
        }
    } // end '--' options

    // Read options
    read_opt_i("g", argv, argc, &p.k_gen);
    read_opt_i("jpc", argv, argc, &p.j_per_c);
    read_opt_i("model", argv, argc, &p.m);
    read_opt_i("c", argv, argc, &p.communities);
    read_opt_i("sp", argv, argc, &p.init_species);
    read_opt_i("x", argv, argc, &n_threads);
    read_opt_d("mu", argv, argc, &p.mu);
    read_opt_d("omega", argv, argc, &p.omega);
    read_opt_d("r", argv, argc, &p.r);
    read_opt_d("w", argv, argc, &p.w);
    read_opt_d("s", argv, argc, &p.s);
    if (read_opt_s("o", argv, argc, p.ofilename) == false)
    {
        sprintf(p.ofilename, "");
    }
    if (read_opt_s("shape", argv, argc, p.shape) == false)
    {
        sprintf(p.shape, "random");
    }

    printf("<?xml version=\"1.0\"?>\n");
    printf("<origin_ssne>\n");
    printf("  <model>");
    if (p.m == MODEL_BDM_NEUTRAL)
    {
        printf("Neutral BDM speciation</model>\n");
    }
    else if (p.m == MODEL_BDM_SELECTION)
    {
        printf("BDM speciation with selection</model>\n");
    }
    printf("  <n>%d</n>\n", n_threads);
    printf("  <shape_metacom>%s</shape_metacom>\n", p.shape);
    printf("  <metacom_size>%d</metacom_size>\n", p.j_per_c * p.communities);
    printf("  <k_gen>%d</k_gen>\n", p.k_gen);
    printf("  <num_comm>%d</num_comm>\n", p.communities);
    printf("  <individuals_per_comm>%d</individuals_per_comm>\n", p.j_per_c);
    printf("  <initial_num_species>%d</initial_num_species>\n", p.init_species);
    printf("  <mutation_rate>%.2e</mutation_rate>\n", p.mu);
    printf("  <omega>%.2e</omega>\n", p.omega);
    if (p.m == MODEL_BDM_SELECTION)
    {
        printf("  <selection>%.2e</selection>\n", p.s);
    }
    if (p.shape[0] == 'r')
    {
        printf("  <radius>%.4f</radius>\n", p.r);
    }
    if (p.shape[0] == 'r' && p.shape[1] == 'e')
    {
        printf("  <width>%.4f</width>\n", p.w);
    }
    printf("  <filename>%s</filename>\n", p.ofilename);

    // The threads:
    pthread_t threads[n_threads];

    const time_t start = time(NULL);

    // Create the threads:
    for (int i = 0; i < n_threads; ++i)
    {
        pthread_create(&threads[i], NULL, sim, (void*)&p);
    }

    // Wait for the threads to end:
    for (int i = 0; i < n_threads; ++i)
    {
        pthread_join(threads[i], NULL);
    }

    const time_t end_t = time(NULL);
    printf("  <seconds>%lu</seconds>\n", (unsigned long)(end_t - start));
    printf("  <time>%s</time>\n", sec_to_string(end_t - start));
    printf("</origin_ssne>\n");
    return EXIT_SUCCESS; // yeppie !
}

///////////////////////////////////////////
// The simulation                        //
///////////////////////////////////////////
void *sim(void *parameters)
{
    const Params P = *((Params*)parameters);

    char *shape = P.shape;
    const int k_gen = P.k_gen;
    const int communities = P.communities;
    const int j_per_c = P.j_per_c;
    const int init_species = P.init_species;
    const int init_pop_size = j_per_c / init_species;
    const double omega = P.omega;
    const double mu = P.mu;
    const double s = P.s;
    const double radius = P.r;
    const double width = P.w;

    // GSL's Taus generator:
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_taus2);
    // Initialize the GSL generator with /dev/urandom:
    const unsigned int seed = devurandom_get_uint();
    gsl_rng_set(rng, seed); // Seed with time
    printf("  <seed>%u</seed>\n", seed);
    // Used to name the output file:
    char *buffer = (char*)malloc(100);
    // Nme of the file:
    sprintf(buffer, "%s%u.xml", P.ofilename, seed);
    // Open the output file:
    FILE *restrict out = fopen(buffer, "w");
    // Store the total num. of species/1000 generations:
    int *restrict total_species = (int*)malloc(k_gen * sizeof(int));
    // Number of speciation events/1000 generations:
    int *restrict speciation_events = (int*)malloc(k_gen * sizeof(int));
    // Number of extinctions/1000 generations:
    int *restrict extinction_events = (int*)malloc(k_gen * sizeof(int));
    // Number of speciation events/vertex:
    int *restrict speciation_per_c = (int*)malloc(communities * sizeof(int));
    // Number of local extinction events/vertex:
    int *restrict extinction_per_c = (int*)malloc(communities * sizeof(int));
    // Store the lifespan of the extinct species:
    ivector lifespan;
    ivector_init0(&lifespan);
    // Store the population size at speciation event:
    ivector pop_size;
    ivector_init0(&pop_size);
    // (x, y) coordinates for the spatial graph:
    double *restrict x = (double*)malloc(communities * sizeof(double));
    double *restrict y = (double*)malloc(communities * sizeof(double));
    for (int i = 0; i < communities; ++i)
    {
        speciation_per_c[i] = 0;
        extinction_per_c[i] = 0;
    }
    // Initialize an empty list of species:
    species_list *restrict list = species_list_init();
    // Initialize the metacommunity and fill them with the initial species evenly:
    for (int i = 0; i < init_species; ++i)
    {
        // Intialize the species and add it to the list:
        species_list_add(list, species_init(communities, 0, 3));
    }
    // To iterate the list;
    slnode *it = list->head;
    // Fill the communities:
    while (it != NULL)
    {
        for (int i = 0; i < communities; ++i)
        {
            it->sp->n[i] = init_pop_size;
            it->sp->genotypes[0][i] = init_pop_size;
        }
        it = it->next;
    }

    // To iterate the list;
    const int remainder = j_per_c - (init_species * init_pop_size);
    for (int i = 0; i < communities; ++i)
    {
        it = list->head;
        for (int j = 0; j < remainder; ++j, it = it->next)
        {
            ++(it->sp->n[i]);
            ++(it->sp->genotypes[0][i]);
        }
    }

    // Test (will be removed for v2.0):
    int sum = 0;
    it = list->head;
    while (it != NULL)
    {
        sum += species_total(it->sp);
        it = it->next;
    }
    assert(sum == j_per_c * communities);

    // Create the metacommunity;
    graph g;
    switch(shape[0])
    {
    case 's':
        shape = "star";
        graph_get_star(&g, communities);
        break;
    case 'c':
        if (shape[1] == 'o')
        {
            shape = "complete";
            graph_get_complete(&g, communities);
            break;
        }
        else
        {
            shape = "circle";
            graph_get_circle(&g, communities);
            break;
        }
    case 'r':
        if (shape[1] == 'e')
        {
            shape = "rectangle";
            graph_get_rec_crgg(&g, communities, width, radius, x, y, rng);
            break;
        }
        else
        {
            shape = "random";
            graph_get_crgg(&g, communities, radius, x, y, rng);
            break;
        }
    default:
        shape = "random";
        graph_get_crgg(&g, communities, radius, x, y, rng);
    }
    // Setup the cumulative jagged array for migration:
    double **cumul = setup_cumulative_list(&g, omega);

    fprintf(out, "<?xml version=\"1.0\"?>\n");
    fprintf(out, "<simulation>\n");
    fprintf(out, "  <model>");
    if (P.m == MODEL_BDM_NEUTRAL)
    {
        fprintf(out, "Neutral BDM speciation</model>\n");
    }
    else if (P.m == MODEL_BDM_SELECTION)
    {
        fprintf(out, "BDM speciation with selection</model>\n");
    }
    fprintf(out, "  <seed>%u</seed>\n", seed);
    fprintf(out, "  <shape_metacom>%s</shape_metacom>\n", shape);
    fprintf(out, "  <metacom_size>%d</metacom_size>\n", j_per_c * communities);
    fprintf(out, "  <k_gen>%d</k_gen>\n", k_gen);
    fprintf(out, "  <num_comm>%d</num_comm>\n", communities);
    fprintf(out, "  <individuals_per_comm>%d</individuals_per_comm>\n", j_per_c);
    fprintf(out, "  <initial_num_species>%d</initial_num_species>\n", init_species);
    fprintf(out, "  <mutation_rate>%.2e</mutation_rate>\n", mu);
    fprintf(out, "  <omega>%.2e</omega>\n", omega);
    if (P.m == MODEL_BDM_SELECTION)
    {
        fprintf(out, "  <selection>%.2e</selection>\n", s);
    }
    if (shape[0] == 'r')
    {
        fprintf(out, "  <radius>%.4f</radius>\n", radius);
    }
    if (shape[0] == 'r' && shape[1] == 'e')
    {
        fprintf(out, "  <width>%.4f</width>\n", width);
    }
    // To select the species and genotypes to pick and replace:
    slnode *s0 = list->head; // species0
    slnode *s1 = list->head; // species1
    int g0 = 0;
    int g1 = 0;
    int v1 = 0; // Vertex of the individual 1

    /////////////////////////////////////////////
    // Groups of 1 000 generations             //
    /////////////////////////////////////////////
    for (int k = 0; k < k_gen; ++k)
    {
        extinction_events[k] = 0;
        speciation_events[k] = 0;

        /////////////////////////////////////////////
        // 1 000 generations                       //
        /////////////////////////////////////////////
        for (int gen = 0; gen < 1000; ++gen)
        {
            const int current_date = (k * 1000) + gen;
            /////////////////////////////////////////////
            // A single generation                     //
            /////////////////////////////////////////////
            for (int t = 0; t < j_per_c; ++t)
            {
                /////////////////////////////////////////////
                // A single time step (for each community) //
                /////////////////////////////////////////////
                for (int c = 0; c < communities; ++c)
                {
                    // Select the species and genotype of the individual to be replaced
                    int position = (int)(gsl_rng_uniform(rng) * j_per_c);
                    s0 = list->head;
                    int index = s0->sp->n[c];
                    while (index <= position)
                    {
                        s0 = s0->next;
                        index += s0->sp->n[c];
                    }
                    position = (int)(gsl_rng_uniform(rng) * s0->sp->n[c]);
                    if (position < s0->sp->genotypes[0][c])
                    {
                        g0 = 0;
                    }
                    else if (position < (s0->sp->genotypes[0][c] + s0->sp->genotypes[1][c]))
                    {
                        g0 = 1;
                    }
                    else
                    {
                        g0 = 2;
                    }
                    // Choose the vertex for the individual
                    const double r_v1 = gsl_rng_uniform(rng);
                    v1 = 0;
                    while (r_v1 > cumul[c][v1])
                    {
                        ++v1;
                    }
                    v1 = g.adj_list[c][v1];
                    // species of the new individual
                    position = (int)(gsl_rng_uniform(rng) * j_per_c);
                    s1 = list->head;
                    index = s1->sp->n[v1];
                    while (index <= position)
                    {
                        s1 = s1->next;
                        index += s1->sp->n[v1];
                    }
                    if (v1 == c) // local remplacement
                    {
                        const double r = gsl_rng_uniform(rng);
                        const int aa = s1->sp->genotypes[0][v1];
                        const int Ab = s1->sp->genotypes[1][v1];
                        const int AB = s1->sp->genotypes[2][v1];

                        // The total fitness of the population 'W':
                        const double w = aa + Ab * (1.0 + s) + AB * (1.0 + s) * (1.0 + s);

                        if (r < aa / w)
                        {
                            g1 = gsl_rng_uniform(rng) < mu ? 1 : 0;
                        }
                        else
                        {
                            if (AB == 0 || r < (aa + Ab * (1.0 + s)) / w)
                            {
                                g1 = gsl_rng_uniform(rng) < mu ? 2 : 1;
                            }
                            else
                            {
                                g1 = 2;
                            }
                        }
                    }
                    else
                    { // Migration event
                        g1 = 0;
                    }
                    // Apply the changes
                    s0->sp->n[c]--;
                    s0->sp->genotypes[g0][c]--;
                    s1->sp->n[c]++;
                    s1->sp->genotypes[g1][c]++;

                    ////////////////////////////////////////////
                    // Check for local extinction             //
                    ////////////////////////////////////////////
                    if (s0->sp->n[c] == 0)
                    {
                        extinction_per_c[c]++;
                    }
                    ////////////////////////////////////////////
                    // Check for speciation                   //
                    ////////////////////////////////////////////
                    else if (s0->sp->genotypes[2][c] > 0 && s0->sp->genotypes[0][c] == 0 && s0->sp->genotypes[1][c] == 0)
                    {
                        species_list_add(list, species_init(communities, current_date, 3)); // Add the new species

                        const int pop = s0->sp->n[c];
                        list->tail->sp->n[c] = pop;
                        list->tail->sp->genotypes[0][c] = pop;
                        s0->sp->n[c] = 0;
                        s0->sp->genotypes[2][c] = 0;

                        // To keep info on patterns of speciation...
                        ivector_add(&pop_size, pop);
                        ++speciation_events[k];
                        ++speciation_per_c[c];
                    }

                } // End 'c'

            } // End 't'

            // Remove extinct species from the list and store the number of extinctions.
            extinction_events[k] += species_list_rmv_extinct2(list, &lifespan, current_date);

        } // End 'g'

        total_species[k] = list->size;

    } // End 'k'

    //////////////////////////////////////////////////
    // PRINT THE FINAL RESULTS                      //
    //////////////////////////////////////////////////
    fprintf(out, "  <global>\n");
    fprintf(out, "    <proper_edges>%d</proper_edges>\n", graph_edges(&g));
    fprintf(out, "    <links_per_c>%.4f</links_per_c>\n", (double)graph_edges(&g) / communities);

    fprintf(out, "    <avr_lifespan>%.4f</avr_lifespan>\n", imean(lifespan.array, lifespan.size));
    fprintf(out, "    <median_lifespan>%.4f</median_lifespan>\n", imedian(lifespan.array, lifespan.size));

    fprintf(out, "    <avr_pop_size_speciation>%.4f</avr_pop_size_speciation>\n", imean(pop_size.array, pop_size.size));
    fprintf(out, "    <median_pop_size_speciation>%.4f</median_pop_size_speciation>\n", imedian(pop_size.array, pop_size.size));

    fprintf(out, "    <speciation_per_k_gen>");
    int i = 0;
    for (; i < k_gen - 1; ++i)
    {
        fprintf(out, "%d ", speciation_events[i]);
    }
    fprintf(out, "%d</speciation_per_k_gen>\n", speciation_events[i]);

    fprintf(out, "    <extinctions_per_k_gen>");
    for (i = 0; i < k_gen - 1; ++i)
    {
        fprintf(out, "%d ", extinction_events[i]);
    }
    fprintf(out, "%d</extinctions_per_k_gen>\n", extinction_events[i]);

    fprintf(out, "    <extant_species_per_k_gen>");
    for (i = 0; i < k_gen - 1; ++i)
    {
        fprintf(out, "%d ", total_species[i]);
    }
    fprintf(out, "%d</extant_species_per_k_gen>\n", total_species[i]);

    // Print global distribution
    fprintf(out, "    <species_distribution>");
    ivector species_distribution;
    ivector_init1(&species_distribution, 128);
    it = list->head;
    while (it != NULL)
    {
        ivector_add(&species_distribution, species_total(it->sp));
        it = it->next;
    }
    ivector_sort_asc(&species_distribution);
    ivector_print(&species_distribution, out);
    fprintf(out, "</species_distribution>\n");

    double *octaves;
    int oct_num = biodiversity_octaves(species_distribution.array, species_distribution.size, &octaves);
    fprintf(out, "    <octaves>");
    for (i = 0; i < oct_num; ++i)
    {
        fprintf(out, "%.2f ", octaves[i]);
    }
    fprintf(out, "%.2f</octaves>\n", octaves[i]);
    fprintf(out, "  </global>\n");

    // Print info on all vertices
    double *restrict ric_per_c = (double*)malloc(communities * sizeof(double));
    for (int c = 0; c < communities; ++c)
    {
        fprintf(out, "  <vertex>\n");
        fprintf(out, "    <id>%d</id>\n", c);
        if (shape[0] == 'r')
        {
            fprintf(out, "    <xcoor>%.4f</xcoor>\n", x[c]);
            fprintf(out, "    <ycoor>%.4f</ycoor>\n", y[c]);
        }
        fprintf(out, "    <degree>%d</degree>\n", g.num_e[c] + 1);
        fprintf(out, "    <speciation_events>%d</speciation_events>\n", speciation_per_c[c]);
        fprintf(out, "    <extinction_events>%d</extinction_events>\n", extinction_per_c[c]);

        int vertex_richess = 0;
        ivector_rmvall(&species_distribution);
        it = list->head;
        while (it != NULL)
        {
            ivector_add(&species_distribution, it->sp->n[c]);
            it = it->next;
        }
        // Sort the species distribution and remove the 0s
        ivector_sort_asc(&species_distribution);
        ivector_trim_small(&species_distribution, 1);

        ric_per_c[c] = (double)species_distribution.size;
        fprintf(out, "    <species_richess>%d</species_richess>\n", species_distribution.size);
        fprintf(out, "    <species_distribution>");
        ivector_print(&species_distribution, out);
        fprintf(out, "</species_distribution>\n");

        // Print octaves
        free(octaves);
        oct_num = biodiversity_octaves(species_distribution.array, species_distribution.size, &octaves);
        fprintf(out, "    <octaves>");
        for (i = 0; i < oct_num - 1; ++i)
        {
            fprintf(out, "%.2f ", octaves[i]);
        }
        fprintf(out, "%.2f</octaves>\n", octaves[i]);
        fprintf(out, "  </vertex>\n");
    }

    fprintf(out, "</simulation>\n");

    // GraphML output:
    sprintf(buffer, "%s%u.graphml", P.ofilename, seed);
    FILE *outgml = fopen(buffer, "w");
    graph_graphml(&g, outgml, seed);

    // Print to SVG files.
    sprintf(buffer, "%s%u.svg", P.ofilename, seed);
    FILE *outsvg = fopen(buffer, "w");
    graph_svg(&g, x, y, 400.0, 20.0, outsvg);

    sprintf(buffer, "%s%u-speciation.svg", P.ofilename, seed);
    FILE *outsvgspe = fopen(buffer, "w");
    double *spe_per_c = (double*)malloc(communities * sizeof(double));
    for (int c = 0; c < communities; ++c)
    {
        spe_per_c[c] = (double)speciation_per_c[c];
    }
    scale_0_1(spe_per_c, communities);
    graph_svg_abun(&g, x, y, 400.0, 20.0, spe_per_c, 2, outsvgspe);

    sprintf(buffer, "%s%u-richness.svg", P.ofilename, seed);
    FILE *outsvgric = fopen(buffer, "w");
    scale_0_1(ric_per_c, communities);
    graph_svg_abun(&g, x, y, 400.0, 20.0, ric_per_c, 1, outsvgric);

    //////////////////////////////////////////////////
    // EPILOGUE...                                  //
    //////////////////////////////////////////////////
    // Close files;
    fclose(out);
    fclose(outgml);
    fclose(outsvg);
    fclose(outsvgspe);
    fclose(outsvgric);
    // Free arrays;
    free(x);
    free(y);
    free(ric_per_c);
    free(spe_per_c);
    free(buffer);
    free(total_species);
    free(octaves);
    free(speciation_per_c);
    free(extinction_per_c);
    free(speciation_events);
    free(extinction_events);
    // Free structs;
    species_list_free(list);
    ivector_free(&species_distribution);
    ivector_free(&lifespan);
    ivector_free(&pop_size);
    graph_free(&g);
    gsl_rng_free(rng);

    return NULL;
}

double **setup_cumulative_list(const graph *g, double omega)
{
    const int num_v = g->num_v;
    double **cumul = (double**)malloc(num_v * sizeof(double*));
    for (int i = 0; i < num_v; ++i)
    {
        cumul[i] = (double*)malloc(g->num_e[i] * sizeof(double));
    }
    for (int i = 0; i < num_v; ++i)
    {
        for (int j = 0; j < g->num_e[i]; ++j)
        {
            cumul[i][j] = g->w_list[i][j];
        }
    }
    for (int i = 0; i < num_v; ++i)
    {
        for (int j = 0; j < g->num_e[i]; ++j)
        {
            if (i != g->adj_list[i][j])
            {
                cumul[i][j] = omega;
            }
            else
            {
                cumul[i][j] = 1.0;
            }
        }
    }
    for (int i = 0; i < num_v; ++i)
    {
        double sum = 0.0;
        const int num_e = g->num_e[i];
        for (int j = 0; j < num_e; ++j)
        {
            sum += cumul[i][j];
        }
        for (int j = 0; j < num_e; ++j)
        {
            cumul[i][j] /= sum;
        }
    }
    for (int i = 0; i < num_v; ++i)
    {
        const int num_e = g->num_e[i];
        for (int j = 1; j < num_e - 1; ++j)
        {
            cumul[i][j] += cumul[i][j - 1];
        }
        cumul[i][num_e - 1] = 1.0;
    }
    /*
    graph_print(g, stdout);
    for (int i = 0; i < num_v; ++i)
    {
        const int num_e = g->num_e[i];
        for (int j = 0; j < num_e; ++j)
        {
            printf("%.4f   ", cumul[i][j]);
        }
        printf("\n");
    }
    */
    return cumul;
}


/*
gsl-sprng.h

Code declaring a new GSL random number type "gsl_rng_sprng20"
which is a thin wrapper over the SPRNG 2.0 parallel random
number generator.

To use, just add the line:
#include "gsl-sprng.h"
immediately after the line:
#include <gsl/gsl_rng.h>
near the start of your code. The new type should now be available.
Make sure you alloc the rng on each processor. If you wish to
set a seed, you should set it to be the same on each processor.

Darren Wilkinson
d.j.wilkinson@ncl.ac.uk
http://www.staff.ncl.ac.uk/d.j.wilkinson/

Last updated: 28/8/2002

*/

#define SIMPLE_SPRNG
#define USE_MPI
#include "sprng.h"

static void sprng_set(void * vstate,unsigned long int s)
{
  init_sprng(DEFAULT_RNG_TYPE,s,SPRNG_DEFAULT);
}

static unsigned long sprng_get(void * vstate)
{
  return( (long) isprng() );
}

static double sprng_get_double(void * vstate)
{
  return( (double) sprng());
}

static const gsl_rng_type sprng_type =
  {"sprng20",        /* name */
   0x7fffffffUL,     /* RAND_MAX */
   0,                /* RAND_MIN */
   0,                /* size of state - not sure about this */
   &sprng_set,          /* initialisation */
   &sprng_get,          /* get integer RN */
   &sprng_get_double};  /* get double RN */

const gsl_rng_type *gsl_rng_sprng20 = &sprng_type;

/* eof */


#pragma once

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

namespace angonoka::cli {
/**
    Pretty-print values.

    This class is mostly used for specializing user-defined
    formatters for the fmt library. We don't want to specialize
    the formatter for the original type because we might not
    always want a human-readable output.

    Instead of specializing fmt::formatter<std::chrono::seconds>
    we specialize fmt::formatter<humanize<std::chrono::seconds>>
    thus not interfering with the original formatter.

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

namespace detail {
    /**
        Enum that tells if a human-readable duration
        printing should continue parsing durations of a
        smaller scale or stop.
    */
    enum DurationParsing : bool { continue_, stop };

    /**
        Helper function to print durations in human-readable form.

        @param article  "a" or "an"
        @param name     Duration (seconds, minutes, etc)

        @return Formatter function
    */
    template <typename T>
    constexpr auto
    humanize_duration(gsl::czstring article, gsl::czstring name)
    {
        return [=](auto total, auto& ctx) {
            const auto dur = std::chrono::round<T>(total);
            if (dur == dur.zero()) return DurationParsing::continue_;
            const auto ticks = dur.count();
            if (ticks == 1) {
                fmt::format_to(
                    ctx.out(),
                    "about {} {}",
                    article,
                    name);
            } else {
                fmt::format_to(ctx.out(), "{} {}s", ticks, name);
            }
            return DurationParsing::stop;
        };
    }
} // namespace detail
} // namespace angonoka::cli

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

    template <typename FormatContext>
    constexpr auto format(const value_type& obj, FormatContext& ctx)
    {
        using angonoka::cli::detail::humanize_duration;
        using namespace std::literals::chrono_literals;
        using namespace std::chrono;
        constexpr auto min_threshold = 5s;
        if (obj.value <= min_threshold)
            return format_to(ctx.out(), "a few seconds");
        [&](auto&&... fns) {
            (fns(obj.value, ctx) || ...);
        }(humanize_duration<months>("a", "month"),
          humanize_duration<days>("a", "day"),
          humanize_duration<hours>("an", "hour"),
          humanize_duration<minutes>("a", "minute"),
          humanize_duration<seconds>("a", "second"));
        return ctx.out();
    }
};
} // namespace fmt


#ifndef _BLAS_UTIL_H_
#define _BLAS_UTIL_H_

#ifdef __BLAS_LEGACY__
#include <math.h>
#include "utils/cblas.h"
//#include <lapacke.h> //! hasn't used
#define _D2_MALLOC_SCALAR(x)       (SCALAR *) malloc( (x) *sizeof(SCALAR))
#define _D2_MALLOC_INT(x)       (int *) malloc( (x) *sizeof(int))
#define _D2_MALLOC_SIZE_T(x)       (size_t *) malloc( (x) *sizeof(size_t))
#define _D2_CALLOC_SCALAR(x)       (SCALAR *) calloc( (x) , sizeof(SCALAR))
#define _D2_CALLOC_INT(x)       (int *) calloc( (x) , sizeof(int))
#define _D2_CALLOC_SIZE_T(x)       (size_t *) calloc( (x) , sizeof(size_t))
#define _D2_FREE(x)         free(x)

#elif defined __APPLE__
#include <Accelerate/Accelerate.h>
#define _D2_MALLOC_SCALAR(x)       (SCALAR *) malloc( (x) *sizeof(SCALAR))
#define _D2_MALLOC_INT(x)       (int *) malloc( (x) *sizeof(int))
#define _D2_MALLOC_SIZE_T(x)       (size_t *) malloc( (x) *sizeof(size_t))
#define _D2_CALLOC_SCALAR(x)       (SCALAR *) calloc( (x) , sizeof(SCALAR))
#define _D2_CALLOC_INT(x)       (int *) calloc( (x) , sizeof(int))
#define _D2_CALLOC_SIZE_T(x)       (size_t *) calloc( (x) , sizeof(size_t))
#define _D2_FREE(x)         free(x)

#elif defined __USE_MKL__
#include <mkl.h>
#define _D2_MALLOC_SCALAR(x)       (SCALAR *) mkl_malloc( (x) *sizeof(SCALAR), 16)
#define _D2_MALLOC_INT(x)       (int *) mkl_malloc( (x) *sizeof(int), 16)
#define _D2_MALLOC_SIZE_T(x)       (size_t *) mkl_malloc( (x) *sizeof(size_t), 16)
#define _D2_CALLOC_SCALAR(x)       (SCALAR *) mkl_calloc( (x) , sizeof(SCALAR), 16)
#define _D2_CALLOC_INT(x)       (int *) mkl_calloc( (x) , sizeof(int), 16)
#define _D2_CALLOC_SIZE_T(x)       (size_t *) mkl_calloc( (x) , sizeof(size_t), 16)
#define _D2_FREE(x)         mkl_free(x)

#endif

#endif /* _BLAS_UTIL_H_ */


// Copyright 2011-2021 GameParadiso, Inc. All Rights Reserved.

#pragma once

#include <map>
#include <unordered_map>
#include <ranges>

#include <glm/vec3.hpp>
#include <glm/vec4.hpp>
#include <glm/mat4x4.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/ext/matrix_transform.hpp>
#include <glm/ext/matrix_clip_space.hpp>
#include <glm/ext/scalar_constants.hpp>

#include <gsl/gsl>

using namespace std::string_literals;


//! @file
//! Various functions used during experiences (dgemmsy)
//!
//! @author
//!    Copyright (C) 2009-2011 BigDFT group
//!    This file is distributed under the terms of the
//!    GNU General Public License, see ~/COPYING file
//!    or http://www.gnu.org/copyleft/gpl.txt .
//!    For the list of contributors, see ~/AUTHORS
//! Eric Bainville, Mar 2010

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <malloc.h>
#include <math.h>
#include <unistd.h>
#include <xmmintrin.h>

// Set to 1 to use BLAS axpy call to accumulate partial results.
#ifndef USE_AXPY
#define USE_AXPY 1
/*#define ACML_BLAS 1*/
#endif

#if USE_AXPY
#if defined(MKL_BLAS) || defined(WIN32)
#include <mkl_cblas.h>
#else
#if defined(ACML_BLAS)
#include <acml.h>
#define cblas_daxpy daxpy
#else
#include <cblas.h>
#endif
#endif
#endif

#include "utils.h"

// System-specific functions

#include <unistd.h>
void * allocPages(size_t sz)
{
  size_t page_size = sysconf(_SC_PAGESIZE);
  size_t extra = sz % page_size;
  if (extra != 0) sz = (sz - extra) + page_size; // round to next page
  void * address = 0;
  int status = posix_memalign(&address,page_size,sz);
  if (status != 0) return 0; // Failed
  return address;
}
void freePages(void * address)
{
  free(address);
}
double * allocDoubles(int n)
{
  void * address = 0;
  int status = posix_memalign(&address,16,n*sizeof(double));
  if (status != 0) return 0; // Failed
  return address;
}
float * allocFloats(int n)
{
  void * address = 0;
  int status = posix_memalign(&address,16,n*sizeof(float));
  if (status != 0) return 0; // Failed
  return address;
}
void freeDoubles(double * x)
{
  free(x);
}
void freeFloats(float * x)
{
  free(x);
}

// Reference implementations

void gemm_block_2x2_ref(const double * a,const double * b,long long int n,double * y,long long int ldy)
{
  int k;
  for (k=0;k<n;k+=2)
  {
    y[0] += a[2*k] * b[4*k];
    y[0] += a[2*k+1] * b[4*k+1];
    y[1] += a[2*k] * b[4*k+2];
    y[1] += a[2*k+1] * b[4*k+3];
    y[ldy] += a[2*k+2] * b[4*k];
    y[ldy] += a[2*k+3] * b[4*k+1];
    y[ldy+1] += a[2*k+2] * b[4*k+2];
    y[ldy+1] += a[2*k+3] * b[4*k+3];
  }
}

void gemm_block_2x4_ref(const double * a,const double * b,long long int n,double * y,long long int ldy)
{
  int k;
  double * work;
  for (k=0;k<n;k+=2)
  {
    work = y;
    work[0] += a[2*k]   * b[4*k];
    work[0] += a[2*k+1] * b[4*k+1];
    work += ldy;
    work[0] += a[2*k]   * b[4*k+2];
    work[0] += a[2*k+1] * b[4*k+3];
    work += ldy;
    work[0] += a[2*k]   * b[4*k+4];
    work[0] += a[2*k+1] * b[4*k+5];
    work += ldy;
    work[0] += a[2*k]   * b[4*k+6];
    work[0] += a[2*k+1] * b[4*k+7];

    work = y + 1;
    work[0] += a[2*k+2] * b[4*k];
    work[0] += a[2*k+3] * b[4*k+1];
    work += ldy;
    work[0] += a[2*k+2] * b[4*k+2];
    work[0] += a[2*k+3] * b[4*k+3];
    work += ldy;
    work[0] += a[2*k+2] * b[4*k+4];
    work[0] += a[2*k+3] * b[4*k+5];
    work += ldy;
    work[0] += a[2*k+2] * b[4*k+6];
    work[0] += a[2*k+3] * b[4*k+7];
  }
}

int dgemmsy_base(dgemmsyBaseArgs * args)
{
  int status = 0; // return value
  int row,col;
  size_t slice_size,ywork_size;
  int p2; // P rounded to next multiple of 4
  double *a_slice,*b_slice,*y_work;
  ComputeData cdata;
  TransposeData tdata;
  int slice_n,n,p;
  const double *a,*b;
  double *y;
  int lda,ldb,ldy;
  int transa,transb;
  BlockPattern_2x4_Proc pattern;
  void * pattern_arg;
  double alpha;

  if (args == 0) return -2;

  pattern = args->params.pattern;
  pattern_arg = args->params.pattern_arg;
  slice_n = args->params.slice_n;

  transa = args->transa;
  transb = args->transb;
  n = args->n;
  p = args->p;
  a = args->a;
  lda = args->lda;
  b = args->b;
  ldb = args->ldb;
  y = args->y;
  ldy = args->ldy;
  alpha = args->alpha;

  // Check dimensions
  if (slice_n < 8) return -1;
  if ( n <= 0 || p <= 0 ) return -1;
  p2 = (p+3) & 0x7FFFFFFC;

  // printf("N=%d P=%d LDA=%d LDB=%d LDY=%d  P2=%d\n",n,p,lda,ldb,ldy,p2);

  // Check other arguments
  if (a == 0 || b == 0 || y == 0 || pattern == 0) return -5;

  // Allocate memory
  slice_size = slice_n * p2 * sizeof(double); // Slice size in bytes
  ywork_size = p2 * p2 * sizeof(double); // Result size in bytes
  a_slice = (double *)allocPages(slice_size);
  b_slice = (double *)allocPages(slice_size);
  y_work = (double *)allocPages(ywork_size);
  if (a_slice == 0 || b_slice == 0 || y_work == 0) { status = -3; goto END; }
  memset(y_work,0,ywork_size);

  // Loop on all slices and accumulate products
  initComputeData(&cdata,p2,slice_n,a_slice,b_slice,y_work);
  for (row=0;row<n;row+=slice_n)
  {
    int s = slice_n;
    if (row+s > n) s = n-row; // Limit size of last slice if needed

    // 2-pack A slice
    if (transa) tpack_2(s,p,a+lda*row,lda, slice_n,p2,a_slice);
    else npack_2(s,p,a+row,lda, slice_n,p2,a_slice);

    // 4-pack B slice
    if (transb) tpack_4(s,p,b+ldb*row,ldb, slice_n,p2,b_slice);
    else npack_4(s,p,b+row,ldb, slice_n,p2,b_slice);

    pattern(pattern_arg,p2,Compute_visitor,&cdata);
  }
  cleanupComputeData(&cdata);

  // Complete result by symmetry
  initTransposeData(&tdata,p2,y_work);
  pattern(pattern_arg,p2,Transpose_visitor,&tdata);
  cleanupTransposeData(&tdata);

  // Combine and store (untransposed) result. If we are multithreading,
  // we must protect the update with a mutex, since all threads
  // will update the same Y.
  if (args->yMutex != 0)
    {
      int locked = pthread_mutex_lock(args->yMutex);
      if (locked != 0) status = -4;
    }
  for (col=0;col<p;col++)
    {
      double * yy = y+ldy*col;
      double * yy_work = y_work+p2*col;
#if USE_AXPY
      cblas_daxpy(p,alpha,yy_work,1,yy,1);
#else
      if (alpha == 1)
	{
	  for (row=0;row<p;row++) yy[row] += yy_work[row];
	}
      else
	{
	  for (row=0;row<p;row++) yy[row] += alpha * yy_work[row];
	}
#endif
    }
  if (args->yMutex != 0)
    {
      int unlocked = pthread_mutex_unlock(args->yMutex);
      if (unlocked != 0) status = -4;
    }

END:
  // Cleanup
  freePages(y_work);
  freePages(a_slice);
  freePages(b_slice);
  return status;
}

void npack_2(int n,int p,const double * a,int lda,int s,int q,double * slice)
{
  int row,col,icol;
  double * x = 0;
  int oddn = 0;
  if (n&1) { n--; oddn=1; }
  for (col=0;col<q;col++)
    {
      icol = col&1;
      x = slice + s*(col-icol) + (icol<<1);
      row = 0;
      if (col < p)
	{
	  for ( ;row<n;row+=2)
	    {
	      x[2*row] = a[row];
	      x[2*row+1] = a[row+1];
	    }
	  if (oddn) { x[2*row] = a[row]; x[2*row+1] = 0; row+=2; }
	}
      for ( ;row<s;row+=2)
	{
	  x[2*row] = 0;
	  x[2*row+1] = 0;
	}
      a += lda;
    }
}

void npack_4(int n,int p,const double * a,int lda,int s,int q,double * slice)
{
  int row,col,icol;
  double * x = 0;
  int oddn = 0;
  if (n&1) { n--; oddn=1; }
  for (col=0;col<q;col++)
    {
      icol = col&3;
      x = slice + s*(col-icol) + (icol<<1);
      row = 0;
      if (col < p)
	{
	  for ( ;row<n;row+=2)
	    {
	      x[4*row] = a[row];
	      x[4*row+1] = a[row+1];
	    }
	  if (oddn) { x[4*row] = a[row]; x[4*row+1] = 0; row+=2; }
	}
      for ( ;row<s;row+=2)
	{
	  x[4*row] = 0;
	  x[4*row+1] = 0;
	}
      a += lda;
    }
}

void tpack_2(int n,int p,const double * a,int lda,int s,int q,double * slice)
{
  int row,col,icol;
  double * x = 0;
  const double * aa;
  int da = lda<<1;
  int oddn = 0;
  if (n&1) { n--; oddn=1; }
  for (col=0;col<q;col++)
    {
      icol = col&1;
      x = slice + s*(col-icol) + (icol<<1);
      row = 0;
      if (col < p)
	{
	  aa = a;
	  for ( ;row<n;row+=2)
	    {
	      x[2*row] = aa[0];
	      x[2*row+1] = aa[lda];
	      aa += da;
	    }
	  if (oddn) { x[2*row] = aa[0]; x[2*row+1] = 0; row += 2; }
	}
      for ( ;row<s;row+=2)
	{
	  x[2*row] = 0;
	  x[2*row+1] = 0;
	}
      a++;
    }
}

void tpack_4(int n,int p,const double * a,int lda,int s,int q,double * slice)
{
  int row,col,icol;
  double * x = 0;
  const double * aa;
  int da = lda<<1;
  int oddn = 0;
  if (n&1) { n--; oddn=1; }
  for (col=0;col<q;col++)
    {
      icol = col&3;
      x = slice + s*(col-icol) + (icol<<1);
      row = 0;
      if (col < p)
	{
	  aa = a;
	  for ( ;row<n;row+=2)
	    {
	      x[4*row] = aa[0];
	      x[4*row+1] = aa[lda];
	      aa += da;
	    }
	  if (oddn) { x[4*row] = aa[0]; x[4*row+1] = 0; row += 2; }
	}
      for ( ;row<s;row+=2)
	{
	  x[4*row] = 0;
	  x[4*row+1] = 0;
	}
      a++;
    }
}


// MIT License
//
// Copyright (c) 2021 Daniel Robertson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef HX711_DISCOVERY_H_82654AF1_3AA3_4885_9BE7_FA38117DE328
#define HX711_DISCOVERY_H_82654AF1_3AA3_4885_9BE7_FA38117DE328

#include "HX711.h"
#include <chrono>
#include <vector>
#include <pthread.h>
#include <sched.h>
#include <thread>
#include <algorithm>
#include <cmath>
#include <gsl/gsl_statistics.h>

namespace HX711 {

using namespace std::chrono;

struct TimingResult {
public:
    Value v;
    high_resolution_clock::time_point start;
    high_resolution_clock::time_point waitStart;
    high_resolution_clock::time_point waitEnd;
    high_resolution_clock::time_point convertStart;
    high_resolution_clock::time_point convertEnd;
    high_resolution_clock::time_point end;

    std::chrono::microseconds getWaitTime() const noexcept {
        return std::chrono::duration_cast<std::chrono::microseconds>(
            this->waitEnd - this->waitStart);
    }

    std::chrono::microseconds getConversionTime() const noexcept {
        return std::chrono::duration_cast<std::chrono::microseconds>(
            this->convertEnd - this->convertStart);
    }

    std::chrono::microseconds getTotalTime() const noexcept {
        return std::chrono::duration_cast<std::chrono::microseconds>(
            this->end - this->start);
    }

};

class TimingCollection : public std::vector<TimingResult> {
public:
    struct Stats {
        double min;
        double max;
        double med;
        double std;

        bool inRange(const double n) const noexcept {
            const double minDev = this->med - this->std;
            const double maxDev = this->med + this->std;
            return n >= minDev && n <= maxDev;
        }

    };

protected:

    Stats _statsFromVector(std::vector<double>& vec) const noexcept {

        Stats s;

        std::sort(vec.begin(), vec.end());

        s.min = gsl_stats_min(vec.data(), 1, vec.size());
        s.max = gsl_stats_max(vec.data(), 1, vec.size());
        s.med = gsl_stats_median_from_sorted_data(vec.data(), 1, vec.size());

        //double work[vec.size()];
        s.std = gsl_stats_sd(vec.data(), 1, vec.size());

        return s;

    }

public:

    Stats getWaitTimeStats() const noexcept {

        std::vector<double> vec;
        vec.reserve(this->size());

        for(auto it = this->cbegin(); it != this->cend(); ++it) {
            vec.push_back(static_cast<double>(it->getWaitTime().count()));
        }

        return _statsFromVector(vec);

    }

    Stats getConversionTimeStats() const noexcept {

        std::vector<double> vec;
        vec.reserve(this->size());

        for(auto it = this->cbegin(); it != this->cend(); ++it) {
            vec.push_back(static_cast<double>(it->getConversionTime().count()));
        }

        return _statsFromVector(vec);

    }

    Stats getTotalTimeStats() const noexcept {

        std::vector<double> vec;
        vec.reserve(this->size());

        for(auto it = this->cbegin(); it != this->cend(); ++it) {
            vec.push_back(static_cast<double>(it->getTotalTime().count()));
        }

        return _statsFromVector(vec);

    }

};

class Discovery : public HX711 {
public:

    Discovery(const int dataPin, const int clockPin, const Rate rate) :
        HX711(dataPin, clockPin, rate) {
            this->connect();
    }

    TimingCollection getTimings(const std::size_t samples) {

        using namespace std::chrono;

        TimingCollection vec;
        vec.reserve(samples);

        for(size_t i = 0; i < samples; ++i) {

            TimingResult tr;

            tr.start = high_resolution_clock::now();

            tr.waitStart = high_resolution_clock::now();
            while(!this->isReady()) ;
            tr.waitEnd = high_resolution_clock::now();

            tr.convertStart = high_resolution_clock::now();
            tr.v = this->readValue();
            tr.convertEnd = high_resolution_clock::now();

            tr.end = high_resolution_clock::now();

            vec.push_back(tr);

        }

        return vec;

    }

    std::vector<std::chrono::nanoseconds> getTimeToReady(const std::size_t samples) {

        using namespace std::chrono;

        std::vector<nanoseconds> timings;
        timings.reserve(samples);

        //do an initial read
        while(!this->isReady()) ;
        this->readValue();

        for(std::size_t i = 0; i < samples; ++i) {

            high_resolution_clock::time_point start = high_resolution_clock::now();
            while(!this->isReady()) ;
            high_resolution_clock::time_point end = high_resolution_clock::now();

            timings.push_back(end - start);

            this->readValue();

        }

        return timings;

    }

};
};
#endif


#ifndef PITCH_H
#define PITCH_H

#include <cstring>

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>

#define DECL_PITCH(name)  bool pitch_ ## name (gsl_vector *frame, double *f0res, double *T0res)

DECL_PITCH(AMDF);

#endif // PITCH_H


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_DETAIL_CBLAS_H
#define BOOST_NUMERIC_BINDINGS_BLAS_DETAIL_CBLAS_H

#ifdef HAS_OpenBLAS
#define OPENBLAS_CONST_FLOAT_CAST(x) reinterpret_cast<const float*>(x)
#define OPENBLAS_CONST_DOUBLE_CAST(x) reinterpret_cast<const double*>(x)
#define OPENBLAS_FLOAT_CAST(x) reinterpret_cast<float*>(x)
#define OPENBLAS_DOUBLE_CAST(x) reinterpret_cast<double*>(x)
#define OPENBLAS_OPENBLAS_COMPLEX_FLOAT_CAST(x) reinterpret_cast<openblas_complex_float*>(x)
#define OPENBLAS_OPENBLAS_COMPLEX_DOUBLE_CAST(x) reinterpret_cast<openblas_complex_double*>(x)

#else
#define OPENBLAS_CONST_FLOAT_CAST(x) x
#define OPENBLAS_CONST_DOUBLE_CAST(x) x
#define OPENBLAS_FLOAT_CAST(x) x
#define OPENBLAS_DOUBLE_CAST(x) x
#define OPENBLAS_OPENBLAS_COMPLEX_FLOAT_CAST(x) x
#define OPENBLAS_OPENBLAS_COMPLEX_DOUBLE_CAST(x) x
#endif

//
// MKL-specific CBLAS include
//
#if defined BOOST_NUMERIC_BINDINGS_BLAS_MKL

extern "C" {
#include <mkl_cblas.h>
//#include <mkl_service.h>
//
// mkl_types.h defines P4 macro which breaks MPL, undefine it here.
//
#undef P4
}

//
// Default CBLAS include
//
#else

extern "C" {
#include <cblas.h>
}

#endif
#endif


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <mpi.h>
#include <petsc.h>
#include <petscvec.h>
#include <petscmat.h>
#include <petscksp.h>
#include <petscpc.h>
#include <petscsnes.h>

#include <petscversion.h>
#if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR >=3) )
  #if (PETSC_VERSION_MINOR >=6)
     #include "petsc/private/kspimpl.h"
  #else
     #include "petsc-private/kspimpl.h"   /*I "petscksp.h" I*/
  #endif
#else
  #include "private/kspimpl.h"   /*I "petscksp.h" I*/
#endif

#include <StGermain/StGermain.h>
#include <StgDomain/StgDomain.h>
#include <StgFEM/StgFEM.h>
#include <PICellerator/PICellerator.h>
#include <Underworld/Underworld.h>
#include <Solvers/Solvers.h>

#include "petsccompat.h"

#include "TestKSP.h"

#define KSPTEST         "test"

EXTERN_C_BEGIN
EXTERN PetscErrorCode PETSCKSP_DLLEXPORT KSPCreate_TEST(KSP);
EXTERN_C_END

#undef __FUNCT__
#define __FUNCT__ "KSPRegisterTEST"
PetscErrorCode PETSCKSP_DLLEXPORT KSPRegisterTEST(const char path[])
{
    PetscErrorCode ierr;

    PetscFunctionBegin;
    ierr = Stg_KSPRegister(KSPTEST, path, "KSPCreate_TEST", KSPCreate_TEST );CHKERRQ(ierr);
    PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "KSPSolve_TEST"
PetscErrorCode  KSPSolve_TEST(KSP ksp)
{
    PetscPrintf( PETSC_COMM_WORLD, "----- In KSP Solver %s\n",__func__);
    PetscFunctionReturn(0);

}

#undef __FUNCT__
#define __FUNCT__ "KSPDestroy_TEST"
PetscErrorCode KSPDestroy_TEST(KSP ksp)
{
    PetscErrorCode ierr;
    PetscFunctionBegin;
    ierr = KSPDefaultDestroy(ksp);CHKERRQ(ierr);
    PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "KSPSetFromOptions_TEST"
PetscErrorCode KSPSetFromOptions_TEST(KSP ksp)
{

    PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "KSPView_TEST"
PetscErrorCode KSPView_TEST(KSP ksp,PetscViewer viewer)
{
    PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "KSPSetUp_TEST"
PetscErrorCode KSPSetUp_TEST(KSP ksp)
{
    PetscFunctionReturn(0);
}

EXTERN_C_BEGIN
#undef __FUNCT__
#define __FUNCT__ "KSPCreate_TEST"
PetscErrorCode PETSCKSP_DLLEXPORT KSPCreate_TEST(KSP ksp)
{
    PetscErrorCode ierr;
    PetscFunctionBegin;

    #if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR >= 2 ) )
    ierr = KSPSetSupportedNorm(ksp,KSP_NORM_NONE,PC_LEFT,1);CHKERRQ(ierr);
    #endif
    /*
       Sets the functions that are associated with this data structure
       (in C++ this is the same as defining virtual functions)
    */
    ksp->ops->setup                = KSPSetUp_TEST;
    ksp->ops->solve                = KSPSolve_TEST;
    ksp->ops->destroy              = KSPDestroy_TEST;
    ksp->ops->view                 = KSPView_TEST;
    ksp->ops->setfromoptions       = KSPSetFromOptions_TEST;
    ksp->ops->buildsolution        = KSPDefaultBuildSolution;
    ksp->ops->buildresidual        = KSPDefaultBuildResidual;

    PetscFunctionReturn(0);
}
EXTERN_C_END


/*
 * Copyright 2014 Marc Normandin
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 *     Unless required by applicable law or agreed to in writing, software
 *     distributed under the License is distributed on an "AS IS" BASIS,
 *     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *     See the License for the specific language governing permissions and
 *     limitations under the License.
 */
#ifndef INC_PARTICLE_H
#define INC_PARTICLE_H

#include <iostream>
#include <vector>
#include <algorithm>
#include <functional>
#include <cmath>
#include <cstdlib>
#include <gsl/gsl_rng.h>
#include <limits>
#include <fstream>
#include <cassert>

#include "rng.h"
#include "dim.h"

class Particle
{
public:
    typedef std::vector<dim_t> dvector;

    Particle();

    Particle(const std::vector<Dim>& dim);

    Particle(const dvector& pos, const prob_t fitness = -1.0 * std::numeric_limits<dim_t>::max());

    const dvector& getPosition() const;

    dvector getVelocity() const;

    double getFitness() const;

    dvector getPBestPosition() const;

    double getPBestFitness() const;

    void updateFitness(const prob_t newFitness);

    void updatePosition(const Particle& GBest, const std::vector<Dim>& dim, const double C1, const double C2,
                        const RandomNumberGenerator& rng, const double inertiaWeight);

    size_t size() const;

    friend bool operator<(const Particle& p1, const Particle& p2);

private:
    dvector     mPos;
    dvector     mVel;
    prob_t      mFitness;

    // The particles best position
    dvector		mPBestPos;
    prob_t 		mPBestFitness;
};

bool operator<(const Particle& p1, const Particle& p2);

#endif // #ifndef INC_PARTICLE_H


#include <glib.h>
#include <gsl/gsl_rng.h>
#include <stdlib.h>
#include "util.h"

hash_t *hash_create () {
  return (char*) g_hash_table_new(g_direct_hash, g_direct_equal);
}

void hash_set (hash_t *table, size_t key, size_t value) {
  g_hash_table_replace((GHashTable*)table, (gpointer)key, (gpointer)value);
}

size_t hash_get (hash_t *table, size_t key, size_t value) {
  size_t orig_key;
  size_t result;
  if (g_hash_table_lookup_extended((GHashTable*)table, (gpointer)key, (gpointer*)&orig_key, (gpointer*)&result)) {
    return result;
  }
  return value;
}

void hash_clear (hash_t *table) {
  return g_hash_table_remove_all((GHashTable*)table);
}

void hash_destroy (hash_t *table) {
  return g_hash_table_destroy((GHashTable*)table);
}

gsl_rng *r = 0;

size_t random_range (size_t lo, size_t hi) {
  if (!r) {
    gsl_rng_env_setup();
    const gsl_rng_type *T = gsl_rng_default;
    r = gsl_rng_alloc (T);
  }
  size_t n = hi - lo;
  return lo + gsl_rng_uniform_int(r, n);//todo this range might not be big enough/type issues may occur here.
}

double random_uniform () {
  if (!r) {
    gsl_rng_env_setup();
    const gsl_rng_type *T = gsl_rng_default;
    r = gsl_rng_alloc (T);
  }
  return gsl_rng_uniform(r);
}

void random_choose (size_t *samples, size_t n, size_t lo, size_t hi) {
  hash_t *table = hash_create();
  for (size_t i = 0; i < n; i++) {
    size_t j = random_range(lo, hi);
    samples[i] = hash_get(table, j, j);
    hash_set(table, j, hash_get(table, lo, lo));
    lo++;
  }
  hash_destroy(table);
}

int size_t_cmp(const void *a, const void *b)
{
    const size_t *ia = (const size_t *)a;
    const size_t *ib = (const size_t *)b;
    return *ia  - *ib;
}

void quicksort(size_t *stuff, size_t n)
{
  if (n < 2) return;

  size_t pivot = stuff[n / 2];

  size_t i, j;
  for (i = 0, j = n - 1; ; i++, j--)
  {
    while (stuff[i] < pivot) i++;
    while (stuff[j] > pivot) j--;

    if (i >= j) break;

    size_t temp = stuff[i];
    stuff[i]     = stuff[j];
    stuff[j]     = temp;
  }

  quicksort(stuff, i);
  quicksort(stuff + i, n - i);
}

void sort (size_t *stuff, size_t n) {
  //quicksort(stuff, n);
  qsort(stuff, n, sizeof(size_t), size_t_cmp);
}

size_t search (const size_t *stuff, size_t lo, size_t hi, size_t key) {
  size_t len = hi - lo;
  size_t half;
  size_t mid;
  while (len > 0) {
    half = len >> 1;
    mid = lo + half;
    if (stuff[mid] >= key) {
      len = half;
    } else {
      lo = mid;
      lo++;
      len = len - half - 1;
    }
  }
  return lo;
}

size_t search_strict (const size_t *stuff, size_t lo, size_t hi, size_t key) {
  size_t len = hi - lo;
  size_t half;
  size_t mid;
  while (len > 0) {
    half = len >> 1;
    mid = lo + half;
    if (stuff[mid] > key) {
      len = half;
    } else {
      lo = mid;
      lo++;
      len = len - half - 1;
    }
  }
  return lo;
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//
// 			     //////////////////////////////////////////////////////////
//  			     //							     //
// 			     //   	        hybridMANTIS v1.0		     //
// 			     //                fastDETECT2 - C code                  //
//			     //		   (optical photons transport)		     //
//  			     //							     //
//                           //               used for Load Balancing                //
//			     //							     //
//			     //////////////////////////////////////////////////////////
//
//
//
//
// ****Disclaimer****
//  This software and documentation (the "Software") were developed at the Food and Drug Administration (FDA) by employees of the Federal Government in
//  the course of their official duties. Pursuant to Title 17, Section 105 of the United States Code, this work is not subject to copyright protection
//  and is in the public domain. Permission is hereby granted, free of charge, to any person obtaining a copy of the Software, to deal in the Software
//  without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, or sell copies of the
//  Software or derivatives, and to permit persons to whom the Software is furnished to do so. FDA assumes no responsibility whatsoever for use by other
//  parties of the Software, its source code, documentation or compiled executables, and makes no guarantees, expressed or implied, about its quality,
//  reliability, or any other characteristic. Further, use of this code in no way implies endorsement by the FDA or confers any advantage in regulatory
//  decisions. Although this software can be redistributed and/or modified freely, we ask that any derivative works bear some notice that they are
//  derived from it, and any modified versions bear some notice that they have been modified.
//
//	Detailed comments are available in "hybridMANTIS_cuda_ver1_0.cu" and "hybridMANTIS_c_ver1_0.c" files.
//
//	Associated publication: Sharma Diksha, Badal Andreu and Badano Aldo, "hybridMANTIS: a CPU-GPU Monte Carlo method for modeling indirect x-ray detectors with
//				columnar scintillators". Physics in Medicine and Biology, 57(8), pp. 2357–2372 (2012)
//
//
//	File:   	hybridMANTIS_c_ver1_0_LB.c
//	Author: 	Diksha Sharma (US Food and Drug Administration)
//	Email: 		diksha.sharma@fda.hhs.gov
//	Last updated:  	Apr 13, 2012
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////
//
//      Header libraries
//
/////////////////////////////////////////

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

/////////////////////////////////////////
//
//      Global variables
//
/////////////////////////////////////////

#define max_photon_per_EDE 900000	// maximum number of optical photons that can be generated per energy deposition event (EDE)

#ifndef USING_CUDA
	#define mybufsizeT 2304000	// CPU buffer size: # of events sent to the CPU
#endif

/////////////////////////////////////////
//
//      Include kernel program
//
/////////////////////////////////////////
#include "kernel_cuda_c_ver1_0_LB.cu"

////////////////////////////////////////////////////////////////////////////
//				MAIN PROGRAM			          //
////////////////////////////////////////////////////////////////////////////

#ifndef USING_CUDA

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// cpuoptlb():   Performs optical transport for finding optimal load using load balancing in the CPU
//	  	 Input arguments: cptime, cpubufsize
//
//		 cptime: 	time taken by GPU to call this routine
//		 cpubufsize:  	CPU buffer size defined by user in PENELOPE tally code. Only to be used for load balancing. This is different from 'mybufsize'.
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	void cpuoptlb_(double *cptime, int *cpubufsize)
	{

		float dcos[3]={0}; 		// directional cosines
		float normal[3]={0}; 		// normal to surface in case of TIR
		float pos[3] = {0}; 		// new position
		float old_pos[3] = {0};  	// source coordinates

		// command line arguments
		float xdetector, ydetector, radius, height, n_C, n_IC, top_absfrac, bulk_abscoeff, beta, d_min, lbound_x, lbound_y, ubound_x, ubound_y, d_max, yield, sensorRefl;
		int pixelsize, num_primary, min_optphotons, max_optphotons, num_bins;

		int nbytes = (*cpubufsize)*sizeof(struct start_info);
		struct start_info *structa;
		structa = (struct start_info*) malloc(nbytes);
		if( structa == NULL )
			printf("\n Struct start_info array CANNOT BE ALLOCATED - %d !!", (*cpubufsize));

		// get cpu time
		clock_t start, end;
		double num_sec;

		// get current time stamp to initialize seed input for RNG
		time_t seconds;
		seconds = time (NULL);
		struct timeval tv;
		gettimeofday(&tv,NULL);

		float rr=0.0f, theta=0.0f;
		float r=0.0f;		// random number
		float norm=0.0f;
		int jj=0;

		// initialize random number generator (RANECU)
		int seed_input = 271828182 ; // ranecu seed input
		int seed[2];

		// gsl variables
		const gsl_rng_type * Tgsl;
		gsl_rng * rgsl;
		double mu_gsl;
		int my_index=0;
		int result_algo = 0;
		unsigned long long int *num_rebound;

		// output image variables
		int xdim = 0;
		int ydim = 0;
		int indexi=0, indexj=0;

	      	// create a generator chosen by the environment variable GSL_RNG_TYPE
	       	gsl_rng_env_setup();
	       	Tgsl = gsl_rng_default;
	       	rgsl = gsl_rng_alloc (Tgsl);

 		// copy to local variables from PENELOPE buffers
		xdetector = inputargs_.detx;		// x dimension of detector (in um). x in (0,xdetector)
		ydetector = inputargs_.dety;		// y dimension of detector (in um). y in (0,ydetector)
		height = inputargs_.detheight;		// height of column and thickness of detector (in um). z in range (-H/2, H/2)
		radius = inputargs_.detradius;		// radius of column (in um).
		n_C = inputargs_.detnC;			// refractive index of columns
		n_IC = inputargs_.detnIC;		// refractive index of intercolumnar material
		top_absfrac = inputargs_.dettop;	// column's top surface absorption fraction (0.0, 0.5, 0.98)
		bulk_abscoeff = inputargs_.detbulk;	// column's bulk absorption coefficient (in um^-1) (0.001, 0.1 cm^-1)
		beta = inputargs_.detbeta;		// roughness coefficient of column walls
		d_min = inputargs_.detdmin;		// minimum distance a photon can travel when transmitted from a column
		d_max = inputargs_.detdmax;
		lbound_x = inputargs_.detlboundx;	// x lower bound of region of interest of output image (in um)
		lbound_y = inputargs_.detlboundy;	// y lower bound (in um)
		ubound_x = inputargs_.detuboundx;	// x upper bound (in um)
		ubound_y = inputargs_.detuboundy;	// y upper bound (in um)
		yield = inputargs_.detyield;		// yield (/eV)
		pixelsize = inputargs_.detpixel;	// 1 pixel = pixelsize microns (in um)
		sensorRefl = inputargs_.detsensorRefl;	// Non-Ideal sensor reflectivity (%)
		num_primary = inputargs_.mynumhist;	// total number of primaries to be simulated
		min_optphotons = inputargs_.minphotons;	// minimum number of optical photons detected to be included in PHS
		max_optphotons = inputargs_.maxphotons;	// maximum number of optical photons detected to be included in PHS
		num_bins = inputargs_.mynumbins;	// number of bins for genrating PHS

		// dimensions of PRF image
		xdim = ceil((ubound_x - lbound_x)/pixelsize);
		ydim = ceil((ubound_y - lbound_y)/pixelsize);
		unsigned long long int myimage[xdim][ydim];

		// memory for storing histogram of # photons detected/primary
		int *h_num_detected_prim = 0;
		h_num_detected_prim = (int*)malloc(sizeof(int)*num_primary);

		for(indexj=0; indexj < num_primary; indexj++)
		  h_num_detected_prim[indexj] = 0;

		// start the clock
		start = clock();

	for(my_index = 0; my_index < (*cpubufsize); my_index++)		// iterate over x-rays
	{

		// reset the global counters
		num_generatedT = 0;
		num_detectT=0;
		num_abs_topT=0;
		num_abs_bulkT=0;
		num_lostT=0;
		num_outofcolT=0;
		num_theta1T=0;
		photon_distanceT=0.0f;

		// copying fortran buffer into *structa

		// units in the penelope output file are in cm. Convert to microns.
		structa[my_index].str_x = optical_.xbufopt[my_index] * 10000.0f;	// x-coordinate of interaction event.
		structa[my_index].str_y = optical_.ybufopt[my_index] * 10000.0f;	// y-coordinate
		structa[my_index].str_z = optical_.zbufopt[my_index] * 10000.0f;	// z-coordinate
		structa[my_index].str_E = optical_.debufopt[my_index];			// energy deposited
		structa[my_index].str_histnum = optical_.nbufopt[my_index];		// x-ray history number

		// sample # optical photons based on light yield and energy deposited for this interaction event (using Poisson distribution)
		mu_gsl = (double)structa[my_index].str_E * yield;
		structa[my_index].str_N = gsl_ran_poisson(rgsl,mu_gsl);

		if(structa[my_index].str_N > max_photon_per_EDE)
		{
			printf("\n\n CPU str_n exceeds max photons. program is exiting - %d !! \n\n",structa[my_index].str_N);
			exit(0);
		}

		num_rebound = (unsigned long long int*) malloc(structa[my_index].str_N*sizeof(unsigned long long int));
		if(num_rebound == NULL)
			printf("\n Error allocating num_rebound memory !\n");

		// initialize the RANECU generator in a position far away from the previous history:
		seed_input = (int)(seconds/3600+tv.tv_usec);			// seed input=seconds passed since 1970+current time in micro secs
		init_PRNG(my_index, 50000, seed_input, seed);      		// intialize RNG

		for(jj=0; jj<structa[my_index].str_N; jj++)
			num_rebound[jj] = 0;

		// reset the vectors
		dcos[0]=0.0f; dcos[1]=0.0f; dcos[2]=0.0f;
		normal[0]=0.0f; normal[1]=0.0f; normal[2]=0.0f;

		// set starting location of photon
		pos[0] = structa[my_index].str_x; pos[1] = structa[my_index].str_y; pos[2] = structa[my_index].str_z;
		old_pos[0] = structa[my_index].str_x; old_pos[1] = structa[my_index].str_y; old_pos[2] = structa[my_index].str_z;

		// initializing the direction cosines for the first particle in each core
		r = (ranecu(seed) * 2.0f) - 1.0f; // random number between (-1,1)

		while(fabs(r) <= 0.01f)
		 {
		   	r = (ranecu(seed) * 2.0f) - 1.0f;
		 }

		dcos[2] = r;		// random number between (-1,1)
		rr = sqrt(1.0f-r*r);
		theta=ranecu(seed)*twopipen;
		dcos[0]=rr*cos(theta);
		dcos[1]=rr*sin(theta);

		norm = sqrt(dcos[0]*dcos[0] + dcos[1]*dcos[1] + dcos[2]*dcos[2]);

		if ((norm < (1.0f - epsilon)) || (norm > (1.0f + epsilon)))	// normalize
		 {
			dcos[0] = dcos[0]/norm;
			dcos[1] = dcos[1]/norm;
			dcos[2] = dcos[2]/norm;
		 }

		local_counterT=0;
		while(local_counterT < structa[my_index].str_N)
		 {

			absorbedT = 0;
			detectT = 0;
			bulk_absT = 0;

			// set starting location of photon
			pos[0] = structa[my_index].str_x; pos[1] = structa[my_index].str_y; pos[2] = structa[my_index].str_z;
			old_pos[0] = structa[my_index].str_x; old_pos[1] = structa[my_index].str_y; old_pos[2] = structa[my_index].str_z;
			num_generatedT++;
			result_algo = 0;

			while(result_algo == 0)
			 {
			  	result_algo = algoT(normal, old_pos, pos, dcos, num_rebound, seed, structa[my_index], &myimage[0][0], xdetector, ydetector, radius, height, n_C, n_IC, top_absfrac, bulk_abscoeff, beta, d_min, pixelsize, lbound_x, lbound_y, ubound_x, ubound_y, sensorRefl, d_max, ydim, h_num_detected_prim);
			 }

		 }

		// release resources
		free(num_rebound);

	}	// my_index loop ends

	// end the clock
	end = clock();

	num_sec = (double)((end - start)/CLOCKS_PER_SEC);
        *cptime = num_sec;

	// release resources
	free(structa);
	free(h_num_detected_prim);

		return;
	}	// C main() ends

#endif


#include "vector.h"

#include "math.h"
#include "mem.h"
#include "string_utils.h"
#include "ut_utils.h"

typedef struct {
    char *name;
    char age;
} nage_t;

void free_nage_t(void *pnage)
{
    nage_t *nage = pnage;
    ufree(nage->name);
    ufree(nage);
}

void *copy_nage_t(const void *pnage)
{
    const nage_t *nage = pnage;

    nage_t *n = umalloc(sizeof(*n));
    UASSERT(n);

    n->name = ustring_dup(nage->name);
    n->age = nage->age;

    return n;
}

void test_uvector_copy(bool verbose)
{
    (void)verbose;

    uvector_t *v = uvector_create();
    uvector_set_void_destroyer(v, free_nage_t);
    uvector_set_void_copier(v, copy_nage_t);

    nage_t *n = umalloc(sizeof(*n));

    *n = (nage_t){ustring_dup("john"), 34};
    uvector_append(v, G_PTR(n)); // v owns *n data

    uvector_t *v2 = uvector_copy(v); // v2 has shallow copy of *n2
    uvector_t *v3 = uvector_deep_copy(v); // v3 has deep copy of *n2, must set own

    uvector_destroy(v);
    uvector_destroy(v2);
    uvector_destroy(v3);
}

void test_uvector_bsearch(void)
{
    uvector_t *v = uvector_create();
    uvector_append(v, G_INT(1));
    uvector_append(v, G_INT(2));
    uvector_append(v, G_INT(4));
    uvector_append(v, G_INT(3));

    uvector_sort(v); // [1, 2, 3, 4]]
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(1)), 0);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(2)), 1);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(3)), 2);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(4)), 3);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(400)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(-400)), SIZE_MAX);

    uvector_remove_at(v, 1); // [1, 3, 4]
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(1)), 0);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(2)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(3)), 1);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(4)), 2);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(400)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(-400)), SIZE_MAX);

    uvector_pop_back(v); // [1, 3]
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(1)), 0);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(2)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(3)), 1);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(4)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(400)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(-400)), SIZE_MAX);

    uvector_pop_at(v, 0); // [3]
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(1)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(2)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(3)), 0);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(4)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(400)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(-400)), SIZE_MAX);

    uvector_pop_at(v, 0);
    UASSERT_INT_EQ(uvector_get_size(v), 0);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(1)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(2)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(3)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(4)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(400)), SIZE_MAX);
    UASSERT_INT_EQ(uvector_bsearch(v, G_INT(-400)), SIZE_MAX);

    uvector_destroy(v);
}

void test_uvector_next_permutation(bool verbose)
{
    uvector_t *v = uvector_create();
    uvector_append(v, G_INT(4));
    uvector_append(v, G_INT(3));
    uvector_append(v, G_INT(1));
    uvector_append(v, G_INT(2));

    do {
        if (verbose)
            uvector_print(v);
    } while (uvector_next_permutation(v));

    uvector_destroy(v);
}

void test_uvector_serialization(bool verbose)
{
    char *vs;
    uvector_t *v = uvector_create();

    vs = uvector_as_str(v);
    UASSERT_STR_EQ(vs, "[]");
    ufree(vs);

    uvector_append(v, G_INT(11));
    uvector_append(v, G_INT(22));
    uvector_append(v, G_INT(33));
    uvector_append(v, G_STR(ustring_dup("44")));
    uvector_append(v, G_CSTR("5"));

    uvector_append(v, G_VECTOR(uvector_deep_copy(v)));
    uvector_append(v, G_VECTOR(uvector_deep_copy(v)));

    if (verbose)
        uvector_print(v);
    vs = uvector_as_str(v);
    UASSERT_STR_EQ(vs,
                  "[11, 22, 33, \"44\", \"5\", [11, 22, 33, \"44\", \"5\"], [11, 22, 33, \"44\", \"5\", [11, 22, 33, \"44\", \"5\"]]]");
    ufree(vs);
    uvector_destroy(v);
}

void test_uvector_api()
{
    uvector_t *v = uvector_create();
    uvector_append(v, G_INT(0));
    UASSERT(uvector_get_capacity(v) >= VECTOR_INITIAL_CAPACITY);
    uvector_reserve_capacity(v, 1000);
    UASSERT_INT_EQ(uvector_get_capacity(v), 1000);
    uvector_destroy(v);

    v = uvector_create_with_size(3, G_NULL());
    UASSERT_INT_EQ(uvector_get_size(v), 3);

    uvector_append(v, G_INT(11));
    uvector_append(v, G_INT(22));
    uvector_append(v, G_INT(33));
    UASSERT_INT_EQ(uvector_get_size(v), 6);
    UASSERT(G_IS_NULL(uvector_get_at(v, 0)));
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 3)), 11);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 4)), 22);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 5)), 33);

    uvector_swap(v, 4, 5);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 4)), 33);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 5)), 22);

    uvector_resize(v, 0, G_NULL());
    UASSERT(uvector_is_empty(v));

    uvector_append(v, G_INT(44));
    UASSERT_INT_EQ(uvector_get_size(v), 1);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 0)), 44);

    uvector_set_at(v, 0, G_INT(2222));
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 0)), 2222);

    ugeneric_t *a = uvector_get_cells(v);
    UASSERT_INT_EQ(G_AS_INT(a[0]), 2222);
    uvector_destroy(v);

    v = uvector_create();
    UASSERT_INT_EQ(uvector_get_size(v), 0);
    UASSERT(uvector_is_empty(v));
    uvector_append(v, G_INT(11));
    UASSERT(!uvector_is_empty(v));
    UASSERT_INT_EQ(uvector_get_size(v), 1);
    UASSERT_INT_EQ(G_AS_INT(uvector_pop_back(v)), 11);
    UASSERT(uvector_is_empty(v));
    UASSERT_INT_EQ(uvector_get_size(v), 0);

    uvector_resize(v, 0, G_NULL());
    uvector_append(v, G_INT(200));
    uvector_append(v, G_INT(100));
    UASSERT_INT_EQ(uvector_get_size(v), 2);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 0)), 200);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 1)), 100);
    uvector_insert_at(v, 1, G_INT(300));
    UASSERT_INT_EQ(uvector_get_size(v), 3);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 0)), 200);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 1)), 300);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 2)), 100);
    UASSERT_INT_EQ(G_AS_INT(uvector_pop_at(v, 0)), 200);
    UASSERT_INT_EQ(uvector_get_size(v), 2);
    UASSERT_INT_EQ(G_AS_INT(uvector_pop_at(v, 0)), 300);
    UASSERT_INT_EQ(uvector_get_size(v), 1);
    UASSERT_INT_EQ(G_AS_INT(uvector_pop_at(v, 0)), 100);
    UASSERT_INT_EQ(uvector_get_size(v), 0);
    uvector_resize(v, 1000, G_NULL());
    UASSERT_INT_EQ(uvector_get_size(v), 1000);
    uvector_destroy(v);

    v = uvector_create();
    uvector_append(v, G_STR(ustring_dup("1")));
    uvector_append(v, G_STR(ustring_dup("2")));
    uvector_append(v, G_STR(ustring_dup("3")));
    uvector_append(v, G_STR(ustring_dup("4")));
    uvector_resize(v, 2, G_NULL());
    uvector_resize(v, 1, G_NULL());
    uvector_clear(v);
    uvector_append(v, G_STR(ustring_dup("5")));
    UASSERT_INT_EQ(uvector_get_size(v), 1);
    uvector_destroy(v);

    v = uvector_create();
    uvector_append(v, G_STR(ustring_dup("string")));
    uvector_set_at(v, 0, G_STR(ustring_dup("string2")));
    uvector_destroy(v);

    v = uvector_create();
    uvector_append(v, G_INT(123));
    UASSERT_INT_EQ(G_AS_INT(uvector_get_at(v, 0)), 123);
    UASSERT_INT_EQ(G_AS_INT(uvector_get_back(v)), 123);
    uvector_t *vcopy = uvector_copy(v);
    UASSERT(uvector_compare(v, vcopy) == 0);
    uvector_destroy(v);
    uvector_destroy(vcopy);

    v = uvector_create();
    uvector_reserve_capacity(v, 1024);
    uvector_shrink_to_size(v);
    uvector_destroy(v);

    v = uvector_create();
    uvector_append(v, G_STR(ustring_dup("1")));
    uvector_append(v, G_INT(42));
    uvector_append(v, G_BOOL(false));
    UASSERT(uvector_contains(v, G_STR("1")));
    UASSERT(uvector_contains(v, G_CSTR("1")));
    UASSERT(uvector_contains(v, G_BOOL(false)));
    UASSERT(uvector_contains(v, G_INT(42)));
    UASSERT(!uvector_contains(v, G_STR("2")));
    UASSERT(!uvector_contains(v, G_BOOL(true)));
    UASSERT(!uvector_contains(v, G_INT(100)));
    uvector_destroy(v);

#if defined(__unix__) && defined(ENABLE_UASSERT_INPUT)
    // Check asserts.
    UASSERT_ABORTS(uvector_append(NULL, G_INT(0)));
    UASSERT_ABORTS(uvector_pop_back(NULL));
    UASSERT_ABORTS(uvector_get_cells(NULL));
    UASSERT_ABORTS(uvector_get_size(NULL));
    UASSERT_ABORTS(uvector_resize(NULL, 100, G_NULL()));
    UASSERT_ABORTS(uvector_reserve_capacity(NULL, 100));
    UASSERT_ABORTS(uvector_get_capacity(NULL));
    UASSERT_ABORTS(uvector_is_empty(NULL));
    UASSERT_ABORTS(uvector_swap(NULL, 1, 2));
    UASSERT_ABORTS(uvector_insert_at(NULL, 1, G_INT(100)));
    UASSERT_ABORTS(uvector_get_at(NULL, 1));
    UASSERT_ABORTS(uvector_set_at(NULL, 1, G_INT(0)));
    uvector_destroy(NULL);

    v = uvector_create_with_size(0, G_NULL());
    UASSERT_ABORTS(uvector_get_at(v, 1));
    UASSERT_ABORTS(uvector_set_at(v, 1, G_INT(0)));

    uvector_resize(v, 100, G_NULL());
    UASSERT_ABORTS(uvector_swap(v, 150, 250));
    UASSERT_ABORTS(uvector_swap(v, 50, 250));
    UASSERT_ABORTS(uvector_insert_at(v, 500, G_INT(400)));
    uvector_destroy(v);
#endif

}

int cmp(const char *s1, const char *s2)
{
    ugeneric_t gv1 = ugeneric_parse(s1);
    ugeneric_t gv2 = ugeneric_parse(s2);
    UASSERT_NO_ERROR(gv1);
    UASSERT_NO_ERROR(gv2);
    uvector_t *v1 = G_AS_PTR(gv1);
    uvector_t *v2 = G_AS_PTR(gv2);
    int diff = uvector_compare(v1, v2);
    uvector_destroy(v1);
    uvector_destroy(v2);
    return diff;
}

void test_uvector_compare(void)
{
    uvector_t *v1 = uvector_create();
    uvector_t *v2 = uvector_create();
    uvector_append(v1, G_STR(ustring_dup("s1")));
    uvector_append(v2, G_STR(ustring_dup("s1")));
    UASSERT(uvector_compare(v1, v2) == 0);
    uvector_set_at(v1, 0, G_STR(ustring_dup("s3")));
    UASSERT(uvector_compare(v1, v2) != 0);
    uvector_set_at(v2, 0, G_STR(ustring_dup("s4")));
    UASSERT(uvector_compare(v1, v2) < 0);
    uvector_destroy(v1);
    uvector_destroy(v2);

    UASSERT(cmp("[]", "[]") == 0);
    UASSERT(cmp("[1]", "[1]") == 0);
    UASSERT(cmp("[1, 2, 3]", "[1, 2, 3]") == 0);
    UASSERT(cmp("[1, 2]", "[1, 2, 3]") < 0);
    UASSERT(cmp("[1, 2, 3]", "[1, 2]") > 0);
    UASSERT(cmp("[3, 2, 3]", "[1, 2, 3]") > 0);
}

/*
double f(double x, void *p)
{
    uvector_t *v = p;
    return G_AS_REAL(uvector_get_at(v, x));
}

void test_gnuplot(void)
{
#include <gsl/gsl_math.h>
#include <gsl/gsl_deriv.h>

    gsl_function F;
    double result, abserr;
    #define N 10

    gnuplot_attrs_t a = {
        .xlabel = "xlabel",
        .ylabel = "ylabel",
        .data_label = "samples",
        .title = "chart",
    };

    uvector_t *v = uvector_create();
    for (size_t i = 0; i < N; i++)
    {
        uvector_append(v, G_REAL(sin(i/50.0)));
    }
    uvector_dump_to_gnuplot(v, &a, stdout);

    uvector_t *dv = uvector_create();
    F.function = &f;
    F.params = v;

    for (size_t i = 0; i < N; i++)
    {
        UASSERT(gsl_deriv_central(&F, (double)i, 1e-8, &result, &abserr) == 0);
        uvector_append(dv, G_REAL(result));
    }
    uvector_dump_to_gnuplot(dv, &a, stdout);

    uvector_destroy(v);
    uvector_destroy(dv);
}
*/

void _check_slice(const uvector_t *v, size_t b, size_t e, size_t s, char *exp)
{
    uvector_t *slice = uvector_get_slice(v, b, e, s);
    char *str = uvector_as_str(slice);
    UASSERT_STR_EQ(str, exp);
    ufree(str);
    uvector_destroy(slice);
}

void test_uvector_slice(void)
{
    ugeneric_t g = ugeneric_parse("[1, 2, 3, 4, 5, 6, 7, 8, 9, 10]");
    UASSERT_NO_ERROR(g);
    uvector_t *v = G_AS_PTR(g);

    _check_slice(v, 0, 10, 1, "[1, 2, 3, 4, 5, 6, 7, 8, 9, 10]");
    _check_slice(v, 0, 10, 2, "[1, 3, 5, 7, 9]");
    _check_slice(v, 1, 10, 2, "[2, 4, 6, 8, 10]");
    _check_slice(v, 0, 10, 3, "[1, 4, 7, 10]");
    _check_slice(v, 1, 10, 3, "[2, 5, 8]");
    _check_slice(v, 0, 10, 4, "[1, 5, 9]");
    _check_slice(v, 1, 10, 4, "[2, 6, 10]");
    _check_slice(v, 0, 10, 5, "[1, 6]");
    _check_slice(v, 1, 10, 5, "[2, 7]");
    _check_slice(v, 0, 10, 6, "[1, 7]");
    _check_slice(v, 1, 10, 6, "[2, 8]");
    _check_slice(v, 0, 10, 9, "[1, 10]");
    _check_slice(v, 1, 10, 9, "[2]");
    _check_slice(v, 1, 1, 1, "[]");
    _check_slice(v, 1, 1, 5, "[]");
    _check_slice(v, 0, 10, 25, "[1]");

    uvector_destroy(v);
}

void test_uvector_data_ownership(void)
{
    uvector_t *v = uvector_create();
    UASSERT(uvector_is_data_owner(v));
    //uvector_append(v, G_CSTR("s1"));
    //uvector_append(v, G_CSTR("s1"));

    // shallow copy
    uvector_t *copy1 = uvector_copy(v);
    UASSERT(!uvector_is_data_owner(copy1));

    // deep copy
    uvector_t *copy2 = uvector_deep_copy(v);
    UASSERT(uvector_is_data_owner(copy2));
    uvector_drop_data_ownership(copy2);
    UASSERT(!uvector_is_data_owner(copy2));

    // kick the bucket
    uvector_destroy(v);
    uvector_destroy(copy1);
    uvector_destroy(copy2);
}

void _check_reverse(const char *in, const char *rev)
{
    ugeneric_t g = ugeneric_parse(in);
    UASSERT_NO_ERROR(g);
    uvector_t *v = G_AS_PTR(g);
    uvector_reverse(v);
    char *t = uvector_as_str(v);
    UASSERT_STR_EQ(t, rev);
    ufree(t);
    uvector_destroy(v);
}

void test_uvector_reverse(void)
{
    _check_reverse("[]", "[]");
    _check_reverse("[1]", "[1]");
    _check_reverse("[1, 2]", "[2, 1]");
    _check_reverse("[1, 2, 3]", "[3, 2, 1]");
    _check_reverse("[1, 2, 3, 4]", "[4, 3, 2, 1]");
    _check_reverse("[1, 2, 3, 4, 5]", "[5, 4, 3, 2, 1]");
}

int main(int argc, char **argv)
{
//    test_gnuplot();

    (void)argv;
    test_uvector_api();
    test_uvector_serialization(argc > 1);
    test_uvector_next_permutation(argc > 1);
    test_uvector_copy(argc > 1);
    test_uvector_bsearch();
    test_uvector_compare();
    test_uvector_slice();
    test_uvector_data_ownership();
    test_uvector_reverse();

    return EXIT_SUCCESS;
}


/*
 * The MIT License is a permissive free software license, which permits reuse
 * within both open source and proprietary software. The software is licensed
 * as is, and no warranty is given as to fitness for purpose or absence of
 * infringement of third parties' rights, such as patents. Generally use for
 * research activities is allowed regardless of any third party patents, but
 * commercial use may be subject to a separate license.
 *
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2015 Tuomo Raitio
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 *
 *
 *
 * <><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><>
 *              GlottHMM Speech Parameter Extractor
 * <><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><>
 *
 * This program reads a speech file and extracts speech
 * parameters using glottal inverse filtering.
 *
 * This program has been written in Aalto University,
 * Department of Signal Processign and Acoustics, Espoo, Finland
 *
 * Author: Tuomo Raitio
 * Acknowledgements: Antti Suni, Paavo Alku, Martti Vainio
 *
 * File Analysis.c
 * Version: 1.1
 *
 */

/***********************************************/
/*                 INCLUDE                     */
/***********************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <sndfile.h> 				/* Read and write wav */
#include <gsl/gsl_vector.h>		/* GSL, Vector */
#include <gsl/gsl_matrix.h>		/* GSL, Matrix */
#include <gsl/gsl_fft_real.h>		/* GSL, FFT */
#include <libconfig.h>				/* Configuration file */
#include "AnalysisFunctions.h"

/*******************************************************************/
/*                          MAIN                                   */
/*******************************************************************/

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

	/* Check command line format */
	if(Check_command_line(argc) == EXIT_FAILURE)
		return EXIT_FAILURE;

	/* Read default configuration file and assign parameters */
	struct config_t *conf_def = Read_config(argv[2]);
	if(conf_def == NULL)
		return EXIT_FAILURE;
	PARAM params;
	if(Assign_config_parameters(conf_def,&params,DEF_CONF) == EXIT_FAILURE) {
		config_destroy(conf_def);
		free(conf_def);
		return EXIT_FAILURE;
	}

	/* Read user configuration file and assign parameters */
	if(argc == 4) {
		struct config_t *conf_usr = Read_config(argv[3]);
		if(conf_usr == NULL)
			return EXIT_FAILURE;
		if(Assign_config_parameters(conf_usr,&params,USR_CONF) == EXIT_FAILURE) {
			config_destroy(conf_usr);
			free(conf_usr);
			return EXIT_FAILURE;
		}
	}

	/* Check the validity of the parameters */
	if(Check_parameter_validity(&params) == EXIT_FAILURE)
		return EXIT_FAILURE;

	/* Read soundfile, define sampling frequency, invert signal if requested */
	gsl_vector *signal = Read_soundfile(argv[1], &params);
	if(signal == NULL)
		return EXIT_FAILURE;

	/* Read external F0 file if requested */
	gsl_vector *fundf;
	if(Read_external_f0(&fundf,&params) == EXIT_FAILURE) {
		gsl_vector_free(signal);
		return EXIT_FAILURE;
	}

	/* High-pass filtering */
	if(HighPassFilter(signal,&params) == EXIT_FAILURE) {
		gsl_vector_free(signal);
		return EXIT_FAILURE;
	}

	/*******************************************************************/
	/*                    ALLOCATE MEMORY                              */
	/*******************************************************************/

	/* Allocate memory for variables */
	int i,j,index;
	double f0 = 0;
	gsl_vector *frame,*frame0,*glottal,*gain,*uvgain,*f0_frame,*glottal_f0,*f0_frame0,*glottsig,*glottsig_f0;
	gsl_vector *source_signal,*h1h2,*naq,*ph1h2,*pnaq;
	gsl_matrix *fundf_candidates, *LSF, *LSF2, *bp_gain, *spectral_tilt, *HNR, *waveform, *harmonics,
			*fftmatrix_vt, *fftmatrix_src, *fftmatrix_uv;
	Allocate_variables(&frame,&frame0,&glottal,&gain,&uvgain,&f0_frame,&glottal_f0,&f0_frame0,&glottsig,
			&glottsig_f0,&source_signal,&fundf_candidates,&LSF,&LSF2,&bp_gain,&spectral_tilt,&HNR,
			&waveform,&harmonics,&h1h2,&naq,&fftmatrix_vt,&fftmatrix_src,&fftmatrix_uv,&params);

	/*******************************************************************/
	/*                   ALLOCATE PULSE LIBRARY                        */
	/*******************************************************************/

	/* Allocate memory for pulse library variables */
	gsl_vector *pulse_inds,*pulse_lengths,*pulse_pos,*pgain;
	gsl_matrix *gpulses,*gpulses_rs,*plsf,*ptilt,*pharm,*phnr,*pwaveform;
	Allocate_pulselib_variables(&gpulses,&gpulses_rs,&pulse_inds,&pulse_pos,&pulse_lengths,
			&plsf,&ptilt,&pharm,&phnr,&pwaveform,&pgain,&ph1h2,&pnaq,&params);

	/********************************************************************/
	/*                    EXTRACT PARAMETERS                            */
	/********************************************************************/

	/* Analysis: Start reading signal vector */
	for(index=0; index<params.n_frames; index++) {

		/* Print progress */
		Print_progress(index,params.n_frames,argv[1],params.FS);

		/* Get samples to frames */
		Get_samples_to_frames(signal,frame,frame0,f0_frame,f0_frame0,params.shift,index);

		/* Gain extraction */
		Gain(frame, gain, index, USE_WINDOWING);
		UnvoicedGain(frame, uvgain, index, USE_WINDOWING, params.unvoiced_frame_length);
		BandPassGain(frame, bp_gain, &params, index);

		/* Preliminary inverse filtering for GCI detection (e.g. in GCI-weighted SWLP) */
		if(params.lp_method == LP_METHOD_ID_WLP && params.lp_weighting == LP_WEIGHTING_ID_GCI) {
			InverseFiltering_long(f0_frame, f0_frame0, glottsig_f0, fundf, glottsig, index, &params);
			FundF(glottsig_f0, frame, fundf_candidates, fundf, bp_gain, index, &params);
			InverseFiltering_long(frame, frame0, glottsig, fundf, glottsig, index, &params);
		}

		/* Inverse filtering */
		InverseFiltering(frame, frame0, glottal, LSF, LSF2, spectral_tilt, fundf, glottsig, fftmatrix_vt,
				fftmatrix_src, fftmatrix_uv, index, &params);
		InverseFiltering_long(f0_frame, f0_frame0, glottal_f0, fundf, glottsig_f0, index, &params);

		/* Fundamental frequency estimation. Define a f0 value for the frame (even if unvoiced) */
		FundF(glottal_f0, frame, fundf_candidates, fundf, bp_gain, index, &params);
		f0 = Define_current_f0(fundf,f0,index);

		/* Estimate Harmonic-to-Noise Ratio (HNR) and magnitudes of the first N harmonics */
		Harmonic_analysis(glottal_f0, harmonics, HNR, h1h2, f0, params.FS, index);

		/* Extract pulses, pitch-synchronous spectrum estimation */
		Extract_pulses(f0_frame,glottal_f0,fundf,naq,gpulses,gpulses_rs,pulse_pos,pulse_inds,pulse_lengths,
				plsf,ptilt,pharm,phnr,pgain,ph1h2,pnaq,LSF,spectral_tilt,harmonics,h1h2,HNR,gain,pwaveform,waveform,index,&params);

		/* Construct source signal */
		Construct_source(source_signal, glottal, glottal_f0, index, &params);
	}

	/********************************************************************/
	/*                POSTPROCESSING OF PARAMETERS                      */
	/********************************************************************/

	/* Add missing frames to the beginning and the end of parameter vectors/matrices.
	 * This is due to the analysis scheme, where the analysis starts
	 * and ends with whole frames instead of zero-padding */
	Add_missing_frames(&fundf,&fundf_candidates,&gain,&uvgain,&HNR,&spectral_tilt,&LSF,&LSF2,&harmonics,&waveform,&naq,&h1h2,
			&fftmatrix_vt,&fftmatrix_src,&fftmatrix_uv,&params);

	/* Postprocess F0 */
	F0_postprocess(fundf,fundf_candidates,&params);

	/* Median filtering */
	MedFilt5_matrix(HNR);
	MedFilt5_matrix(harmonics);
	MedFilt5(h1h2);
	MedFilt5(naq);

	/* Replace unvoiced fetures */
	for(i=0; i<fundf->size; i++) {
		if(gsl_vector_get(fundf, i) == 0) {
			if(params.sep_vuv_spectrum == 0) {
				for(j=0; j<params.lpc_order_vt; j++)
					gsl_matrix_set(LSF, i, j, gsl_matrix_get(LSF2, i, j));
			}
			for(j=0; j<fftmatrix_vt->size2; j++)
				gsl_matrix_set(fftmatrix_vt, i, j, gsl_matrix_get(fftmatrix_uv, i, j));
			gsl_vector_set(gain,i,gsl_vector_get(uvgain,i));
		}
	}

	/* Noise reduction */
	Noise_reduction(gain,&params);

	/* Fill possible gaps in waveform and NAQ */
	Fill_waveform_gaps(fundf,waveform);
	Fill_naq_gaps(fundf,naq);

	/* Select new values to pulse parameters according to refined parameters */
	Select_new_refined_values(fundf,gain,LSF,spectral_tilt,HNR,harmonics,pgain,plsf,ptilt,
			phnr,pharm,pulse_pos,pulse_lengths,waveform,h1h2,ph1h2,naq,pnaq,&params);

	/* Select only unique pulses */
	Select_unique_pulses(&gpulses,&gpulses_rs,&pulse_lengths,&pulse_pos,&pulse_inds,&plsf,
			&ptilt,&phnr,&pharm,&pwaveform,&pgain,&ph1h2,&pnaq,&params);

	/* Select only one pulse per frame */
	Select_one_pulse_per_frame(&gpulses,&gpulses_rs,&pulse_lengths,&pulse_pos,&pulse_inds,&plsf,
				&ptilt,&phnr,&pharm,&pwaveform,&pgain,&ph1h2,&pnaq,&params);

	/********************************************************************/
	/*                      FORMANT ENHANCEMENT                         */
	/********************************************************************/

	/* Formant enhancement by LSFs*/
	if(params.formant_enh_method == FORMANT_ENH_ID_LSF)
		LSF_Postfilter(LSF, &params);

	/* Formant Enhancement by re-estimating LPC and modifying the autocorrelation */
	if(params.formant_enh_method == FORMANT_ENH_ID_LPC)
		LPC_Postfilter(LSF, &params);

	/* Differentiate LSFs if requested */
	if(params.differential_lsf == 1) {
		Differentiate_LSFs(&LSF);
		Differentiate_LSFs(&LSF2);
		Differentiate_LSFs(&spectral_tilt);
	}

	/* Convert F0 to logarithmic scale */
	Convert_F0_to_log(fundf,&params);

	/********************************************************************/
	/*                    WRITE PARAMETERS TO FILE                      */
	/********************************************************************/

	/* Open files for writing parameters, free memory */
	Write_parameters_to_file(argv[1],LSF,LSF2,spectral_tilt,HNR,harmonics,waveform,fundf,gain,h1h2,naq,source_signal,
			fftmatrix_vt,fftmatrix_src,&params);
	Free_variables(frame,frame0,signal,glottal,glottsig,glottsig_f0,uvgain,f0_frame,f0_frame0,glottal_f0,bp_gain,
			fundf_candidates,fftmatrix_uv);

	/**************************************************************/
	/*            WRITE  PULSE LIBRARY TO FILE                    */
	/**************************************************************/

	/* Select unique pulses and write pulse library data to file, free memory */
	Write_pulselibrary_to_file(argv[1],gpulses,gpulses_rs,pulse_lengths,pulse_pos,pulse_inds,plsf,
			ptilt,phnr,pharm,pwaveform,pgain,ph1h2,pnaq,&params);

	/* Finish */
	printf("\nFinished analysis.\n\n");
	return EXIT_SUCCESS;
}

/***********/
/*   EOF   */
/***********/


/*
 * Copyright 2020 Makani Technologies LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SIM_MATH_LINALG_H_
#define SIM_MATH_LINALG_H_

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <stdint.h>

// We use GSL style conventions here to maintain consistency with the
// GSL library.  See GSL docs for comment.

#define GSL_VECTOR_STACK_ALLOC(v_, size_)                               \
  double v_##_data[size_];                                              \
  gsl_block v_##_block = {size_, v_##_data};                            \
  gsl_vector v_##_vector = {size_, 1, v_##_block.data, &v_##_block, 0}; \
  gsl_vector *v_ = &v_##_vector;

#define GSL_MATRIX_STACK_ALLOC(m_, size1_, size2_)                \
  double m_##_data[(size1_) * (size2_)];                          \
  gsl_block m_##_block = {(size1_) * (size2_), m_##_data};        \
  gsl_matrix m_##_matrix = {size1_,          size2_,      size2_, \
                            m_##_block.data, &m_##_block, 0};     \
  gsl_matrix *m_ = &m_##_matrix;

double gsl_matrix_trace(const gsl_matrix *m);
const gsl_vector *gsl_matrix_diag(const gsl_matrix *m, gsl_vector *d);
void gsl_matrix_disp(const gsl_matrix *m);
void gsl_vector_disp(const gsl_vector *v);
const gsl_vector *gsl_vector_saturate(const gsl_vector *x, double low,
                                      double high, gsl_vector *y);
double gsl_vector_norm_bound(const gsl_vector *x, double low);
double gsl_vector_dot(const gsl_vector *x, const gsl_vector *y);

#endif  // SIM_MATH_LINALG_H_


/*
 * optimize_tnc.h
 *
 *  Created on: Feb 9, 2010
 *      Author: smitty
 */

#ifndef _OPTIMIZE_STATE_RECONSTRUCTOR_NLOPT_H_
#define _OPTIMIZE_STATE_RECONSTRUCTOR_NLOPT_H_

#include <nlopt.h>

#include "state_reconstructor.h"
#include "rate_model.h"

#include <armadillo>
using namespace arma;

void optimize_sr_nlopt(RateModel * _rm,StateReconstructor * _sr, mat * _free_mask, int _nfree);

#endif /* _OPTIMIZE_STATE_RECONSTRUCTOR_NLOPT_H_ */