/**
 * @file instances_helper.hpp
 * @author francois.hamonic@gmail.com
 * @brief parse specific instances datas
 * @version 0.1
 * @date 2020-05-08
 */
#ifndef INSTANCES_HELPER_HPP
#define INSTANCES_HELPER_HPP

#include <math.h>
#include <algorithm>
#include <execution>
#include <numeric>
#include <random>

#include <lemon/adaptors.h>

#include <boost/range/algorithm/sort.hpp>

#include "landscape/mutable_landscape.hpp"
#include "solvers/concept/instance.hpp"

#include "fast-cpp-csv-parser/csv.h"
#include "utils/random_chooser.hpp"

void addCostNoise(Instance & instance, double deviation_ratio = 0.2,
                  int seed = 456) {
    std::default_random_engine generator(seed);
    std::normal_distribution<double> distribution(1.0, deviation_ratio);

    auto noise = [&generator, &distribution](double value) {
        return std::max(std::numeric_limits<double>::epsilon(),
                        value * distribution(generator));
    };

    for(const RestorationPlan<MutableLandscape>::Option i :
        instance.plan.options())
        instance.plan.setCost(i, noise(instance.plan.getCost(i)));
}

Instance make_instance_aude(const double median,
                            const double fish_ladder_prob) {
    Instance instance;

    MutableLandscape & landscape = instance.landscape;
    const MutableLandscape::Graph & graph = landscape.getNetwork();
    RestorationPlan<MutableLandscape> & plan = instance.plan;

    std::array<MutableLandscape::Node, 45> nodes;
    MutableLandscape::Graph::NodeMap<double> troncons_lengths(graph);
    // MutableLandscape::Graph::NodeMap<int> depth_id(graph);

    auto p = [median](const double d) {
        return std::exp(d / median * std::log(0.5));
    };

    io::CSVReader<4> patches("../landscape_opt_datas/Aude/aude.patches");
    patches.read_header(io::ignore_extra_column, "id", "length", "x", "y");
    int id;
    double length, X, Y;
    while(patches.read_row(id, length, X, Y)) {
        MutableLandscape::Node u = landscape.addNode(length, Point(X, Y));
        nodes[id] = u;
        troncons_lengths[u] = length;
    }

    io::CSVReader<3> links("../landscape_opt_datas/Aude/aude.links");
    links.read_header(io::ignore_extra_column, "source_id", "target_id", "dam");
    int source_id, target_id, dam;
    while(links.read_row(source_id, target_id, dam)) {
        MutableLandscape::Node u = nodes[source_id];
        MutableLandscape::Node v = nodes[target_id];

        const double prob = p((troncons_lengths[u] + troncons_lengths[v]) / 2);

        if(!dam) {
            MutableLandscape::Arc a =
                landscape.addArc(nodes[source_id], nodes[target_id], prob);
            continue;
        }
        MutableLandscape::Arc a =
            landscape.addArc(nodes[source_id], nodes[target_id], 0);
        RestorationPlan<MutableLandscape>::Option option = plan.addOption(1);
        plan.addArc(option, a, fish_ladder_prob * prob);
    }

    return instance;
}

Instance make_instance_quebec_leam(double pow, double thresold, double median,
                                   double decreased_prob,
                                   Point orig = Point(240548, 4986893),
                                   Point dim = Point(32360, 20000)) {
    Instance instance;

    MutableLandscape & landscape = instance.landscape;
    const MutableLandscape::Graph & graph = landscape.getNetwork();
    RestorationPlan<MutableLandscape> & plan = instance.plan;

    auto p = [median, pow](const double d) {
        return std::exp(std::pow(d, pow) / std::pow(median, pow) *
                        std::log(0.5));
    };

    std::array<MutableLandscape::Node, 8248> node_correspondance;
    node_correspondance.fill(lemon::INVALID);

    using ThreatData = struct {
        MutableLandscape::Node node;
        double area;
    };
    std::vector<ThreatData> threaten_list;

    io::CSVReader<5> patches(
        "../landscape_opt_datas/quebec_leam_v3/raw/sommets_leam_v3.csv");
    patches.read_header(io::ignore_extra_column, "count", "area", "xcoord",
                        "ycoord", "count2050");
    int id;
    double area, xcoord, ycoord, count2050;
    while(patches.read_row(id, area, xcoord, ycoord, count2050)) {
        if(xcoord < orig.x) continue;
        if(xcoord >= orig.x + dim.x) continue;
        if(ycoord < orig.y) continue;
        if(ycoord >= orig.y + dim.y) continue;

        MutableLandscape::Node u =
            landscape.addNode(count2050, Point(xcoord, ycoord));
        node_correspondance[id] = u;

        if(area == count2050) continue;
        if(area > 0 && count2050 == 0) {
            threaten_list.push_back(ThreatData{u, area});
            continue;
        }

        const double area_loss = area - count2050;
        RestorationPlan<MutableLandscape>::Option option =
            plan.addOption(area_loss);
        plan.addNode(option, u, area_loss);
    }

    io::CSVReader<3> links(
        "../landscape_opt_datas/quebec_leam_v3/raw/aretes_leam_v3.csv");
    links.read_header(io::ignore_extra_column, "from", "to", "Dist");
    int from, to;
    double Dist;
    while(links.read_row(from, to, Dist)) {
        MutableLandscape::Node u = node_correspondance[from];
        MutableLandscape::Node v = node_correspondance[to];
        if(u == lemon::INVALID || v == lemon::INVALID) continue;
        double probability = p(Dist);
        if(probability < thresold) continue;
        landscape.addArc(u, v, probability);
        landscape.addArc(v, u, probability);
    }

    for(ThreatData data : threaten_list) {
        MutableLandscape::Node v1 = data.node;
        RestorationPlan<MutableLandscape>::Option option =
            plan.addOption(data.area);
        landscape.setCoords(v1, landscape.getCoords(v1) + Point(-200, 0));
        MutableLandscape::Node v2 =
            landscape.addNode(0, landscape.getCoords(v1) + Point(200, 0));

        for(MutableLandscape::Graph::OutArcIt a(graph, v1), next_a = a;
            a != lemon::INVALID; a = next_a) {
            ++next_a;
            landscape.changeSource(a, v2);
        }
        MutableLandscape::Arc v1v2 = landscape.addArc(v1, v2, decreased_prob);
        plan.addArc(option, v1v2, 1);
        plan.addNode(option, v2, data.area);
    }

    return instance;
}

Instance make_instance_quebec_frog(double pow, double thresold, double median) {
    Instance instance;

    MutableLandscape & landscape = instance.landscape;
    const MutableLandscape::Graph & graph = landscape.getNetwork();
    RestorationPlan<MutableLandscape> & plan = instance.plan;

    auto p = [median, pow](const double d) {
        return std::exp(std::pow(d, pow) / std::pow(median, pow) *
                        std::log(0.5));
    };

    std::array<MutableLandscape::Node, 3032> node_correspondance;
    node_correspondance.fill(lemon::INVALID);

    using ThreatData = struct {
        MutableLandscape::Node node;
        double area;
    };
    std::vector<ThreatData> threaten_list;

    io::CSVReader<5> patches(
        "../landscape_opt_datas/quebec_438_RASY/vertices_438_RASY.csv");
    patches.read_header(io::ignore_extra_column, "name", "area", "xcoord",
                        "ycoord", "per_menace");
    int id;
    double area_in_2000, xcoord, ycoord, per_menace;
    while(patches.read_row(id, area_in_2000, xcoord, ycoord, per_menace)) {
        area_in_2000 /= 100;
        const double area_loss_by_2050 = area_in_2000 * per_menace;
        const double area_in_2050 = area_in_2000 - area_loss_by_2050;

        MutableLandscape::Node u =
            landscape.addNode(area_in_2050, Point(xcoord, ycoord));
        node_correspondance[id] = u;

        if(per_menace == 0) continue;
        if(per_menace == 1) {
            threaten_list.push_back(ThreatData{u, area_in_2000});
            continue;
        }

        RestorationPlan<MutableLandscape>::Option option =
            plan.addOption(area_loss_by_2050);
        plan.addNode(option, u, area_loss_by_2050);
    }

    io::CSVReader<3> links(
        "../landscape_opt_datas/quebec_438_RASY/edges_438_RASY.csv");
    links.read_header(io::ignore_extra_column, "from", "to", "Dist");
    int from, to;
    double Dist;
    while(links.read_row(from, to, Dist)) {
        MutableLandscape::Node u = node_correspondance[from];
        MutableLandscape::Node v = node_correspondance[to];
        if(u == lemon::INVALID || v == lemon::INVALID) continue;
        double probability = p(Dist);
        if(probability < thresold) continue;
        landscape.addArc(u, v, probability);
        landscape.addArc(v, u, probability);
    }

    for(ThreatData data : threaten_list) {
        MutableLandscape::Node v1 = data.node;
        RestorationPlan<MutableLandscape>::Option option =
            plan.addOption(data.area);
        landscape.setCoords(v1, landscape.getCoords(v1) + Point(-200, 0));
        MutableLandscape::Node v2 =
            landscape.addNode(0, landscape.getCoords(v1) + Point(200, 0));

        for(MutableLandscape::Graph::OutArcIt a(graph, v1), next_a = a;
            a != lemon::INVALID; a = next_a) {
            ++next_a;
            landscape.changeSource(a, v2);
        }
        MutableLandscape::Arc v1v2 = landscape.addArc(v1, v2, 0);
        plan.addArc(option, v1v2, 1);
        plan.addNode(option, v1, data.area);
    }

    return instance;
}

Instance make_instance_biorevaix_level_2_v7(const double restoration_coef = 2,
                                            const double distance_coef = 1) {
    Instance instance;
    MutableLandscape & landscape = instance.landscape;
    const MutableLandscape::Graph & graph = landscape.getNetwork();
    RestorationPlan<MutableLandscape> & plan = instance.plan;

    auto prob = [distance_coef](const double cost) {
        return cost == 1     ? std::pow(1, distance_coef)
               : cost == 10  ? std::pow(0.98, distance_coef)
               : cost == 150 ? std::pow(0.8, distance_coef)
               : cost == 300 ? std::pow(0.6, distance_coef)
               : cost == 800 ? std::pow(0.4, distance_coef)
                             : 0;
    };

    std::array<MutableLandscape::Node, 688402> nodes;
    nodes.fill(lemon::INVALID);
    MutableLandscape::Graph::NodeMap<double> node_prob(graph, 0.0);
    MutableLandscape::Graph::NodeMap<RestorationPlan<MutableLandscape>::Option>
        troncon_option(graph);
    std::array<RestorationPlan<MutableLandscape>::Option, 2008> id_tronc_option;
    id_tronc_option.fill(-1);

    io::CSVReader<7> patches(
        "../landscape_opt_datas/BiorevAix/vertexN2_v7.txt");
    patches.read_header(io::ignore_extra_column, "N2_id", "X", "Y", "N4_id",
                        "area2", "area4", "cost_mode");
    int N2_id, N4_id;
    int area2, area4;
    double X, Y, cost;
    while(patches.read_row(N2_id, X, Y, N4_id, area2, area4, cost)) {
        if(!area2) continue;
        // if(!area4) continue;
        if(cost == 1000) continue;
        MutableLandscape::Node u =
            landscape.addNode((cost == 1 ? 1 : 0), Point(X, Y));
        nodes[N2_id] = u;
        node_prob[u] = prob(cost);
        troncon_option[u] = -1;

        if(cost != 800) continue;
        RestorationPlan<MutableLandscape>::Option & option =
            id_tronc_option[N4_id];
        if(option == -1) option = plan.addOption(0);
        troncon_option[u] = option;
        plan.setCost(option, plan.getCost(option) + 1);
    }

    io::CSVReader<2> links("../landscape_opt_datas/BiorevAix/AL_N2.txt");
    links.read_header(io::ignore_extra_column, "from", "to");
    int from, to;
    while(links.read_row(from, to)) {
        const MutableLandscape::Node u = nodes[from];
        const MutableLandscape::Node v = nodes[to];
        if(u == lemon::INVALID || v == lemon::INVALID) continue;
        if(node_prob[u] == 0 || node_prob[v] == 0) continue;

        RestorationPlan<MutableLandscape>::Option option_u = troncon_option[u];
        RestorationPlan<MutableLandscape>::Option option_v = troncon_option[v];
        if(option_u > 0 && option_v > 0 && option_u != option_v) {
            const MutableLandscape::Node w = landscape.addNode(
                0, (landscape.getCoords(u) + landscape.getCoords(v)) / 2);

            const MutableLandscape::Arc uw =
                landscape.addArc(u, w, std::sqrt(node_prob[u]));
            const MutableLandscape::Arc wu =
                landscape.addArc(w, u, std::sqrt(node_prob[u]));
            const MutableLandscape::Arc wv =
                landscape.addArc(w, v, std::sqrt(node_prob[v]));
            const MutableLandscape::Arc vw =
                landscape.addArc(v, w, std::sqrt(node_prob[v]));

            const double restored_prob_u =
                std::pow(node_prob[u], 1 / (2 * restoration_coef));
            const double restored_prob_v =
                std::pow(node_prob[v], 1 / (2 * restoration_coef));

            plan.addArc(option_u, uw, restored_prob_u);
            plan.addArc(option_u, wu, restored_prob_u);
            plan.addArc(option_v, vw, restored_prob_v);
            plan.addArc(option_v, wv, restored_prob_v);
            continue;
        };

        double probability = std::sqrt(node_prob[u] * node_prob[v]);
        probability = std::max(std::min(probability, 1.0), 0.0);

        const MutableLandscape::Arc uv = landscape.addArc(u, v, probability);
        const MutableLandscape::Arc vu = landscape.addArc(v, u, probability);

        if(option_u < 0 && option_v < 0) continue;

        RestorationPlan<MutableLandscape>::Option option =
            std::max(option_u, option_v);
        double restored_prob =
            std::pow(node_prob[u],
                     1 / (2 * (option_u >= 0 ? restoration_coef : 1))) *
            std::pow(node_prob[v],
                     1 / (2 * (option_v >= 0 ? restoration_coef : 1)));
        if(restored_prob <= probability) continue;
        plan.addArc(option, uv, restored_prob);
        plan.addArc(option, vu, restored_prob);
    }
    return instance;
}

Instance make_instance_biorevaix_level_2_all_troncons(
    const double restoration_coef = 2, const double distance_coef = 1) {
    Instance instance;
    MutableLandscape & landscape = instance.landscape;
    const MutableLandscape::Graph & graph = landscape.getNetwork();
    RestorationPlan<MutableLandscape> & plan = instance.plan;

    auto prob = [distance_coef](const double cost) {
        return cost == 1     ? std::pow(1, distance_coef)
               : cost == 10  ? std::pow(0.98, distance_coef)
               : cost == 150 ? std::pow(0.8, distance_coef)
               : cost == 300 ? std::pow(0.6, distance_coef)
               : cost == 800 ? std::pow(0.4, distance_coef)
                             : 0;
    };

    std::array<MutableLandscape::Node, 98344> nodes;
    nodes.fill(lemon::INVALID);
    MutableLandscape::Graph::NodeMap<double> node_prob(graph, 0.0);

    io::CSVReader<6> patches(
        "../landscape_opt_datas/BiorevAix/vertexN2_v7.txt");
    patches.read_header(io::ignore_extra_column, "N2_id", "X", "Y", "area2",
                        "area4", "cost_mode");
    int N2_id;
    int area2, area4;
    double X, Y, cost;
    while(patches.read_row(N2_id, X, Y, area2, area4, cost)) {
        // if(!area2) continue;
        // if(!area4) continue;
        if(cost == 1000) continue;
        MutableLandscape::Node u =
            landscape.addNode((cost == 1 ? 1 : 0), Point(X, Y));
        nodes[N2_id] = u;
        node_prob[u] = prob(cost);
    }

    io::CSVReader<2> links("../landscape_opt_datas/BiorevAix/AL_N2.txt");
    links.read_header(io::ignore_extra_column, "from", "to");
    int from, to;
    while(links.read_row(from, to)) {
        const MutableLandscape::Node u = nodes[from];
        const MutableLandscape::Node v = nodes[to];
        if(u == lemon::INVALID || v == lemon::INVALID) continue;
        if(node_prob[u] == 0 || node_prob[v] == 0) continue;

        double probability = std::sqrt(node_prob[u] * node_prob[v]);
        probability = std::max(std::min(probability, 1.0), 0.0);

        const MutableLandscape::Arc uv = landscape.addArc(u, v, probability);
        const MutableLandscape::Arc vu = landscape.addArc(v, u, probability);
    }

    std::array<RestorationPlan<MutableLandscape>::Option, 5460> troncon_option;
    for(int i=0; i<5460; ++i) {
        troncon_option[i] = plan.addOption(0);
    }
    io::CSVReader<2> troncons(
        "../landscape_opt_datas/BiorevAix/croisemt_troncon_hexagN2.txt");
    troncons.read_header(io::ignore_extra_column, "troncon_id", "N2_id");
    int troncon_id;
    while(troncons.read_row(troncon_id, N2_id)) {
        const MutableLandscape::Node u = nodes[N2_id];

        RestorationPlan<MutableLandscape>::Option option =
            troncon_option[troncon_id];

        plan.setCost(option, plan.getCost(option) + 1);

        for(MutableLandscape::Graph::OutArcIt uv(graph, u);
            uv != lemon::INVALID; ++uv) {
            const MutableLandscape::Node v = graph.target(uv);
            const double probability = landscape.getProbability(uv);
            double restored_prob =
                std::pow(node_prob[u], 1 / (2 * restoration_coef)) *
                std::pow(node_prob[v], 1 / 2);
            if(restored_prob <= probability) continue;
            plan.addArc(option, uv, restored_prob);
        }
    }
    return instance;
}

Instance make_instance_marseille(double pow, double thresold, double median,
                                 int nb_friches = 100) {
    Instance instance;

    MutableLandscape & landscape = instance.landscape;
    const MutableLandscape::Graph & graph = landscape.getNetwork();
    RestorationPlan<MutableLandscape> & plan = instance.plan;

    auto d = [&landscape](MutableLandscape::Node u, MutableLandscape::Node v) {
        return std::sqrt(
            (landscape.getCoords(u) - landscape.getCoords(v)).normSquare());
    };
    auto p = [median, pow](const double d) {
        return std::exp(std::pow(d, pow) / std::pow(median, pow) *
                        std::log(0.5));
    };

    using FricheData = struct {
        Point p;
        double area;
        double price;
        MutableLandscape::Node node;
    };
    RandomChooser<FricheData> friches_chooser(9876);
    std::vector<FricheData> friches_list;

    io::CSVReader<5> patches(
        "../landscape_opt_datas/Marseille/vertices_marseillec.csv");
    patches.read_header(io::ignore_extra_column, "category", "x", "y", "area2",
                        "price_rel");
    std::string category;
    double x, y, area, price_rel;
    while(patches.read_row(category, x, y, area, price_rel)) {
        if(category.compare("\"massif\"") == 0) {
            landscape.addNode(20, Point(x, y));
            continue;
        }
        if(category.compare("\"parc\"") == 0) {
            landscape.addNode(area, Point(x, y));
            continue;
        }
        if(category.compare("\"friche\"") == 0) {
            friches_chooser.add(
                FricheData{Point(x, y), area, price_rel * area, lemon::INVALID},
                1);
            continue;
        }
        assert(false);
    }
    for(int i = 0; i < nb_friches; i++) {
        if(!friches_chooser.canPick()) break;
        FricheData data = friches_chooser.pick();
        MutableLandscape::Node u = landscape.addNode(0, data.p);
        data.node = u;
        friches_list.push_back(data);
    }

    for(MutableLandscape::NodeIt u(graph); u != lemon::INVALID; ++u) {
        for(MutableLandscape::NodeIt v(graph); v != lemon::INVALID; ++v) {
            if(v < u || u == v) continue;
            double dist = d(u, v);
            double probability = p(dist);
            if(probability < thresold) continue;
            landscape.addArc(u, v, probability);
            landscape.addArc(v, u, probability);
        }
    }

    for(FricheData data : friches_list) {
        MutableLandscape::Node v1 = data.node;
        RestorationPlan<MutableLandscape>::Option option =
            plan.addOption(data.price);
        MutableLandscape::Node v2 = landscape.addNode(
            0, landscape.getCoords(v1) + Point(0.0001, 0.0001));

        for(MutableLandscape::Graph::OutArcIt a(graph, v1), next_a = a;
            a != lemon::INVALID; a = next_a) {
            ++next_a;
            landscape.changeSource(a, v2);
        }
        MutableLandscape::Arc v1v2 = landscape.addArc(v1, v2, 0);
        plan.addArc(option, v1v2, 1);
        plan.addNode(option, v2, data.area);
    }

    return instance;
}

#endif  // INSTANCES_HELPER_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2011 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file exponentialjump1dmesher.cpp
    \brief mesher for a exponential jump mesher with high
           mean reversion rate and low jump intensity
*/

#include <ql/math/incompletegamma.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/math/distributions/gammadistribution.hpp>
#include <ql/methods/finitedifferences/meshers/exponentialjump1dmesher.hpp>

#include <boost/bind.hpp>

namespace QuantLib {
    ExponentialJump1dMesher::ExponentialJump1dMesher(
          Size steps, Real beta, Real jumpIntensity, Real eta, Real eps)
    : Fdm1dMesher(steps),
      beta_(beta), jumpIntensity_(jumpIntensity), eta_(eta)
   {
        QL_REQUIRE(eps > 0.0 && eps < 1.0, "eps > 0.0 and eps < 1.0");
        QL_REQUIRE(steps > 1, "minimum number of steps is two");

        const Real start = 0.0;
        const Real end   = 1.0-eps;
        const Real dx    = (end-start)/(steps-1);

        for (Size i=0; i < steps; ++i) {
            const Real p = start + i*dx;
            locations_[i] = -1.0/eta*std::log(1.0-p);
        }
        for (Size i=0; i < steps-1; ++i) {
            dminus_[i+1] = dplus_[i] = locations_[i+1]-locations_[i];
        }
        dplus_.back() = dminus_.front() = Null<Real>();
    }

    Real ExponentialJump1dMesher::jumpSizeDensity(Real x, Time t) const {
        const Real a    = 1.0-jumpIntensity_/beta_;
        const Real norm = 1.0-std::exp(-jumpIntensity_*t);
        const Real gammaValue
            = std::exp(GammaFunction().logValue(1.0-jumpIntensity_/beta_));
        return jumpIntensity_*gammaValue/norm
                    *( incompleteGammaFunction(a, x*std::exp(beta_*t)*eta_)
                      -incompleteGammaFunction(a, x*eta_))
                    *std::pow(eta_, jumpIntensity_/beta_)
                    /(beta_*std::pow(x, a));
    }

    Real ExponentialJump1dMesher::jumpSizeDensity(Real x) const {
        const Real a = 1.0-jumpIntensity_/beta_;
        const Real gammaValue
                = std::exp(GammaFunction().logValue(jumpIntensity_/beta_));
        return std::exp(-x*eta_)*std::pow(x, -a) * std::pow(eta_, 1.0-a)
                / gammaValue;
    }

    Real ExponentialJump1dMesher::jumpSizeDistribution(Real x, Time t) const {
        const Real xmin = std::min(x, 1.0e-100);

        return GaussLobattoIntegral(1000000, 1e-12)(
            boost::bind(&ExponentialJump1dMesher::jumpSizeDensity, this, _1, t),
            xmin, std::max(x, xmin));
    }

    Real ExponentialJump1dMesher::jumpSizeDistribution(Real x) const {
        const Real a    = jumpIntensity_/beta_;
        const Real xmin = std::min(x, QL_EPSILON);
        const Real gammaValue
                = std::exp(GammaFunction().logValue(jumpIntensity_/beta_));

        const Real lowerEps =
            (std::pow(xmin, a)/a - std::pow(xmin, a+1)/(a+1))/gammaValue;

        return lowerEps + GaussLobattoIntegral(10000, 1e-12)(
            boost::bind(&ExponentialJump1dMesher::jumpSizeDensity, this, _1),
            xmin/eta_, std::max(x, xmin/eta_));
    }
}


#ifndef gaussian_process_regression_hpp
#define gaussian_process_regression_hpp

#include <Eigen/Core>

namespace mathtoolbox
{
    class GaussianProcessRegression
    {
    public:
        // Construction with input data
        GaussianProcessRegression(const Eigen::MatrixXd& X, const Eigen::VectorXd& y);

        // Estimation methods
        double EstimateY(const Eigen::VectorXd& x) const;
        double EstimateVariance(const Eigen::VectorXd& x) const;

        // Hyperparameters setup methods
        void SetHyperparameters(double sigma_squared_f,
                                double sigma_squared_n,
                                const Eigen::VectorXd& length_scales);
        void PerformMaximumLikelihood(double sigma_squared_f_initial,
                                      double sigma_squared_n_initial,
                                      const Eigen::VectorXd& length_scales_initial);

        // Getter methods
        const Eigen::MatrixXd& GetX() const { return X; }
        const Eigen::VectorXd& GetY() const { return y; }

    private:

        // Data points
        Eigen::MatrixXd X;
        Eigen::VectorXd y;

        // Derivative data
        Eigen::MatrixXd K;
        Eigen::MatrixXd K_inv;

        // Hyperparameters
        double          sigma_squared_f;
        double          sigma_squared_n;
        Eigen::VectorXd length_scales;
    };
}

#endif /* gaussian_process_regression_hpp */


/**
 * @file
 * @brief Implementation of JacobianInverseGramian() test for geometry objects
 * @author Anian Ruoss
 * @date   2019-02-11 17:53:17
 * @copyright MIT License
 */

#include "check_jacobian_inverse_gramian.h"

#include <gtest/gtest.h>

#include <Eigen/Eigen>

namespace lf::geometry::test_utils {

void checkJacobianInverseGramian(const lf::geometry::Geometry &geom,
                                 const Eigen::MatrixXd &eval_points) {
  const size_t num_points = eval_points.cols();
  const size_t dim_local = geom.DimLocal();
  const size_t dim_global = geom.DimGlobal();

  Eigen::MatrixXd jacobians = geom.Jacobian(eval_points);
  Eigen::MatrixXd jacInvGrams = geom.JacobianInverseGramian(eval_points);

  EXPECT_EQ(jacInvGrams.rows(), dim_global)
      << "JacobianInverseGramian has " << jacInvGrams.rows()
      << " rows instead of " << dim_global;
  EXPECT_EQ(jacInvGrams.cols(), num_points * dim_local)
      << "JacobianInverseGramian has " << jacInvGrams.cols()
      << " cols instead of " << num_points * dim_local;

  for (int j = 0; j < num_points; ++j) {
    Eigen::MatrixXd jacInvGram =
        jacInvGrams.block(0, j * dim_local, dim_global, dim_local);
    Eigen::MatrixXd jacobian =
        jacobians.block(0, j * dim_local, dim_global, dim_local);

    EXPECT_TRUE(jacInvGram.isApprox(
        jacobian * (jacobian.transpose() * jacobian).inverse()))
        << "JacobianInverseGramian incorrect at point " << eval_points.col(j);
  }
}

}  // namespace lf::geometry::test_utils


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010,2012 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

template<typename MatrixType> void schur(int size = MatrixType::ColsAtCompileTime)
{
  typedef typename ComplexSchur<MatrixType>::ComplexScalar ComplexScalar;
  typedef typename ComplexSchur<MatrixType>::ComplexMatrixType ComplexMatrixType;

  // Test basic functionality: T is triangular and A = U T U*
  for(int counter = 0; counter < g_repeat; ++counter) {
    MatrixType A = MatrixType::Random(size, size);
    ComplexSchur<MatrixType> schurOfA(A);
    VERIFY_IS_EQUAL(schurOfA.info(), Success);
    ComplexMatrixType U = schurOfA.matrixU();
    ComplexMatrixType T = schurOfA.matrixT();
    for(int row = 1; row < size; ++row) {
      for(int col = 0; col < row; ++col) {
        VERIFY(T(row,col) == (typename MatrixType::Scalar)0);
      }
    }
    VERIFY_IS_APPROX(A.template cast<ComplexScalar>(), U * T * U.adjoint());
  }

  // Test asserts when not initialized
  ComplexSchur<MatrixType> csUninitialized;
  VERIFY_RAISES_ASSERT(csUninitialized.matrixT());
  VERIFY_RAISES_ASSERT(csUninitialized.matrixU());
  VERIFY_RAISES_ASSERT(csUninitialized.info());

  // Test whether compute() and constructor returns same result
  MatrixType A = MatrixType::Random(size, size);
  ComplexSchur<MatrixType> cs1;
  cs1.compute(A);
  ComplexSchur<MatrixType> cs2(A);
  VERIFY_IS_EQUAL(cs1.info(), Success);
  VERIFY_IS_EQUAL(cs2.info(), Success);
  VERIFY_IS_EQUAL(cs1.matrixT(), cs2.matrixT());
  VERIFY_IS_EQUAL(cs1.matrixU(), cs2.matrixU());

  // Test maximum number of iterations
  ComplexSchur<MatrixType> cs3;
  cs3.setMaxIterations(ComplexSchur<MatrixType>::m_maxIterationsPerRow * size).compute(A);
  VERIFY_IS_EQUAL(cs3.info(), Success);
  VERIFY_IS_EQUAL(cs3.matrixT(), cs1.matrixT());
  VERIFY_IS_EQUAL(cs3.matrixU(), cs1.matrixU());
  cs3.setMaxIterations(1).compute(A);
  VERIFY_IS_EQUAL(cs3.info(), size > 1 ? NoConvergence : Success);
  VERIFY_IS_EQUAL(cs3.getMaxIterations(), 1);

  MatrixType Atriangular = A;
  Atriangular.template triangularView<StrictlyLower>().setZero();
  cs3.setMaxIterations(1).compute(Atriangular); // triangular matrices do not need any iterations
  VERIFY_IS_EQUAL(cs3.info(), Success);
  VERIFY_IS_EQUAL(cs3.matrixT(), Atriangular.template cast<ComplexScalar>());
  VERIFY_IS_EQUAL(cs3.matrixU(), ComplexMatrixType::Identity(size, size));

  // Test computation of only T, not U
  ComplexSchur<MatrixType> csOnlyT(A, false);
  VERIFY_IS_EQUAL(csOnlyT.info(), Success);
  VERIFY_IS_EQUAL(cs1.matrixT(), csOnlyT.matrixT());
  VERIFY_RAISES_ASSERT(csOnlyT.matrixU());

  if (size > 1 && size < 20)
  {
    // Test matrix with NaN
    A(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN();
    ComplexSchur<MatrixType> csNaN(A);
    VERIFY_IS_EQUAL(csNaN.info(), NoConvergence);
  }
}

void test_schur_complex()
{
  CALL_SUBTEST_1(( schur<Matrix4cd>() ));
  CALL_SUBTEST_2(( schur<MatrixXcf>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4)) ));
  CALL_SUBTEST_3(( schur<Matrix<std::complex<float>, 1, 1> >() ));
  CALL_SUBTEST_4(( schur<Matrix<float, 3, 3, Eigen::RowMajor> >() ));

  // Test problem size constructors
  CALL_SUBTEST_5(ComplexSchur<MatrixXf>(10));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

//[equals
//` Shows the predicate equals, which returns true if two geometries are spatially equal

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

#include <boost/assign.hpp>

int main()
{
    using boost::assign::tuple_list_of;

    typedef boost::tuple<int, int> point;

    boost::geometry::model::polygon<point> poly1, poly2;
    boost::geometry::exterior_ring(poly1) = tuple_list_of(0, 0)(0, 5)(5, 5)(5, 0)(0, 0);
    boost::geometry::exterior_ring(poly2) = tuple_list_of(5, 0)(0, 0)(0, 5)(5, 5)(5, 0);

    std::cout
        << "polygons are spatially "
        << (boost::geometry::equals(poly1, poly2) ? "equal" : "not equal")
        << std::endl;

    boost::geometry::model::box<point> box;
    boost::geometry::assign_values(box, 0, 0, 5, 5);

    std::cout
        << "polygon and box are spatially "
        << (boost::geometry::equals(box, poly2) ? "equal" : "not equal")
        << std::endl;

    return 0;
}

//]

//[equals_output
/*`
Output:
[pre
polygons are spatially equal
polygon and box are spatially equal
]
*/
//]


/* Boost numeric test of the adams-bashforth steppers test file

 Copyright 2013 Karsten Ahnert
 Copyright 2013-2015 Mario Mulansky

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE numeric_adams_bashforth

#include <iostream>
#include <cmath>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef double value_type;

typedef boost::array< double , 2 > state_type;

// harmonic oscillator, analytic solution x[0] = sin( t )
struct osc
{
    void operator()( const state_type &x , state_type &dxdt , const double t ) const
    {
        dxdt[0] = x[1];
        dxdt[1] = -x[0];
    }
};

BOOST_AUTO_TEST_SUITE( numeric_adams_bashforth_test )

/* generic test for all adams bashforth steppers */
template< class Stepper >
struct perform_adams_bashforth_test
{
    void operator()( void )
    {
        Stepper stepper;
        const int o = stepper.order()+1; //order of the error is order of approximation + 1

        const state_type x0 = {{ 0.0 , 1.0 }};
        state_type x1 = x0;
        double t = 0.0;
        double dt = 0.2;
        // initialization, does a number of steps already to fill internal buffer, t is increased
        stepper.initialize( osc() , x1 , t , dt );
        double A = std::sqrt( x1[0]*x1[0] + x1[1]*x1[1] );
        double phi = std::asin(x1[0]/A) - t;
        // do a number of steps to fill the buffer with results from adams bashforth
        for( size_t n=0 ; n < stepper.steps ; ++n )
        {
            stepper.do_step( osc() , x1 , t , dt );
            t += dt;
        }
        // now we do the actual step
        stepper.do_step( osc() , x1 , t , dt );
        // only examine the error of the adams-bashforth step, not the initialization
        const double f = 2.0 * std::abs( A*sin(t+dt+phi) - x1[0] ) / std::pow( dt , o ); // upper bound

        std::cout << o << " , "
                  << Stepper::initializing_stepper_type::order_value+1 << " , "
                  << f << std::endl;

        /* as long as we have errors above machine precision */
        while( f*std::pow( dt , o ) > 1E-16 )
        {
            x1 = x0;
            t = 0.0;
            stepper.initialize( osc() , x1 , t , dt );
            A = std::sqrt( x1[0]*x1[0] + x1[1]*x1[1] );
            phi = std::asin(x1[0]/A) - t;
            // now we do the actual step
            stepper.do_step( osc() , x1 , t , dt );
            // only examine the error of the adams-bashforth step, not the initialization
            std::cout << "Testing dt=" << dt << " , " << std::abs( A*sin(t+dt+phi) - x1[0] ) << std::endl;
            BOOST_CHECK_LT( std::abs( A*sin(t+dt+phi) - x1[0] ) , f*std::pow( dt , o ) );
            dt *= 0.5;
        }
    }
};

typedef mpl::vector<
    adams_bashforth< 2 , state_type > ,
    adams_bashforth< 3 , state_type > ,
    adams_bashforth< 4 , state_type > ,
    adams_bashforth< 5 , state_type > ,
    adams_bashforth< 6 , state_type > ,
    adams_bashforth< 7 , state_type > ,
    adams_bashforth< 8 , state_type >
    > adams_bashforth_steppers;

BOOST_AUTO_TEST_CASE_TEMPLATE( adams_bashforth_test , Stepper, adams_bashforth_steppers )
{
    perform_adams_bashforth_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()


#include "spline.h"

#include <Eigen/Dense>
#include <cmath>
#include <iomanip>
#include <iostream>

#include "quadpp.h"

int main()
{
  const std::size_t npts = 1001;
  const double low = 0, high = 1;

  Eigen::ArrayXd xs, rs, ys, jac;
  quadpp::SemiInfiniteIntegralMesh(npts, low, high, xs, rs, jac);
  ys = jac * Eigen::exp(-rs);
  ys.tail(1) = 0;

  auto result1 =
      quadpp::spline::Integrate(xs, ys, quadpp::spline::SplineType::Steffen);
  auto result2 =
      quadpp::spline::Integrate(xs, ys, quadpp::spline::SplineType::Cubic);
  auto result3 =
      quadpp::spline::Integrate(xs, ys, quadpp::spline::SplineType::Akima);

  std::cout << "Integrating exp(-r) from 0 to ∞."
            << "\n";
  std::cout << "Using Steffen splines: " << std::setprecision(10) << std::fixed
            << result1 << "\n";
  std::cout << "Using Cubic splines: " << result2 << "\n";
  std::cout << "Using Akima splines: " << result3 << "\n";
}


// (C) Copyright 2007-2009 Andrew Sutton
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <vector>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/directed_graph.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/closeness_centrality.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/property_maps/constant_property_map.hpp>

using namespace std;
using namespace boost;

// number of vertices in the graph
static const unsigned N = 5;

template <typename Graph>
struct vertex_vector
{
    typedef graph_traits<Graph> traits;
    typedef vector<typename traits::vertex_descriptor> type;
};

template <typename Graph>
void build_graph(Graph& g,
                 typename vertex_vector<Graph>::type& v)
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    // add vertices
    for(size_t i = 0; i < N; ++i) {
        v[i] = add_vertex(g);
    }

    // add edges
    add_edge(v[0], v[1], g);
    add_edge(v[1], v[2], g);
    add_edge(v[2], v[0], g);
    add_edge(v[3], v[4], g);
    add_edge(v[4], v[0], g);
}

template <typename Graph>
void test_undirected()
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor Edge;

    typedef exterior_vertex_property<Graph, double> CentralityProperty;
    typedef typename CentralityProperty::container_type CentralityContainer;
    typedef typename CentralityProperty::map_type CentralityMap;

    typedef exterior_vertex_property<Graph, int> DistanceProperty;
    typedef typename DistanceProperty::matrix_type DistanceMatrix;
    typedef typename DistanceProperty::matrix_map_type DistanceMatrixMap;

    typedef constant_property_map<Edge, int> WeightMap;

    Graph g;
    vector<Vertex> v(N);
    build_graph(g, v);

    CentralityContainer centralities(num_vertices(g));
    DistanceMatrix distances(num_vertices(g));

    CentralityMap cm(centralities, g);
    DistanceMatrixMap dm(distances, g);

    WeightMap wm(1);

    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));
    all_closeness_centralities(g, dm, cm);

    BOOST_ASSERT(cm[v[0]] == double(1)/5);
    BOOST_ASSERT(cm[v[1]] == double(1)/7);
    BOOST_ASSERT(cm[v[2]] == double(1)/7);
    BOOST_ASSERT(cm[v[3]] == double(1)/9);
    BOOST_ASSERT(cm[v[4]] == double(1)/6);
}

template <typename Graph>
void test_directed()
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor Edge;

    typedef exterior_vertex_property<Graph, double> CentralityProperty;
    typedef typename CentralityProperty::container_type CentralityContainer;
    typedef typename CentralityProperty::map_type CentralityMap;

    typedef exterior_vertex_property<Graph, int> DistanceProperty;
    typedef typename DistanceProperty::matrix_type DistanceMatrix;
    typedef typename DistanceProperty::matrix_map_type DistanceMatrixMap;

    typedef constant_property_map<Edge, int> WeightMap;

    Graph g;
    vector<Vertex> v(N);
    build_graph(g, v);

    CentralityContainer centralities(num_vertices(g));
    DistanceMatrix distances(num_vertices(g));

    CentralityMap cm(centralities, g);
    DistanceMatrixMap dm(distances, g);

    WeightMap wm(1);

    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));
    all_closeness_centralities(g, dm, cm);

    BOOST_ASSERT(cm[v[0]] == double(0));
    BOOST_ASSERT(cm[v[1]] == double(0));
    BOOST_ASSERT(cm[v[2]] == double(0));
    BOOST_ASSERT(cm[v[3]] == double(1)/10);
    BOOST_ASSERT(cm[v[4]] == double(0));
}

int
main(int, char *[])
{
    typedef undirected_graph<> Graph;
    typedef directed_graph<> Digraph;

    test_undirected<Graph>();
    test_directed<Digraph>();
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/QR>
#include <Eigen/SVD>

template <typename MatrixType>
void cod() {
  typedef typename MatrixType::Index Index;

  Index rows = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
  Index cols = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
  Index cols2 = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
  Index rank = internal::random<Index>(1, (std::min)(rows, cols) - 1);

  typedef typename MatrixType::Scalar Scalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime,
                 MatrixType::RowsAtCompileTime>
      MatrixQType;
  MatrixType matrix;
  createRandomPIMatrixOfRank(rank, rows, cols, matrix);
  CompleteOrthogonalDecomposition<MatrixType> cod(matrix);
  VERIFY(rank == cod.rank());
  VERIFY(cols - cod.rank() == cod.dimensionOfKernel());
  VERIFY(!cod.isInjective());
  VERIFY(!cod.isInvertible());
  VERIFY(!cod.isSurjective());

  MatrixQType q = cod.householderQ();
  VERIFY_IS_UNITARY(q);

  MatrixType z = cod.matrixZ();
  VERIFY_IS_UNITARY(z);

  MatrixType t;
  t.setZero(rows, cols);
  t.topLeftCorner(rank, rank) =
      cod.matrixT().topLeftCorner(rank, rank).template triangularView<Upper>();

  MatrixType c = q * t * z * cod.colsPermutation().inverse();
  VERIFY_IS_APPROX(matrix, c);

  MatrixType exact_solution = MatrixType::Random(cols, cols2);
  MatrixType rhs = matrix * exact_solution;
  MatrixType cod_solution = cod.solve(rhs);
  VERIFY_IS_APPROX(rhs, matrix * cod_solution);

  // Verify that we get the same minimum-norm solution as the SVD.
  JacobiSVD<MatrixType> svd(matrix, ComputeThinU | ComputeThinV);
  MatrixType svd_solution = svd.solve(rhs);
  VERIFY_IS_APPROX(cod_solution, svd_solution);

  MatrixType pinv = cod.pseudoInverse();
  VERIFY_IS_APPROX(cod_solution, pinv * rhs);
}

template <typename MatrixType, int Cols2>
void cod_fixedsize() {
  enum {
    Rows = MatrixType::RowsAtCompileTime,
    Cols = MatrixType::ColsAtCompileTime
  };
  typedef typename MatrixType::Scalar Scalar;
  int rank = internal::random<int>(1, (std::min)(int(Rows), int(Cols)) - 1);
  Matrix<Scalar, Rows, Cols> matrix;
  createRandomPIMatrixOfRank(rank, Rows, Cols, matrix);
  CompleteOrthogonalDecomposition<Matrix<Scalar, Rows, Cols> > cod(matrix);
  VERIFY(rank == cod.rank());
  VERIFY(Cols - cod.rank() == cod.dimensionOfKernel());
  VERIFY(cod.isInjective() == (rank == Rows));
  VERIFY(cod.isSurjective() == (rank == Cols));
  VERIFY(cod.isInvertible() == (cod.isInjective() && cod.isSurjective()));

  Matrix<Scalar, Cols, Cols2> exact_solution;
  exact_solution.setRandom(Cols, Cols2);
  Matrix<Scalar, Rows, Cols2> rhs = matrix * exact_solution;
  Matrix<Scalar, Cols, Cols2> cod_solution = cod.solve(rhs);
  VERIFY_IS_APPROX(rhs, matrix * cod_solution);

  // Verify that we get the same minimum-norm solution as the SVD.
  JacobiSVD<MatrixType> svd(matrix, ComputeFullU | ComputeFullV);
  Matrix<Scalar, Cols, Cols2> svd_solution = svd.solve(rhs);
  VERIFY_IS_APPROX(cod_solution, svd_solution);
}

template<typename MatrixType> void qr()
{
  using std::sqrt;
  typedef typename MatrixType::Index Index;

  Index rows = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE), cols = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE), cols2 = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE);
  Index rank = internal::random<Index>(1, (std::min)(rows, cols)-1);

  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::RealScalar RealScalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> MatrixQType;
  MatrixType m1;
  createRandomPIMatrixOfRank(rank,rows,cols,m1);
  ColPivHouseholderQR<MatrixType> qr(m1);
  VERIFY_IS_EQUAL(rank, qr.rank());
  VERIFY_IS_EQUAL(cols - qr.rank(), qr.dimensionOfKernel());
  VERIFY(!qr.isInjective());
  VERIFY(!qr.isInvertible());
  VERIFY(!qr.isSurjective());

  MatrixQType q = qr.householderQ();
  VERIFY_IS_UNITARY(q);

  MatrixType r = qr.matrixQR().template triangularView<Upper>();
  MatrixType c = q * r * qr.colsPermutation().inverse();
  VERIFY_IS_APPROX(m1, c);

  // Verify that the absolute value of the diagonal elements in R are
  // non-increasing until they reach the singularity threshold.
  RealScalar threshold =
      sqrt(RealScalar(rows)) * numext::abs(r(0, 0)) * NumTraits<Scalar>::epsilon();
  for (Index i = 0; i < (std::min)(rows, cols) - 1; ++i) {
    RealScalar x = numext::abs(r(i, i));
    RealScalar y = numext::abs(r(i + 1, i + 1));
    if (x < threshold && y < threshold) continue;
    if (!test_isApproxOrLessThan(y, x)) {
      for (Index j = 0; j < (std::min)(rows, cols); ++j) {
        std::cout << "i = " << j << ", |r_ii| = " << numext::abs(r(j, j)) << std::endl;
      }
      std::cout << "Failure at i=" << i << ", rank=" << rank
                << ", threshold=" << threshold << std::endl;
    }
    VERIFY_IS_APPROX_OR_LESS_THAN(y, x);
  }

  MatrixType m2 = MatrixType::Random(cols,cols2);
  MatrixType m3 = m1*m2;
  m2 = MatrixType::Random(cols,cols2);
  m2 = qr.solve(m3);
  VERIFY_IS_APPROX(m3, m1*m2);
}

template<typename MatrixType, int Cols2> void qr_fixedsize()
{
  using std::sqrt;
  using std::abs;
  enum { Rows = MatrixType::RowsAtCompileTime, Cols = MatrixType::ColsAtCompileTime };
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::RealScalar RealScalar;
  int rank = internal::random<int>(1, (std::min)(int(Rows), int(Cols))-1);
  Matrix<Scalar,Rows,Cols> m1;
  createRandomPIMatrixOfRank(rank,Rows,Cols,m1);
  ColPivHouseholderQR<Matrix<Scalar,Rows,Cols> > qr(m1);
  VERIFY_IS_EQUAL(rank, qr.rank());
  VERIFY_IS_EQUAL(Cols - qr.rank(), qr.dimensionOfKernel());
  VERIFY_IS_EQUAL(qr.isInjective(), (rank == Rows));
  VERIFY_IS_EQUAL(qr.isSurjective(), (rank == Cols));
  VERIFY_IS_EQUAL(qr.isInvertible(), (qr.isInjective() && qr.isSurjective()));

  Matrix<Scalar,Rows,Cols> r = qr.matrixQR().template triangularView<Upper>();
  Matrix<Scalar,Rows,Cols> c = qr.householderQ() * r * qr.colsPermutation().inverse();
  VERIFY_IS_APPROX(m1, c);

  Matrix<Scalar,Cols,Cols2> m2 = Matrix<Scalar,Cols,Cols2>::Random(Cols,Cols2);
  Matrix<Scalar,Rows,Cols2> m3 = m1*m2;
  m2 = Matrix<Scalar,Cols,Cols2>::Random(Cols,Cols2);
  m2 = qr.solve(m3);
  VERIFY_IS_APPROX(m3, m1*m2);
  // Verify that the absolute value of the diagonal elements in R are
  // non-increasing until they reache the singularity threshold.
  RealScalar threshold =
      sqrt(RealScalar(Rows)) * (std::abs)(r(0, 0)) * NumTraits<Scalar>::epsilon();
  for (Index i = 0; i < (std::min)(int(Rows), int(Cols)) - 1; ++i) {
    RealScalar x = numext::abs(r(i, i));
    RealScalar y = numext::abs(r(i + 1, i + 1));
    if (x < threshold && y < threshold) continue;
    if (!test_isApproxOrLessThan(y, x)) {
      for (Index j = 0; j < (std::min)(int(Rows), int(Cols)); ++j) {
        std::cout << "i = " << j << ", |r_ii| = " << numext::abs(r(j, j)) << std::endl;
      }
      std::cout << "Failure at i=" << i << ", rank=" << rank
                << ", threshold=" << threshold << std::endl;
    }
    VERIFY_IS_APPROX_OR_LESS_THAN(y, x);
  }
}

// This test is meant to verify that pivots are chosen such that
// even for a graded matrix, the diagonal of R falls of roughly
// monotonically until it reaches the threshold for singularity.
// We use the so-called Kahan matrix, which is a famous counter-example
// for rank-revealing QR. See
// http://www.netlib.org/lapack/lawnspdf/lawn176.pdf
// page 3 for more detail.
template<typename MatrixType> void qr_kahan_matrix()
{
  using std::sqrt;
  using std::abs;
  typedef typename MatrixType::Index Index;
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::RealScalar RealScalar;

  Index rows = 300, cols = rows;

  MatrixType m1;
  m1.setZero(rows,cols);
  RealScalar s = std::pow(NumTraits<RealScalar>::epsilon(), 1.0 / rows);
  RealScalar c = std::sqrt(1 - s*s);
  RealScalar pow_s_i(1.0); // pow(s,i)
  for (Index i = 0; i < rows; ++i) {
    m1(i, i) = pow_s_i;
    m1.row(i).tail(rows - i - 1) = -pow_s_i * c * MatrixType::Ones(1, rows - i - 1);
    pow_s_i *= s;
  }
  m1 = (m1 + m1.transpose()).eval();
  ColPivHouseholderQR<MatrixType> qr(m1);
  MatrixType r = qr.matrixQR().template triangularView<Upper>();

  RealScalar threshold =
      std::sqrt(RealScalar(rows)) * numext::abs(r(0, 0)) * NumTraits<Scalar>::epsilon();
  for (Index i = 0; i < (std::min)(rows, cols) - 1; ++i) {
    RealScalar x = numext::abs(r(i, i));
    RealScalar y = numext::abs(r(i + 1, i + 1));
    if (x < threshold && y < threshold) continue;
    if (!test_isApproxOrLessThan(y, x)) {
      for (Index j = 0; j < (std::min)(rows, cols); ++j) {
        std::cout << "i = " << j << ", |r_ii| = " << numext::abs(r(j, j)) << std::endl;
      }
      std::cout << "Failure at i=" << i << ", rank=" << qr.rank()
                << ", threshold=" << threshold << std::endl;
    }
    VERIFY_IS_APPROX_OR_LESS_THAN(y, x);
  }
}

template<typename MatrixType> void qr_invertible()
{
  using std::log;
  using std::abs;
  typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
  typedef typename MatrixType::Scalar Scalar;

  int size = internal::random<int>(10,50);

  MatrixType m1(size, size), m2(size, size), m3(size, size);
  m1 = MatrixType::Random(size,size);

  if (internal::is_same<RealScalar,float>::value)
  {
    // let's build a matrix more stable to inverse
    MatrixType a = MatrixType::Random(size,size*2);
    m1 += a * a.adjoint();
  }

  ColPivHouseholderQR<MatrixType> qr(m1);
  m3 = MatrixType::Random(size,size);
  m2 = qr.solve(m3);
  //VERIFY_IS_APPROX(m3, m1*m2);

  // now construct a matrix with prescribed determinant
  m1.setZero();
  for(int i = 0; i < size; i++) m1(i,i) = internal::random<Scalar>();
  RealScalar absdet = abs(m1.diagonal().prod());
  m3 = qr.householderQ(); // get a unitary
  m1 = m3 * m1 * m3;
  qr.compute(m1);
  VERIFY_IS_APPROX(absdet, qr.absDeterminant());
  VERIFY_IS_APPROX(log(absdet), qr.logAbsDeterminant());
}

template<typename MatrixType> void qr_verify_assert()
{
  MatrixType tmp;

  ColPivHouseholderQR<MatrixType> qr;
  VERIFY_RAISES_ASSERT(qr.matrixQR())
  VERIFY_RAISES_ASSERT(qr.solve(tmp))
  VERIFY_RAISES_ASSERT(qr.householderQ())
  VERIFY_RAISES_ASSERT(qr.dimensionOfKernel())
  VERIFY_RAISES_ASSERT(qr.isInjective())
  VERIFY_RAISES_ASSERT(qr.isSurjective())
  VERIFY_RAISES_ASSERT(qr.isInvertible())
  VERIFY_RAISES_ASSERT(qr.inverse())
  VERIFY_RAISES_ASSERT(qr.absDeterminant())
  VERIFY_RAISES_ASSERT(qr.logAbsDeterminant())
}

void test_qr_colpivoting()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( qr<MatrixXf>() );
    CALL_SUBTEST_2( qr<MatrixXd>() );
    CALL_SUBTEST_3( qr<MatrixXcd>() );
    CALL_SUBTEST_4(( qr_fixedsize<Matrix<float,3,5>, 4 >() ));
    CALL_SUBTEST_5(( qr_fixedsize<Matrix<double,6,2>, 3 >() ));
    CALL_SUBTEST_5(( qr_fixedsize<Matrix<double,1,1>, 1 >() ));
  }

  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( cod<MatrixXf>() );
    CALL_SUBTEST_2( cod<MatrixXd>() );
    CALL_SUBTEST_3( cod<MatrixXcd>() );
    CALL_SUBTEST_4(( cod_fixedsize<Matrix<float,3,5>, 4 >() ));
    CALL_SUBTEST_5(( cod_fixedsize<Matrix<double,6,2>, 3 >() ));
    CALL_SUBTEST_5(( cod_fixedsize<Matrix<double,1,1>, 1 >() ));
  }

  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( qr_invertible<MatrixXf>() );
    CALL_SUBTEST_2( qr_invertible<MatrixXd>() );
    CALL_SUBTEST_6( qr_invertible<MatrixXcf>() );
    CALL_SUBTEST_3( qr_invertible<MatrixXcd>() );
  }

  CALL_SUBTEST_7(qr_verify_assert<Matrix3f>());
  CALL_SUBTEST_8(qr_verify_assert<Matrix3d>());
  CALL_SUBTEST_1(qr_verify_assert<MatrixXf>());
  CALL_SUBTEST_2(qr_verify_assert<MatrixXd>());
  CALL_SUBTEST_6(qr_verify_assert<MatrixXcf>());
  CALL_SUBTEST_3(qr_verify_assert<MatrixXcd>());

  // Test problem size constructors
  CALL_SUBTEST_9(ColPivHouseholderQR<MatrixXf>(10, 20));

  CALL_SUBTEST_1( qr_kahan_matrix<MatrixXf>() );
  CALL_SUBTEST_2( qr_kahan_matrix<MatrixXd>() );
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example blas2.cpp
*
*   In this tutorial the BLAS level 2 functionality in ViennaCL is demonstrated.
*
*   We start with including the required header files:
**/

// System headers
#include <iostream>

// uBLAS headers
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

// ViennaCL headers
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/direct_solve.hpp"
#include "viennacl/linalg/prod.hpp"       //generic matrix-vector product
#include "viennacl/linalg/norm_2.hpp"     //generic l2-norm for vectors
#include "viennacl/linalg/lu.hpp"         //LU substitution routines

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

// Make `boost::numeric::ublas` available under the shortcut `ublas`:
using namespace boost::numeric;

/**
* We do not need any auxiliary functions in this example, so let us start directly in main():
**/
int main()
{
  typedef float       ScalarType;

  /**
  * Set up some uBLAS vectors and a matrix.
  * They will be later used for filling the ViennaCL objects with data.
  **/
  ublas::vector<ScalarType> rhs(12);
  for (unsigned int i = 0; i < rhs.size(); ++i)
    rhs(i) = random<ScalarType>();
  ublas::vector<ScalarType> rhs2 = rhs;
  ublas::vector<ScalarType> result = ublas::zero_vector<ScalarType>(10);
  ublas::vector<ScalarType> result2 = result;
  ublas::vector<ScalarType> rhs_trans = rhs;
  rhs_trans.resize(result.size(), true);
  ublas::vector<ScalarType> result_trans = ublas::zero_vector<ScalarType>(rhs.size());

  ublas::matrix<ScalarType> matrix(result.size(),rhs.size());

  /**
  * Fill the uBLAS-matrix
  **/
  for (unsigned int i = 0; i < matrix.size1(); ++i)
    for (unsigned int j = 0; j < matrix.size2(); ++j)
      matrix(i,j) = random<ScalarType>();

  /**
  * Use some plain STL types:
  **/
  std::vector< ScalarType > stl_result(result.size());
  std::vector< ScalarType > stl_rhs(rhs.size());
  std::vector< std::vector<ScalarType> > stl_matrix(result.size());
  for (unsigned int i=0; i < result.size(); ++i)
  {
    stl_matrix[i].resize(rhs.size());
    for (unsigned int j = 0; j < matrix.size2(); ++j)
    {
      stl_rhs[j] = rhs[j];
      stl_matrix[i][j] = matrix(i,j);
    }
  }

  /**
  * Set up some ViennaCL objects (initialized with zeros) and then copy data from the uBLAS objects.
  **/
  viennacl::vector<ScalarType> vcl_rhs(rhs.size());
  viennacl::vector<ScalarType> vcl_result(result.size());
  viennacl::matrix<ScalarType> vcl_matrix(result.size(), rhs.size());
  viennacl::matrix<ScalarType> vcl_matrix2(result.size(), rhs.size());

  viennacl::copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  viennacl::copy(matrix, vcl_matrix);     //copy from ublas dense matrix type to ViennaCL type

  /**
  * Some basic matrix operations with ViennaCL are as follows:
  **/
  vcl_matrix2 = vcl_matrix;
  vcl_matrix2 += vcl_matrix;
  vcl_matrix2 -= vcl_matrix;
  vcl_matrix2 = vcl_matrix2 + vcl_matrix;
  vcl_matrix2 = vcl_matrix2 - vcl_matrix;

  viennacl::scalar<ScalarType> vcl_3(3.0);
  vcl_matrix2 *= ScalarType(2.0);
  vcl_matrix2 /= ScalarType(2.0);
  vcl_matrix2 *= vcl_3;
  vcl_matrix2 /= vcl_3;

  /**
  * A matrix can be cleared directly:
  **/
  vcl_matrix.clear();

  /**
  * Other ways of data transfers between matrices in main memory and a ViennaCL matrix:
  **/
  viennacl::copy(stl_matrix, vcl_matrix); //alternative: copy from STL vector< vector<> > type to ViennaCL type

  //for demonstration purposes (no effect):
  viennacl::copy(vcl_matrix, matrix); //copy back from ViennaCL to ublas type.
  viennacl::copy(vcl_matrix, stl_matrix); //copy back from ViennaCL to STL type.

  /**
  *
 Matrix-Vector Products

  *
  * Compute matrix-vector products
  **/
  std::cout << "----- Matrix-Vector product -----" << std::endl;
  result = ublas::prod(matrix, rhs);                            //the ublas way
  stl_result = viennacl::linalg::prod(stl_matrix, stl_rhs);     //using STL
  vcl_result = viennacl::linalg::prod(vcl_matrix, vcl_rhs);     //the ViennaCL way

  /**
  * Compute transposed matrix-vector products
  **/
  std::cout << "----- Transposed Matrix-Vector product -----" << std::endl;
  result_trans = prod(trans(matrix), rhs_trans);

  viennacl::vector<ScalarType> vcl_rhs_trans(rhs_trans.size());
  viennacl::vector<ScalarType> vcl_result_trans(result_trans.size());
  viennacl::copy(rhs_trans.begin(), rhs_trans.end(), vcl_rhs_trans.begin());
  vcl_result_trans = viennacl::linalg::prod(trans(vcl_matrix), vcl_rhs_trans);

  /**
  *
Direct Solver

  *
  * In order to demonstrate the direct solvers, we first need to setup suitable square matrices.
  * This is again achieved by running the setup on the CPU and then copy the data over to ViennaCL types:
  **/
  ublas::matrix<ScalarType> tri_matrix(10,10);
  for (std::size_t i=0; i<tri_matrix.size1(); ++i)
  {
    for (std::size_t j=0; j<i; ++j)
      tri_matrix(i,j) = 0.0;

    for (std::size_t j=i; j<tri_matrix.size2(); ++j)
      tri_matrix(i,j) = matrix(i,j);
  }

  viennacl::matrix<ScalarType> vcl_tri_matrix = viennacl::identity_matrix<ScalarType>(tri_matrix.size1());
  viennacl::copy(tri_matrix, vcl_tri_matrix);

  // Bring vectors to correct size:
  rhs.resize(tri_matrix.size1(), true);
  rhs2.resize(tri_matrix.size1(), true);
  vcl_rhs.resize(tri_matrix.size1(), true);

  viennacl::copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  vcl_result.resize(10);

  /**
  * Run a triangular solver on the upper triangular part of the matrix:
  **/
  std::cout << "----- Upper Triangular solve -----" << std::endl;
  result = ublas::solve(tri_matrix, rhs, ublas::upper_tag());                                    //ublas
  vcl_result = viennacl::linalg::solve(vcl_tri_matrix, vcl_rhs, viennacl::linalg::upper_tag());  //ViennaCL

  /**
  * Inplace variants of triangular solvers:
  **/
  ublas::inplace_solve(tri_matrix, rhs, ublas::upper_tag());                                //ublas
  viennacl::linalg::inplace_solve(vcl_tri_matrix, vcl_rhs, viennacl::linalg::upper_tag());  //ViennaCL

  /**
  * Set up a full system for full solver using LU factorizations:
  **/
  std::cout << "----- LU factorization -----" << std::endl;
  std::size_t lu_dim = 300;
  ublas::matrix<ScalarType> square_matrix(lu_dim, lu_dim);
  ublas::vector<ScalarType> lu_rhs(lu_dim);
  viennacl::matrix<ScalarType> vcl_square_matrix(lu_dim, lu_dim);
  viennacl::vector<ScalarType> vcl_lu_rhs(lu_dim);

  for (std::size_t i=0; i<lu_dim; ++i)
    for (std::size_t j=0; j<lu_dim; ++j)
      square_matrix(i,j) = random<ScalarType>();

  //put some more weight on diagonal elements:
  for (std::size_t j=0; j<lu_dim; ++j)
  {
    square_matrix(j,j) += ScalarType(10.0);
    lu_rhs(j) = random<ScalarType>();
  }

  viennacl::copy(square_matrix, vcl_square_matrix);
  viennacl::copy(lu_rhs, vcl_lu_rhs);
  viennacl::linalg::lu_factorize(vcl_square_matrix);
  viennacl::linalg::lu_substitute(vcl_square_matrix, vcl_lu_rhs);
  viennacl::copy(square_matrix, vcl_square_matrix);
  viennacl::copy(lu_rhs, vcl_lu_rhs);

  /**
  * Full solver with Boost.uBLAS:
  **/
  ublas::lu_factorize(square_matrix);
  ublas::inplace_solve (square_matrix, lu_rhs, ublas::unit_lower_tag ());
  ublas::inplace_solve (square_matrix, lu_rhs, ublas::upper_tag ());

  /**
  * Full solver with ViennaCL:
  **/
  viennacl::linalg::lu_factorize(vcl_square_matrix);
  viennacl::linalg::lu_substitute(vcl_square_matrix, vcl_lu_rhs);

  /**
  *  That's it.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


/* Boost test/pow.cpp
 * test the pow function
 *
 * Copyright Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors nor Polytechnic University make any
 * representation about the suitability of this software for any
 * purpose. It is provided "as is" without express or implied warranty.
 *
 * $Id: pow.cpp,v 1.3 2003/02/05 17:34:36 gmelquio Exp $
 */

#include <boost/numeric/interval.hpp>
#include <boost/test/minimal.hpp>

bool test_pow(double al, double au, double bl, double bu, int p) {
  typedef boost::numeric::interval<double> I;
  I b = pow(I(al, au), p);
  return b.lower() == bl && b.upper() == bu;
}

int test_main(int, char *[]) {
  BOOST_TEST(test_pow(2, 3, 8, 27, 3));
  BOOST_TEST(test_pow(2, 3, 16, 81, 4));
  BOOST_TEST(test_pow(-3, 2, -27, 8, 3));
  BOOST_TEST(test_pow(-3, 2, 0, 81, 4));
  BOOST_TEST(test_pow(-3, -2, -27, -8, 3));
  BOOST_TEST(test_pow(-3, -2, 16, 81, 4));

  BOOST_TEST(test_pow(2, 4, 1./64, 1./8, -3));
  BOOST_TEST(test_pow(2, 4, 1./256, 1./16, -4));
  BOOST_TEST(test_pow(-4, -2, -1./8, -1./64, -3));
  BOOST_TEST(test_pow(-4, -2, 1./256, 1./16, -4));

  BOOST_TEST(test_pow(2, 3, 1, 1, 0));
  BOOST_TEST(test_pow(-3, 2, 1, 1, 0));
  BOOST_TEST(test_pow(-3, -2, 1, 1, 0));

  return 0;
}


/*
 * @brief A general filter class. Replicates "filt" in Matlab
 * @author Jenna Reher (jreher@caltech.edu)
 */

#ifndef SMOOTHING_HPP
#define SMOOTHING_HPP

#include <Eigen/Dense>
#include <iostream>

namespace cassie_common_toolbox {

class ButterFilter
{
public:
    ButterFilter();
    ButterFilter(Eigen::VectorXd &b, Eigen::VectorXd &a);
    void reconfigure(Eigen::VectorXd &b, Eigen::VectorXd &a);
    void update(double x_raw);
    double getValue();
    void reset();

private:
    long nb;
    long na;
    Eigen::VectorXd y_; // Array of previously filtered values
    Eigen::VectorXd x_; // Array of raw values
    Eigen::VectorXd b_; // filter coefficient
    Eigen::VectorXd a_; // filter coefficient
};

class MovingAverage
{
public:
    MovingAverage();
    MovingAverage(int nSamples, int dim);
    void reset();
    void reconfigure(int nSamples, int dim);
    void update(Eigen::VectorXd &raw);
    Eigen::VectorXd getValue();
private:
    int nSamples;
    int cur_index;
    Eigen::MatrixXd array;
};

}

#endif // SMOOTHING_HPP


/*
* compile with flags:   g++ test.cc tree_classifier.cc   -std=c++14 -larmadillo -I ../../include/
* author: Yuzhen Liu
* Date: 2019.5.8 17:35
*/

#include <iostream>
#include <armadillo>
#include <cmath>
#include <stdlib.h>
#include <datasets/datasets.h>
#include <probalistic/naive_bayes.h>

using namespace std;
using namespace arma;

pair<umat, uvec> generate_data() {
    int n_sample = 100;
    int n_feature = 4;
    umat x(n_feature, n_sample);
    uvec y(n_sample);

    // generate n samples iteratively
    srand((unsigned)time(NULL));
    for(int i =0; i < n_sample; i++) {
        x(0, i) = rand() % 10;
        x(1, i) = rand() % 10 + 100;
        x(2, i) = rand() % 10;
        x(3, i) = rand() % 10;

        if (x(0, i) <= 5 && x(1, i) > 105 && x(2, i) > 5 )  y(i) = 0;
        else if (x(0, i) > 5 && x(1, i) == 106 && x(2, i) <= 5 && x(3, i) > 5)  y(i) = 1;
        else if (x(0, i) <= 5 && x(1, i) <= 105 && x(2, i) < 5)  y(i) = 2;
        else y(i) = 3;
    }
    return make_pair(x, y);
}

int main() {
    pair<umat, uvec> p = generate_data();
    mat x = conv_to<mat>::from(p.first);
    uvec y = p.second;

    Datasets dataset = Datasets("iris");
    mat x = dataset.x;
    uvec y = conv_to<uvec>::from(dataset.y);

    // Naive_Bayes bayes_classifier = Naive_Bayes();
    // bayes_classifier.train(x, mat(0, 0), y);
    // uvec res = bayes_classifier.predict(x, mat(0, 0));

    uvec dis = res - y;
    int count =0;
    for (int i =0; i < dis.n_elem; i++) {
        if (dis(i) == 0) count++;
    }
    // dis.print();
    cout << "The accuracy is: " << (count * 100/ dis.n_elem) << "%." << endl;

    return 0;
}


// Copyright 2004 The Trustees of Indiana University.

// 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_RANDOM_LAYOUT_HPP
#define BOOST_GRAPH_RANDOM_LAYOUT_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/mpl/if.hpp>

namespace boost {

template<typename Graph, typename PositionMap, typename Dimension,
         typename RandomNumberGenerator>
void
random_graph_layout(const Graph& g, PositionMap position_map,
                    Dimension minX, Dimension maxX,
                    Dimension minY, Dimension maxY,
                    RandomNumberGenerator& gen)
{
  typedef typename mpl::if_<is_integral<Dimension>,
                            uniform_int<Dimension>,
                            uniform_real<Dimension> >::type distrib_t;
  typedef typename mpl::if_<is_integral<Dimension>,
                            RandomNumberGenerator&,
                            uniform_01<RandomNumberGenerator, Dimension> >
    ::type gen_t;

  gen_t my_gen(gen);
  distrib_t x(minX, maxX);
  distrib_t y(minY, maxY);
  typename graph_traits<Graph>::vertex_iterator vi, vi_end;
  for(tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    position_map[*vi].x = x(my_gen);
    position_map[*vi].y = y(my_gen);
  }
}

} // end namespace boost

#endif // BOOST_GRAPH_RANDOM_LAYOUT_HPP


#include <memory>
#include <vector>

#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>

#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

TEST(OpengvPoseEstimation, KneipP3p) {
  opengv::bearingVectors_t bearing_vectors;
  opengv::points_t points;

  points.push_back(opengv::point_t(0, -1, 5));
  points.push_back(opengv::point_t(-1, 0, 0));
  points.push_back(opengv::point_t(1, 0, 10));

  bearing_vectors.push_back(opengv::bearingVector_t(0, -1, 6).normalized());
  bearing_vectors.push_back(opengv::bearingVector_t(-1, 0, 1).normalized());
  bearing_vectors.push_back(opengv::bearingVector_t(1, 0, 11).normalized());

  opengv::absolute_pose::CentralAbsoluteAdapter adapter(
      bearing_vectors, points);

  opengv::transformations_t p3p_kneip_transformations;
  p3p_kneip_transformations = opengv::absolute_pose::p3p_kneip(adapter);

  CHECK_EQ(4u, p3p_kneip_transformations.size());

  Eigen::Matrix<double, 3, 4> expected_transform;
  expected_transform << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, -1;

  const bool is_1st_guess_matching =
      p3p_kneip_transformations[0].isApprox(expected_transform, 1e-10);
  const bool is_2nd_guess_matching =
      p3p_kneip_transformations[1].isApprox(expected_transform, 1e-10);
  const bool is_3rd_guess_matching =
      p3p_kneip_transformations[2].isApprox(expected_transform, 1e-10);
  const bool is_4th_guess_matching =
      p3p_kneip_transformations[3].isApprox(expected_transform, 1e-10);

  EXPECT_TRUE(
      is_1st_guess_matching || is_2nd_guess_matching || is_3rd_guess_matching ||
      is_4th_guess_matching);
}

TEST(OpengvPoseEstimation, KneipP3pRansac) {
  opengv::bearingVectors_t bearing_vectors;
  opengv::points_t points;

  points.push_back(opengv::point_t(0, -1, 0));
  points.push_back(opengv::point_t(-1, 0, 0));
  points.push_back(opengv::point_t(1, 0, 1));
  points.push_back(opengv::point_t(1, 1, 8));

  bearing_vectors.push_back(opengv::bearingVector_t(0, -1, 1).normalized());
  bearing_vectors.push_back(opengv::bearingVector_t(-1, 0, 1).normalized());
  bearing_vectors.push_back(opengv::bearingVector_t(1, 0, 2).normalized());
  bearing_vectors.push_back(opengv::bearingVector_t(1, 1, 9).normalized());

  opengv::absolute_pose::CentralAbsoluteAdapter adapter(
      bearing_vectors, points);

  opengv::sac::Ransac<
      opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem>
      ransac;
  std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem>
      absposeproblem_ptr(
          new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(
              adapter, opengv::sac_problems::absolute_pose::
                           AbsolutePoseSacProblem::KNEIP));
  ransac.sac_model_ = absposeproblem_ptr;
  ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0) * 0.5 / 800.0));
  ransac.max_iterations_ = 50;

  ransac.computeModel();
  Eigen::Matrix<double, 3, 4> expected_transform;
  expected_transform << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, -1;
  EXPECT_NEAR_EIGEN(expected_transform, ransac.model_coefficients_, 1e-10);
}

MAPLAB_UNITTEST_ENTRYPOINT


/*
 * Website:
 *      https://github.com/wo3kie/dojo
 *
 * Author:
 *      Lukasz Czerwinski
 *
 * Compilation:
 *      g++ --std=c++11 countBits.cpp -o countBits
 *
 * Usage:
 *      $ ./countBits
 */

#include <cassert>
#include <random>

#include <boost/preprocessor/repetition/repeat.hpp>

constexpr int countBits8(unsigned char i){
    return
        i == 0
        ? 0
        : (i & 1) + countBits8(i / 2);
}

#define COUNT_BITS_CALCULATE(z, n, data) \
    countBits8(n),

int countBits(
    unsigned char const * begin,
    unsigned char const * const end
){
    constexpr int bits[256] = {
        BOOST_PP_REPEAT(256, COUNT_BITS_CALCULATE, "")
    };

    int result = 0;

    for( ; begin != end; ++begin){
        result += bits[ * begin ];
    }

    return result;
}

template<typename T>
int countBits(T const & t){
    unsigned char const * const begin
        = reinterpret_cast< unsigned char const * >( & t );

    return countBits(begin, begin + sizeof(T));
}

int main(){
    for(int i = 0; i < 256; ++i){
        assert(countBits8(i) == __builtin_popcount(i));
    }

    for(int i = 0; i < 256; ++i){
        assert(countBits(i) == __builtin_popcount(i));
    }

    for(int i = 0; i < 1e4; ++i){
        int const random = std::rand();
        assert(countBits(random) == __builtin_popcount(random));
    }
}


#include <boost/test/unit_test.hpp>

#include "math/point_typed.h" // header to test

using namespace biosim;

BOOST_AUTO_TEST_SUITE(suite_point_typed)

BOOST_AUTO_TEST_CASE(point_typed_ctor) {
  math::point p_data({1.0, 2.0, 3.0});
  math::point_typed p(p_data);
  BOOST_CHECK(p.get_type() == math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK(p.get_point() == p_data);

  p = math::point_typed(p_data, math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p.get_type() == math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p.get_point() == p_data);
}

BOOST_AUTO_TEST_CASE(point_typed_cmp_operator) {
  math::point p_cart({3.0, 4.0, 5.0});
  math::point p_cart2({3.0, 4.0, 6.0});
  math::point p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  math::point p_cart_again = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                                 .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK(p_cart == p_cart);
  BOOST_CHECK(p_cart == p_cart_again);
  BOOST_CHECK(p_cart != p_cart2);
  BOOST_CHECK(p_cart != p_cyl);
}

BOOST_AUTO_TEST_CASE(point_typed_convert) {
  math::point p_cart({3.0, 4.0, 5.0});
  math::point p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p_cyl[0] == 5.0);
  BOOST_CHECK_CLOSE(p_cyl[1], 0.927295, 1e-3);
  BOOST_CHECK(p_cyl[2] == 5.0);
  math::point p_cart2 = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                            .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK_CLOSE(p_cart2[0], 3.0, 1e-3);
  BOOST_CHECK_CLOSE(p_cart2[1], 4.0, 1e-3);
  BOOST_CHECK(p_cart2[2] == 5.0);

  p_cart = math::point({-3.0, 4.0, 5.0});
  p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p_cyl[0] == 5.0);
  BOOST_CHECK_CLOSE(p_cyl[1], 2.214297, 1e-3);
  BOOST_CHECK(p_cyl[2] == 5.0);
  p_cart2 = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK_CLOSE(p_cart2[0], -3.0, 1e-3);
  BOOST_CHECK_CLOSE(p_cart2[1], 4.0, 1e-3);
  BOOST_CHECK(p_cart2[2] == 5.0);

  p_cart = math::point({-3.0, -4.0, 5.0});
  p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p_cyl[0] == 5.0);
  BOOST_CHECK_CLOSE(p_cyl[1], -2.214297, 1e-3);
  BOOST_CHECK(p_cyl[2] == 5.0);
  p_cart2 = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK_CLOSE(p_cart2[0], -3.0, 1e-3);
  BOOST_CHECK_CLOSE(p_cart2[1], -4.0, 1e-3);
  BOOST_CHECK(p_cart2[2] == 5.0);

  p_cart = math::point({3.0, -4.0, 5.0});
  p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p_cyl[0] == 5.0);
  BOOST_CHECK_CLOSE(p_cyl[1], -0.927295, 1e-3);
  BOOST_CHECK(p_cyl[2] == 5.0);
  p_cart2 = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK_CLOSE(p_cart2[0], 3.0, 1e-3);
  BOOST_CHECK_CLOSE(p_cart2[1], -4.0, 1e-3);
  BOOST_CHECK(p_cart2[2] == 5.0);

  p_cart = math::point({0.0, 4.0, 5.0});
  p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p_cyl[0] == 4.0);
  BOOST_CHECK_CLOSE(p_cyl[1], 1.5708, 1e-3);
  BOOST_CHECK(p_cyl[2] == 5.0);
  p_cart2 = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK_SMALL(p_cart2[0], 1e-5);
  BOOST_CHECK_CLOSE(p_cart2[1], 4.0, 1e-3);
  BOOST_CHECK(p_cart2[2] == 5.0);

  p_cart = math::point({0.0, -4.0, 5.0});
  p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p_cyl[0] == 4.0);
  BOOST_CHECK_CLOSE(p_cyl[1], -1.5708, 1e-3);
  BOOST_CHECK(p_cyl[2] == 5.0);
  p_cart2 = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK_SMALL(p_cart2[0], 1e-5);
  BOOST_CHECK_CLOSE(p_cart2[1], -4.0, 1e-3);
  BOOST_CHECK(p_cart2[2] == 5.0);

  p_cart = math::point({0.0, 0.0, 5.0});
  p_cyl = math::point_typed(p_cart).to_type(math::point_typed::coordinate_type::cylindrical);
  BOOST_CHECK(p_cyl[0] == 0.0);
  BOOST_CHECK(p_cyl[1] == 0.0);
  BOOST_CHECK(p_cyl[2] == 5.0);
  p_cart2 = math::point_typed(p_cyl, math::point_typed::coordinate_type::cylindrical)
                .to_type(math::point_typed::coordinate_type::cartesian);
  BOOST_CHECK_SMALL(p_cart2[0], 1e-5);
  BOOST_CHECK_SMALL(p_cart2[1], 1e-5);
  BOOST_CHECK(p_cart2[2] == 5.0);
}

BOOST_AUTO_TEST_SUITE_END()


//####### Test module for rng_wrapper ####################################

//Define Module name
 #define BOOST_TEST_MODULE "RNG wrapper"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

#include <cstdint>
#include <random>

//Include Boost unit tests library
#include <boost/test/unit_test.hpp>

//Units choice. Not relevant here, but avoids compile-time warning
#define PXRMP_USE_SI_UNITS
#include "rng_wrapper.hpp"

using namespace picsar::multi_physics;

// ------------- Tests --------------

//***STL***

//***Constructors

//Test STL rng_wrapper constructors generic
template<typename T>
void  rng_stl_wrapper_constructors(uint64_t seed)
{
    std::mt19937_64 rng{seed};

    stl_rng_wrapper<T> wrp1{seed};
    stl_rng_wrapper<T> wrp2(move(rng));

    BOOST_CHECK_EQUAL( wrp1.unf(0.0,1.0), wrp2.unf(0.0,1.0));
}

//Test STL rng_wrapper constructors(double precision)
BOOST_AUTO_TEST_CASE( rng_stl_wrapper_constructors_double )
{
    rng_stl_wrapper_constructors<double>(2391892344079);
}

//Test STL rng_wrapper constructors(single precision)
BOOST_AUTO_TEST_CASE( rng_stl_wrapper_constructors_single )
{
    rng_stl_wrapper_constructors<float>(2391892344079);
}

//***Uniform distribution

//Test STL rng_wrapper unf generic
template<typename T>
void  rng_stl_wrapper_unf(uint64_t seed)
{
    stl_rng_wrapper<T> wrp{seed};
    size_t how_many = 10000;
    T a = static_cast<T>(-7.0);
    T b = static_cast<T>(11.1);

    for (size_t d = 0; d <= how_many; ++d){
        T qq = wrp.unf(a,b);
        BOOST_TEST( qq >= a);
        BOOST_TEST( qq < b);
    }
}

//Test STL rng_wrapper unf (double precision)
BOOST_AUTO_TEST_CASE( rng_stl_wrapper_unf_double )
{
    rng_stl_wrapper_unf<double>(2391892344079);
}

//Test STL rng_wrapper unf (single precision)
BOOST_AUTO_TEST_CASE( rng_stl_wrapper_unf_single )
{
    rng_stl_wrapper_unf<float>(2391892344079);
}

//Test STL rng_wrapper exp generic
template<typename T>
void  rng_stl_wrapper_exp(uint64_t seed)
{
    stl_rng_wrapper<T> wrp{seed};
    size_t how_many = 10000;
    T l = static_cast<T>(1.0);

    for (size_t d = 0; d <= how_many; ++d){
        T qq = wrp.exp(l);
        BOOST_TEST( qq >= 0.0);
    }
}

//Test STL rng_wrapper exp (double precision)
BOOST_AUTO_TEST_CASE( rng_stl_wrapper_exp_double )
{
    rng_stl_wrapper_exp<double>(2391892344079);
}

//Test STL rng_wrapper exp (single precision)
BOOST_AUTO_TEST_CASE( rng_stl_wrapper_exp_single )
{
    rng_stl_wrapper_exp<float>(2391892344079);
}

//***Exponential distribution


#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
//#include <Eigen/Dense>
//#include <Eigen/Core>

using namespace std;
using namespace Eigen;
using namespace pcl;

// This function displays the help
void showHelp(char * program_name)
{
	std::cout << std::endl;
	std::cout << "Usage: " << program_name << " cloud_filename.[pcd|ply]" << std::endl;
	std::cout << "-h:  Show this help." << std::endl;
}

// This is the main function
int main (int argc, char** argv)
{
// Show help
	if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help"))
	{
		showHelp (argv[0]);
		return 0;
	}
// Fetch point cloud filename in arguments | Works with PCD and PLY files
	std::vector<int> filenames;
	bool file_is_pcd = false;
	filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply");

	if (filenames.size () != 1)
	{
		filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
		if (filenames.size () != 1)
		{
			showHelp (argv[0]);
			return -1;
		}
		else
		{
			file_is_pcd = true;
		}
	}
// Load file | Works with PCD and PLY files
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
	if (file_is_pcd)
	{
		if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0)
		{
			std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
			showHelp (argv[0]);
			return -1;
		}
	}
	else
	{
		if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0)
		{
			std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
			showHelp (argv[0]);
			return -1;
		}
	}
	cout<<"Width of the cloud: "<<source_cloud->width<<endl;
	cout<<"Height of the cloud: "<<source_cloud->height<<endl;
	int d=source_cloud->width;
	MatrixXf cloud_data(d,3);
	for(int i=0;i<d;i++)
	{
		cloud_data(i,0) = source_cloud->points.at(i).x+2;
		cloud_data(i,1) = source_cloud->points.at(i).y+2;
		cloud_data(i,2) = source_cloud->points.at(i).z+2;
	}
	cout<<cloud_data<<endl;
//!! Create a point cloud containing part of the pointcloud data
	PointCloud<pcl::PointXYZ>::Ptr cloud_test (new PointCloud<pcl::PointXYZ>);
	cloud_test->width    = d;
	cloud_test->height   = 1;
	cloud_test->is_dense = false;
	cloud_test->points.resize (cloud_test->width * cloud_test->height);
	for (size_t i = 0; i < cloud_test->points.size (); ++i)
	{
		cloud_test->points[i].x = source_cloud->points.at(i).x;
		cloud_test->points[i].y = source_cloud->points.at(i).y;
		cloud_test->points[i].z = source_cloud->points.at(i).z;
	}
	cout << "Test " << cloud_test->points.size () << " data points:"<< endl;
	for (size_t i = 0; i < cloud_test->points.size (); ++i)
	{
		cout << "    " << cloud_test->points[i].x << " " <<cloud_test->points[i].y << " " << cloud_test->points[i].z << endl;
	}
	//cloud_data<< source_cloud->points;
  /* Reminder: how transformation matrices work :

           |-------> This column is the translation
    | 1 0 0 x |  \
    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
    | 0 0 1 z |  /
    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)

    METHOD #1: Using a Matrix4f
    This is the "manual" method, perfect to understand but error prone !
  */
	Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
	float theta = M_PI/4; // The angle of rotation in radians
	transform_1 (0,0) = cos (theta);
	transform_1 (0,1) = -sin(theta);
	transform_1 (1,0) = sin (theta);
	transform_1 (1,1) = cos (theta);
  //    (row, column)

  // Define a translation of 2.5 meters on the x axis.
	transform_1 (0,3) = 2.5;
  // Print the transformation
	printf ("Method #1: using a Matrix4f\n");
	std::cout << transform_1 << std::endl;
  /*  METHOD #2: Using a Affine3f
    This method is easier and less error prone
  */
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
	transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians arround Z axis
	transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
// Print the transformation
	printf ("\nMethod #2: using an Affine3f\n");
	std::cout << transform_2.matrix() << std::endl;

 // Executing the transformation
	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
 // You can either apply transform_1 or transform_2; they are the same
	pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);

 // Visualization
	printf(  "\nPoint cloud colors :  white  = original point cloud\n"
      "                        red  = transformed point cloud\n");
	pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");

// Define R,G,B colors for the point cloud
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
// We add the point cloud to the viewer and pass the color handler
	viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
	viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_test_color_handler (cloud_test, 255, 20, 255);
// We add the point cloud to the viewer and pass the color handler
	viewer.addPointCloud (cloud_test, cloud_test_color_handler, "test_cloud");

	viewer.addCoordinateSystem (0.1, 0);
	viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "test_cloud");
  //viewer.setPosition(800, 400); // Setting visualiser window position

	while (!viewer.wasStopped ())
	{ // Display the visualiser until 'q' key is pressed
		viewer.spinOnce ();
	}

	return 0;
}


#include <cmath>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

#ifdef WITH_EIGEN
#include <Eigen/Dense>
#endif

template <typename Vector>
void get_data(Vector& x, Vector& y, Vector& err) {
  // get number of data points
  const unsigned n = x.size();
  assert(y.size() == n && err.size() == n);

  // compute data
  for (unsigned i = 0; i < n; ++i) {
    *(x.data() + i) = 1.0 * i / n;
    *(y.data() + i) = sin(2.0 * M_PI * i / n);
    *(err.data() + i) = exp(-0.1 * i);
  }
}

template <typename Vector>
void plot(unsigned n) {
  // get the data
  Vector x(n), y(n), err(n);
  get_data(x, y, err);

  // plot errorbar plot
  plt::errorbar(x, y, err);
}

int main() {
  // create figure
  plt::figure();

  // plot with std::vector
  plot<std::vector<double>>(10);

  // plot with Eigen::VectorXd, if specified
  #ifdef WITH_EIGEN
  plot<Eigen::VectorXd>(13);
  #endif

  // show plot
  plt::show();

  return 0;
}