#include "MCHMM.h"
#include <cmath>
#include "Sampler.h"
#include <random>
#include <chrono>
#include <glog/logging.h>
#include <cassert>
#include <boost/math/distributions/students_t.hpp>
using namespace std;
using namespace google;

MCHMM::MCHMM(){
    pi = new vector<Sample>();
    m = new vector<Sample>();
    v = new vector<Sample>();
}

/**
 * @brief MCHMM::forward Goes one step forward in time according to the HMM distributions
 * @param observations Observations needed to do the reasoning based on the HMM
 * @param N Number of samples used in the resampling step of any sampling inside this method
 * @return Alpha DETree for the next time step distribution
 */
DETree * MCHMM::forward(vector<Observation> *observations, size_t N){
    vector<Sample> alpha_samples[2];
    Sampler sampler;
    size_t T = observations->size();

    alpha_samples[0] = sampler.resample_from(pi_tree, N);

    // STEP 2
    size_t t = 0;
    for (t = 1; t < T; t++){
        // STEP 2(a)
        vector<Sample> temp = sampler.likelihood_weighted_resampler(alpha_samples[(t - 1) % 2], N);
        double sum_densities = 0.0;

        for (size_t i = 0; i < temp.size(); i++){
            // STEP 2(b)
            Sample x = sampler.sample_given(m_tree, temp[i]);

            for (size_t i = 0; i < temp[i].size(); i++){
                x.values.pop_back();
            }

            // STEP 2(c)
            Sample v_temp = (*observations)[t].combine(x);
            double density = v_tree->density_value(v_temp, rho);
            x.p = density;

            sum_densities += density;
            temp[i] = x;
        }

        // Normalizing the probabilities
        for (size_t i = 0; i < temp.size(); i++){
            temp[i].p = temp[i].p / sum_densities;
        }

        // STEP 2(d)
        alpha_samples[t % 2] = temp;
    }

    vector<Sample> temp = sampler.likelihood_weighted_resampler(alpha_samples[(t - 1) % 2], N);
    for (size_t i = 0; i < temp.size(); i++){
        // STEP 2(b)
        Sample x = sampler.sample_given(m_tree, temp[i]);
        x.p = 1.0 / temp.size();
        temp[i] = x;
    }

    return new DETree(temp, pi_low_limit, pi_high_limit);
}

/**
 * @brief MCHMM::gamma It perform the forward-backward algorithms and combine the results to get the
 * gamma distribution, it is useful mostly for getting the most probable states
 * @param observations Observations needed to do the reasoning based on the HMM
 * @param N Number of samples used in the resampling step of any sampling inside this method
 * @return Gamma DETrees for each observation
 */
vector<DETree *> MCHMM::gamma(vector<Observation> *observations, size_t N){
    Sampler sampler;
    size_t T = observations->size();

    vector<Sample> alpha_samples[2];
    vector<Sample> beta_samples[2];

    vector<DETree*> alpha_trees;
    vector<DETree*> beta_trees;
    vector<DETree*> gamma_trees;

    // STEP 1
    alpha_samples[0] = sampler.resample_from(pi_tree, N);
    alpha_trees.push_back(new DETree(alpha_samples[0], pi_low_limit, pi_high_limit));

    // STEP 2
    for (size_t t = 1; t < T; t++){
        // STEP 2(a)
        vector<Sample> temp = sampler.likelihood_weighted_resampler(alpha_samples[(t - 1) % 2], N);
        double sum_densities = 0.0;

        for (size_t i = 0; i < temp.size(); i++){
            // STEP 2(b)
            Sample x = sampler.sample_given(m_tree, temp[i]);

            for (size_t i = 0; i < temp[i].size(); i++){
                x.values.pop_back();
            }

            // STEP 2(c)
            Sample v_temp = (*observations)[t].combine(x);
            double density = v_tree->density_value(v_temp, rho);
            x.p = density;

            sum_densities += density;
            temp[i] = x;
        }

        // Normalizing the probabilities
        for (size_t i = 0; i < temp.size(); i++){
            temp[i].p = temp[i].p / sum_densities;
        }

        // STEP 2(d)
        alpha_samples[t % 2] = temp;
        alpha_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
    }

    // STEP 3
    beta_samples[0] = sampler.uniform_sampling(pi_low_limit, pi_high_limit, N);
    beta_trees.push_back(new DETree(beta_samples[0], pi_low_limit, pi_high_limit));

    // STEP 4
    for (size_t t = T - 1; t >= 1; t--){
        // STEP 4(a)
        int index_t = ((T) - (t + 1)) % 2;
        vector<Sample> temp = sampler.likelihood_weighted_resampler(beta_samples[index_t], N);
        double sum_densities = 0.0;

        for (size_t i = 0; i < temp.size(); i++){
            // STEP 4(b)
            Sample x = sampler.sample_given(m_tree, temp[i]);

            for (size_t i = 0; i < temp[i].size(); i++){
                x.values.pop_back();
            }

            // STEP 4(c)
            Sample v_temp = (*observations)[t].combine(x);
            double density = v_tree->density_value(v_temp, rho);
            x.p = density;

            sum_densities += density;
            temp[i] = x;
        }

        // Normalizing the probabilities
        for (size_t i = 0; i < temp.size(); i++){
            temp[i].p = temp[i].p / sum_densities;
        }

        // STEP 4(d)
        beta_samples[(index_t + 1) % 2] = temp;
        beta_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
    }

    // STEP 5
    for (size_t t = 0; t < T; t++){
        vector<Sample> temp;
        double sum_density = 0.0;

        int index_t = (T) - (t + 1);

        // STEP 5(a)
        for (size_t j = 0; j < N / 2; j++){
            Sample sample = sampler.sample(alpha_trees[t]);
            sample.p = (*beta_trees[index_t]).density_value(sample, rho);
            sum_density += sample.p;
            temp.push_back(sample);
        }

        // STEP 5(b)
        for (size_t j = 0; j < N - (N / 2); j++){
            Sample sample = sampler.sample(beta_trees[index_t]);
            sample.p = (*alpha_trees[t]).density_value(sample, rho);
            sum_density += sample.p;
            temp.push_back(sample);
        }

        // Normalizing the probabilities
        for (size_t i = 0; i < temp.size(); i++){
            temp[i].p = temp[i].p / sum_density;
        }

        gamma_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
    }

    alpha_samples[0].clear();
    alpha_samples[1].clear();
    beta_samples[0].clear();
    beta_samples[1].clear();

    for (size_t i = 0; i < alpha_trees.size(); i++){
        delete alpha_trees[i];
    }

    for (size_t i = 0; i < beta_trees.size(); i++){
        delete beta_trees[i];
    }

    assert(gamma_trees.size() == T);

    return gamma_trees;
}

/**
 * @brief MCHMM::learn_hmm it takes in some observations and perform the EM as many iterations as the max_iteration arg
 * @param observations Observations needed for learning the HMM distributions
 * @param max_iteration Maximum number of iterations performed for the EM
 * @param N Number of samples used in the resampling step of any sampling inside this method
 */
void MCHMM::learn_hmm_KL(vector<Observation> *observations, double threshold, size_t max_iteration, int N){
    if (observations->size() < 2){
        LOG(ERROR) << "Not enough observation data!";
        return;
    }

    Sampler sampler;
    size_t T = observations->size();

    if (pi->size() < 1 || v->size() < 1 || m->size() < 1){
        LOG(INFO) << "Init HMM Randomly!";
        init_hmm_randomly(N, N, N);
    }

    bool cond = true;
    size_t iteration = 0;
    DETree* old_gamma_tree = NULL;

    vector<Sample> test_samples = sampler.uniform_sampling(pi_low_limit, pi_high_limit, 1000);

    while (cond){
        vector<Sample> alpha_samples[2];
        vector<Sample> beta_samples[2];

        vector<DETree*> alpha_trees;
        vector<DETree*> beta_trees;
        vector<DETree*> gamma_trees;

        /////////////////E STEP/////////////////
        {
            // STEP 1
            alpha_samples[0] = sampler.resample_from(pi_tree, N);
            alpha_trees.push_back(new DETree(alpha_samples[0], pi_low_limit, pi_high_limit));

            // STEP 2
            for (size_t t = 1; t < T; t++){
                // STEP 2(a)
                vector<Sample> temp = sampler.likelihood_weighted_resampler(alpha_samples[(t - 1) % 2], N);
                double sum_densities = 0.0;

                for (size_t i = 0; i < temp.size(); i++){
                    // STEP 2(b)
                    Sample x = sampler.sample_given(m_tree, temp[i]);

                    for (size_t i = 0; i < temp[i].size(); i++){
                        x.values.pop_back();
                    }

                    // STEP 2(c)
                    Sample v_temp = (*observations)[t].combine(x);
                    double density = v_tree->density_value(v_temp, rho);
                    x.p = density;

                    sum_densities += density;
                    temp[i] = x;
                }

                // Normalizing the probabilities
                for (size_t i = 0; i < temp.size(); i++){
                    temp[i].p = temp[i].p / sum_densities;
                }

                // STEP 2(d)
                alpha_samples[t % 2] = temp;
                alpha_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
            }

            // STEP 3
            beta_samples[0] = sampler.uniform_sampling(pi_low_limit, pi_high_limit, N);
            beta_trees.push_back(new DETree(beta_samples[0], pi_low_limit, pi_high_limit));

            // STEP 4
            for (size_t t = T - 1; t >= 1; t--){
                // STEP 4(a)
                int index_t = ((T) - (t + 1)) % 2;
                vector<Sample> temp = sampler.likelihood_weighted_resampler(beta_samples[index_t], N);
                double sum_densities = 0.0;

                for (size_t i = 0; i < temp.size(); i++){
                    // STEP 4(b)
                    Sample x = sampler.sample_given(m_tree, temp[i]);

                    for (size_t i = 0; i < temp[i].size(); i++){
                        x.values.pop_back();
                    }

                    // STEP 4(c)
                    Sample v_temp = (*observations)[t].combine(x);
                    double density = v_tree->density_value(v_temp, rho);
                    x.p = density;

                    sum_densities += density;
                    temp[i] = x;
                }

                // Normalizing the probabilities
                for (size_t i = 0; i < temp.size(); i++){
                    temp[i].p = temp[i].p / sum_densities;
                }

                // STEP 4(d)
                beta_samples[(index_t + 1) % 2] = temp;
                beta_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
            }

            // STEP 5
            for (size_t t = 1; t < T; t++){
                vector<Sample> temp;
                double sum_density = 0.0;

                int index_t = (T) - (t);

                // STEP 5(a)
                for (int j = 0; j < N / 2; j++){
                    Sample sample = sampler.sample(alpha_trees[t]);
                    sample.p = (*beta_trees[index_t]).density_value(sample, rho);
                    sum_density += sample.p;
                    temp.push_back(sample);
                }

                // STEP 5(b)
                for (int j = 0; j < N - (N / 2); j++){
                    Sample sample = sampler.sample(beta_trees[index_t]);
                    sample.p = (*alpha_trees[t]).density_value(sample, rho);
                    sum_density += sample.p;
                    temp.push_back(sample);
                }

                // Normalizing the probabilities
                for (size_t i = 0; i < temp.size(); i++){
                    temp[i].p = temp[i].p / sum_density;
                }

                gamma_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
            }

            alpha_samples[0].clear();
            alpha_samples[1].clear();
            beta_samples[0].clear();
            beta_samples[1].clear();

            LOG(INFO) << "End of E Step at iteration: " << iteration;
        }

        /////////////////M STEP/////////////////
        {
            unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
            std::default_random_engine gen(seed);

            vector<Sample> * temp_m = new vector<Sample>();
            vector<Sample> * temp_v = new vector<Sample>();
            vector<Sample> * temp_pi = new vector<Sample>();

            // STEP 1
            for (int i = 0; i < N; i++){
                uniform_real_distribution<double> dist(1, T - 2);
                int t = dist(gen);

                Sample x = sampler.sample(gamma_trees[t]);
                Sample x_prime = sampler.sample(gamma_trees[t + 1]);

                Sample temp = x.combine(x_prime.values);
                temp.p = 1.0 / N;
                temp_m->push_back(temp);
            }

            // STEP 2
            for (int i = 0; i < N; i++){
                uniform_real_distribution<double> dist(1, T - 1);
                int t = dist(gen);

                Sample x = sampler.sample(gamma_trees[t]);

                Sample temp = (*observations)[t].combine(x);
                temp.p = 1.0 / N;
                temp_v->push_back(temp);
            }

            // STEP 3
            int pi_size = pi->size();
            for (int i = 0; i < pi_size; i++){
                temp_pi->push_back(sampler.sample(gamma_trees[0]));
            }

            delete pi;
            pi = temp_pi;
            pi_tree->create_tree(*pi, pi_low_limit, pi_high_limit);

            delete m;
            m =  temp_m;
            m_tree->create_tree(*m, m_low_limit, m_high_limit);

            delete v;
            v = temp_v;
            v_tree->create_tree(*v, v_low_limit, v_high_limit);

            LOG(INFO) << "End of M Step at iteration: " << iteration;

        }

        /////////////////ANNEALING/////////////////
        if (rho > 0.01)
            rho = rho * rho_bar;

        /////////////////SAMPLE SET SIZE/////////////////
        if (N < (int)max_sample_size)
            N = N; // * eta;

        /////////////////STOP CONDITION/////////////////
        {
            LOG(INFO) << "Iteration " << iteration + 1 << " Finished!" << "\n";
            iteration++;
            if (iteration >= max_iteration){
                cond = false;
            }

            if (iteration > 1){ // Do the KL if we have at least on previous HMM parameters set!!!
                // Generate a lot of samples uniformly

                // Find the density estimations for each generated sample
                vector<double> estimates_old;
                vector<double> estimates_new;
                double sum_old = 0.0;
                double sum_new = 0.0;
                for (size_t r = 0; r < test_samples.size(); r++){
                    estimates_old.push_back(old_gamma_tree->density_value(test_samples[r], 0.5));
                    estimates_new.push_back(gamma_trees.back()->density_value(test_samples[r], 0.5));

                    sum_old += estimates_old.back();
                    sum_new += estimates_new.back();
                }

                // Normalize the density values
                for (size_t r = 0; r < test_samples.size(); r++){
                    estimates_old[r] = estimates_old[r] / sum_old;
                    estimates_new[r] = estimates_new[r] / sum_new;
                }

                // Compute the KL divergence factor
                double KLD = KLD_compute(estimates_old, estimates_new);

                LOG(ERROR) << "KLD: " << KLD;

                // If KLD < threshold --> STOP
                if (KLD < threshold){
                    cond = false;
                }
            }
        }

        for (size_t i = 0; i < alpha_trees.size(); i++){
            delete alpha_trees[i];
        }

        for (size_t i = 0; i < beta_trees.size(); i++){
            delete beta_trees[i];
        }

        for (size_t i = 0; i < gamma_trees.size() - 1; i++){
            delete gamma_trees[i];
        }

        if (old_gamma_tree){
            delete old_gamma_tree;
        }

        old_gamma_tree = gamma_trees.back();
    }

    initialized = true;
}

/**
 * @brief MCHMM::KLD_compute computes the Kullback-Leibler divergence of two distributions
 * @param P The true distribution (in this application, the true is our old distribution)
 * @param Q The estimated distribution (in this application, the estimated is our new distribution)
 * @return KLD value
 */
double MCHMM::  KLD_compute(vector<double> P, vector<double> Q){
    double KLD = 0.0;
    for (size_t i = 0; i < P.size(); i++){
        KLD += P[i] * std::log(P[i] / Q[i]);
    }
    return KLD;
}

vector<Sample> MCHMM::get_uniform_samples_from_pi(size_t N){
    Sampler sampler;
    return sampler.uniform_sampling(pi_low_limit, pi_high_limit, N);
}

/**
 * @brief MCHMM::learn_hmm it takes in some observations and perform the EM as many iterations as the max_iteration arg
 * @param observations Observations needed for learning the HMM distributions
 * @param max_iteration Maximum number of iterations performed for the EM
 * @param N Number of samples used in the resampling step of any sampling inside this method
 */
void MCHMM::learn_hmm(vector<Observation> *observations, size_t max_iteration, int N){

    if (observations->size() < 2){
        LOG(ERROR) << "Not enough observation data!";
        return;
    }

    Sampler sampler;
    size_t T = observations->size();

    if (pi->size() < 1 || v->size() < 1 || m->size() < 1){
        LOG(INFO) << "Init HMM Randomly!";
        init_hmm_randomly(N, N, N);
    }

    bool cond = true;
    size_t iteration = 0;

    while (cond){
        vector<Sample> alpha_samples[2];
        vector<Sample> beta_samples[2];

        vector<DETree*> alpha_trees;
        vector<DETree*> beta_trees;
        vector<DETree*> gamma_trees;

        /////////////////E STEP/////////////////
        {
            // STEP 1
            alpha_samples[0] = sampler.resample_from(pi_tree, N);
            alpha_trees.push_back(new DETree(alpha_samples[0], pi_low_limit, pi_high_limit));

            // STEP 2
            for (size_t t = 1; t < T; t++){
                // STEP 2(a)
                vector<Sample> temp = sampler.likelihood_weighted_resampler(alpha_samples[(t - 1) % 2], N);
                double sum_densities = 0.0;

                for (size_t i = 0; i < temp.size(); i++){
                    // STEP 2(b)
                    Sample x = sampler.sample_given(m_tree, temp[i]);

                    for (size_t i = 0; i < temp[i].size(); i++){
                        x.values.pop_back();
                    }

                    // STEP 2(c)
                    Sample v_temp = (*observations)[t].combine(x);
                    double density = v_tree->density_value(v_temp, rho);
                    x.p = density;

                    sum_densities += density;
                    temp[i] = x;
                }

                // Normalizing the probabilities
                for (size_t i = 0; i < temp.size(); i++){
                    temp[i].p = temp[i].p / sum_densities;
                }

                // STEP 2(d)
                alpha_samples[t % 2] = temp;
                alpha_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
            }

            // STEP 3
            beta_samples[0] = sampler.uniform_sampling(pi_low_limit, pi_high_limit, N);
            beta_trees.push_back(new DETree(beta_samples[0], pi_low_limit, pi_high_limit));

            // STEP 4
            for (size_t t = T - 1; t >= 1; t--){
                // STEP 4(a)
                int index_t = ((T) - (t + 1)) % 2;
                vector<Sample> temp = sampler.likelihood_weighted_resampler(beta_samples[index_t], N);
                double sum_densities = 0.0;

                for (size_t i = 0; i < temp.size(); i++){
                    // STEP 4(b)
                    Sample x = sampler.sample_given(m_tree, temp[i]);

                    for (size_t i = 0; i < temp[i].size(); i++){
                        x.values.pop_back();
                    }

                    // STEP 4(c)
                    Sample v_temp = (*observations)[t].combine(x);
                    double density = v_tree->density_value(v_temp, rho);
                    x.p = density;

                    sum_densities += density;
                    temp[i] = x;
                }

                // Normalizing the probabilities
                for (size_t i = 0; i < temp.size(); i++){
                    temp[i].p = temp[i].p / sum_densities;
                }

                // STEP 4(d)
                beta_samples[(index_t + 1) % 2] = temp;
                beta_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
            }

            // STEP 5
            for (size_t t = 1; t < T; t++){
                vector<Sample> temp;
                double sum_density = 0.0;

                int index_t = (T) - (t);

                // STEP 5(a)
                for (int j = 0; j < N / 2; j++){
                    Sample sample = sampler.sample(alpha_trees[t]);
                    sample.p = (*beta_trees[index_t]).density_value(sample, rho);
                    sum_density += sample.p;
                    temp.push_back(sample);
                }

                // STEP 5(b)
                for (int j = 0; j < N - (N / 2); j++){
                    Sample sample = sampler.sample(beta_trees[index_t]);
                    sample.p = (*alpha_trees[t]).density_value(sample, rho);
                    sum_density += sample.p;
                    temp.push_back(sample);
                }

                // Normalizing the probabilities
                for (size_t i = 0; i < temp.size(); i++){
                    temp[i].p = temp[i].p / sum_density;
                }

                gamma_trees.push_back(new DETree(temp, pi_low_limit, pi_high_limit));
            }

            alpha_samples[0].clear();
            alpha_samples[1].clear();
            beta_samples[0].clear();
            beta_samples[1].clear();

            LOG(INFO) << "End of E Step at iteration: " << iteration;
        }

        /////////////////M STEP/////////////////
        {
            unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
            std::default_random_engine gen(seed);

            vector<Sample> * temp_m = new vector<Sample>();
            vector<Sample> * temp_v = new vector<Sample>();
            vector<Sample> * temp_pi = new vector<Sample>();

            // STEP 1
            for (int i = 0; i < N; i++){
                uniform_real_distribution<double> dist(1, T - 2);
                int t = dist(gen);

                Sample x = sampler.sample(gamma_trees[t]);
                Sample x_prime = sampler.sample(gamma_trees[t + 1]);

                Sample temp = x.combine(x_prime.values);
                temp.p = 1.0 / N;
                temp_m->push_back(temp);
            }

            // STEP 2
            for (int i = 0; i < N; i++){
                uniform_real_distribution<double> dist(1, T - 1);
                int t = dist(gen);

                Sample x = sampler.sample(gamma_trees[t]);

                Sample temp = (*observations)[t].combine(x);
                temp.p = 1.0 / N;
                temp_v->push_back(temp);
            }

            // STEP 3
            int pi_size = pi->size();
            for (int i = 0; i < pi_size; i++){
                temp_pi->push_back(sampler.sample(gamma_trees[0]));
            }

            delete pi;
            pi = temp_pi;
            pi_tree->create_tree(*pi, pi_low_limit, pi_high_limit);

            delete m;
            m =  temp_m;
            m_tree->create_tree(*m, m_low_limit, m_high_limit);

            delete v;
            v = temp_v;
            v_tree->create_tree(*v, v_low_limit, v_high_limit);

            LOG(INFO) << "End of M Step at iteration: " << iteration;

        }

        /////////////////ANNEALING/////////////////
        if (rho > 0.01)
            rho = rho * rho_bar;

        /////////////////SAMPLE SET SIZE/////////////////
        if (N < (int)max_sample_size)
            N = N; // * eta;

        /////////////////STOP CONDITION/////////////////
        LOG(INFO) << "Iteration " << iteration + 1 << " Finished!" << "\n";
        iteration++;
        if (iteration >= max_iteration){
            cond = false;
        }

        for (size_t i = 0; i < alpha_trees.size(); i++){
            delete alpha_trees[i];
        }

        for (size_t i = 0; i < beta_trees.size(); i++){
            delete beta_trees[i];
        }

        for (size_t i = 0; i < gamma_trees.size(); i++){
            delete gamma_trees[i];
        }
    }

    initialized = true;
}

/**
 * @brief MCHMM::set_distributions Instead of learning the distributions one can use this to prime the HMM with pre-collected samples
 * @param pi Samples collected for the initial state distribution PI
 * @param m Samples collected for the transition distribution M
 * @param v Samples collected for the observation distribution NU
 * @param rho Amount of effect that different levels of the DETree have on the computed density value (default: 0.5)
 */
void MCHMM::set_distributions(vector<Sample> *pi, vector<Sample> *m, vector<Sample> *v, double rho){
    this->pi = new vector<Sample>();
    this->m = new vector<Sample>();
    this->v = new vector<Sample>();

    for (size_t i = 0; i < pi->size(); i++){
        this->pi->push_back((*pi)[i]);
    }

    for (size_t i = 0; i < m->size(); i++){
        this->m->push_back((*m)[i]);
    }

    for (size_t i = 0; i < v->size(); i++){
        this->v->push_back((*v)[i]);
    }

    this->rho = rho;

    pi_tree = new DETree(*pi, pi_low_limit, pi_high_limit);
    m_tree = new DETree(*m, m_low_limit, m_high_limit);
    v_tree = new DETree(*v, v_low_limit, v_high_limit);

    initialized = true;
}

void MCHMM::set_limits(vector<double> *pi_low_limit, vector<double> *pi_high_limit,
                       vector<double> *m_low_limit, vector<double> *m_high_limit,
                       vector<double> *v_low_limit, vector<double> *v_high_limit
                       )
{
    this->pi_low_limit = pi_low_limit;
    this->pi_high_limit = pi_high_limit;

    this->m_low_limit = m_low_limit;
    this->m_high_limit = m_high_limit;

    this->v_low_limit = v_low_limit;
    this->v_high_limit = v_high_limit;
}

void MCHMM::init_hmm_randomly(int sample_size_pi, int sample_size_m, int sample_size_v){

    if (pi_low_limit == NULL){
        LOG(FATAL) << "Please set the limits first and then run this method!!!";
    }

    for (int i = 0; i < sample_size_pi; i++){
        Sample sample;
        sample.init_rand(pi_low_limit, pi_high_limit);
        sample.p = 1.0 / sample_size_pi;
        pi->push_back(sample);
    }

    for (int i = 0; i < sample_size_m; i++){
        Sample sample;
        sample.init_rand(m_low_limit, m_high_limit);
        sample.p = 1.0 / sample_size_m;
        m->push_back(sample);
    }

    for (int i = 0; i < sample_size_v; i++){
        Sample sample;
        sample.init_rand(v_low_limit, v_high_limit);
        sample.p = 1.0 / sample_size_v;
        v->push_back(sample);
    }

    pi_tree = new DETree(*pi, pi_low_limit, pi_high_limit);
    m_tree = new DETree(*m, m_low_limit, m_high_limit);
    v_tree = new DETree(*v, v_low_limit, v_high_limit);

    initialized = true;
}

double MCHMM::_rho(){
    return this->rho;
}

bool MCHMM::initialized_(){
    return initialized;
}

DETree* MCHMM::pi_tree_(){
    return pi_tree;
}


#ifndef INCLUDED_RANGECLASSIFICATION
#include "RangeClassification.h"
#define INCLUDED_RANGECLASSIFICATION
#endif

// External headers.
#ifndef INCLUDED_ALGORITHM
#include <algorithm>
#define INCLUDED_ALGORITHM
#endif

#ifndef INCLUDED_BOOST_FOREACH
#include <boost/foreach.hpp>
#define INCLUDED_BOOST_FOREACH
#endif

#ifndef INCLUDED_QWT_SCALE_ENGINE
#include <qwt_scale_engine.h>
#define INCLUDED_QWT_SCALE_ENGINE
#endif

// Project headers.
#ifndef INCLUDED_DAL_MATHUTILS
#include "dal_MathUtils.h"
#define INCLUDED_DAL_MATHUTILS
#endif

// Module headers.

/*!
  \file
  This file contains the implementation of the RangeClassification class.
*/

namespace {

template<typename T, class BackInsertIterator>
void classifyLinear(
         T min,
         T max,
         size_t nrClasses,
         BackInsertIterator it)
{
  assert(dal::smallerOrComparable(min, max));
  assert(nrClasses > 0);

  QwtLinearScaleEngine scaleEngine;
  QwtScaleDiv scaleDivision(scaleEngine.divideScale(min, max, nrClasses, 0));
  QwtValueList const& ticks(scaleDivision.ticks(QwtScaleDiv::MajorTick));

  BOOST_FOREACH(double value, ticks) {
    *it++ = value;
  }
}

template<typename T, class BackInsertIterator>
void classifyLog10(
         T min,
         T max,
         size_t nrClasses,
         BackInsertIterator it)
{
  assert(dal::smallerOrComparable(min, max));
  assert(nrClasses > 0);

  QwtLog10ScaleEngine scaleEngine;
  QwtScaleDiv scaleDivision(scaleEngine.divideScale(min, max, nrClasses, 0));
  QwtValueList const& ticks(scaleDivision.ticks(QwtScaleDiv::MajorTick));

  BOOST_FOREACH(double value, ticks) {
    *it++ = value;
  }
}

} // Anonymous namespace

namespace ag {

//------------------------------------------------------------------------------
// DEFINITION OF STATIC RANGECLASSIFICATION MEMBERS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF RANGECLASSIFICATION MEMBERS
//------------------------------------------------------------------------------

template<typename T>
RangeClassification<T>::RangeClassification()

  : Classification<T>(),
    _min(T(0)),
    _max(T(1))

{
  classify(_min, _max, 10, Linear);
}

template<typename T>
RangeClassification<T>::RangeClassification(
         T min,
         T max,
         size_t nrClasses,
         Algorithm algorithm)

  : Classification<T>(),
    _min(min),
    _max(max)

{
  classify(_min, _max, nrClasses, algorithm);
}

template<typename T>
RangeClassification<T>::RangeClassification(
         T min,
         T max,
         T minCutoff,
         T maxCutoff,
         size_t nrClasses,
         Algorithm algorithm)

  : Classification<T>(),
    _min(min),
    _max(max)

{
  classify(minCutoff, maxCutoff, nrClasses, algorithm);
}

//! Destructor.
/*!
*/
template<typename T>
RangeClassification<T>::~RangeClassification()
{
}

template<typename T>
void RangeClassification<T>::classify(
         T minCutoff,
         T maxCutoff,
         size_t nrClasses,
         Algorithm algorithm)
{
  this->clear();

  switch(algorithm) {
    case Linear: {
      classifyLinear(minCutoff, maxCutoff, nrClasses,
         std::back_insert_iterator<RangeClassification>(*this));
      break;
    }
    case Log10: {
      // For some reason the upper border is duplicated in some cases.
      // Therefore we postprocess the calculated borders to remove duplicates.
      std::vector<T> borders;
      classifyLog10(minCutoff, maxCutoff, nrClasses,
         std::back_insert_iterator< std::vector<T> >(borders));
      this->insert(this->end(), borders.begin(),
         std::unique(borders.begin(), borders.end()));
      break;
    }
    default: {
      assert(false);
      classify(minCutoff, maxCutoff, nrClasses, Linear);
      break;
    }
  }

  assert(std::unique(this->begin(), this->end()) == this->end());

  _algorithm = algorithm;
}

template<typename T>
size_t RangeClassification<T>::index(
         T value) const
{
  assert(!this->empty());
  assert(dal::greaterOrComparable(value, this->front()));
  assert(dal::smallerOrComparable(value, this->back()));

  size_t i = 1;

  for(i = 0; i < this->size() &&
         dal::smallerOrComparable(this->operator[](i), value); ++i) { }

  return --i;
}

template<typename T>
T RangeClassification<T>::min() const
{
  return _min;
}

template<typename T>
T RangeClassification<T>::max() const
{
  return _max;
}

template<typename T>
T RangeClassification<T>::minCutoff() const
{
  assert(!this->empty());

  return this->front();
}

template<typename T>
T RangeClassification<T>::maxCutoff() const
{
  assert(!this->empty());

  return this->back();
}

template class RangeClassification<float>;
template class RangeClassification<double>;

//------------------------------------------------------------------------------
// DEFINITION OF FREE OPERATORS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF FREE FUNCTIONS
//------------------------------------------------------------------------------

} // namespace ag


#include <iostream>
#include <dlib/svm_threaded.h>

// define types
typedef dlib::matrix<double, 0, 1> col_vec;
// pair def: first is LHS, second is RHS
typedef std::pair<std::vector<col_vec>, std::vector<col_vec>> sample_type;
// associate info between LHS and RHS
typedef std::vector<long> label_type;

// all LHS and RHS are 3-dimensional vector in this example
const unsigned long num_dims = 3;

// pre define
void make_data(std::vector<sample_type> &samples,
               std::vector<label_type> &labels);

// feature extractor
struct feature_extractor
{
    typedef col_vec feature_vector_type;
    typedef col_vec lhs_element;
    typedef col_vec rhs_element;

    unsigned long num_features() const
    {
        return num_dims;
    }

    void get_features(const lhs_element &left,
                      const rhs_element &right,
                      feature_vector_type &feats) const
    {
        feats= dlib::squared(left - right);
    }
};

// serialization - empty since no state
void serialize(const feature_extractor&, std::ostream&) {}
void deserialize(const feature_extractor&, std::istream&) {}

int main()
{
    try
    {
        // get small bit of training data
        std::vector<sample_type> samples;
        std::vector<label_type> labels;
        make_data(samples, labels);

        // trainer
        dlib::structural_assignment_trainer<feature_extractor> trainer;
        // regularization constant c , large c==> overfit
        trainer.set_c(10);
        // use multiple cpu cores
        trainer.set_num_threads(4);

        // train now
        dlib::assignment_function<feature_extractor> assigner = trainer.train(samples, labels);

        // test the assigner on our data
        std::cout << "Test the learned assignment function" << std::endl;
        for (unsigned long i = 0; i < samples.size(); ++i)
        {
            std::vector<long> predicted_assignments = assigner(samples[i]);
            std::cout << "True labels: " << dlib::trans(dlib::mat(labels[i])) << std::endl;
            std::cout << "Predicted labels: " << dlib::trans(dlib::mat(predicted_assignments)) << std::endl;
        }

        // training accuracy
        std::cout << "training accuracy: " << dlib::test_assignment_function(assigner, samples, labels) << std::endl;

    }
    catch (std::exception &e)
    {
        std::cout << "Exception thrown" << std::endl;
        std::cout << e.what() << std::endl;
    }

    std::system("pause");
    return 0;
}

// make data function
void make_data(std::vector<sample_type> &samples,
               std::vector<label_type> &labels)
{
    // make four different vectors
    col_vec A(num_dims), B(num_dims), C(num_dims), D(num_dims);
    A = 1, 0, 0;
    B = 0, 1, 0;
    C = 0, 0, 1;
    D = 0, 1, 1;

    std::vector<col_vec> lhs;
    std::vector<col_vec> rhs;
    label_type mapping;

    lhs.resize(3);
    lhs[0] = A;
    lhs[1] = B;
    lhs[2] = C;

    rhs.resize(3);
    rhs[0] = B;
    rhs[1] = A;
    rhs[2] = C;

    mapping.resize(3);
    mapping[0] = 1;  // lhs 0 matches rhs 1
    mapping[1] = 0;
    mapping[2] = 2;

    samples.push_back(make_pair(lhs, rhs));
    labels.push_back(mapping);
    // ----------------
    lhs[0] = C;
    lhs[1] = A;
    lhs[2] = B;

    rhs[0] = A;
    rhs[1] = B;
    rhs[2] = D;

    mapping[0] = -1;
    mapping[1] = 0;
    mapping[2] = 1;

    samples.push_back(make_pair(lhs, rhs));
    labels.push_back(mapping);

    // -----------------
    lhs[0] = A;
    lhs[1] = B;
    lhs[2] = C;

    rhs.resize(4);
    rhs[0] = C;
    rhs[1] = B;
    rhs[2] = A;
    rhs[3] = D;

    mapping[0] = 2;
    mapping[1] = 1;
    mapping[2] = 0;

    samples.push_back(make_pair(lhs, rhs));
    labels.push_back(mapping);
    //-------------------------------
    lhs.resize(2);
    lhs[0] = B;
    lhs[1] = C;

    rhs.resize(3);
    rhs[0] = C;
    rhs[1] = A;
    rhs[2] = D;

    mapping.resize(2);
    mapping[0] = -1;
    mapping[1] = 0;

    samples.push_back(make_pair(lhs, rhs));
    labels.push_back(mapping);

    //-------------------------
    lhs.resize(3);
    lhs[0] = D;
    lhs[1] = B;
    lhs[2] = C;

    // rhs will be empty.  So none of the items in lhs can match anything.
    rhs.resize(0);

    mapping.resize(3);
    mapping[0] = -1;
    mapping[1] = -1;
    mapping[2] = -1;

    samples.push_back(make_pair(lhs, rhs));
    labels.push_back(mapping);
}


#ifdef _WIN32
#pragma warning(disable:4503)
#pragma warning(push)
#pragma warning(disable:4996 4251 4275 4800 4190 4244)
#endif
#include <vector>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"

#ifdef _MSC_VER
#include <intrin.h>
#define __builtin_popcount __popcnt
#endif

#ifdef _WIN32
#pragma warning(pop)
#endif

#include <boost/math/special_functions/erf.hpp>
#include "Block.h"
#include "ParamValidator.h"

using std::vector;
using std::string;
using cv::Mat;

namespace charliesoft
{
  BLOCK_BEGIN_INSTANTIATION(CORE_filter);
  inline double hamming_distance(char* vals1, char* vals2, int nbVals)
  {
    int output = 0;
    int i;
    int nbValsPacked = nbVals - 3;
    for (i = 0; i < nbValsPacked; i += 4) {
      output += __builtin_popcount(
        *((uint32_t *)(&vals1[i])) ^ *((uint32_t *)(&vals2[i])));
    }
    //if nbVals is not 4bytes packed
    for (; i < nbVals; i++)
      output += __builtin_popcount((unsigned char)vals1[i] ^ (unsigned char)vals2[i]);
    return output;
  };

  template<typename T>
  inline double euclidean2_distance(T* vals1, T* vals2, int nbVals)
  {
    double output = 0;
    int i;
    for (i = 0; i < nbVals; i ++) {
      T tmp = vals1[i] - vals2[i];
      output += static_cast<double>(tmp*tmp);
    }
    return output;
  };

  template<typename T>
  cv::Mat get_distances_float_opt(cv::Mat features, float sigma, cv::Mat points, double boostThreshold)
  {
    static cv::Mat output(cv::Size(features.rows, features.rows), CV_64FC1);
    if (output.rows != features.rows)
      output = Mat(cv::Size(features.rows, features.rows), CV_64FC1);

    double* tmp = new double[features.cols];
    float* tmpP = new float[points.cols];
    float* ptrP = points.ptr<float>();

    double sigma_carre = 2 * sigma*sigma;

    double* ptr = output.ptr<double>();
    T* val = features.ptr<T>();
    int nbFeatures = features.cols;
    int totalSwitch = 0;
    for (int i = 0; i < output.rows; i++)
    {
      int realAdress = i*output.cols;
      ptr[realAdress + i] = 1;

      T* val1 = &val[i*features.cols];
      int nbCopy = 1;
      for (int j = i + 1; j < output.cols; j++)
      {
        double test = ptr[realAdress + j] = exp(
          -euclidean2_distance(val1, &val[j*nbFeatures], nbFeatures)
          / sigma_carre);
        if (test > boostThreshold)//points are the same!
        {
          if (i + nbCopy < j)
          {
            //switch (i+nbCopy)th line with jth feature:
            memcpy(tmp, &val[j*nbFeatures], sizeof(T)*nbFeatures);
            memcpy(&val[j*nbFeatures], &val[(i + nbCopy)*nbFeatures], sizeof(T)*nbFeatures);
            memcpy(&val[(i + nbCopy)*nbFeatures], tmp, sizeof(T)*nbFeatures);

            //switch also the points
            float* pt1 = &ptrP[j*points.cols];
            float* pt2 = &ptrP[(i + nbCopy)*points.cols];
            memcpy(tmpP, pt1, sizeof(float)*points.cols);
            memcpy(pt1, pt2, sizeof(float)*points.cols);
            memcpy(pt2, tmpP, sizeof(float)*points.cols);

            //also switch the value of previously computed distance:
            for (int cpt = 0; cpt <= i; cpt++)
              std::swap(ptr[cpt*output.cols + j], ptr[cpt*output.cols + i + nbCopy]);

          }
          nbCopy++;
        }
      }
      //copy the distances of ith point:
      for (int cpt = 1; cpt < nbCopy; cpt++)
        memcpy(&ptr[realAdress + cpt*output.cols + i + cpt], &ptr[realAdress + i + cpt], sizeof(double)*(output.cols - (i + cpt)));

      nbCopy--;
      i += nbCopy;//skip the same points...
      totalSwitch += nbCopy;
    }
    delete[] tmp;
    delete[] tmpP;
    cv::Mat cleanImg;
    output.convertTo(cleanImg, CV_8UC1, 512);
    cv::imwrite("avecOpt.bmp", cleanImg);
    return output;
  }

  cv::Mat get_distances_binary_opt(cv::Mat features, double mu, int nbBits, cv::Mat points, double boostThreshold)
  {
    static cv::Mat output(cv::Size(features.rows, features.rows), CV_64FC1);
    if (output.rows != features.rows)
      output = Mat(cv::Size(features.rows, features.rows), CV_64FC1);

    double* tmp = new double[features.cols];
    float* tmpP = new float[points.cols];
    float* ptrP = points.ptr<float>();

    double* ptr = output.ptr<double>();
    char* val = features.ptr<char>();

    int nbFeatures = features.cols;
    int sizeFeatures = features.cols * 8;
    int totalSwitch = 0;
    for (int i = 0; i < output.rows; i++)
    {
      int realAdress = i*output.cols;
      ptr[realAdress + i] = 1;

      char* val1 = &val[i*features.cols];
      int nbCopy = 1;
      for (int j = i + 1; j < output.cols; j++)
      {
        double distTmp = hamming_distance(
          val1, &val[j*features.cols], nbFeatures);

        double test = ptr[realAdress + j] = (pow(mu, (int)distTmp) * pow(1. - mu, (int)(nbBits - distTmp)));

        if (distTmp / nbBits > boostThreshold)//points are the same!
        {
          if (i + nbCopy < j)
          {
            //switch (i+nbCopy)th line with jth feature:
            memcpy(tmp, &val[j*nbFeatures], nbFeatures);
            memcpy(&val[j*nbFeatures], &val[(i + nbCopy)*nbFeatures], nbFeatures);
            memcpy(&val[(i + nbCopy)*nbFeatures], tmp, nbFeatures);

            //switch also the points
            float* pt1 = &ptrP[j*points.cols];
            float* pt2 = &ptrP[(i + nbCopy)*points.cols];
            memcpy(tmpP, pt1, sizeof(float)*points.cols);
            memcpy(pt1, pt2, sizeof(float)*points.cols);
            memcpy(pt2, tmpP, sizeof(float)*points.cols);

            //also switch the value of previously computed distance:
            for (int cpt = 0; cpt <= i; cpt++)
              std::swap(ptr[cpt*output.cols + j], ptr[cpt*output.cols + i + nbCopy]);
          }
          nbCopy++;
        }
      }
      //copy the distances of ith point:
      for (int cpt = 1; cpt < nbCopy; cpt++)
        memcpy(&ptr[realAdress + cpt*output.cols + i + cpt], &ptr[realAdress + i + cpt], sizeof(double)*(output.cols - (i + cpt)));

      nbCopy--;
      i += nbCopy;//skip the same points...
      totalSwitch += nbCopy;
    }
    delete[] tmp;
    delete[] tmpP;
    return output;
  }

  template<typename T>
  cv::Mat get_distances_float(cv::Mat features, float sigma)
  {
    static cv::Mat output(cv::Size(features.rows, features.rows), CV_64FC1);
    if (output.rows != features.rows)
      output = Mat(cv::Size(features.rows, features.rows), CV_64FC1);

    float sigma_carre = 2 * sigma*sigma;

    double* ptr = output.ptr<double>();
    T* val = features.ptr<T>();
    int nbFeatures = features.cols;
    for (int i = 0; i < features.rows; i++)
    {
      int realAdress = i*features.rows;
      ptr[realAdress + i] = 0;//diagonal distance is null...
      T* val1 = &val[i*features.cols];
      for (int j = i + 1; j < features.rows; j++)
      {
        ptr[realAdress + j] = exp(
          -euclidean2_distance(val1, &val[j*features.cols], nbFeatures)
          / sigma_carre);
      }
    }
    return output;
  }

  cv::Mat get_distances_binary(cv::Mat features, double mu, int nbBits)
  {
    static cv::Mat output(cv::Size(features.rows, features.rows), CV_64FC1);
    if (output.rows != features.rows)
      output = Mat(cv::Size(features.rows, features.rows), CV_64FC1);
    output.setTo(0);

    double* ptr = output.ptr<double>();
    char* val = features.ptr<char>();
    int nbFeatures = features.cols;
    for (int i = 0; i < features.rows; i++)
    {
      int realAdress = i*features.rows;
      char* val1 = &val[i*features.cols];
      for (int j = i + 1; j < features.rows; j++)
      {
        double dist = hamming_distance(
          val1, &val[j*features.cols], nbFeatures);

        ptr[realAdress + j] = (pow(mu, (int)dist)) * (pow(1. - mu, (int)(nbBits - dist)));
      }
    }
    return output;
  }

  std::vector<double> get_criterions(cv::Mat distances)
  {
    std::vector<double> output;
    for (int i = 0; i < distances.cols; i++)
      output.push_back(0);
    //double divisor = 1.;// (1. / ((distances.rows - 1.) * pow((sigma*sqrt(2 * CV_PI)), D)));

    for (int i = 0; i < distances.cols; i++)
    {
      double* dist_i = distances.ptr<double>(i);
      double crit_i = 0;
      for (int j = i + 1; j < distances.rows; j++)
      {
        double crit_i = dist_i[j];
        output[i] += crit_i;
        output[j] += crit_i;
      }
    }

    return output;
  };

  double get_threshold_float(double p, double D, double sigma, double N)
  {
    double gamma = 2.*pow(boost::math::erf_inv(2 * p - 1), 2);
    double v = sigma * sigma * (D + 2. * sqrt(gamma*(D - gamma))) / (D - 2 * gamma);
    double Ci = pow(2. * CV_PI * v, -D / 2.);

    return Ci*(N - 1)*pow(sqrt(CV_PI*2.) * sigma, D);
  }

  double get_threshold_binary(double p, double D, double mu, double N)
  {
    double v = 0;
    double gamma = 2.*pow(boost::math::erf_inv(2 * p - 1), 2);

    if (p < 0.5)
      v = (2 * mu*D + gamma + sqrt(gamma * (8 * mu * D + gamma))) / (2.*D);
    if (p > 0.5)
      v = (2 * mu*D + gamma - sqrt(gamma * (8 * mu * D + gamma))) / (2.*D);
    if (p == 0.5)
      v = mu;

    double Ci = pow(1. - v, D);

    return Ci;
  }

  BLOCK_END_INSTANTIATION(CORE_filter, AlgoType::imgProcess, BLOCK__CORE_NAME);

  BEGIN_BLOCK_INPUT_PARAMS(CORE_filter);
  //Add parameters, with following parameters:
  ADD_PARAMETER(toBeLinked, Matrix, "BLOCK__CORE_IN_POINTS", "BLOCK__CORE_IN_POINTS_HELP");
  ADD_PARAMETER(toBeLinked, Matrix, "BLOCK__CORE_IN_DESC", "BLOCK__CORE_IN_DESC_HELP");
  ADD_PARAMETER_FULL(false, Float, "BLOCK__CORE_IN_THRESHOLD", "BLOCK__CORE_IN_THRESHOLD_HELP", 90.f);
  ADD_PARAMETER_FULL(false, Float, "BLOCK__CORE_IN_OPTIM_THRESHOLD", "BLOCK__CORE_IN_OPTIM_THRESHOLD_HELP", .75f);
  END_BLOCK_PARAMS();

  BEGIN_BLOCK_OUTPUT_PARAMS(CORE_filter);
  ADD_PARAMETER(toBeLinked, Matrix, "BLOCK__CORE_OUT_POINTS", "BLOCK__CORE_OUT_POINTS_HELP");
  ADD_PARAMETER(toBeLinked, Matrix, "BLOCK__CORE_OUT_DESC", "BLOCK__CORE_OUT_DESC_HELP");
  END_BLOCK_PARAMS();

  BEGIN_BLOCK_SUBPARAMS_DEF(CORE_filter);
  END_BLOCK_PARAMS();

  CORE_filter::CORE_filter() :Block("BLOCK__CORE_NAME", true){
    _myInputs["BLOCK__CORE_IN_POINTS"].addValidator({ new ValNeeded() });
    _myInputs["BLOCK__CORE_IN_THRESHOLD"].addValidator({ new ValRange(0,100) });
  };

  template <typename T>
  vector<size_t> sort_indexes(const vector<T> &v) {

    // initialize original index locations
    vector<size_t> idx(v.size());
    for (size_t i = 0; i != idx.size(); ++i) idx[i] = i;

    // sort indexes based on comparing values in v
    std::sort(idx.begin(), idx.end(),
      [&v](size_t i1, size_t i2) {return v[i1] < v[i2]; });

    return idx;
  }

  bool CORE_filter::run(bool oneShot){
    double percent = _myInputs["BLOCK__CORE_IN_THRESHOLD"].get<double>() / 100.;
    double percentOpt = _myInputs["BLOCK__CORE_IN_OPTIM_THRESHOLD"].get<double>();
    cv::Mat points = _myInputs["BLOCK__CORE_IN_POINTS"].get<cv::Mat>().clone();
    cv::Mat desc = _myInputs["BLOCK__CORE_IN_DESC"].get<cv::Mat>().clone();
    if (points.empty() || desc.empty())
    {
      _myOutputs["BLOCK__CORE_OUT_POINTS"] = points;
      _myOutputs["BLOCK__CORE_OUT_DESC"] = desc;
      return true;//not a problem, just nothing to produce...
    }
    int nbChanels = points.channels();
    if (nbChanels != 1)
      points = points.reshape(1, points.rows);
    if (points.depth() != CV_32F)
      points.convertTo(points, CV_32F);

    float sigma = 32.135f;
    double mu = 0.1;

    int dataSize = 8 * desc.cols;
    bool isBinary = true, isFloat = false;
    if (desc.depth() == CV_16U || desc.depth() == CV_16S)
      dataSize = 16 * desc.cols;
    if (desc.depth() == CV_32S)
      dataSize = 32 * desc.cols;
    if (desc.depth() == CV_32F)
    {
      dataSize = desc.cols;//we just need the number of values, not the number of bytes...
      isBinary = false;
      isFloat = true;
    }
    if (desc.depth() == CV_64F)
    {
      dataSize = desc.cols;//we just need the number of values, not the number of bytes...
      isBinary = false;
      isFloat = false;
    }
    std::vector<double> crit;
    cv::Mat distances;
    if (isBinary)
    {
      if (percentOpt >= 1)
        distances = get_distances_binary(desc, mu, dataSize);
      else
      {
        auto tick = boost::posix_time::microsec_clock::local_time();/*
        distances = get_distances_binary_opt(desc, 0.1, dataSize, points, percentOpt);
        distances = get_distances_binary_opt(desc, 0.1, dataSize, points, percentOpt);
        distances = get_distances_binary_opt(desc, 0.1, dataSize, points, percentOpt);
        distances = get_distances_binary_opt(desc, 0.1, dataSize, points, percentOpt);
        distances = get_distances_binary_opt(desc, 0.1, dataSize, points, percentOpt);*/
        distances = get_distances_binary_opt(desc, mu, dataSize, points, percentOpt);
        auto now = boost::posix_time::microsec_clock::local_time();
        static long long elapsed = 0;
        static int nbEchantillons = 0;
        nbEchantillons++;
        elapsed += (now - tick).total_microseconds();

        tick = boost::posix_time::microsec_clock::local_time();
        cv::Mat distances1 = get_distances_binary(desc, mu, dataSize);/*
        distances1 = get_distances_binary(desc, 0.1, dataSize);
        distances1 = get_distances_binary(desc, 0.1, dataSize);
        distances1 = get_distances_binary(desc, 0.1, dataSize);
        distances1 = get_distances_binary(desc, 0.1, dataSize);
        distances1 = get_distances_binary(desc, 0.1, dataSize);*/
        now = boost::posix_time::microsec_clock::local_time();
        static long long elapsed1 = 0;
        elapsed1 += (now - tick).total_microseconds();
        std::cout << elapsed / nbEchantillons << " sans optim : " << elapsed1 / nbEchantillons << " gain : " << elapsed * 100 / elapsed1 << std::endl;
        distances1.ptr<double>();
      }
    }
    else
    {
      if (percentOpt >= 1)
      {
        if (isFloat)
          distances = get_distances_float<float>(desc, sigma);
        else
          distances = get_distances_float<double>(desc, sigma);
      }
      else
      {
        if (isFloat)
        {
          auto tick = boost::posix_time::microsec_clock::local_time();
          distances = get_distances_float_opt<float>(desc, sigma, points, percentOpt);/*
          distances = get_distances_float_opt<float>(desc, sigma, points, percentOpt);
          distances = get_distances_float_opt<float>(desc, sigma, points, percentOpt);
          distances = get_distances_float_opt<float>(desc, sigma, points, percentOpt);
          distances = get_distances_float_opt<float>(desc, sigma, points, percentOpt);
          distances = get_distances_float_opt<float>(desc, sigma, points, percentOpt);*/
          auto now = boost::posix_time::microsec_clock::local_time();
          auto elapsed = (now - tick).total_microseconds();

          tick = boost::posix_time::microsec_clock::local_time();
          cv::Mat distances1 = get_distances_float<float>(desc, sigma);/*
          distances1 = get_distances_float<float>(desc, sigma);
          distances1 = get_distances_float<float>(desc, sigma);
          distances1 = get_distances_float<float>(desc, sigma);
          distances1 = get_distances_float<float>(desc, sigma);
          distances1 = get_distances_float<float>(desc, sigma);*/
          now = boost::posix_time::microsec_clock::local_time();
          auto elapsed1 = (now - tick).total_microseconds();
          std::cout << elapsed << " sans optim : " << elapsed1 << " gain : " << elapsed * 100 / elapsed1 << std::endl;
          distances1.ptr<double>();
        }
        else
          distances = get_distances_float_opt<double>(desc, sigma, points, percentOpt);
      }
    }
    crit = get_criterions(distances);

    std::vector<size_t> indices = sort_indexes(crit);

    int nbVals = 0;
    if (!isBinary)
    {
      //first count the correct values:
      double threshold = get_threshold_float(percent, desc.cols, sigma, crit.size());
      for (size_t i = 0; i < crit.size(); i++) {
        if (crit[i] < threshold)
          nbVals++;
        else
          break;
      }
    }
    else
    {
      //first count the correct values:
      double threshold = get_threshold_binary(percent, desc.cols, mu, crit.size());
      for (size_t i = 0; i < crit.size(); i++) {
        if (crit[i] < threshold)
          nbVals++;
        else
          break;
      }
    }
    //nbVals = (int)(points.rows*percent);

    cv::Mat outPoints(cv::Size(points.cols, nbVals), points.type());
    cv::Mat outDesc(cv::Size(desc.cols, nbVals), desc.type());
    for (int i = 0; i < nbVals; i++) {
      int bestIndice = indices[i];
      memcpy(outPoints.ptr<float>(i), points.ptr<float>(bestIndice), points.cols*sizeof(float));

      if (desc.depth() == CV_8U || desc.depth() == CV_8S)
        memcpy(outDesc.ptr<char>(i), desc.ptr<char>(bestIndice), desc.cols*sizeof(char));

      if (desc.depth() == CV_16U || desc.depth() == CV_16S)
        memcpy(outDesc.ptr<short>(i), desc.ptr<short>(bestIndice), desc.cols*sizeof(short));

      if (desc.depth() == CV_32S || desc.depth() == CV_32F)
        memcpy(outDesc.ptr<float>(i), desc.ptr<float>(bestIndice), desc.cols*sizeof(float));

      if (desc.depth() == CV_64F)
        memcpy(outDesc.ptr<double>(i), desc.ptr<double>(bestIndice), desc.cols*sizeof(double));
    }
    _myOutputs["BLOCK__CORE_OUT_POINTS"] = outPoints;
    _myOutputs["BLOCK__CORE_OUT_DESC"] = outDesc;

    return true;
  };
};


#ifndef __ENVIRE_CORE_ALIGNED_BOUNDING_BOX__
#define __ENVIRE_CORE_ALIGNED_BOUNDING_BOX__

#include <Eigen/Geometry>
#include <boost/intrusive_ptr.hpp>
#include <envire_core/BoundingVolume.hpp>

namespace envire { namespace core
{

    class AlignedBoundingBox : public BoundingVolume
    {
    protected:
        Eigen::AlignedBox<double,3> box;

    public:
        AlignedBoundingBox();
        AlignedBoundingBox(const Eigen::AlignedBox<double,3>& box);
        void extend(const Eigen::Vector3d& point);
        void extend(const boost::intrusive_ptr<BoundingVolume>& bv);
        bool intersects(const boost::intrusive_ptr<BoundingVolume>& bv) const;
        boost::intrusive_ptr<BoundingVolume> intersection(const boost::intrusive_ptr<BoundingVolume>& bv) const;
        bool contains(const Eigen::Vector3d& point) const;
        bool contains(const boost::intrusive_ptr<BoundingVolume>& bv) const;
        double exteriorDistance(const Eigen::Vector3d& point) const;
        double exteriorDistance(const boost::intrusive_ptr<BoundingVolume>& bv) const;
        Eigen::Vector3d center() const;
        void clear();

    };

}}

#endif


#include <boost/graph/small_world_generator.hpp>


//
//  vpgl_perspective_camera.cpp
//  calib
//
//  Created by jimmy on 2017-07-26.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#include "perspective_camera.h"
#include <Eigen/Geometry>

namespace cvx {
    perspective_camera::perspective_camera()
    {

    }
    perspective_camera::~perspective_camera()
    {

    }

    perspective_camera::perspective_camera( const perspective_camera& other )
    {
        if (&other == this) {
            return;
        }
        K_ = other.K_;
        camera_center_ = other.camera_center_;
        R_ = other.R_;
    }

    void perspective_camera::set_calibration(const double fl, const double u, const double v)
    {
        K_.fill(0.0);
        K_(0, 0) = K_(1, 1) = fl;
        K_(0, 2) = u;
        K_(1, 2) = v;
        K_(2, 2) = 1.0;
    }

    void perspective_camera::set_calibration( const Eigen::Matrix3d& K )
    {
        K_ = K;
    }
    void perspective_camera::set_camera_center( const Eigen::Vector3d& camera_center )
    {
        camera_center_ = camera_center;

    }
    void perspective_camera::set_translation(const Eigen::Vector3d& t)
    {
        Eigen::Matrix3d Rt = R_.transpose();
        Eigen::Vector3d cv = -(Rt * t);
        camera_center_ = cv;
    }

    void perspective_camera::set_rotation(const Eigen::Vector3d& rvector)
    {
        //@todo
        //printf("This function is not tested\n");
        double mag = rvector.norm();
        Eigen::Quaternion<double> q;
        if (mag > 0.0) {
            Eigen::Vector3d r = rvector/mag;
            Eigen::AngleAxisd aa(mag, r);
            q = Eigen::Quaternion<double>(aa);
        }
        else { // identity rotation is a special case
            q = Eigen::Quaternion<double>(1, 0, 0, 0);
        }
        R_ = q.normalized().toRotationMatrix();
    }

    void perspective_camera::set_rotation( const Eigen::Matrix3d& R )
    {
        R_ = R;
    }
}


#include <boost/python/class.hpp>
#include <boost/python/module.hpp>

#include <boost/math/constants/constants.hpp>

#include <string>
#include <utility>

namespace py = boost::python;

namespace
{
    auto constexpr pi = boost::math::constants::pi<double>();

    struct some_class_t final
    {
        explicit some_class_t(std::string n)
            : name(std::move(n))
            , number_(0.0)
        {
        }

        void set_number(double const n)
        {
            number_ = n > pi ? -1 : n;
        }

        double get_number() const
        {
            return number_;
        }

        std::string name;

    private:
        double number_;
    };
}

// clang-format off
BOOST_PYTHON_MODULE(MODULE_NAME) // NOLINT
{
   py::class_<some_class_t>("SomeClass", py::init<std::string>())
      .def_readwrite("name", &some_class_t::name)
      .add_property("number", &some_class_t::get_number, &some_class_t::set_number)
      ;
}
// clang-format on


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * 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.
 */

///
/// \file fast_esprit.hpp
///
#ifndef MXPFIT_ESPRIT_HPP
#define MXPFIT_ESPRIT_HPP

#include <algorithm>
#include <type_traits>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>

#include <mxpfit/exponential_sum.hpp>
#include <mxpfit/prony_like_method_common.hpp>

namespace mxpfit
{
///
/// ### ESPRIT
///
/// \brief ESPRIT method for finding parameters of exponential sum
/// approximation from sampled data on uniform grid.
///
/// \tparam T  Scalar type of function values.
///
/// #### Description
///
/// This class implements the Estimation of Signal Parameters via Rotational
/// Invariance Techniques (ESPRIT) method for parameter estimation of decaying
/// exponential sum funcitons.
///
/// This class is implemented only for the benchmarking purpose to compare with
/// the fast ESPRIT algorithm. DO NOT use the class for practical applications,
/// use `FastESPRIT` class instead, which is much faster than ESPRIT.
///
///
/// #### References
///
/// 1. D. Potts and M. Tasche,"Parameter estimation for nonincreasing
///    exponential sums by Prony-like methods", Linear Algebra Appl. **439**
///    (2013) 1024-1039.
///    [DOI: https://doi.org/10.1016/j.laa.2012.10.036]
///
template <typename T>
class ESPRIT
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using Index         = Eigen::Index;

    using Vector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVector    = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using ComplexVector = Eigen::Matrix<ComplexScalar, Eigen::Dynamic, 1>;

    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using RealMatrix =
        Eigen::Matrix<RealScalar, Eigen::Dynamic, Eigen::Dynamic>;
    using ComplexMatrix =
        Eigen::Matrix<ComplexScalar, Eigen::Dynamic, Eigen::Dynamic>;

    using ResultType =
        typename detail::gen_prony_like_method_result<T>::ResultType;

private:
    enum
    {
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex,
        Alignment = Eigen::internal::traits<Matrix>::Alignment
    };

    Index m_rows;
    Index m_cols;
    Index m_max_terms;
    Matrix m_matH;

public:
    ///
    /// Default constructor
    ///
    ESPRIT() = default;

    ///
    /// Constructor with memory preallocation
    ///
    /// \param[in] N  Number of sampling points
    /// \param[in] L  Window size. This is equals to the number of rows of
    ///               generalized Hankel matrix.
    /// \param[in] M  Maxumum number of terms used for the exponential sum.
    /// \pre  `N >= M >= 1` and `N - L + 1 >= M >= 1`.
    ///
    ESPRIT(Index N, Index L, Index M)
        : m_rows(L), m_cols(N - L + 1), m_max_terms(M), m_matH(m_rows, m_cols)
    {
        assert(m_rows >= M && m_cols >= M && M >= 1);
    }

    ///
    /// Destructor
    ///
    ~ESPRIT()
    {
    }

    ///
    /// Memory reallocation
    ///
    /// \param[in] N  Number of sampling points
    /// \param[in] L  Window size. This is equals to the number of rows of
    ///               generalized Hankel matrix.
    /// \param[in] M  Maxumum number of terms used for the exponential sum.
    /// \pre  `N >= M >= 1` and `N - L + 1 >= M >= 1`.
    ///
    void resize(Index N, Index L, Index M)
    {
        m_rows = L;
        m_cols = N - L + 1;
        assert(m_rows >= M && m_cols >= M && M >= 1);
        m_max_terms = M;
        m_matH.resize(m_rows, m_cols);
    }

    ///
    /// \return Number of sampling points.
    ///
    Index size() const
    {
        return m_rows + m_cols - 1;
    }

    ///
    /// Fit signals by a exponential sum
    ///
    /// \param[in] f The array of signals sampled on the equispaced grid. The
    ///    first `size()` elemnets of `nterms` are used as a sampled data. In
    ///    case `f.size() < size()` then, last `size() - f.size()` elements are
    ///    padded by zeros.
    /// \param[in] eps  Small positive number `(0 < eps < 1)` that
    ///    controlls the accuracy of the fit.
    /// \param[in] x0  Argument of first sampling point
    /// \param[in] delta Spacing between neighboring sample points.
    ///
    template <typename VectorT>
    ResultType compute(const Eigen::MatrixBase<VectorT>& h, RealScalar x0,
                       RealScalar delta, RealScalar eps);
};

template <typename T>
template <typename VectorT>
typename ESPRIT<T>::ResultType
ESPRIT<T>::compute(const Eigen::MatrixBase<VectorT>& h, RealScalar x0,
                   RealScalar delta, RealScalar eps)
{
    assert(h.size() == size() && "Number of data points mismatch.");
    //
    // Form rectangular Hankel matrix from sequance h.
    //
    for (Index j = 0; j < m_cols; ++j)
    {
        for (Index i = 0; i < m_rows; ++i)
        {
            m_matH.coeffRef(i, j) = h(i + j);
        }
    }
    // const Index nr = m_matH.rows();
    const Index nc = m_matH.cols();

    //-------------------------------------------------------------------------
    // Compute roots of Prony polynomials H = U * S * W^H
    //-------------------------------------------------------------------------
    Eigen::BDCSVD<Matrix> svd(m_matH, Eigen::ComputeThinV);

    const auto& sigma = svd.singularValues();
    Index nterms      = 0;
    // extract rank of matrix H from singular values
    while (nterms < m_max_terms)
    {
        if (sigma(nterms) < eps)
        {
            break;
        }
        ++nterms;
    }

    if (nterms == 0)
    {
        return ResultType();
    }

    ComplexVector roots(nterms);
    ComplexVector weights(nterms);

    {
        // --- Form the views of matrix W
        // Matrix W excluding the last row
        auto W0 = svd.matrixV().block(0, 0, nc - 1, nterms);
        // Matrix W excluding the first row
        auto W1 = svd.matrixV().block(1, 0, nc - 1, nterms);
        // adjoint of the last row of matrix W
        auto nu = svd.matrixV().block(nc - 1, 0, 1, nterms).adjoint();
        //
        // Compute the spectral matrix G = pinv(W0) * W1, where pinv indicate
        // the Moore-Penrose pseudo-inverse. The computation of the
        // pseudo-inverse of W0 can be avoided.
        //
        Matrix G(W0.adjoint() * W1);
        Vector phi(G.adjoint() * nu);
        auto scal = RealScalar(1) / (RealScalar(1) - nu.squaredNorm());
        G += scal * nu * phi.adjoint();
        //
        // Prony roots \f$\{z_i\{\}\f$ are the eigenvalues of matrix G. The
        // exponents for approximation are obtained as \f$ \log z_i \f$
        //
        roots = G.eigenvalues();
    }

    //----------------------------------------------------------------------
    // Solve overdetermined Vandermonde system to obtain the weights
    //----------------------------------------------------------------------
    {
        // Create Vandermonde matrix from prony roots
        ComplexMatrix matV(h.size(), nterms);
        for (Index j = 0; j < matV.cols(); ++j)
        {
            auto x     = roots(j);
            auto v     = ComplexScalar(1);
            matV(0, j) = v;
            for (Index i = 1; i < matV.rows(); ++i)
            {
                v *= x;
                matV(i, j) = v;
            }
        }
        // Solve least-squares problem V x = h
        weights = matV.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV)
                      .solve(h.template cast<ComplexScalar>());
    }

    // Rescale the exponents and weights, and return the result
    return detail::gen_prony_like_method_result<T>::create(
        roots.array(), weights.array(), x0, delta);
}

} // namespace mxpfit

#endif /* MXPFIT_ESPRIT_HPP */


#include <gtest/gtest.h>
#include "scheme/util/template_loop.hh"
#include <iostream>
#include <vector>
#include <boost/function.hpp>
#include <boost/bind.hpp>

#include "scheme/util/SimpleArray.hh"

namespace scheme {
namespace util {

using std::cout;
using std::endl;

void testfunc(SimpleArray<3,int> const & /*a*/){}

TEST(NESTED_FOR,interface_test){
    SimpleArray<3,int> lb(0,0,3), ub(4,2,3);
    NESTED_FOR<3>(lb,ub,testfunc);
}

template<class Indices>
struct TestFun {
    size_t sum,ncalls;
    TestFun() : sum(0),ncalls(0) {}
    void func_to_call(int dummyi, int dummyj, Indices const & indices){
        // std::cerr << "func_to_call " << dummyi << " " << dummyj << " " << indices.transpose() << std::endl;
        sum += indices.sum() + dummyi + dummyj;
        ++ncalls;
    }
};

TEST(NESTED_FOR,boost_bind_functor){
    static const size_t DIM=2;
    typedef SimpleArray<DIM,int> IDX;
    IDX lb(0,0),ub(4,2);
    {
        TestFun<IDX> mytest;
        boost::function<void(IDX)> functor = boost::bind( &TestFun<IDX>::func_to_call, &mytest, 0, 0, _1 );
        NESTED_FOR<DIM>( lb, ub, functor );
        ASSERT_EQ( mytest.ncalls, (ub-lb+1).prod() );
        ASSERT_EQ( mytest.sum   , 45 );
    }
    {
        TestFun<IDX> mytest;
        boost::function<void(IDX)> functor = boost::bind( &TestFun<IDX>::func_to_call, &mytest, 18, 329, _1 );
        NESTED_FOR<DIM>( lb, ub, functor );
        ASSERT_EQ( mytest.ncalls, (ub-lb+1).prod() );
        ASSERT_EQ( mytest.sum   , 5250 );
    }
}

template<class Indices>
struct RecordCalls {
    // std::vector<Indices,Eigen::aligned_allocator<Indices> > calls;
    std::vector<Indices> calls;
    void operator()(Indices const & i){
        calls.push_back(i);
    }
};

TEST(NESTED_FOR,check_all_output){
    {
        static const size_t DIM=1;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0),ub(4);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }
    }

    {
        static const size_t DIM=2;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10),ub(4,-4 );
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}
    }

    {
        static const size_t DIM=3;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3),ub(4,-4 ,3);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}
    }

    {
        static const size_t DIM=4;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3,6),ub(4,-4 ,7,8);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[3] = lb[3]; idx[3] <= ub[3]; ++idx[3]){
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}}
    }

    {
        static const size_t DIM=5;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3,6,2),ub(4,-4 ,7,8,4);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[4] = lb[4]; idx[4] <= ub[4]; ++idx[4]){
        for(idx[3] = lb[3]; idx[3] <= ub[3]; ++idx[3]){
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}}}
    }

    {
        static const size_t DIM=6;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3,6,2,7),ub(4,-4 ,7,8,4,8);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[5] = lb[5]; idx[5] <= ub[5]; ++idx[5]){
        for(idx[4] = lb[4]; idx[4] <= ub[4]; ++idx[4]){
        for(idx[3] = lb[3]; idx[3] <= ub[3]; ++idx[3]){
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}}}}
    }

    {
        static const size_t DIM=7;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3,6,2,7,0),ub(4,-4 ,7,8,4,8,1);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[6] = lb[6]; idx[6] <= ub[6]; ++idx[6]){
        for(idx[5] = lb[5]; idx[5] <= ub[5]; ++idx[5]){
        for(idx[4] = lb[4]; idx[4] <= ub[4]; ++idx[4]){
        for(idx[3] = lb[3]; idx[3] <= ub[3]; ++idx[3]){
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}}}}}
    }

    {
        static const size_t DIM=8;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3,6,2,7,0,34),ub(4,-4 ,7,8,4,8,1,35);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[7] = lb[7]; idx[7] <= ub[7]; ++idx[7]){
        for(idx[6] = lb[6]; idx[6] <= ub[6]; ++idx[6]){
        for(idx[5] = lb[5]; idx[5] <= ub[5]; ++idx[5]){
        for(idx[4] = lb[4]; idx[4] <= ub[4]; ++idx[4]){
        for(idx[3] = lb[3]; idx[3] <= ub[3]; ++idx[3]){
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}}}}}}
    }

    {
        static const size_t DIM=9;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3,6,2,7,0,34,1),ub(2,-7 ,7,8,4,8,1,35,2);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[8] = lb[8]; idx[8] <= ub[8]; ++idx[8]){
        for(idx[7] = lb[7]; idx[7] <= ub[7]; ++idx[7]){
        for(idx[6] = lb[6]; idx[6] <= ub[6]; ++idx[6]){
        for(idx[5] = lb[5]; idx[5] <= ub[5]; ++idx[5]){
        for(idx[4] = lb[4]; idx[4] <= ub[4]; ++idx[4]){
        for(idx[3] = lb[3]; idx[3] <= ub[3]; ++idx[3]){
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}}}}}}}
    }

    {
        static const size_t DIM=10;
        typedef SimpleArray<DIM,int> IDX;
        IDX lb(0,-10,3,6,2,7,0,34,1,12),ub(2,-8 ,5,8,4,8,1,35,2,13);
        RecordCalls<IDX> recorder;
        NESTED_FOR<DIM>( lb, ub, recorder );
        ASSERT_EQ( recorder.calls.size(), (ub-lb+1).prod() );
        size_t count = 0;
        IDX idx;
        for(idx[9] = lb[9]; idx[9] <= ub[9]; ++idx[9]){
        for(idx[8] = lb[8]; idx[8] <= ub[8]; ++idx[8]){
        for(idx[7] = lb[7]; idx[7] <= ub[7]; ++idx[7]){
        for(idx[6] = lb[6]; idx[6] <= ub[6]; ++idx[6]){
        for(idx[5] = lb[5]; idx[5] <= ub[5]; ++idx[5]){
        for(idx[4] = lb[4]; idx[4] <= ub[4]; ++idx[4]){
        for(idx[3] = lb[3]; idx[3] <= ub[3]; ++idx[3]){
        for(idx[2] = lb[2]; idx[2] <= ub[2]; ++idx[2]){
        for(idx[1] = lb[1]; idx[1] <= ub[1]; ++idx[1]){
        for(idx[0] = lb[0]; idx[0] <= ub[0]; ++idx[0]){
            for(size_t j=0; j < DIM; ++j){
                ASSERT_EQ( recorder.calls[count][j], idx[j] );
            }
            ++count;
        }}}}}}}}}}
    }

}

}
}


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file set_test.cpp
/// \brief Basic regression test for set expressions.

#define BOOST_TEST_MODULE set_test
#include <boost/test/included/unit_test_framework.hpp>

#include "mcrl2/data/parse.h"
#include "mcrl2/data/rewriter.h"
#include "mcrl2/data/set.h"

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::data::sort_set;
using namespace mcrl2::data::sort_fset;

template <typename Predicate>
void test_data_expression(const std::string& s, const variable_vector& v, Predicate p)
{
  std::cerr << "testing data expression " << s << std::endl;
  data_expression e = parse_data_expression(s, v);
  BOOST_CHECK(p(e));
}

void test_expression(const std::string& evaluate, const std::string& expected, data::rewriter r)
{
  const data_expression d1 = parse_data_expression(evaluate);
  const data_expression rd1 =r(d1);
  const data_expression d2 = parse_data_expression(expected);
  const data_expression rd2 =r(d2);

  if (rd1!=rd2)
  {
    std::cerr << "------------------------------------------------------\n";
    std::cerr << "Evaluating: " << evaluate << "\n";
    std::cerr << "Result: " << rd1 << "\n";
    std::cerr << "Expected result: " << expected << "\n";
    std::cerr << "Evaluated expected result " << rd2 << "\n";
    BOOST_CHECK(rd1 == rd2);
    std::cerr << "------------------------------------------------------\n";
  }
}

void set_expression_test()
{
  data::data_specification specification;

  specification.add_context_sort(sort_pos::pos());
  specification.add_context_sort(sort_set::set_(sort_pos::pos()));
  specification.add_context_sort(sort_set::set_(sort_bool::bool_()));

  data::rewriter normaliser(specification);

  variable_vector v;
  v.push_back(parse_variable("s:Set(Nat)"));

  test_data_expression("{x : Nat | x < 10}", v, sort_set::is_constructor_application);
  test_data_expression("!s", v, sort_set::is_complement_application);
  test_data_expression("s * {}", v, sort_set::is_intersection_application);
  test_data_expression("s * {1,2,3}", v, sort_set::is_intersection_application);
  test_data_expression("s - {3,1,2}", v, sort_set::is_difference_application);
  test_data_expression("1 in s", v, sort_set::is_in_application);
  test_data_expression("{} + s", v, sort_set::is_union_application);
  test_data_expression("(({} + s) - {20}) * {40}", v, sort_set::is_intersection_application);
  test_data_expression("{10} < s", v, is_less_application<data_expression>);
  test_data_expression("s <= {10}", v, is_less_equal_application<data_expression>);
  test_data_expression("{20} + {30}", v, sort_set::is_union_application);

  test_expression("{1,2}", "{2,1}", normaliser);

  data_expression t1d1a = parse_data_expression("{1,2,3}");
  data_expression t1d2a = parse_data_expression("{2,1}");
  BOOST_CHECK(normaliser(t1d1a) != normaliser(t1d2a));

  data_expression t2d1 = parse_data_expression("{1,2} == {1,2}");
  data_expression t2d2 = parse_data_expression("true");
  BOOST_CHECK(normaliser(t2d1) == normaliser(t2d2));

  data_expression t2d1a = parse_data_expression("{1,2,3} == {1,2,1}");
  data_expression t2d2a = parse_data_expression("false");
  BOOST_CHECK(normaliser(t2d1a) == normaliser(t2d2a));

  data_expression t3d1 = parse_data_expression("({1,2} != {2,3})");
  data_expression t3d2 = parse_data_expression("true");
  BOOST_CHECK(normaliser(t3d1) == normaliser(t3d2));

  data_expression t4d1 = parse_data_expression("(!{1,2}) == {1,2}");
  data_expression t4d2 = parse_data_expression("false");
  BOOST_CHECK(normaliser(t4d1) == normaliser(t4d2));

  data_expression t5d1 = parse_data_expression("(!!{1,2}) == {2,1}");
  data_expression t5d2 = parse_data_expression("true");
  BOOST_CHECK(normaliser(t5d1) == normaliser(t5d2));

  data_expression e = parse_data_expression("{20}", v);
  BOOST_CHECK(sort_fset::is_cons_application(normaliser(e)));

  e = parse_data_expression("{20, 30, 40}", v);
  BOOST_CHECK(sort_fset::is_cons_application(normaliser(e)));

  test_expression("{} == { b: Bool | true } - { true, false }","true",normaliser);

  test_expression("{ b: Bool | true } - { true, false } == {}","true", normaliser);

  // Check the operation == on sets.
  test_expression("{} == ({true} - {true})","true",normaliser);  // {true}-{true} is a trick to type {} == {}.
  test_expression("{} == {false}", "false",normaliser);
  test_expression("{} == {true}", "false",normaliser);
  test_expression("{true} == {true}", "true",normaliser);
  test_expression("{false} == {false}", "true",normaliser);
  test_expression("{true} == {false, true}", "false",normaliser);
  test_expression("{false, true} == {false}", "false",normaliser);
  test_expression("{false, true} == {true, false}", "true",normaliser);

  // Check the operation < on sets.
  test_expression("{} < ({true} - {true})","false",normaliser);  // {true}-{true} is a trick to type {} == {}.
  test_expression("{true} < {false}", "false",normaliser);
  test_expression("{false} < {true}", "false",normaliser);
  test_expression("{true} < {true}", "false",normaliser);
  test_expression("{false} < {false}", "false",normaliser);
  test_expression("{true} < {false, true}", "true",normaliser);
  test_expression("{false} < {false, true}", "true",normaliser);
  test_expression("{true} < {true, false}", "true",normaliser);
  test_expression("{false} < {true, false}", "true",normaliser);
  test_expression("{true, false} < {true}", "false",normaliser);
  test_expression("{true, false} < {false}", "false",normaliser);
  test_expression("{false, true} < {true}", "false",normaliser);
  test_expression("{false, true} < {false}", "false",normaliser);
  test_expression("{true, false} < {false, true}", "false",normaliser);
  test_expression("{true, false} < {true, false}", "false",normaliser);
  test_expression("{false, true} < {false, true}", "false",normaliser);
  test_expression("{false, true} < {true, false}", "false",normaliser);
  test_expression("{false, false} < {false}", "false",normaliser);

  // Check the operation <= on sets.
  test_expression("{} <= ({true}-{true})","true",normaliser);  // {true} - {true} is a trick to type {} == {}.
  test_expression("{true} <= {false}", "false",normaliser);
  test_expression("{false} <= {true}", "false",normaliser);
  test_expression("{true} <= {true}", "true",normaliser);
  test_expression("{false} <= {false}", "true",normaliser);
  test_expression("{true} <= {false, true}", "true",normaliser);
  test_expression("{false} <= {false, true}", "true",normaliser);
  test_expression("{true} <= {true, false}", "true",normaliser);
  test_expression("{false} <= {true, false}", "true",normaliser);
  test_expression("{true, false} <= {true}", "false",normaliser);
  test_expression("{true, false} <= {false}", "false",normaliser);
  test_expression("{false, true} <= {true}", "false",normaliser);
  test_expression("{false, true} <= {false}", "false",normaliser);
  test_expression("{true, false} <= {false, true}", "true",normaliser);
  test_expression("{true, false} <= {true, false}", "true",normaliser);
  test_expression("{false, true} <= {false, true}", "true",normaliser);
  test_expression("{false, true} <= {true, false}", "true",normaliser);
  test_expression("{false, false} <= {false}", "true",normaliser);

}

BOOST_AUTO_TEST_CASE(test_main)
{
  set_expression_test();
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_SIMD_LOGICAL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_SIMD_LOGICAL_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF(is_simd_logical_
                         , (typename A0,typename X)
                         , (detail::is_native<X>)
                         , bd::cpu_
                         , bs::pack_<bd::arithmetic_<A0>,X>
                         )
  {
    BOOST_FORCEINLINE bool operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0, signed>;
      iA0 tmp = bitwise_cast<iA0>(a0);
      return bs::all(logical_or(is_equal(tmp, Mone<iA0>()), is_eqz(tmp)));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF(is_simd_logical_
                         , (typename A0,typename X)
                         , (detail::is_native<X>)
                         , bd::cpu_
                         , bs::pack_<bs::logical_<A0>,X>
                         )

  {
    BOOST_FORCEINLINE bool operator()(A0 const&) const
    {
      return true;
    }
  };
} } }

#endif


//          Copyright Carl Philipp Reh 2009 - 2021.
// 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)

#include <fcppt/declare_strong_typedef.hpp>
#include <fcppt/make_ref.hpp>
#include <fcppt/strong_typedef.hpp>
#include <fcppt/algorithm/repeat.hpp>
#include <fcppt/random/variate.hpp>
#include <fcppt/random/distribution/basic.hpp>
#include <fcppt/random/distribution/parameters/uniform_int.hpp>
#include <fcppt/random/generator/minstd_rand.hpp>
#include <fcppt/random/generator/seed_from_chrono.hpp>
#include <fcppt/type_iso/boost_units.hpp>
#include <fcppt/type_iso/strong_typedef.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <iostream>
#include <fcppt/config/external_end.hpp>

int main()
{
  //![random_complex_distribution]
  using meter = boost::units::quantity<boost::units::si::length, int>;

  FCPPT_DECLARE_STRONG_TYPEDEF(meter, radius);

  using distribution = fcppt::random::distribution::basic<
      fcppt::random::distribution::parameters::uniform_int<radius>>;
  //![random_complex_distribution]

  using generator_type = fcppt::random::generator::minstd_rand;

  generator_type generator(fcppt::random::generator::seed_from_chrono<generator_type::seed>());

  using variate = fcppt::random::variate<generator_type, distribution>;

  //![random_complex_variate]
  variate rng(
      fcppt::make_ref(generator),
      distribution(
          distribution::param_type::min(radius(0 * boost::units::si::meter)),
          distribution::param_type::max(radius(10 * boost::units::si::meter))));
  //![random_complex_variate]

  //![random_complex_output]
  fcppt::algorithm::repeat(10U, [&rng] { std::cout << rng().get().value() << ' '; });
  //![random_complex_output]

  std::cout << '\n';

  using meter_distribution = fcppt::random::distribution::basic<
      fcppt::random::distribution::parameters::uniform_int<meter>>;

  using meter_variate = fcppt::random::variate<generator_type, meter_distribution>;

  meter_variate meter_rng(
      fcppt::make_ref(generator),
      meter_distribution(
          meter_distribution::param_type::min(0 * boost::units::si::meter),
          meter_distribution::param_type::max(10 * boost::units::si::meter)));

  fcppt::algorithm::repeat(10U, [&meter_rng] { std::cout << meter_rng().value() << ' '; });

  std::cout << '\n';
}