#include "model_distribution.hpp"

#include "monte_carlo_integration.hpp"
#include "multinomial.hpp"
#include "multivariate_guassian.hpp"
#include "mvi3/mvi3.hpp"
#include "types.hpp"
#include "utils.hpp"

#include <algorithm>
#include <boost/log/trivial.hpp>
#include <numeric>
#include <vector>

namespace FilterModel {
ModelDistribution::ModelDistribution(const std::vector<category_counts_t> &data,
                                     std::default_random_engine &generator, const Options &options)
    : data(data), generator(generator), options(options){};

std::vector<std::vector<double>> ModelDistribution::distribution(
    const std::vector<std::vector<alpha_t>> &alphas_per_object, double epsilon,
    const delta_t &delta) const {
    // See function definition for description.
    std::vector<std::vector<double>> log_alpha_likelyhoods_per_object(data.size(),
                                                                      std::vector<double>());
    for (int obs_index = 0; obs_index < data.size(); ++obs_index) {
        const category_counts_t &object_counts = data.at(obs_index);
        const std::vector<alpha_t> &alphas = alphas_per_object.at(obs_index);

        int n = std::accumulate(object_counts.begin(), object_counts.end(), 0);
        std::vector<double> p = {1 - epsilon, epsilon};
        Multinomial n_positive_distribution(n, p);

        for (int alpha_index = 0; alpha_index < alphas.size(); ++alpha_index) {
            const alpha_t &alpha = alphas.at(alpha_index);

            // n^+ goes from 0 to the sum of k_i for which alpha_i is true
            // When alpha_i is false, then all of the k_i corresponding must be noise reads.
            const category_counts_t filtered_object_counts = filter_by_alpha(alpha, object_counts);
            const int max_n_positive =
                std::accumulate(filtered_object_counts.begin(), filtered_object_counts.end(), 0);

            std::vector<double> log_p_k_n_positive_given_all_terms;
            // n_positive is the number of observations not made by the error process.
            for (int n_positive = 0; n_positive <= max_n_positive; ++n_positive) {
                // n_negative is the number of observations made by the error process.
                int n_negative = n - n_positive;
                double log_p_n_plus_given_n_epsilon =
                    n_positive_distribution.log_pdf(std::vector<int>({n_positive, n_negative}));

                double log_p_k_positive_given_alpha_n_positive =
                    calculate_log_p_k_positive_given_alpha_n_positive(n_positive, alpha);

                double log_sum_over_k_negative = calculate_log_sum_over_k_negative(
                    n_positive, n_negative, alpha, delta, object_counts);

                double log_p_k_n_positive_given_all = log_p_n_plus_given_n_epsilon +
                                                      log_p_k_positive_given_alpha_n_positive +
                                                      log_sum_over_k_negative;

                log_p_k_n_positive_given_all_terms.push_back(log_p_k_n_positive_given_all);
            }

            std::vector<double> p_k_n_positive_given_all =
                exp<double>(log_p_k_n_positive_given_all_terms);

            log_alpha_likelyhoods_per_object.at(obs_index).push_back(
                std::log(stable_sum<double>(p_k_n_positive_given_all)));
        }
    }
    return log_alpha_likelyhoods_per_object;
}

std::vector<std::vector<double>> ModelDistribution::distribution(const std::vector<alpha_t> &alphas,
                                                                 double epsilon,
                                                                 const delta_t &delta) const {
    std::vector<std::vector<alpha_t>> alphas_per_obs(data.size(), alphas);
    return distribution(alphas_per_obs, epsilon, delta);
}

double ModelDistribution::calculate_log_p_k_positive_given_alpha_n_positive(int n_positive,
                                                                            const alpha_t &alpha) {
    int sum_alpha = accumulate(alpha.begin(), alpha.end(), 0);

    // 1/(n^+ + sum alpha_i - 1 choose n^+)
    // lgamma(x + 1) = log(x!)
    double log_p_k_given_alpha_n_positive =
        lgamma(n_positive + 1) + lgamma(sum_alpha) - lgamma(n_positive + sum_alpha);
    return log_p_k_given_alpha_n_positive;
}

double ModelDistribution::calculate_log_sum_over_k_negative(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) const {
    // TODO(joschnie): Check for viability and return -1 if unviable.

    int n = n_positive + n_negative;

    delta_t delta_for_alpha_true = filter_by_alpha(alpha, delta);

    double sum_over_k_negative;
    if (options.comparison) {
        sum_over_k_negative = calculate_sum_over_k_negative_exact(n_positive, n_negative, alpha,
                                                                  delta, object_counts);
        double sum_over_k_negative_approx = calculate_sum_over_k_negative_approx(
            n_positive, n_negative, alpha, delta, object_counts);
        double error = sum_over_k_negative - sum_over_k_negative_approx;

        BOOST_LOG_TRIVIAL(info) << "Approximation error: " << std::to_string(error);
    } else {
        if (!can_use_normal_approx(n_negative, delta_for_alpha_true)) {
            sum_over_k_negative = calculate_sum_over_k_negative_exact(n_positive, n_negative, alpha,
                                                                      delta, object_counts);
        } else {
            sum_over_k_negative = calculate_sum_over_k_negative_approx(n_positive, n_negative,
                                                                       alpha, delta, object_counts);
        }
    }

    return std::log(sum_over_k_negative);
}

double ModelDistribution::calculate_sum_over_k_negative_exact(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) {
    Multinomial k_negative_distribution(n_negative, delta);

    std::vector<double> log_p_k_positive_given_delta_n_negative;

    iterate_over_k_negatives(
        [&log_p_k_positive_given_delta_n_negative, &k_negative_distribution](
            int k_1_negative, int k_2_negative, int k_3_negative) {
            log_p_k_positive_given_delta_n_negative.push_back(
                k_negative_distribution.log_pdf({k_1_negative, k_2_negative, k_3_negative}));
        },
        n_positive, n_negative, alpha, object_counts);

    std::vector<double> p_k_positive_given_delta_n_negative =
        exp(log_p_k_positive_given_delta_n_negative);
    return stable_sum<double>(p_k_positive_given_delta_n_negative);
}

void ModelDistribution::iterate_over_k_negatives(std::function<void(int, int, int)> f,
                                                 int n_positive, int n_negative,
                                                 const alpha_t &alpha,
                                                 const category_counts_t &object_counts) {
    // Because some alpha_i might be 0, we have to fix some of the k^- values to start with.
    std::vector<int> fixed_k_negative = {0, 0, 0};
    for (int i = 0; i < 3; ++i) {
        if (alpha.at(i) == false) {
            fixed_k_negative.at(i) = object_counts.at(i);
        }
    }

    int k_1_negative_start = std::max({object_counts[0] - n_positive, fixed_k_negative[0],
                                       n_negative - object_counts[1] - object_counts[2]});
    int k_1_negative_end =
        std::min(n_negative - fixed_k_negative[1] - fixed_k_negative[2], object_counts[0]);
    for (int k_1_negative = k_1_negative_start; k_1_negative <= k_1_negative_end; ++k_1_negative) {
        int k_2_negative_start = std::max({object_counts[1] - n_positive, fixed_k_negative[1],
                                           n_negative - k_1_negative - object_counts[2]});
        int k_2_negative_end =
            std::min(n_negative - k_1_negative - fixed_k_negative[2], object_counts[1]);
        for (int k_2_negative = k_2_negative_start; k_2_negative <= k_2_negative_end;
             ++k_2_negative) {
            int k_3_negative = n_negative - k_1_negative - k_2_negative;
            if (alpha[2] == false && k_3_negative == object_counts[2] ||
                alpha[2] == true && k_3_negative >= 0 && k_3_negative <= object_counts[2]) {
                f(k_1_negative, k_2_negative, k_3_negative);
            }
        }
    }
}

double ModelDistribution::calculate_sum_over_k_negative_approx(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) {
    std::vector<bool> object_count_nonzero(object_counts.size(), false);
    std::transform(object_counts.begin(), object_counts.end(), object_count_nonzero.begin(),
                   [](int count) { return count > 0; });

    std::vector<bool> effective_alpha;
    for (int i = 0; i < alpha.size(); ++i) {
        effective_alpha.push_back(alpha.at(i) && object_count_nonzero.at(i));
    }

    category_counts_t filtered_object_counts = filter_by_alpha(effective_alpha, object_counts);

    if (effective_alpha == std::vector<bool>({0, 0, 0})) {
        return 0;
    }

    Multinomial m_fixed =
        Multinomial(n_negative, delta)
            .fix_dimensions(not_v(effective_alpha),
                            filter_by_alpha(effective_alpha, object_counts, false));

    double sum_over_k_negative_approx;
    if (m_fixed.p.size() > 1) {
        MultivariateGuassian mg = MultivariateGuassian::from_multinomial(m_fixed);
        std::vector<std::vector<double>> hyperplanes =
            ModelDistribution::get_hyperplanes(m_fixed.n, filtered_object_counts);
        mg.shift_hyperplanes(hyperplanes);

        MVI3::Mvi3 mvi3;
        double log_sum_over_k_negative_approx =
            std::log(mvi3.integrate(12456, -1, 10, 10, mg.get_covariance(), hyperplanes)) +
            m_fixed.log_adjust;

        sum_over_k_negative_approx = std::exp(log_sum_over_k_negative_approx);
    } else {
        if (filtered_object_counts.size() > 0 && filtered_object_counts.back() >= m_fixed.n) {
            sum_over_k_negative_approx = std::exp(m_fixed.log_adjust);
            // TODO(joschnei): condition above is wrong. Need to check both n+ and n-b
        } else {
            sum_over_k_negative_approx = 0;
        }
    }
    return sum_over_k_negative_approx;
}

double ModelDistribution::calculate_sum_over_k_negative_approx_2(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) {
    Multinomial m_fixed =
        Multinomial(n_negative, delta)
            .fix_dimensions(not_v(alpha), filter_by_alpha(alpha, object_counts, false));
    MultivariateGuassian mg = MultivariateGuassian::from_multinomial(m_fixed);

    std::function<double(std::vector<double>)> multivariate_guassian_density =
        [mg](std::vector<double> point) { return mg.density(point); };

    std::function<std::vector<double>(std::default_random_engine &)> sampler =
        [m_fixed, object_counts = filter_by_alpha(alpha, object_counts)](
            std::default_random_engine &generator) {
            int dim = m_fixed.p.size();

            std::vector<std::uniform_real_distribution<double>> dists;
            for (int i = 0; i < dim - 1; ++i) {
                dists.push_back(std::uniform_real_distribution<double>(0.0, object_counts.at(i)));
            }

            std::vector<double> point;
            double last_component;
            do {
                point = std::vector<double>();
                for (std::uniform_real_distribution<double> dist : dists) {
                    point.push_back(dist(generator));
                }
                last_component = m_fixed.n - std::accumulate(point.begin(), point.end(), 0.0);
            } while (!(0 <= last_component && last_component <= object_counts.back()));
            return point;
        };

    double volume;
    if (m_fixed.p.size() == 3) {
        volume = n_negative * object_counts.at(2) - std::pow(object_counts.at(2), 2) / 2.0;
        if (n_negative > object_counts.at(0)) {
            volume -= 1.0 / 2.0 * std::pow(n_negative - object_counts.at(0), 2);
            if (n_negative - object_counts.at(2) > object_counts.at(0)) {
                volume +=
                    1.0 / 2.0 * std::pow(n_negative - object_counts.at(2) - object_counts.at(0), 2);
            }
        }
        if (n_negative > object_counts.at(1)) {
            volume -= 1.0 / 2.0 * std::pow(n_negative - object_counts.at(1), 2);
            if (n_negative - object_counts.at(2) > object_counts.at(1)) {
                volume +=
                    1.0 / 2.0 * std::pow(n_negative - object_counts.at(2) - object_counts.at(1), 2);
            }
        }
        if (n_negative > object_counts.at(0) + object_counts.at(1)) {
            volume +=
                1.0 / 2.0 * std::pow(n_negative - object_counts.at(0) - object_counts.at(1), 2);
        }
        if (n_negative - object_counts.at(2) < 0) {
            volume += 1.0 / 2.0 * std::pow(n_negative - object_counts.at(2), 2.0);
        }
    } else if (m_fixed.p.size() == 2) {
        volume = std::min(n_negative, object_counts.at(0)) -
                 std::max(0, n_negative - object_counts.at(1));
    }

    return std::exp(
        std::log(integrate(multivariate_guassian_density, sampler, generator, volume, 500000)) +
        m_fixed.log_adjust);
}

/**
 * Assumes n_negative has already been adjusted, objct_counts has already been filtered.
 */
std::vector<std::vector<double>> ModelDistribution::get_hyperplanes(
    int n_negative, category_counts_t object_counts) {
    std::vector<std::vector<double>> hyperplanes;

    // Of the the remaining dimensions, pick the last one to equal n - sum k_i.
    category_count_t k_fixed = object_counts.back();
    object_counts.pop_back();

    // sum k_i^- = n^- => k_1^- = n^- - sum k_i^- (from 2)
    // k_1^- >= 0 => sum k_i^- (from 2) <= n^-
    std::vector<double> k_fixed_negative_lower_constraint(object_counts.size(), 1);
    k_fixed_negative_lower_constraint.push_back(n_negative);
    hyperplanes.push_back(k_fixed_negative_lower_constraint);

    // sum k_i^- = n^+- => k_1^- = n^- - sum k_i^- (from 2)
    // k_1^- <= k_1 => - sum k_i^- (from 2) <= k_1 - n^-
    std::vector<double> k_fixed_negative_upper_constraint(object_counts.size(), -1);
    k_fixed_negative_upper_constraint.push_back(k_fixed - n_negative);
    hyperplanes.push_back(k_fixed_negative_upper_constraint);

    // k_i^+ and k_i^- are both >= 0.
    // k_i^- >= 0 implies -k_i^- <= 0.
    // k_i+ >= 0 implies k_i - k_i^- >= 0 implies k_i^- <= k_i
    for (int i = 0; i < object_counts.size(); ++i) {
        std::vector<double> k_i_minus_constraint(object_counts.size(), 0);
        k_i_minus_constraint.at(i) = -1;
        k_i_minus_constraint.push_back(0);
        hyperplanes.push_back(k_i_minus_constraint);

        std::vector<double> k_i_plus_constraint(object_counts.size(), 0);
        k_i_plus_constraint.at(i) = 1;
        k_i_plus_constraint.push_back(object_counts.at(i));
        hyperplanes.push_back(k_i_plus_constraint);
    }

    return hyperplanes;
}

bool ModelDistribution::can_use_normal_approx(int n_negative, const delta_t &delta) const {
    if (options.exact) {
        return false;
    }
    for (double p : delta) {
        if (n_negative * p <= 5.0) {
            return false;
        }
    }
    return true;
}
}  // namespace FilterModel


/*!
* \file GrapheneExp.cpp
*
* \brief Tight-binding model for graphene photo-excitation
* Solves the ODE system for a wave with a few-cycle envelope (exp)
*
* The excitation
* is a slowly varying envelope with exponential shape
* See e.g. http://link.aps.org/doi/10.1103/PhysRevB.91.045439
*
* \author Author: D. Gagnon <denisg6@hotmail.com>
*/

// Include some headers
#include <iostream>
#include <fstream>
#include <armadillo>
#include <cmath>
#include <complex>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/numeric/odeint.hpp>

#include "GrapheneExp.hpp"
#include "Utils.hpp"

/// Main function
int main(int argc, char *argv[])
{
    // Parse parameter file
    boost::property_tree::ptree pt;
    boost::property_tree::ini_parser::read_ini("GrapheneExp.ini", pt);

    // Problem parameters
    double E0 = std::stof(pt.get<std::string>("Parameters.E0"));            // Peak electric field in V/m
    double tau = std::stof(pt.get<std::string>("Parameters.tau"));        // Pulse duration in fs

    // Parameter sweep
    double xmin = std::stof(pt.get<std::string>("Sweep.xmin")); // Minimum frequency in sweep
    double xmax = std::stof(pt.get<std::string>("Sweep.xmax")); // Minimum frequency in sweep
    int x_elem = std::stoi(pt.get<std::string>("Sweep.xnum"));

    // Parameter sweep
    double ymin = std::stof(pt.get<std::string>("Sweep.ymin")); // Minimum frequency in sweep
    double ymax = std::stof(pt.get<std::string>("Sweep.ymax")); // Minimum frequency in sweep
    int y_elem = std::stoi(pt.get<std::string>("Sweep.ynum"));

    // Meshgrid (vectors of parameters)
    auto xvec = arma::linspace(xmin, xmax, x_elem);
    auto yvec = arma::linspace(ymin, ymax, y_elem);

    // Variables to store probability values
    auto prob_mat = arma::mat(y_elem,x_elem);

    // Initial value of time, integration time and interval

    double tinit = -4.0*tau;
    double inttime = 8.0*tau;
    double dt = inttime/100;

    // Variables for loops
    unsigned id, id2;

    // Loop for each K value and compute probability
    for (id=0; id < y_elem; id++)
    {
        for (id2=0; id2 < x_elem; id2++)
        {

            // Initialize tight-binding model
            tight_binding_exp model(xvec[id2],yvec[id],tau,E0);

            // Prepare initial states (negative and positive eigenstates)
            state_type psi = EigenState(model, xvec[id2], yvec[id], -1.0);
            state_type eigen_p = EigenState(model, xvec[id2], yvec[id], 1.0);

            // Integrate
            size_t steps = boost::numeric::odeint::integrate( model,
                           psi, tinit, inttime, dt );

            // Calculate the projection
            std::complex<double> projection = Projection(eigen_p,psi);

            prob_mat(id,id2) = std::abs(projection)*std::abs(projection);

        }

    }

    // Save ODE integration data
    prob_mat.save("probability.dat", arma::raw_ascii);

    // Save parameters vector
    xvec.save("xvec.dat", arma::raw_ascii);
    yvec.save("yvec.dat", arma::raw_ascii);

    // For post-processing: time evolution of vector potential after ODE integration
    int numtimes = ceil(inttime/dt);
    auto times = arma::linspace(tinit,tinit+inttime,numtimes);

    // Initialize output file
    std::ofstream outfile("potential.dat");
    outfile << std::scientific << std::setprecision(10);

    // Create tight binding object
    tight_binding_exp model(0.0,0.0,tau,E0);

    for (size_t i=0; i < numtimes; i++)
    {
        outfile << times(i) << " "
                << model.Gx(times(i)) << " "
                << model.Gy(times(i)) << std::endl;
    }

    return 0;

}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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 <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <iostream>
#include <fstream>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, undirectedS,
    no_property, property < edge_weight_t, int > > Graph;
  typedef graph_traits < Graph >::edge_descriptor Edge;
  typedef std::pair<int, int> E;

  const int num_nodes = 5;
  E edge_array[] = { E(0, 2), E(1, 3), E(1, 4), E(2, 1), E(2, 3),
    E(3, 4), E(4, 0), E(4, 1)
  };
  int weights[] = { 1, 1, 2, 7, 3, 1, 1, 1 };
  std::size_t num_edges = sizeof(edge_array) / sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph g(num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  for (std::size_t j = 0; j < num_edges; ++j) {
    Edge e; bool inserted;
    boost::tie(e, inserted) = add_edge(edge_array[j].first, edge_array[j].second, g);
    weightmap[e] = weights[j];
  }
#else
  Graph g(edge_array, edge_array + num_edges, weights, num_nodes);
#endif
  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
  std::vector < Edge > spanning_tree;

  kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

  std::cout << "Print the edges in the MST:" << std::endl;
  for (std::vector < Edge >::iterator ei = spanning_tree.begin();
       ei != spanning_tree.end(); ++ei) {
    std::cout << source(*ei, g) << " <--> " << target(*ei, g)
      << " with weight of " << weight[*ei]
      << std::endl;
  }

  std::ofstream fout("figs/kruskal-eg.dot");
  fout << "graph A {\n"
    << " rankdir=LR\n"
    << " size=\"3,3\"\n"
    << " ratio=\"filled\"\n"
    << " edge[style=\"bold\"]\n" << " node[shape=\"circle\"]\n";
  graph_traits<Graph>::edge_iterator eiter, eiter_end;
  for (boost::tie(eiter, eiter_end) = edges(g); eiter != eiter_end; ++eiter) {
    fout << source(*eiter, g) << " -- " << target(*eiter, g);
    if (std::find(spanning_tree.begin(), spanning_tree.end(), *eiter)
        != spanning_tree.end())
      fout << "[color=\"black\", label=\"" << get(edge_weight, g, *eiter)
           << "\"];\n";
    else
      fout << "[color=\"gray\", label=\"" << get(edge_weight, g, *eiter)
           << "\"];\n";
  }
  fout << "}\n";
  return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::example::model::exponential.cpp              //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 <stdexcept>
#include <string> //needed?
#include <vector>
#include <limits>
#include <ostream>
#include <fstream>
#include <algorithm>
#include <iterator>
// #include <boost/archive/binary_oarchive.hpp>
// #include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>

#include <boost/arithmetic/equal.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/iterator/range_cycle.hpp>
#include <boost/range.hpp>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/iterator/range_cycle.hpp>

#include <boost/standard_distribution/distributions/normal.hpp>

#include <boost/statistics/survival/data/include.hpp>
#include <boost/statistics/survival/model/models/exponential/include.hpp>
#include <libs/statistics/survival/model/example/exponential.h>

// Must come after the model to be used
#include <boost/statistics/model/include.hpp>

#include <libs/statistics/survival/model/example/exponential.h>

void example_exponential(std::ostream& out){

    out << "-> example_model_exponential : ";

    // Steps shown in this example:
    //
    // Loads the first batch of a set of records
    // Creates events at given time
    // Evaluates the likelihoods and posteriors

    using namespace boost;
    using namespace statistics;
    namespace surv = survival;

    // [ Types ]
    // Value
    typedef double                                  val_;
    typedef std::vector<val_>                       vals_;
    typedef surv::constant<val_>                    const_;

    // I/O
    typedef boost::archive::text_oarchive oa_;
    typedef boost::archive::text_iarchive ia_;

   // Records
    typedef surv::data::record<val_>                record_;
    typedef std::vector<record_>                    records_;
    typedef range_iterator<records_>::type          it_record_;

    // Events
    typedef surv::data::event<val_>                 event_;
    typedef std::vector<event_>                     events_;
    typedef range_iterator<events_>::type           it_event_;

    // Covariates
    typedef val_                                    x_;
    typedef vals_                                   r_x_;
    typedef range_cycle<>                           range_cycle_;
    typedef range_cycle_::apply<r_x_>::type         x_cycle_;

    // Model
    typedef surv::model::exponential::model<val_>   model_;
    typedef val_                                    par_;
    typedef vals_                                   pars_;

    // [ Constants ]
    const val_ entry_bound = const_::inf_;
    const val_ par = 2.0;
    const char* batches_path
        = "/Users/erwann/projets/2009/Xcode/survival/build/Release/batches";

    // [ Variables ]
    long n_record;

    // [ Upload first batch of records ]
    records_ records;
    {
        std::ifstream ifs(batches_path);
        if(ifs.good()){
            ia_ ia(ifs);
            ia >> records;
        }else{
            std::string str = "error opening : ";
            str.append( batches_path );
            throw std::runtime_error(str);
        }
        ifs.close();
    }
    n_record = boost::size( records );

    // [ Events ]
    events_ events;
    events.reserve( size(records) );
    surv::data::events(
        begin(records),
        end(records),
        entry_bound,
        std::back_inserter(events)
    );

    // [ Covariates ]
    r_x_ r_x;
    {
        using namespace boost::assign;
        r_x += -0.5, 0.5;
    }
    x_cycle_ x_cycle = range_cycle_::make(r_x,0,n_record);
    out << "size(x_cycle) = " << size(x_cycle) << std::endl;
    BOOST_ASSERT( size(x_cycle)>=size(events) );
    // Resize x_cycle to a size that matches that of events
    x_cycle.advance_end( - (size(x_cycle) - size(events)) );
    BOOST_ASSERT( size(x_cycle) == size(events) );

    // Model
    model_ model;

    // Pars
    pars_ pars;
    {
        using namespace assign;
        pars += -2.0, -1.0, 0.0, 1.0, 2.0;
    }

    // [ Likelihood ]

    typedef math::normal_distribution<val_> mprior_;
    mprior_ mprior;

    out << '(';
    out << model::log_likelihood<val_>(
        model::make_model_data(
            model,
            r_x[0],
            events[0]
        ),
        par
    );
    out << ',';
    out << model::log_likelihood<val_>(
        model::make_model_data(
            model,
            r_x[1],
            events[1]
        ),
        par
    ) << ')';

    // [ Likelihoods ]
    vals_ lls;
    model::log_likelihoods<val_>(
        model::make_model_dataset(model,r_x,events),
        boost::begin(pars),
        boost::end(pars),
        std::back_inserter(lls)
    );

    // [ Prior ]
    vals_ lprs;
    math::transform<math::fun_wrap::log_unnormalized_pdf_>(
        mprior,
        boost::begin(pars),
        boost::end(pars),
        std::back_inserter(lprs)
    );

    // [ Posteriors ]

    vals_ lpos;
    model::log_posteriors2<val_>(
        model::make_prior_model_dataset(mprior,model,r_x,events),
        boost::begin(pars),
        boost::end(pars),
        std::back_inserter(lpos)
    );

    // Consistency check
    typedef range_iterator<vals_>::type it_val_;
    {

        it_val_ i_lpr = boost::begin(lprs);
        it_val_ i_lpo = boost::begin(lpos);
        out << std::endl;
        out << "log(prior,likelihood,posterior)" << std::endl;
        for(
            it_val_ i_ll = boost::begin(lls);
            i_ll< boost::end(lls);
            i_ll++,i_lpr++,i_lpo++
        ){
            out << '(';
            val_ lpr = *i_lpr;  out << lpr << ',';
            val_ ll = *i_ll;    out << ll << ',';
            val_ lpo = *i_lpo;  out << lpo << ')' << std::endl;
            val_ lpo2 = lpr + ll;
            BOOST_ASSERT(
                arithmetic_tools::equal(
                    lpo,
                    lpo2
                )
            );
        }
    }

    // Consistency check2

    {
        vals_ lpr2s ( size(pars) );
        model::log_posteriors<val_>(
            model::make_prior_model_dataset(mprior,model,r_x,events),
            boost::begin(pars),
            boost::end(pars),
            boost::begin(lls), //subtracted
            boost::begin(lpr2s)
        );

        it_val_ i_lpr = boost::begin(lprs);
        out << std::endl;
        out << "log(prior,prior2)" << std::endl;
        for(
            it_val_ i_lpr2 = boost::begin(lpr2s);
            i_lpr2< boost::end(lpr2s);
            i_lpr2++,i_lpr++
        ){
            out << '(';
            val_ lpr  = *i_lpr;     out << lpr << ',';
            val_ lpr2 = *i_lpr2;    out << lpr2 << ')' << std::endl;
            BOOST_ASSERT(
                arithmetic_tools::equal(
                    lpr,
                    lpr2
                )
            );
        }
    }

    out << "<-" << std::endl;
}


#include <iostream>
#include <limits>

#include <boost/rational.hpp>
#include <boost/safe_numerics/safe_integer.hpp>

int main(int, const char *[]){
    // simple demo of rational library
    const boost::rational<int> r {1, 2};
    std::cout << "r = " << r << std::endl;
    const boost::rational<int> q {-2, 4};
    std::cout << "q = " << q << std::endl;
    // display the product
    std::cout << "r * q = " << r * q << std::endl;

    // problem: rational doesn't handle integer overflow well
    const boost::rational<int> c {1, INT_MAX};
    std::cout << "c = " << c << std::endl;
    const boost::rational<int> d {1, 2};
    std::cout << "d = " << d << std::endl;
    // display the product - wrong answer
    std::cout << "c * d = " << c * d << std::endl;

    // solution: use safe integer in rational definition
    using safe_rational = boost::rational<
        boost::safe_numerics::safe<int>
    >;

    // use rationals created with safe_t
    const safe_rational sc {1, std::numeric_limits<int>::max()};

    std::cout << "c = " << sc << std::endl;
    const safe_rational sd {1, 2};
    std::cout << "d = " << sd << std::endl;
    std::cout << "c * d = ";
    try {
        // multiply them. This will overflow
        std::cout << sc * sd << std::endl;
    }
    catch (std::exception const& e) {
        // catch exception due to multiplication overflow
        std::cout << e.what() << std::endl;
    }

    return 0;
}


#include<stdio.h>
#include"mex.h"

#include "EdgeSE3ProjectDirectWithDirstortG2oLM.cpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

// 李群李代数 库
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace g2o;

// 一次测量的值，包括一个世界坐标系下三维点,以及投影的对应的图像
struct Measurement
{
    Measurement ( Eigen::Vector3d p, float * im) : pos_world (p), image(im){}
    Eigen::Vector3d pos_world;
    float* image;
};

bool calibrationEstimationDirectG2OLM ( const vector<Measurement>& measurements, double *D, double *K, double *imageSize, Eigen::Isometry3d& Tcw, bool verbose, int max_iter)
{
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( verbose );

    // 添加顶点
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        EdgeSE3ProjectDirectWithDirstortG2oLM* edge = new EdgeSE3ProjectDirectWithDirstortG2oLM (
            m.pos_world,
            D, K, imageSize, m.image
        );
        edge->setVertex (0, pose );
        edge->setMeasurement (0);
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    // cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    mexPrintf("edges in graph: %d\n", optimizer.edges().size());
    optimizer.initializeOptimization();
    optimizer.optimize ( max_iter );
    Tcw = pose->estimate();
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    // nlhs represent the number of parameters of the output
    // plhs is a array of the mxarray pointers, each pointing to the output
    // nrhs represents the number of parameters of the input
    // prhs is a array of the mxarray pointers, each pointing to the input

    // prhs[0], 6x1 matrix
    // prhs[1], Mx1 cell, each cell with NX3 points
    // prhs[2], Mx1 cell, each cell with PXQ single matrix
    // prhs[3], 1x2, or 1x3, or 1x4, or 1x5 matrix
    // prhs[4], 3x3 matrix
    // prhs[5], 1x2 matrix
    // prhs[6], 1x1 matrix
    // prhs[7], 1x1 matrix

    if(nrhs < 8){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidNumInputs", "at least 8 input arguments required");
        return;
    }

    // get the init xi
    const size_t *dimArrayOfInitXi = mxGetDimensions(prhs[0]);
    size_t sizeRowsInitXi = *(dimArrayOfInitXi + 0);
    size_t sizeColsInitXi = *(dimArrayOfInitXi + 1);
    if(sizeRowsInitXi != 6 || sizeColsInitXi != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 1st param should be 6x1");
        return;
    }
    double *ptrInitXi = (double *)(mxGetPr(prhs[0]));
    Eigen::Matrix<double, 6, 1> initXi;
    for(int i = 0; i < 6; i++){
        initXi(i, 0) = *(ptrInitXi + i);
    }
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the chessboard lidar points
    const size_t *dimArrayOfChessboardLidarPoints = mxGetDimensions(prhs[1]);
    size_t sizeRowsCellChessboardLidarPoints = *(dimArrayOfChessboardLidarPoints + 0);
    size_t sizeColsCellChessboardLidarPoints = *(dimArrayOfChessboardLidarPoints + 1);
    if(sizeColsCellChessboardLidarPoints != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 2st param should be Mx1 cell");
        return;
    }
    std::vector<std::vector<Eigen::Vector3d>>chessboardLidarPoints;
    size_t total_measurements = 0;
    for(int i = 0; i < sizeRowsCellChessboardLidarPoints; i++){
        // get the dimensions of each cell
        mxArray *cur_mArray_chessboardLidarPoints = mxGetCell(prhs[1], i);
        const size_t *dimArrayOfCurrentChessboardLidarPoints = mxGetDimensions(cur_mArray_chessboardLidarPoints);
        size_t sizeRowsCurrentChessboardLidarPoints = *(dimArrayOfCurrentChessboardLidarPoints + 0);
        size_t sizeColsCurrentChessboardLidarPoints = *(dimArrayOfCurrentChessboardLidarPoints + 1);
        if(sizeColsCurrentChessboardLidarPoints != 3){
            mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 2st param should be cell with Px3");
            return;
        }
        float *cur_ptr_chessboardLidarPoints = (float *)(mxGetPr(cur_mArray_chessboardLidarPoints));
        std::vector<Eigen::Vector3d>currentCellChessboardLidarPoints;
        total_measurements = total_measurements + sizeRowsCurrentChessboardLidarPoints;
        for(int j = 0; j < sizeRowsCurrentChessboardLidarPoints; j++){
            Eigen::Vector3d current_chessboardLidarPoint;
            current_chessboardLidarPoint[0] = cur_ptr_chessboardLidarPoints[0 * sizeRowsCurrentChessboardLidarPoints + j];
            current_chessboardLidarPoint[1] = cur_ptr_chessboardLidarPoints[1 * sizeRowsCurrentChessboardLidarPoints + j];
            current_chessboardLidarPoint[2] = cur_ptr_chessboardLidarPoints[2 * sizeRowsCurrentChessboardLidarPoints + j];
            currentCellChessboardLidarPoints.push_back(current_chessboardLidarPoint);
        }
        chessboardLidarPoints.push_back(currentCellChessboardLidarPoints);
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the chessoard_Dt_Mask
    const size_t *dimArrayOfChessboardMaskDT = mxGetDimensions(prhs[2]);
    size_t sizeRowsCellChessboardMaskDT = *(dimArrayOfChessboardMaskDT + 0);
    size_t sizeColsCellChessboardMaskDT = *(dimArrayOfChessboardMaskDT + 1);
    if(sizeColsCellChessboardMaskDT != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 3st param should be Mx1 cell");
        return;
    }
    std::vector<float *>chessboardMaskDTPtr;
    for(int i = 0; i < sizeRowsCellChessboardMaskDT; i++){
        // get the dimensions of each cell
        mxArray *cur_mArray_chessboardMaskDT = mxGetCell(prhs[2], i);

        float *cur_ptr_chessboardMaskDT = (float *)(mxGetPr(cur_mArray_chessboardMaskDT));
        chessboardMaskDTPtr.push_back(cur_ptr_chessboardMaskDT);
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the distortion parameters
    const size_t *dimArrayOfD = mxGetDimensions(prhs[3]);
    size_t sizeRowsD = *(dimArrayOfD + 0);
    size_t sizeColsD = *(dimArrayOfD + 1);
    if(sizeRowsD != 1 || sizeColsD != 5){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 4st param should be 1x5 array");
        return;
    }
    double *ptrD = (double *)(mxGetPr(prhs[3]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the intrinsic parameters K
    const size_t *dimArrayOfK = mxGetDimensions(prhs[4]);
    size_t sizeRowsK = *(dimArrayOfK + 0);
    size_t sizeColsK = *(dimArrayOfK + 1);
    if(sizeRowsK != 3 || sizeColsK != 3){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 5st param should be 3X3 array");
        return;
    }
    double *ptrK = (double *)(mxGetPr(prhs[4]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the imageSize
    const size_t *dimArrayOfImageSize = mxGetDimensions(prhs[5]);
    size_t sizeRowsImageSize = *(dimArrayOfImageSize + 0);
    size_t sizeColsImageSize = *(dimArrayOfImageSize + 1);
    if(sizeRowsImageSize != 1 || sizeColsImageSize != 2){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 6st param should be 1x2 array");
        return;
    }
    double *ptrImageSize = (double *)(mxGetPr(prhs[5]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the verbose params
    const size_t *dimArrayOfVerbose = mxGetDimensions(prhs[6]);
    size_t sizeRowsVerbose= *(dimArrayOfVerbose + 0);
    size_t sizeColsVerbose = *(dimArrayOfVerbose + 1);
    if(sizeRowsVerbose != 1 || sizeColsVerbose != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 7st param should be 1x1 array");
        return;
    }
    double *ptrVerbose = (double *)(mxGetPr(prhs[6]));

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the maxIter params
    const size_t *dimArrayOfMaxIter = mxGetDimensions(prhs[6]);
    size_t sizeRowsMaxIter = *(dimArrayOfMaxIter + 0);
    size_t sizeColsMaxIter = *(dimArrayOfMaxIter + 1);
    if(sizeRowsMaxIter != 1 || sizeColsMaxIter != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 8st param should be 1x1 array");
        return;
    }
    double *ptrMaxIter = (double *)(mxGetPr(prhs[7]));

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    std::vector<Measurement> measurements;
    std::vector<bool> isInners;
    Eigen::Isometry3d Tlc = Eigen::Isometry3d::Identity();

    // get the init tcl from the init Xi
    Sophus::SE3<double> SE3 = Sophus::SE3<double>::exp(initXi);
    Eigen::Matrix<double, 4, 4> SE3Matrix = SE3.matrix();
    Tlc.matrix() << SE3Matrix;

    for(int i = 0; i < chessboardMaskDTPtr.size(); i++){
        float *current_chessboardMaskDTPtr = chessboardMaskDTPtr[i];
        std::vector<Eigen::Vector3d> current_chessboardLidarPoints = chessboardLidarPoints[i];
        for(int j = 0; j < current_chessboardLidarPoints.size(); j++){
            measurements.push_back(Measurement(current_chessboardLidarPoints[j], current_chessboardMaskDTPtr));
        }
    }
    isInners.resize(measurements.size());

    calibrationEstimationDirectG2OLM ( measurements, ptrD, ptrK, ptrImageSize, Tlc, bool(ptrVerbose[0]), int(ptrMaxIter[0]));

    Sophus::SE3<double> SE3_Rt(Tlc.rotation(), Tlc.translation());

    // cout<<"SE3 = "<<endl<<SE3_Rt.matrix()<<endl;

    Eigen::Matrix<double,6,1> se3_out = SE3_Rt.log();

    // the output xi will be 1x6
    size_t dimArrayOfXi[2] = { 1, 6 };
    plhs[0] = mxCreateNumericArray(2, dimArrayOfXi, mxDOUBLE_CLASS, mxREAL);
    double *out_xi = (double *)mxGetData(plhs[0]);

    for(int i = 0; i < 6; i++){
        out_xi[i] = se3_out(i,0);
    }
}


#include <armadillo>
#include <ForwardBackward.hpp>
#include <HSMM.hpp>
#include <iostream>
#include <json.hpp>
#include <memory>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace std;
using json = nlohmann::json;

void PrintBestWeCanAimFor(int nstates, int ndurations, int min_duration,
        field<ivec> hiddenStates, field<ivec> hiddenDurations) {
    int nseq = hiddenStates.n_elem;
    vec best_pi(nstates, fill::zeros);
    for(int s = 0; s < nseq; s++)
        best_pi(hiddenStates(s)(0))++;
    best_pi /= nseq;
    cout << "Best initial state pmf we can aim at:" << endl << best_pi << endl;

    cout << "Best transition matrix we can aim at:" << endl;
    mat prueba(nstates, nstates, fill::zeros);
    for(int s = 0; s < nseq; s++) {
        for(int i = 0; i < hiddenStates(s).n_elem - 1; i++)
            prueba(hiddenStates(s)(i), hiddenStates(s)(i + 1))++;
    }
    mat pruebasum = sum(prueba, 1);
    for(int i = 0; i < nstates; i++)
        prueba.row(i) /= pruebasum(i);
    cout << prueba << endl;

    cout << "Best duration matrix we can aim at:" << endl;
    mat emp_durations(nstates, ndurations, fill::zeros);
    for(int s = 0; s < nseq; s++) {
        for(int i = 0; i < hiddenStates(s).n_elem; i++)
            emp_durations(hiddenStates(s)(i), hiddenDurations(s)(i)
                    - min_duration)++;
    }
    mat emp_durations_sum = sum(emp_durations, 1);
    for(int i = 0; i < nstates; i++)
        emp_durations.row(i) /= emp_durations_sum(i);
    cout << emp_durations << endl;
}

void reset(HSMM& hsmm, vector<FullProMP> promps) {
    int nstates = hsmm.nstates_;
    int ndurations = hsmm.ndurations_;
    mat transition(hsmm.transition_);
    if (nstates == 1)
        transition.fill(1.0);  // Self-loops allowed in this case.
    else {
        transition.fill(1.0/(nstates-1));
        transition.diag().zeros();  // No self-loops.
    }
    hsmm.setTransition(transition);
    vec pi(hsmm.pi_);
    pi.fill(1.0/nstates);
    hsmm.setPi(pi);
    mat durations(hsmm.duration_);
    durations.fill(1.0/ndurations);
    hsmm.setDuration(durations);

    // Resetting emission.
    for(int i = 0; i < nstates; i++) {
        ProMP new_model = promps[i].get_model();
        vec new_mean = randn(size(new_model.get_mu_w()));
        mat new_Sigma_w(size(new_model.get_Sigma_w()), fill::eye);
        new_Sigma_w *= 10000;
        mat new_Sigma_y(size(new_model.get_Sigma_y()), fill::eye);
        new_Sigma_y *= 1;
        new_model.set_mu_w(new_mean);
        new_model.set_Sigma_w(new_Sigma_w);
        new_model.set_Sigma_y(new_Sigma_y);
        promps[i].set_model(new_model);
    }
    shared_ptr<AbstractEmission> ptr_emission(new ProMPsEmission(promps));
    hsmm.setEmission(ptr_emission);
}

int main() {
    int min_duration = 50;
    mat transition = {{0.0, 0.1, 0.4, 0.5},
                      {0.3, 0.0, 0.6, 0.1},
                      {0.2, 0.2, 0.0, 0.6},
                      {0.4, 0.4, 0.2, 0.0}};
    int nstates = transition.n_rows;
    vec pi = {0.1, 0.2, 0.3, 0.4};
    // mat durations(nstates, ndurations, fill::eye);
    mat durations =  {{0.0, 0.1, 0.4, 0.5},
                      {0.3, 0.0, 0.6, 0.1},
                      {0.2, 0.2, 0.0, 0.6},
                      {0.4, 0.4, 0.2, 0.0}};
    int ndurations = durations.n_cols;
    int n_basis_functions = 4;
    int njoints = 1;

    // Setting a third order polynomial basis function for the ProMP
    int polynomial_order = n_basis_functions - 1;
    shared_ptr<ScalarBasisFun> kernel{ new ScalarPolyBasis(polynomial_order)};

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w(n_basis_functions * njoints);
        mu_w.fill(i * 10);
        mat Sigma_w = (i + 1) * eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.0001*eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(kernel, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission.
    shared_ptr<AbstractEmission> ptr_emission(new ProMPsEmission(promps));

    HSMM promp_hsmm(ptr_emission, transition, pi, durations, min_duration);

    int nseq = 1;
    int nsegments = 50;
    field<ivec> hidden_states, hidden_durations;
    field<field<mat>> multiple_toy_obs = promp_hsmm.sampleMultipleSequences(
            nseq, nsegments, hidden_states, hidden_durations);
    cout << "Generated states and durations for the first sequence" << endl;
    cout << join_horiz(hidden_states(0), hidden_durations(0)) << endl;

    PrintBestWeCanAimFor(nstates, ndurations, min_duration, hidden_states,
            hidden_durations);

    cout << "Original emission parameters" << endl;
    json params_test = promp_hsmm.emission_->to_stream();
    cout << params_test.dump(4) << endl;

    reset(promp_hsmm, promps);

    cout << "Emission parameters after reset" << endl;
    params_test = promp_hsmm.emission_->to_stream();
    cout << params_test.dump(4) << endl;

    // Providing some sparse labels.
    set<int> observed_indexes = {};  //{5,6,7,8,25,26,32,33,39,41,42,43,47};
    field<Labels> mlabels(nseq);
    for(int s = 0; s < nseq; s++) {
        int idx = 0;
        Labels observed_segments;
        for(int i = 0; i < nsegments; i++) {
            int hs = hidden_states(0)(i);
            int dur = hidden_durations(0)(i);
            idx += dur;
            if (observed_indexes.find(i) != observed_indexes.end()) {
                cout << "label hs: " << hs << endl;
                observed_segments.setLabel(idx - 1, dur, hs);
            }
        }
        mlabels(s) = observed_segments;
    }

    // Learning the model from data.
    promp_hsmm.fit(multiple_toy_obs, mlabels, 100, 1e-10);

    cout << "Model parameters after training" << endl;
    json params = promp_hsmm.to_stream();
    cout << params.dump(4) << endl;

    // Running the Viterbi algorithm for the first sequence.
    const field<mat>& toy_obs = multiple_toy_obs(0);
    imat psi_duration(nstates, toy_obs.n_elem, fill::zeros);
    imat psi_state(nstates, toy_obs.n_elem, fill::zeros);
    mat delta(nstates, toy_obs.n_elem, fill::zeros);
    cube log_pdf = promp_hsmm.computeEmissionsLogLikelihood(toy_obs);
    Viterbi(transition, pi, durations, log_pdf, delta, psi_duration, psi_state,
            min_duration, toy_obs.n_elem);
    cout << "Delta last column" << endl;
    cout << delta.col(toy_obs.n_elem - 1) << endl;
    ivec viterbiStates, viterbiDurations;
    viterbiPath(psi_duration, psi_state, delta, viterbiStates,
            viterbiDurations);

    cout << "Viterbi states and durations" << endl;
    cout << join_horiz(viterbiStates, viterbiDurations) << endl;
    int dur_diff = 0;
    int states_diff = 0;
    for(int i = 0; i < viterbiDurations.n_elem; i++) {
        dur_diff += (viterbiDurations[i] != hidden_durations(0)(i));
        states_diff += (viterbiStates[i] != hidden_states(0)(i));
    }
    cout << "The number of mismatches in duration is " << dur_diff << endl;
    cout << "The number of mismatches in hidden states is " << states_diff <<
            endl;
    return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_HESSENBERG_INCLUDE
#define MTL_MATRIX_HESSENBERG_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

/// Hessenberg-Factorization of matrix A with householder-vectors
/** Return Hessenberg matrix and tril(B,-2) are Householder-vectors **/
template <typename Matrix>
Matrix inline hessenberg_factors(const Matrix& A)
{
    vampir_trace<5014> tracer;
    if (num_rows(A) < 3)
	return A;

    using mtl::imax;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A);
    value_type       zero= math::zero(A[0][0]), beta;
    Matrix           B(clone(A));

    for(size_type i= 0; i < ncols-2; i++){
	// mtl::vector::dense_vector<value_type>  v(B[irange(i+1, imax)][i]);
		mtl::vector::dense_vector<value_type, vector::parameters<> >  v(nrows-i-1), w(nrows);
        for (size_type j = 0; j < size(v); j++)
            v[j]= B[j+i+1][i];
        beta= householder(v).second;
        v= householder(v).first;
;
	if( beta != zero){
            w= beta * B[irange(0,imax)][irange(i+1,imax)] * v;
	    //rank_one_update(B[irange(0,imax)][irange(i+1,imax)],-w,v);
            for(size_type row = 0; row < nrows; row++){
                for(size_type col = i+1; col < ncols; col++){
                    B[row][col] -= w[row] * v[col-i-1];
                }
            }
            //vector*Matrix
            for(size_type k=0; k < size(w); k++){
                w[k]= zero;
                for(size_type j = 0; j < size(v); j++){
                    w[k] += beta * v[j] * B[j+i+1][k];
                }
            }
            //rank_one_update(A[irange(i+1,imax)][irange(0,imax)],-v,w);
            for(size_type row = i+1; row < nrows; row++){
                for(size_type col = 0; col < ncols; col++){
                    B[row][col] -= v[row-i-1] * w[col];
                }
            }
	    // B[irange(i+2, imax)][i]= v[irange(1, nrows-i-1)];
            for(size_type row = i+2; row < nrows; row++){
                B[row][i] = v[row-i-1];
            }
        }
    }
    return B;
}

/// Extract Householder vectors from Hessenberg factorization H of some A
template <typename Matrix>
Matrix inline extract_householder_hessenberg(const Matrix& H)
{
    vampir_trace<5015> tracer;
    return Matrix(tril(H, -2));
}

/// Compute Householder vectors from Hessenberg factorization of A
template <typename Matrix>
Matrix inline householder_hessenberg(const Matrix& A)
{
    vampir_trace<5016> tracer;
    return Matrix(tril(hessenberg_factors(A), -2));
}

/// Extract Hessenberg form from factorization H of some A
template <typename Matrix>
Matrix inline extract_hessenberg(const Matrix& H)
{
    vampir_trace<5017> tracer;
    return Matrix(triu(H, -1));
}

/// Hessenberg form of A
template <typename Matrix>
Matrix inline hessenberg(const Matrix& A)
{
    vampir_trace<5018> tracer;
    // return triu(hessenberg_factors(A), -1);
    MTL_THROW_IF(num_rows(A) < 3, matrix_too_small());

    typedef typename Collection<Matrix>::value_type   value_type;
    // typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A);
    value_type       zero= math::zero(A[0][0]);
    Matrix           H(nrows,ncols);

    H= hessenberg_factors(A);

    // H= bands(hessenberg_factors(A), -nrows, -1);
    // set (doubly) strict lower triangle to zero
    for(size_type row = 2; row < nrows; row++){
        for(size_type col = 0; col < row-1; col++){
            H[row][col]= zero;
        }
    }

    return H;
}

/// Return Q where Q'*A*Q == hessenberg(A)
template <typename Matrix>
Matrix inline hessenberg_q(const Matrix& A)
{
    vampir_trace<5013> tracer;
    using std::abs;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A), mini;
    value_type       zero= math::zero(A[0][0]), one= math::one(A[0][0]);
    const magnitude_type two(2);
    Matrix           Q(nrows,ncols);

    MTL_THROW_IF(num_rows(A) < 3, matrix_too_small());

    Q= one;

    //Extract Q
    for(size_type i = 0; i < nrows-2; i++){
		mtl::vector::dense_vector<value_type, vector::parameters<> >   v(nrows-1), w(nrows);
        v[0]= one;
// 	std::cout<< "v=" << v << "\n";
// 	std::cout<< "w=" << w << "\n";
        for(size_type k = 1; k < size(v); k++)
            v[k]= A[nrows-k][i];

	magnitude_type beta= two / abs(dot(v, v)); // abs: x+0i -> x
        if (beta != two) {
            //trans(Vector)*Matrix
            for(size_type k = 0; k < size(w); k++) {
                w[k]= zero;
                for(size_type j = 0; j < size(v); j++){
// 		     std::cout<< "k=" << k << "  j=" << j << "\n";
                    w[k]+= beta * v[j] * Q[j+i+1][k];
		}
            }
            //rank_one_update(Q[irange(i+1,imax)][irange(0,imax)],-v,w);
            for(size_type row = i+1; row < nrows; row++)
                for(size_type col = 0; col < ncols; col++){
// 		    std::cout<< "row=" << row << "  col=" << col << "\n";
                    Q[row][col] -= v[row-1] * w[col];
		}
        }
    }
    return Q;
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_HESSENBERG_INCLUDE


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_geometry_linear_cone_hpp__
#define __multicontact_api_geometry_linear_cone_hpp__

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <pinocchio/spatial/se3.hpp>

#include "multicontact-api/geometry/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"

#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(TYPE, ROWS) \
  EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime == ROWS, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)

namespace multicontact_api {
namespace geometry {

template <typename _Scalar, int _dim, int _Options>
struct LinearCone : public serialization::Serializable<LinearCone<_Scalar, _dim, _Options> > {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef _Scalar Scalar;
  enum { dim = _dim, Options = _Options };
  typedef Eigen::Matrix<Scalar, dim, -1, Options> MatrixDx;
  typedef Eigen::Matrix<Scalar, dim, dim, Options> MatrixD;
  typedef Eigen::Matrix<Scalar, dim, 1, Options> VectorD;
  typedef Eigen::DenseIndex Index;

  /// \brief Default constructor
  LinearCone() : m_rays() {}

  /// \brief Constructor from a set of rays
  template <typename EigenDerived>
  explicit LinearCone(const Eigen::MatrixBase<EigenDerived>& rays)
      //      : m_rays(_dim,rays.cols())
      : m_rays(rays) {
    //        EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(EigenDerived,MatrixDx)
    //        for(int k=0; k<rays.cols(); ++k)
    //          m_rays.col(k) = rays.col(k).normalized();
  }

  /// \brief Contrustor from a given size.
  explicit LinearCone(const Index size) : m_rays(_dim, size) {}

  /// \brief Copy constructor
  template <typename S2, int O2>
  LinearCone(const LinearCone<S2, dim, O2>& other) : m_rays(other.m_rays) {}

  void addRay(const VectorD& ray) {
    m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + 1);
    m_rays.template rightCols<1>() = ray.normalized();
  }

  template <typename EigenDerived>
  void stack(const Eigen::MatrixBase<EigenDerived>& rays) {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(EigenDerived, dim);
    m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + rays.cols());
    m_rays.rightCols(rays.cols()) = rays;
  }

  template <typename S2, int O2>
  void stack(const LinearCone<S2, dim, O2>& other) {
    stack(other.rays());
  }

  /// \returns the rays of the linear cone.
  const MatrixDx& rays() const { return m_rays; }
  MatrixDx& rays() { return m_rays; }

  /// \returns the number of rays, i.e. the number of cols of m_rays
  Index size() const { return m_rays.cols(); }

  template <typename S2, int O2>
  bool operator==(const LinearCone<S2, dim, O2>& other) const {
    return m_rays == other.m_rays;
  }

  template <typename S2, int O2>
  bool operator!=(const LinearCone<S2, dim, O2>& other) const {
    return !(*this == other);
  }

  template <typename S2, int O2>
  bool isApprox(const LinearCone<S2, dim, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return m_rays.isApprox(other.m_rays, prec);
  }

  void disp(std::ostream& os) const { os << "Rays:\n" << m_rays << std::endl; }

  friend std::ostream& operator<<(std::ostream& os, const LinearCone& C) {
    C.disp(os);
    return os;
  }

 protected:
  /// \brief Rays of the linear cone
  MatrixDx m_rays;

 private:
  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
    ar& boost::serialization::make_nvp("rays", m_rays);
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int /*version*/) {
    ar >> boost::serialization::make_nvp("rays", m_rays);
  }

  BOOST_SERIALIZATION_SPLIT_MEMBER()
};

template <typename _Scalar, int _Options>
struct ForceConeTpl : public LinearCone<_Scalar, 3, _Options> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef LinearCone<_Scalar, 3, _Options> Base;
  typedef WrenchConeTpl<_Scalar, _Options> WrenchCone;
  typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
  using typename Base::Scalar;
  enum { dim = Base::dim };
  using Base::rays;
  using Base::size;
  using typename Base::Index;
  using typename Base::MatrixD;
  using typename Base::MatrixDx;
  using typename Base::VectorD;
  using Base::operator==;
  using Base::operator!=;
  //      using Base::isApprox; // Leads to a bug with clang

  typedef MatrixDx Matrix3x;
  typedef VectorD Vector3;
  typedef Eigen::AngleAxis<Scalar> AngleAxis;

  /// \brief Default constructor
  ForceConeTpl() : Base() {}

  template <typename EigenDerived>
  explicit ForceConeTpl(const Eigen::MatrixBase<EigenDerived>& rays) : Base(rays) {}

  explicit ForceConeTpl(const Index size) : Base(size) {}

  /// \returns a linear cone built from a friction coefficient and the number of rays along the Z axis.
  static ForceConeTpl RegularCone(const Scalar mu, const VectorD& direction, const int num_rays,
                                  const Scalar theta_offset = 0.) {
    assert(mu >= 0. && "mu must be positive");
    assert(num_rays >= 1 && "The number of rays must be at least one");

    const VectorD normalized_direction(direction.normalized());
    ForceConeTpl cone(num_rays);

    const Scalar angle = (2. * M_PI) / num_rays;

    const MatrixD Po(MatrixD::Identity() - normalized_direction * normalized_direction.transpose());

    const MatrixD rot_offset(AngleAxis(theta_offset, normalized_direction).toRotationMatrix());
    const VectorD init_direction(rot_offset * (Po * VectorD::Ones()).normalized());
    const MatrixD rot(AngleAxis(angle, normalized_direction).toRotationMatrix());

    VectorD ray((direction + mu * init_direction).normalized());

    for (int k = 0; k < num_rays; ++k) {
      cone.rays().col(k) = ray;
      if (k != num_rays - 1) ray = rot * ray;
    }

    return cone;
  }

  WrenchCone SE3ActOn(const SE3& M) const {
    WrenchCone res(size());
    typedef typename WrenchCone::MatrixDx::ColXpr Col6Xpr;
    typedef typename MatrixDx::ConstColXpr ConstCol3Xpr;

    const typename SE3::Matrix3& R = M.rotation();
    const typename SE3::Vector3& t = M.translation();

    for (Index k = 0; k < size(); ++k) {
      ConstCol3Xpr in_col = rays().col(k);
      Col6Xpr out_col = res.rays().col(k);

      out_col.template head<3>() = R * in_col;
      out_col.template tail<3>() = t.cross(out_col.template head<3>());
    }

    return res;
  }

  template <typename S2, int O2>
  bool isApprox(const ForceConeTpl<S2, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return Base::isApprox(other, prec);
  }

  operator WrenchCone() const {
    WrenchCone res(size());
    res.rays().template topRows<3>() = rays();
    res.rays().template bottomRows<3>().setZero();
    return res;
  }
};

template <typename _Scalar, int _Options>
struct WrenchConeTpl : public LinearCone<_Scalar, 6, _Options> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef LinearCone<_Scalar, 6, _Options> Base;
  typedef ForceConeTpl<_Scalar, _Options> ForceCone;
  typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
  using typename Base::Scalar;
  enum { dim = Base::dim };
  using Base::rays;
  using Base::size;
  using typename Base::Index;
  using typename Base::MatrixDx;
  using typename Base::VectorD;
  using Base::operator==;
  using Base::operator!=;
  //      using Base::isApprox; // Leads to a bug with clang

  typedef MatrixDx Matrix6x;
  typedef VectorD Vector6;

  typedef typename ForceCone::Matrix3x Matrix3x;

  typedef typename Matrix6x::template NRowsBlockXpr<3>::Type LinearBlock;
  typedef typename Matrix6x::template ConstNRowsBlockXpr<3>::Type ConstLinearBlock;

  typedef LinearBlock AngularBlock;
  typedef ConstLinearBlock ConstAngularBlock;

  /// \brief Default constructor
  WrenchConeTpl() : Base() {}

  /// \brief Constructor from a set of rays.
  template <typename EigenDerived>
  explicit WrenchConeTpl(const Eigen::MatrixBase<EigenDerived>& rays) : Base(rays) {}

  /// \brief Constructs a WrenchCone of a given size.
  explicit WrenchConeTpl(const Index size) : Base(size) {}

  /// \brief Constructs a WrenchCone of a given size.
  template <typename S2, int O2>
  explicit WrenchConeTpl(const ForceConeTpl<S2, O2>& force_cone) : Base(force_cone.size()) {
    rays().template topRows<3>() = force_cone.rays();
    rays().template bottomRows<3>().setZero();
  }

  /// \brief Copy constructor
  template <typename S2, int O2>
  WrenchConeTpl(const WrenchConeTpl<S2, O2>& other) : Base(other) {}

  WrenchConeTpl SE3ActOn(const SE3& M) const {
    WrenchConeTpl res(size());
    typedef typename MatrixDx::ColXpr Col6Xpr;
    typedef typename MatrixDx::ConstColXpr ConstCol6Xpr;

    const typename SE3::Matrix3& R = M.rotation();
    const typename SE3::Vector3& t = M.translation();

    for (Index k = 0; k < size(); ++k) {
      ConstCol6Xpr in_col = rays().col(k);
      Col6Xpr out_col = res.rays().col(k);

      out_col.template head<3>() = R * in_col.template head<3>();
      out_col.template tail<3>() = t.cross(out_col.template head<3>()) + R * in_col.template tail<3>();
    }

    return res;
  }

  template <typename S2, int O2>
  bool isApprox(const WrenchConeTpl<S2, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return Base::isApprox(other, prec);
  }

  ConstLinearBlock linear() const { return rays().template topRows<3>(); }
  LinearBlock linear() { return rays().template topRows<3>(); }

  ConstAngularBlock angular() const { return rays().template bottomRows<3>(); }
  AngularBlock angular() { return rays().template bottomRows<3>(); }

  ForceCone toForceCone() const { return ForceCone(linear()); }
};
}  // namespace geometry
}  // namespace multicontact_api

#endif  // ifndef __multicontact_api_geometry_linear_cone_hpp__


/* Boost libs/numeric/odeint/examples/openmp/lorenz_ensemble.cpp

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 Parallelized Lorenz ensembles

 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 <omp.h>
#include <vector>
#include <iostream>
#include <iterator>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>
#include <boost/lexical_cast.hpp>
#include "point_type.hpp"

using namespace std;
using namespace boost::numeric::odeint;

typedef point<double, 3> point_type;
typedef vector< point_type > inner_state_type;
typedef openmp_state<point_type> state_type;

const double sigma = 10.0;
const double b = 8.0 / 3.0;

struct sys_func {
    const vector<double> &R;
    sys_func( vector<double> &R ) : R(R) {}

    void operator()( const state_type &x , state_type &dxdt , double t ) const {
#       pragma omp parallel for
        for(size_t j = 0 ; j < x.size() ; j++) {
            size_t offset = 0;
            for(size_t i = 0 ; i < j ; i++)
                offset += x[i].size();

            for(size_t i = 0 ; i < x[j].size() ; i++) {
                const point_type &xi = x[j][i];
                point_type &dxdti = dxdt[j][i];
                dxdti[0] = -sigma * (xi[0] - xi[1]);
                dxdti[1] = R[offset + i] * xi[0] - xi[1] - xi[0] * xi[2];
                dxdti[2] = -b * xi[2] + xi[0] * xi[1];
            }
        }
    }
};

int main(int argc, char **argv) {
    size_t n = 1024;
    if(argc > 1) n = boost::lexical_cast<size_t>(argv[1]);

    vector<double> R(n);
    const double Rmin = 0.1, Rmax = 50.0;
#   pragma omp parallel for
    for(size_t i = 0 ; i < n ; i++)
        R[i] = Rmin + (Rmax - Rmin) / (n - 1) * i;

    vector<point_type> inner(n, point_type(10, 10, 10));
    state_type state;
    split(inner, state);

    cerr << "openmp_state split " << n << " into";
    for(size_t i = 0 ; i != state.size() ; i++)
        cerr << ' ' << state[i].size();
    cerr << endl;

    typedef runge_kutta4< state_type, double > stepper;

    const double t_max = 10.0, dt = 0.01;

    integrate_const(
        stepper(),
        sys_func(R),
        state,
        0.0, t_max, dt
    );

    unsplit(state, inner);
    std::copy( inner.begin(), inner.end(), ostream_iterator<point_type>(cout, "\n") );

    return 0;
}


//  Copyright John Maddock 2007.
//  Use, modification and distribution are subject to 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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip6

#include <boost/math/distributions/negative_binomial.hpp>

using namespace boost::math;

// Lower quantile rounded down:
double x = quantile(negative_binomial(20, 0.3), 0.05);
// Upper quantile rounded up:
double y = quantile(complement(negative_binomial(20, 0.3), 0.05));

//]

#include <iostream>

int main()
{
   std::cout << x << " " << y << std::endl;
}


/***
 *  File: BDE_score.hpp
 *  Created: June 4, 2012
 *
 *  Author: Olga Nikolova <olga.nikolova@gmail.com>
 */

#ifndef BDE_SCORE_HPP
#define BDE_SCORE_HPP

#include <jaz/math_add.hpp>
#include <jaz/plain_array.hpp>
#include <algorithm>
#include <vector>
#include <cmath> // this doesn't seem to work
#include <boost/math/special_functions/gamma.hpp>

class BDE_score {
public:
	/**
	 */
	typedef unsigned int index_type;

	/**
	 */
	typedef double value_type;

	BDE_score(unsigned int n, unsigned int m,
			  const jaz::plain_array<unsigned char>& D)
    : n_(n), m_(m), D_(D), ess_(1.0) {

		r_ = *std::max_element(D.begin(), D.end()) + 1;
		px_.resize(r_, 0);
		init_bde_x_();
		data_bins_.resize(m_);
		data_index_.resize(m_);
	} // BDE_score

	value_type operator()(index_type xi) { return bde_x_[xi]; }

	template <typename Iter>
	value_type operator()(index_type xi, Iter first, Iter last) {
		if (first == last) {
			return bde_x_[xi];
		}

		if (check_key_(first, last) == false) {
			generate_index_(first, last);
		} // if

		unsigned int k = std::distance(first, last);
		double qi = static_cast<double>(pow(r_, k));

		// Set priors
		double ess_qi = ess_ / qi;
		double ess_riqi = ess_ / ( qi * static_cast<double>(r_) );

		// Compute commonly used Gamma-fn. values
		double lg_ess_qi = boost::math::lgamma(ess_qi);
		double lg_ess_riqi = boost::math::lgamma(ess_riqi);

		// compute internal sum
		int pos = -1;
		const unsigned char* tab = D_.begin() + xi * m_;

		double H = 0.0;
		S_ = 0.0;

		px_.zero();

		for (unsigned int i = 0; i < m_; ++i) {
			px_[tab[data_index_[i].second]]++;
			if (data_bins_[i] == true) {
				// p(Pa)
				double pa_cnt = i - pos; // Nij
				//double p_pa = pa_cnt / m_;

				H = 0.0;
				for (unsigned char j = 0; j < r_; ++j) {
					if (px_[j] != 0) {
						// p(x|Pa)
						//double p_xpa = static_cast<double>(px_[j]) / pa_cnt;
						double p_xpa = static_cast<double>(px_[j]); // Nijk
						//	    H += p_pa * p_xpa * log2(p_xpa);
						H += boost::math::lgamma( p_xpa + ess_riqi ) - lg_ess_riqi;

					}
				} // for j

				S_ += lg_ess_qi - boost::math::lgamma( pa_cnt + ess_qi ) + H;

				//std::cout<< "S( " << xi << ",{ ";
				//for(unsigned int ii = 0; ii < k; ii++)
				//  std::cout << pa_[ii] << ", ";
				//std::cout << "}) = " << lg_ess_qi - boost::math::lgamma( pa_cnt + ess_qi ) << " + " << H << std::endl;

				px_.zero();
				pos = i;
			} // if
		} // for i

		return ((-1)*S_);
	} // operator()

private:
	template <typename Iter> bool check_key_(Iter first, Iter last) {
		unsigned int k = std::distance(first, last);
		if (k != pa_.size()) return false;
		for (unsigned int i = 0; i < k; ++i) if (first[i] != pa_[i]) return false;
		return true;
	} // check_key_

	void init_bde_x_() {
		//    mdl_x_.resize(n_, logn_ + hlogm_ * (r_ - 1));
		bde_x_.resize(n_, 0);
		const unsigned char* tab = D_.begin();

		double ess_ri = ess_ /static_cast<double>(r_);
		double lg_ess_ri = boost::math::lgamma( ess_ri );

		//std::cout<< "ess_=" << ess_ << " ess_ri = " << ess_ri << " Gamma(ess_/ri) = " <<  lg_ess_ri << std::endl;

		for (unsigned int i = 0; i < n_; ++i) {
			px_.zero();
			double H = 0.0;
			for (unsigned int j = 0; j < m_; ++j) px_[tab[j]]++;
			for (unsigned int j = 0; j < r_; ++j) {
				if (px_[j] != 0) {
					//	  double pi = static_cast<double>(px_[j]) / m_;
					double pi = static_cast<double>(px_[j]); // Nijk
					//std::cout << "N_i=" << i << ",k=" << j << " == " << pi << "(Nijk)" << std::endl;
					//H += pi * log2(pi);
					//std::cout << "log(Gamma(Nijk + alpha/ri)) = " <<  boost::math::lgamma(pi + ess_ri ) << std::endl;
					//std::cout << "log(Gamma(alpha/ri)) = " <<  boost::math::lgamma( ess_ri ) << std::endl;

					H += boost::math::lgamma(pi + ess_ri ) - lg_ess_ri ;
				}
			}
			bde_x_[i] = (-1)*(boost::math::lgamma(ess_) - boost::math::lgamma( static_cast<double>(m_) + ess_) + H);
			//std::cout << "BDe(" << i << ", {}) = " <<   boost::math::lgamma(ess_) - boost::math::lgamma( static_cast<double>(m_) + ess_) << " + " << H << " = "   << bde_x_[i] << std::endl;
			tab += m_;
		}
	} // init_mdl_x_

	typedef std::vector<char> key_t;
	typedef std::pair<key_t, unsigned int> data_index_t;

	struct data_index_lt_ {
		bool operator()(const data_index_t& lhs, const data_index_t& rhs) {
			return lhs.first < rhs.first;
		}
	}; // struct data_index_lt_

	// index of D for given set of parents
	std::vector<bool> data_bins_;
	std::vector<data_index_t> data_index_;

	template <typename Iter> void generate_index_(Iter first, Iter last) {
		// store new parents set
		unsigned int k = std::distance(first, last);

		pa_.resize(k);
		std::copy(first, last, pa_.begin());

		// prepare index data
		for (unsigned int i = 0; i < m_; ++i) {
			data_bins_[i] = false;
			data_index_[i].first.resize(k);
			data_index_[i].second = i;
		}

		// generate index
		for (unsigned int i = 0; i < k; ++i) {
			const unsigned char* tab = D_.begin() + pa_[i] * m_;
			for (unsigned int j = 0; j < m_; ++j) data_index_[j].first[i] = tab[j];
		}

		std::sort(data_index_.begin(), data_index_.end(), data_index_lt_());

		// find bins boundaries
		key_t key = data_index_[0].first;

		for (unsigned int i = 0; i < m_; ++i) {
			if (key < data_index_[i].first) {
				key = data_index_[i].first;
				data_bins_[i - 1] = true;
			}
		}

		data_bins_[m_ - 1] = true;
	} // generate_index_

	unsigned int n_;
	unsigned int m_;

	const jaz::plain_array<unsigned char>& D_;

	unsigned int r_;

	jaz::plain_array<unsigned int> px_;

	double S_;
	double ess_;

	// BDE score of x with no parents
	jaz::plain_array<double> bde_x_;

	// set of parents
	std::vector<unsigned int> pa_;

}; // class MDL_score

#endif // MDL_SCORE_HPP


#include <vector>
#include <cmath>
#include <Eigen/Core>

#include "ihgp/Matern32model.hpp"

Matern32model::Matern32model()
{
    magnSigma2 = 1.0;
    lengthScale = 1.0;
    sigma2 = 1.0;
    Matern32model::updateModel();
}

void Matern32model::updateModel()
{
    // Model matrices for Matern v=3/2
    double lambda = sqrt(3.0)/lengthScale;
    F.setZero(2,2); Pinf.setZero(2,2); H.setZero(1,2);
    F << 0.0, 1.0, -lambda*lambda, -2*lambda;
    Pinf << magnSigma2, 0.0, 0.0, magnSigma2*lambda*lambda;
    H << 1.0, 0.0;
    R = sigma2;

    // Derivatives dF
    dF.clear();
    dF.push_back(Eigen::MatrixXd::Zero(2,2));
    dF.push_back(Eigen::MatrixXd::Zero(2,2));
    Eigen::MatrixXd foo(2,2);
    foo << 0, 0, 6/lengthScale/lengthScale/lengthScale, 2*lambda/lengthScale;
    dF.push_back(foo);

    // Derivatives dPinf
    dPinf.clear();
    dPinf.push_back(Eigen::MatrixXd::Zero(2,2));
    foo << 1.0, 0, 0, 3.0/lengthScale/lengthScale;
    dPinf.push_back(foo);
    foo << 0, 0, 0,-6*magnSigma2/lengthScale/lengthScale/lengthScale;
    dPinf.push_back(foo);

    // Derivatives dR
    dR.clear();
    dR.push_back(1.0);
    dR.push_back(0.0);
    dR.push_back(0.0);
}

void Matern32model::setMagnSigma2(const double &val)
{
    magnSigma2 = val;
    Matern32model::updateModel();
}

void Matern32model::setLengthScale(const double &val)
{
    lengthScale = val;
    Matern32model::updateModel();
}

void Matern32model::setSigma2(const double &val)
{
    sigma2 = val;
    Matern32model::updateModel();
}

MatrixXd Matern32model::getF()
{
    return F;
}

MatrixXd Matern32model::getPinf()
{
    return Pinf;
}

MatrixXd Matern32model::getH()
{
    return H;
}

double Matern32model::getR()
{
    return R;
}

vector< MatrixXd > Matern32model::getdF()
{
   return dF;
}

vector< MatrixXd > Matern32model::getdPinf()
{
    return dPinf;
}

vector< double > Matern32model::getdR()
{
    return dR;
}

double Matern32model::getMagnSigma2()
{
    return magnSigma2;
}

double Matern32model::getLengthScale()
{
    return lengthScale;
}

double Matern32model::getSigma2()
{
    return sigma2;
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

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

#ifndef RJMCMC_SIMPLEX_VARIATE_HPP
#define RJMCMC_SIMPLEX_VARIATE_HPP

#include <boost/random/uniform_real.hpp>

namespace rjmcmc {

    template<int N> struct factorial    { enum { value = factorial<N-1>::value * N };};
    template<     > struct factorial<0> { enum { value = 1 }; };

    struct simplex_variate_log_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            double sum = 0;
            double x[N+1];
            for(unsigned int i=0; i<=N; ++i)
            {
                x[i] = -log(rand(e));
                sum += x[i];
            }
            for(unsigned int i=0; i<N; ++i, ++it)
            {
                *it = x[i]/sum;
            }
        }
    };

    struct simplex_variate_sort_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            double x[N];
            for(unsigned int i=0; i<N; ++i) x[i] = rand(e);
            std::sort(x,x+N);
            double prev = 0;
            for(unsigned int i=0; i<N; ++i, ++it)
            {
                *it = x[i]-prev;
                prev = x[i];
            }
        }
    };

    struct simplex_variate_power_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            static_assert(N>1,"simplex_variate_power_policy is only valid for 2D variates or more !");
            double prev = pow(rand(e),1./N);
            for(unsigned int i=N-1; i>1; --i)
            {
                double next = prev*pow(rand(e),1./i);
                *it++ = prev-next;
                prev = next;
            }
            double next = prev*rand(e);
            *it++ = prev-next;
            *it++ = next;
        }
    };

    struct simplex_variate_rejection_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            for(;;)
            {
                double x[N], sum;
                for(unsigned int i=0; i<N; ++i)
                {
                    x[i] = rand(e);
                    sum += x[i];
                }
                if(sum<=1.)
                {
                    for(unsigned int i=0; i<N; ++i)  *it++ = x[i];
                    return;
                }
            }
        }
    };

    struct simplex_variate_reflect_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            static_assert(N==2,"simplex_variate_reflect_policy is only valid for 2D variates !");
            double x = rand(e);
            double y = rand(e);
            if(x+y>1.)
            {
                x = 1.-x;
                y = 1.-y;
            }
            *it++ = x;
            *it++ = y;
        }
    };

    struct simplex_variate_sqrt_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            static_assert(N==2,"simplex_variate_sqrt_policy is only valid for 2D variates, use the generalized simplex_variate_power_policy instead !");
            double x = sqrt(rand(e));
            double y = x*rand(e);
            *it++ = x-y;
            *it++ = y;
        }
    };

    template<int N, typename Policy = simplex_variate_power_policy >
    class simplex_variate
    {
        typedef boost::uniform_real<> rand_type;
        mutable rand_type m_rand;
        const double m_pdf;
        Policy m_policy;
    public:
        typedef double value_type;
        enum { dimension = N };
        template<typename InputIterator>
        inline double pdf(InputIterator it) const {
            double sum = 0;
            for(unsigned int i=0; i<N; ++i, ++it)
            {
                if(*it<0) return 0;
                sum += *it;
            }
            return double(sum <= 1.)*m_pdf;
        }
        template<typename Engine, typename OutputIterator>
        inline double operator()(Engine& e, OutputIterator it) const {
            m_policy.template apply<N>(e,it,m_rand);
            return m_pdf;
        }
        simplex_variate() : m_rand(0,1), m_pdf(1./factorial<N>::value) {}

    };

}; // namespace rjmcmc

#endif // RJMCMC_SIMPLEX_VARIATE_HPP


/*
 * Mesh.cpp
 *
 * This class contains the mesh and mesh properties to be used
 * in the finite element method (FiniteElem.cpp).
 *
 * It constructs the mesh, then constructs the stiffness matrix and
 * calculates the boundary.
 *
 *  Created on: Aug 13, 2016
 *      Author: Ted Kwan
 */

#include "Mesh.h"
#include <cmath>
#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

Mesh::Mesh() {

}

/**
 * Constructor for Mesh. It takes one input, a vector containing the
 * properties of the mesh to be created.
 *
 * @param meshprops - double vector with all of the necessary properties.
 */
Mesh::Mesh(vector<double> meshprops) {
	// Extract the mesh size value.
	h = meshprops[4];
	n = round(1 / h);
	// Create vectors to be used for mesh creation.
	vec xr = linspace<vec>(meshprops[0], meshprops[1], n + 1);
	vec yr = linspace<vec>(meshprops[2], meshprops[3], n + 1);
	// Create the mesh.
	makeMesh(xr, yr);
	// Assemble stiffness matrix

	vector<sp_mat> stiffmass = assembleMatrix();
	stiffness=stiffmass[0];
	mass=stiffmass[1];
	// Calculate boundary nodes.
	findBoundary();
}

/**
 * makeMesh creates the mesh for the 2D Cartesian grid.
 *
 * @param xr - input vector containing the x values.
 * @param yr - input vector containing the y values.
 */
void Mesh::makeMesh(vec xr, vec yr) {
	// Findthe lengths to use.
	uword xrl = xr.n_rows;
	uword yrl = xr.n_rows;
	mat x = zeros<mat>(xrl, yrl);
	mat y = zeros<mat>(xrl, yrl);
	// Construct the meshes. This is just the same
	// as calling meshgrid() in MATLAB.
	for (uword i = 0; i < yrl; i++) {
		x.row(i) = xr.t();
	}
	for (uword i = 0; i < xrl; i++) {
		y.col(i) = yr;
	}
	// Vectorise the values to use for the nodes matrix.
	vec xv = vectorise(x);
	N = xv.n_rows;
	uword ni = x.n_rows;
	// Initialize node matrix.
	node = zeros<mat>(N, 2);
	// put all the values in the node matrix
	node.col(0) = xv;
	node.col(1) = vectorise(y);
	// Map the indices for the nodes to create elem matrix.
	uvec t2nidxMapnz = regspace<uvec>(0, N - ni - 1);
	uvec topNode = regspace<uvec>(ni - 1, ni, N - ni - 1);
	// Set all of the doubled nodes to not be included.
	t2nidxMapnz(topNode) = zeros<uvec>(topNode.n_rows);
	// Find nonzeros in the original indices map.
	uvec nnz = nonzeros(t2nidxMapnz);
	uvec k = zeros<uvec>(nnz.n_rows + 1);
	// Calculate the different values used to create nodes.
	k(span(1, nnz.n_rows)) = nnz;
	uword NE = k.n_rows;
	// Create the elements which will differ by odd and
	// even elements.
	umat elemup = zeros<umat>(NE, 3);
	umat elemdown = zeros<umat>(NE, 3);
	uvec niv = ones<uvec>(k.n_rows);
	niv = niv * ni;
	uvec onek = ones<uvec>(k.n_rows);
	// Map elements to nodes in order.
	elemup.col(0) = k + niv;
	elemup.col(1) = k + niv + onek;
	elemup.col(2) = k;
	elemdown.col(0) = k + onek;
	elemdown.col(1) = k;
	elemdown.col(2) = k + niv + onek;
	// join all columns together and stack them properly.
	elem = join_cols(elemup, elemdown);
	NT = elem.n_rows;
}

/**
 * assembleMatrix assembles the stiffness matrix using the quick
 * construction method from armadillo for sparse matrices.
 *
 * @return - Stiffness matrix as a sparse matrix.
 */
vector<sp_mat> Mesh::assembleMatrix() {
	// Initialize index maps and value vector.
	uvec ii = zeros<uvec>(9 * NT);
	uvec jj = zeros<uvec>(9 * NT);
	vec sA = zeros<vec>(9 * NT);
	// Calculate the area of each node.
	cube ve(NT, 2, 3);
	ve.slice(0) = node.rows(elem.col(2)) - node.rows(elem.col(1));
	ve.slice(1) = node.rows(elem.col(0)) - node.rows(elem.col(2));
	ve.slice(2) = node.rows(elem.col(1)) - node.rows(elem.col(0));
	// Find the area using the dot product on the second dimension.
	area = 0.5*abs((ve.slice(2).col(0) % ve.slice(1).col(1))
					-(ve.slice(2).col(1) % ve.slice(1).col(0)));
	uword index = 0;
	// Loop to map values and indices.
	for (uword i = 0; i < 3; i++) {
		for (uword j = 0; j < 3; j++) {
			// Setup indices to map in this iteration.
			uvec inds = regspace<uvec>(index, index + NT - 1);
			// Setup element maps for indices.
			ii(inds) = elem.col(i);
			jj(inds) = elem.col(j);
			// Calculate values of stiffness matrix at
			// these points.
			mat prod = ve.slice(i) % ve.slice(j);
			// Store calculated value.
			sA(inds) = sum(prod, 1) / (4 * area);
			index = index + NT;
		}
	}
	// Setup index map to be a 2x9NT matrix.
	umat inds = join_horiz(ii, jj);
	// Create the sparse matrix using the same
	// method as sparse(row indices, col indices, values, size)
	sp_mat A(true, inds.t(), sA, N, N, true, true);
	vector<sp_mat> matvec(2);
	matvec[0]=A;
	vec Mv=accumArrayM(join_vert(elem.col(0), join_vert(elem.col(1),elem.col(2)))
			,join_vert(area, join_vert(area,area))/(3.0), N);
	uvec inds2=regspace<uvec>(0,Mv.n_rows-1);
	umat subs2=join_horiz(inds2,inds2);
	subs2.print("indices: ");
	sp_mat M(true,subs2.t(),Mv,N,N,true,true);
	matvec[1]=M;
	return matvec;
}

/**
 * Find the boundary nodes and elements.
 *
 * This method calculates the boundary for the given mesh to be used
 * to set the boundary condition for the finite element method.
 *
 * Values are stored so that they can be accesssed later.
 *
 */
void Mesh::findBoundary() {

	// Setup as two column vectors to find the edges.
	umat e1 = join_vert(join_horiz(elem.col(2), elem.col(1)),
			join_horiz(elem.col(0), elem.col(2)));
	// Calculate all of the values of the edges.
	umat totalEdge = join_vert(e1, join_horiz(elem.col(1), elem.col(0)));
	totalEdge = sort(totalEdge, "ascend", 1);
	vec onev = ones<vec>(totalEdge.n_rows);
	// Create sparse matrix containing the edges which are not being double counted
	// and are thus exterior edges.
	sp_mat fndmat(true, totalEdge.t(), onev, totalEdge.n_rows, totalEdge.n_rows,
			true, true);
	// Initialize edge matrix.
	umat bdEdge = zeros<umat>(totalEdge.n_rows, 2);
	vec s = nonzeros(fndmat);
	uvec ii = zeros<uvec>(s.n_elem);
	uvec jj = zeros<uvec>(s.n_elem);
	uword k = 0;
	// Get the indices for the edges which are exterior edges.
	// The indices map back to nodes.
	for (uword i = 0; i < fndmat.n_rows; i++) {
		for (uword j = 0; j < fndmat.n_cols; j++) {
			double spot = fndmat(i, j);
			// Only find edges where there are two boundary nodes.
			if (spot == 1.0 && k < s.n_elem) {
				ii(k) = i;
				jj(k) = j;
				k++;
			}
		}
	}
	uvec i1 = nonzeros(ii);
	uvec j1 = nonzeros(jj);
	// Setup boolean vector which has whether or not a node is a boundary
	// node.
	isbdNode = zeros<uvec>(N);
	isbdNode.rows(i1) = ones<uvec>(i1.n_rows);
	isbdNode.rows(j1) = ones<uvec>(j1.n_rows);
	isbdNode(0) = 1.0;
	// Find indices of boundary nodes.
	bdNode = find(isbdNode);
	uvec onebd = ones<uvec>(isbdNode.n_rows);
	// Find indices of interior nodes.
	freeNode = find(onebd - isbdNode);
}

vec Mesh::accumArrayM(uvec subs,vec ar,uword N){

	vec S=zeros<vec>(N);

	for(uword i=0;i<N;i++){
		// Get the subscripts.
		uvec q1=find(subs ==(i));
		vec spot;
		double thesum=0.0;
		if(!q1.is_empty()){
			// Find elements at indices q1
			spot=ar.elem(q1);
			// Sum elements.
			thesum=sum(spot);
			// Set at position i.
			S(i)=thesum;
		}
		// If it is empty, add 0.
		else{
			S(i)=0.0;
		}
	}

	return S;
}

Mesh::~Mesh() {
	// TODO Auto-generated destructor stub
}