///
/// \file vandermonde.hpp
///
#ifndef MXPFIT_VANDERMONDE_LEAST_SQUARES_HPP
#define MXPFIT_VANDERMONDE_LEAST_SQUARES_HPP

#include <cassert>

#include <Eigen/Core>
#include <Eigen/IterativeLinearSolvers>

#include <mxpfit/matrix_free_gemv.hpp>
#include <mxpfit/vandermonde_matrix.hpp>

namespace mxpfit
{

namespace detail
{

///
/// \internal
///
/// Compute Cholesky decomposition of the gramian matrix of column Vandermonde
/// matrix.
///
/// This function compute the LDL decomposition of the gramian matrix,
///
/// \f[ G = V^{\ast} V = L^{} D L^{\ast}, \f]
///
/// where \f$ V=[v_{j}^{i}]_{i=1,\dots,m}^{n=1,\dots,n}\f$ is the column
/// Vandermonde matrix, \f$ L \f$ is a lower unit triangular matrix, and \f$ D
/// \f$ is a diagonal matrix.
///
/// \param[in] V  An \f$ m \times n \f$ column Vandermonde matrix to be
///   decomposed.
/// \param[out] ldlt  An \ f$ n \times n \f$ matrix to store the result of
///   decomposition. On exit, the diagonal elements of `ldlt` are those of
///   matrix \f$ D, \f$ and strict lower triangular part contains the off
///   diagonal elements of factor \f$ L \f$.
///
/// \param[out] work  An \f$ n \times 4 \f$ matrix used for workspace
///
template <typename T, typename MatrixT, typename MatrixWork>
void cholesky_vandermonde_gramian(const VandermondeMatrix<T>& V, MatrixT& ldlt,
                                  MatrixWork& work)
{
    using Scalar       = typename VandermondeMatrix<T>::Scalar;
    using CoeffsVector = typename VandermondeMatrix<T>::CoeffsVector;
    using RealScalar   = typename Eigen::NumTraits<Scalar>::Real;
    using Index        = Eigen::Index;

    using Eigen::numext::abs;
    using Eigen::numext::abs2;
    using Eigen::numext::conj;
    using Eigen::numext::real;
    using Eigen::numext::sqrt;

    static const auto tiny   = sqrt(std::numeric_limits<RealScalar>::min());
    constexpr const auto one = Scalar(1);

    auto z        = V.coeffs();
    const Index m = V.rows();
    const Index n = V.cols();

    assert(ldlt.rows() == n && ldlt.cols() == n);
    assert(work.rows() == n && work.cols() >= 4);

    // ----- Initialization
    auto y1 = work.col(0);
    auto y2 = work.col(1);
    auto x1 = work.col(2);
    auto x2 = work.col(3);

    auto gramian = [&](Index i, Index j) {
        const auto arg = conj(z(i)) * z(j);
        return arg == one ? Scalar(m) : (one - std::pow(arg, m)) / (one - arg);
    };

    auto sigma2 = gramian(0, 0);
    auto b0     = ldlt.col(0);
    b0(0)       = one;
    for (Index j = 1; j < n; ++j)
    {
        b0(j) = gramian(j, 0) / sigma2;
    }

    y1 = CoeffsVector::Ones(n) - b0;
    y2 = z.array().conjugate().pow(m);
    y2 -= y2(0) * b0;
    x1 = z.array().conjugate().inverse();
    x1 -= x1(0) * b0;
    x2 = -z.array().conjugate().pow(m - 1);
    x2 -= x2(0) * b0;

    b0(0) = sigma2;

    for (Index k = 1; k < n; ++k)
    {
        auto bk = ldlt.col(k);

        auto mu1 = x1(k);
        auto mu2 = x2(k);
        auto nu1 = conj(y1(k));
        auto nu2 = conj(y2(k));

        auto zk_inv = one / z(k);
        auto denom  = conj(zk_inv) - z(k);

        if (abs(denom) < tiny)
        {
            sigma2         = RealScalar(m);
            const auto bkk = real(ldlt(k, k));
            for (Index j = 0; j < k; ++j)
            {
                const auto bkj = ldlt(k, j);
                sigma2 -= abs2(bkj) * bkk;
            }
        }
        else
        {
            sigma2 = (mu1 * nu1 + mu2 * nu2) / denom;
        }

        bk(k) = sigma2;

        Index nt    = n - k - 1;
        bk.tail(nt) = (conj(mu1) / sigma2 * y1.tail(nt) +
                       conj(mu2) / sigma2 * y2.tail(nt));
        bk.array().tail(nt) /=
            (CoeffsVector::Constant(nt, zk_inv) - z.tail(nt).conjugate())
                .array();

        x1.tail(nt) -= mu1 * bk.tail(nt);
        x2.tail(nt) -= mu2 * bk.tail(nt);
        y1.tail(nt) -= conj(nu1) * bk.tail(nt);
        y2.tail(nt) -= conj(nu2) * bk.tail(nt);
    }

    return;
}

} // namespace detail

///
/// Preconditioner specialized for of Vandermonde matrix
///
template <typename T>
class VandermondePreconditioner
{
public:
    using Scalar     = T;
    using RealScalar = typename Eigen::NumTraits<Scalar>::Real;

    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

    using StorageIndex = typename Matrix::StorageIndex;
    using Index        = Eigen::Index;

    using VandermondeGEMV = MatrixFreeGEMV<VandermondeMatrix<Scalar>>;
    enum
    {
        ColsAtCompileTime    = Eigen::Dynamic,
        MaxColsAtCompileTime = Eigen::Dynamic,
    };

    VandermondePreconditioner() = default;

    ~VandermondePreconditioner() = default;

    Index rows() const
    {
        return m_ldlt.rows();
    }

    Index cols() const
    {
        return m_ldlt.cols();
    }

    VandermondePreconditioner& analyzePattern(const VandermondeGEMV&)
    {
        return *this;
    }

    VandermondePreconditioner& factorize(const VandermondeGEMV& mat)
    {
        const Index n = mat.cols();

        m_ldlt.resize(n, n);
        m_invdiag.resize(n);
        Matrix work(n, 4);
        //
        // Compute Cholesky decomposition of the Gramian matrix of the form
        // \f$ V^{\ast} V = L D L^{\ast} \f$
        //
        detail::cholesky_vandermonde_gramian(mat.nestedExpression(), m_ldlt,
                                             work);

        for (Index i = 0; i < n; ++i)
        {
            if (m_ldlt(i, i) == Scalar())
            {
                m_invdiag(i) = RealScalar(1);
            }
            else
            {
                m_invdiag(i) = RealScalar(1) / m_ldlt(i, i);
            }
        }
        m_is_initialized = true;
        return *this;
    }

    VandermondePreconditioner& compute(const VandermondeGEMV& mat)
    {
        return factorize(mat);
    }

    template <typename Rhs, typename Dest>
    void _solve_impl(const Rhs& b, Dest& x) const
    {
        auto matL = m_ldlt.template triangularView<Eigen::UnitLower>();
        x         = matL.solve(b);
        x.array() *= m_invdiag.array();
        matL.adjoint().solveInPlace(x);
    }

    template <typename Rhs>
    inline const Eigen::Solve<VandermondePreconditioner, Rhs>
    solve(const Eigen::MatrixBase<Rhs>& b) const
    {
        eigen_assert(m_is_initialized &&
                     "VandermondePreconditioner is not initialized.");
        eigen_assert(m_ldlt.cols() == b.rows() &&
                     "VandermondePreconditioner::solve(): invalid "
                     "number of rows of the right hand side matrix b");
        return Eigen::Solve<VandermondePreconditioner, Rhs>(*this, b.derived());
    }

    Eigen::ComputationInfo info()
    {
        return Eigen::Success;
    }

private:
    Matrix m_ldlt;
    Vector m_invdiag;
    bool m_is_initialized;
};

///
/// ### VandermondeLeastSquaresSolver
///
/// Solve a least squares problem, \f$V \boldsymbol{x}=\boldsymbol{b},\f$ where
/// \f$V\f$ is a column Vandermonde matrix.
///
template <typename T>
using VandermondeLeastSquaresSolver =
    Eigen::LeastSquaresConjugateGradient<MatrixFreeGEMV<VandermondeMatrix<T>>,
                                         VandermondePreconditioner<T>>;

} // namespace mxpfit

#endif /* MXPFIT_VANDERMONDE_LEAST_SQUARES_HPP */


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


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


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


/*
 * ground_filter.cpp
 *
 * Created on	: May 19, 2017
 * Author	: Patiphon Narksri
 */
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <velodyne_pointcloud/point_types.h>
#include <opencv/cv.h>

#include <boost/lexical_cast.hpp> //U
#include <boost/chrono.hpp> //U
#include <iostream> //U

enum Label
{
        GROUND = 0,
        VERTICAL = 1,
        UNKNOWN = 3
};

class GroundFilter
{
public:

	GroundFilter();

private:

	ros::NodeHandle n;
        ros::Subscriber sub;
	ros::Publisher vertical_points_pub;
	ros::Publisher ground_points_pub;

        std::string     point_topic;
	int 		sensor_model;
	double 		sensor_height;
	double 		max_slope;
	double 		gap_thres;
        bool            floor_removal;
	double 		radius_coeff_close;
	double		radius_coeff_far;

	double 		point_distance;
	int 		min_point;

	int 		vertical_res;
	int 		horizontal_res;
	double 		limiting_ratio;
	cv::Mat 	index_map;
	Label 		class_label[64];
	double 		optimal_radius[64];

	//These will be deleted
	int		original_point;
	int		remaining_point;
	int		point_after_tf;

	boost::chrono::high_resolution_clock::time_point t1;
	boost::chrono::high_resolution_clock::time_point t2;
	boost::chrono::nanoseconds elap_time;

	void initLabelArray(int model);
	void initRadiusArray(double radius[], int model);
	void initDepthMap(int width);
	void publishPoint(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
				int index[], int &index_size,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &topic);

	void velodyneCallback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg);
	void groundSeparate(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &vertical_points,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &ground_points);

};

GroundFilter::GroundFilter() : n("~")
{

	n.param<std::string>("point_topic", point_topic, "/points_raw");
	//If it set to False it will publish the original PointCloud
 	n.param("remove_floor",  floor_removal,  true);
	//Can be selected between 16, 32 and 64 (Have never tested on 16 though)
        n.param("sensor_model", sensor_model, 64);
	//This is the height of Velodyne measured from center of Velodyne to ground
        n.param("sensor_height", sensor_height, 1.80);
	//Maximum allowable slope i.e., any surface steeper than this angle[deg] will not be removed
        n.param("max_slope", max_slope, 15.0);

	//These parameters have been tested to be the optimal values for this algorithm
	//Shouldn't have to be changed for normal use
        n.param("min_point", min_point, 2);
	n.param("gap_thres", gap_thres, 0.15);
	n.param("radius_coeff_close", radius_coeff_close, 0.2);
	n.param("radius_coeff_far", radius_coeff_far, 0.7);

	//Number of laser rays in vertical direction
	vertical_res 	= 64;
	//Number of laser rays in horizontal direction (will be recalculated every rotation)
	horizontal_res 	= 2000;
	//Use tan instead of max_slope in degree for more efficient computation
	limiting_ratio 	= tan(20.0*M_PI/180);

	vertical_res = sensor_model;
	initLabelArray(sensor_model);
	limiting_ratio = tan(max_slope*M_PI/180);
	initRadiusArray(optimal_radius, sensor_model);

	sub = n.subscribe(point_topic, 10, &GroundFilter::velodyneCallback, this);
        vertical_points_pub = n.advertise<sensor_msgs::PointCloud2>("/points_lanes", 10);
        ground_points_pub = n.advertise<sensor_msgs::PointCloud2>("/points_ground", 10);

}
//This loop calculate the expected spaced between consecutive rings
void GroundFilter::initRadiusArray(double radius[], int model)
{
        if (model == 32)
        {
                double start_angle = 92.0/3;
                double angle_res = 4.0/3;
                for (int i = 0; i < model; i++)
                {
                        if (i == 0)
                        {
                                radius[i] = 999999;
                        } else {
                                double theta = start_angle - i*angle_res;
                                theta = theta*M_PI/180.0;
                                radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + angle_res*M_PI/180.0));
                        }

			if (i <= 12)
			{
				radius[i] = radius_coeff_close*radius[i];
			} else if (i <= 20) {
				radius[i] = radius_coeff_far*radius[i];
			} else {
				radius[i] = radius[20];
			}
                }
        } else {
                for (int i = 0; i < model; i++)
                {
                        if (i < 32)
                        {
                                double start_angle = 73.0/3;
                                double angle_res = 1.0/2;
                                if (i == 0)
                                {
                                        radius[i] = 999999;
                                } else {
                                        double theta = start_angle - i*angle_res;
                                        theta = theta*M_PI/180.0;
                                        radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + angle_res*M_PI/180.0));
                                }
                        } else {
                                double start_angle = 25.0/3;
                                double angle_res = 1.0/3;
                                if (i == 32)
                                {
                                        double theta = start_angle;
                                        theta = theta*M_PI/180.0;
                                        radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + 0.5*M_PI/180.0));
                                } else {
                                        double theta = start_angle - (i-32)*angle_res;
                                        theta = theta*M_PI/180.0;
                                        radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + angle_res*M_PI/180.0));
                                }
                        }

			if (i <= 15)
			{
				radius[i] = radius_coeff_close*radius[i];
			} else if (i <= 40) {
				radius[i] = radius_coeff_far*radius[i];
			} else {
				radius[i] = radius[40];
			}
                }
        }
}

//Create an enum array to store the current status of each point in the same bearing angle
void GroundFilter::initLabelArray(int model)
{
	for(int a = 0; a < vertical_res; a++)
	{
		class_label[a] = UNKNOWN;
	}
}

//Create a depth map that has a size of vertical_res x horizontal_res
void GroundFilter::initDepthMap(int width)
{
	const int mOne = -1;
	index_map = cv::Mat_<int>(vertical_res, width, mOne);
}

//Used for publish the separated PointCloud to a defined topic
void GroundFilter::publishPoint(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
				int index[], int &index_size,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &topic)
{

	velodyne_pointcloud::PointXYZIR point;
	for (int i = 0; i < index_size; i++)
	{
		point.x = msg->points[index[i]].x;
		point.y = msg->points[index[i]].y;
		point.z = msg->points[index[i]].z;
		point.intensity = msg->points[index[i]].intensity;
		point.ring = msg->points[index[i]].ring;
		topic.push_back(point);

		remaining_point++;
	}
	index_size = 0;

}

//Main calculation is done in this function
void GroundFilter::groundSeparate(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
			pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &vertical_points,
			pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &ground_points)
{

        velodyne_pointcloud::PointXYZIR point;

        horizontal_res = int(msg->points.size()*1.8 / vertical_res);
        initDepthMap(horizontal_res);

	original_point = msg->points.size();
	remaining_point = 0;
	point_after_tf = 0;

	//This conversion has some losses
	//Convert coordinate of each point from Cartesian to Spherical (XYZ -> DepthMap)
        for (int i = 0; i < msg->points.size(); i++)
        {
                double u = atan2(msg->points[i].y,msg->points[i].x) * 180/M_PI;
                if (u < 0) u = 360 + u;
                int column = horizontal_res - (int)((double)horizontal_res * u / 360.0) - 1;
                int row = vertical_res - 1 - msg->points[i].ring;
		index_map.at<int>(row, column) = i;
        }

	//Iterate through each bearing angle (each horizontal angle)
	for (int i = 0; i < horizontal_res; i++)
        {
                Label point_class[vertical_res];
		int unknown_index[vertical_res];
		int point_index[vertical_res];
		int unknown_index_size = 0;
		int point_index_size = 0;
		double z_ref = 0;
		double r_ref = 0;
		//Initialize the enum array to be UKNOWN in every elements before calculation
		//Of each bearing angle
		std::copy(class_label, class_label + vertical_res, point_class);

		//Iterate through each vertical angle (each laser ray) starting from lowest ray
		for (int j = vertical_res - 1; j >= 0; j--)
                {
			//If the point has already been processed and already classified
			//It will not be processed again
                        if (index_map.at<int>(j,i) > -1 && point_class[j] == UNKNOWN)
                        {
				point_after_tf++;
				double x0 = msg->points[index_map.at<int>(j, i)].x;
				double y0 = msg->points[index_map.at<int>(j, i)].y;
				double z0 = msg->points[index_map.at<int>(j, i)].z;
				double r0 = sqrt(x0*x0 + y0*y0);
				double r_diff = r0 - r_ref;
				double z_diff = fabs(z0 - z_ref);
				double pair_angle = z_diff/r_diff;
				//Check if the angle between the current and the previous point is less than a defined maximum_slope
				//If the angle is less than maximum_slope, add the current point to "Candidate group"
				if (((pair_angle > 0 && pair_angle < limiting_ratio) && z_diff < gap_thres) || point_index_size == 0)
				{
					r_ref = r0;
					z_ref = z0;
					point_index[point_index_size] = j;
					point_index_size++;
				} else {
					//If the angle exceeds the maximum slope
					//Check number of point in "Candidate group", if exceeds the minimum_point threshold
					//Publish them as ground points
					if (point_index_size > min_point)
					{
						for (int m = 0; m < point_index_size; m++)
						{

								int index = index_map.at<int>(point_index[m],i);
								point.x = msg->points[index].x;
								point.y = msg->points[index].y;
								point.z = msg->points[index].z;
								point.intensity = msg->points[index].intensity;
								point.ring = msg->points[index].ring;
								ground_points.push_back(point);
								point_class[point_index[m]] = GROUND;

								remaining_point++;
						}
						point_index_size = 0;
					//If the number of point in "Candidate group" is less than the threshold, continue the calculation
					} else {
						for (int m = 0; m < point_index_size; m++)
						{
							int index = index_map.at<int>(point_index[m],i);
							point.x = msg->points[index].x;
							point.y = msg->points[index].y;
							point.z = msg->points[index].z;
							point.intensity = msg->points[index].intensity;
							point.ring = msg->points[index].ring;
							unknown_index[unknown_index_size] = index;
							unknown_index_size++;
						}
						point_index_size = 0;
					}

					r_ref = r0;
					z_ref = z0;
					point_index[point_index_size] = j;
					point_index_size++;
				}
  			}
			//If the highest ray is reached
                        if (j == 0)
                        {
				//First, check if the "Candidate group" contain any point, if so classify them using
				//the same criteria as above
				if (point_index_size != 0)
				{
					if (point_index_size > min_point)
					{
						for (int m = 0; m < point_index_size; m++)
						{

								int index = index_map.at<int>(point_index[m],i);
								point.x = msg->points[index].x;
								point.y = msg->points[index].y;
								point.z = msg->points[index].z;
								point.intensity = msg->points[index].intensity;
								point.ring = msg->points[index].ring;
								ground_points.push_back(point);
								point_class[point_index[m]] = GROUND;

								remaining_point++;
						}
						point_index_size = 0;
					} else {
						for (int m = 0; m < point_index_size; m++)
						{
							int index = index_map.at<int>(point_index[m],i);
							point.x = msg->points[index].x;
							point.y = msg->points[index].y;
							point.z = msg->points[index].z;
							point.intensity = msg->points[index].intensity;
							point.ring = msg->points[index].ring;
							unknown_index[unknown_index_size] = index;
							unknown_index_size++;
						}
						point_index_size = 0;
					}
				}
				//Lastly, the remaining unknown points are checked using different approach
				//Check if the radial distance between two consecutive points is less than
				//point_distance threshold, if so classify them as vertical
				double centroid = 0;
				int centroid_ring = 0;
				int cluster_index[vertical_res];
				int cluster_index_size = 0;
				for (int m = unknown_index_size - 1; m >= 0; m--)
				{
					double x0 = msg->points[unknown_index[m]].x;
					double y0 = msg->points[unknown_index[m]].y;
					double r0 = sqrt(x0*x0 + y0*y0);
					double r_diff = fabs(r0 - centroid);
					point_distance = optimal_radius[centroid_ring];
					if ((r_diff < point_distance) || cluster_index_size == 0)
					{
						cluster_index[cluster_index_size] = unknown_index[m];
						cluster_index_size++;
						centroid = r0;
						centroid_ring = msg->points[unknown_index[m]].ring;
					} else {
						if(cluster_index_size > 1)
						{
							publishPoint(msg, cluster_index	, cluster_index_size, vertical_points);
						} else {
							publishPoint(msg, cluster_index, cluster_index_size, ground_points);
						}

						cluster_index[cluster_index_size] = unknown_index[m];
						cluster_index_size++;
						centroid = r0;
						centroid_ring = msg->points[unknown_index[m]].ring;
					}
					if (m == 0)
					{
						if(cluster_index_size > 1)
						{
							publishPoint(msg, cluster_index, cluster_index_size, vertical_points);
						} else {
							publishPoint(msg, cluster_index, cluster_index_size, ground_points);
						}
					}
				}
                        }
                }
	}
}

void GroundFilter::velodyneCallback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg)
{
	t1 = boost::chrono::high_resolution_clock::now();

	pcl::PointCloud<velodyne_pointcloud::PointXYZIR> vertical_points;
	pcl::PointCloud<velodyne_pointcloud::PointXYZIR> ground_points;
	vertical_points.header = msg->header;
        ground_points.header = msg->header;
        vertical_points.clear();
        ground_points.clear();

	groundSeparate(msg, vertical_points, ground_points);

	if (!floor_removal)
	{
		vertical_points = *msg;
	}

	vertical_points_pub.publish(vertical_points);
        ground_points_pub.publish(ground_points);

	t2 = boost::chrono::high_resolution_clock::now();
        elap_time = (boost::chrono::duration_cast<boost::chrono::nanoseconds>(t2-t1));
        std::cout << "Computational time for each frame is " << elap_time << " for total " << remaining_point << " points" << std::endl;
        //std::cout << "Original point is " << original_point << " The remaining point is " << remaining_point <<
	//" Lost point is " << original_point - remaining_point << " Point after transform " << point_after_tf<<
	//" Real missing point is " << point_after_tf - remaining_point  << std::endl;
}

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

        ros::init(argc, argv, "ground_filter");
	GroundFilter node;
        ros::spin();

	return 0;

}


#include <Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include "principal_curvatures_can.hpp"

#define R 2
#define r 1
#define N 10000
#define THETA_SCALE (M_PI / 2)
#define THETA_OFFSET (M_PI / 2)
#define PHI_SCALE (M_PI / 4)
#define SEARCH_RADIUS 0.1

int main(void) {
  Eigen::VectorXf theta = THETA_SCALE * Eigen::VectorXf::Random(N) + Eigen::VectorXf::Constant(N, THETA_OFFSET);
  Eigen::VectorXf phi = PHI_SCALE * Eigen::VectorXf::Random(N);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>());
  for(int i=0;i<N;i++) {
    pcl::PointXYZ point;
    point.x = (R + r * cos(theta(i))) * cos(phi(i));
    point.y = (R + r * cos(theta(i))) * sin(phi(i));
    point.z = r * sin(theta(i));
    cloud->points.push_back(point);
  }
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
  normal_estimation.setInputCloud(cloud);
  normal_estimation.setSearchMethod(tree);
  normal_estimation.setRadiusSearch(SEARCH_RADIUS);
  normal_estimation.setViewPoint(0, 0, std::numeric_limits<float>::infinity());
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>());
  normal_estimation.compute(*normals);
  PrincipalCurvaturesEstimationCAN curvature_estimation;
  curvature_estimation.setInputCloud(cloud);
  curvature_estimation.setInputNormals(normals);
  curvature_estimation.setSearchMethod(tree);
  curvature_estimation.setRadiusSearch(SEARCH_RADIUS);
  pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr curvatures (new pcl::PointCloud<pcl::PrincipalCurvatures>());
  curvature_estimation.compute(*curvatures);
  for(int i=0;i<N;i++) {
    pcl::PrincipalCurvatures curve_point = curvatures.get()->points[i];
    std::cout
      << theta(i) << " "
      << phi(i) << " "
      << curve_point.pc1 * curve_point.pc2 << " "
      << cos(theta(i)) / (r * (R + r * cos(theta(i))))
      << std::endl;
  }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/sqr.hpp>
namespace boost { namespace simd { namespace ext
{

  template<std::uintmax_t Exp, std::uintmax_t Odd = Exp%2>
  struct pow_expander;

  template<std::uintmax_t Exp>
  struct pow_expander<Exp, 0ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const& a0) BOOST_NOEXCEPT
    {
      return pow_expander<Exp/2>::call(sqr(a0));
    }
  };

  template<std::uintmax_t Exp>
  struct pow_expander<Exp, 1ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const& a0) BOOST_NOEXCEPT
    {
      return a0*pow_expander<Exp/2>::call(sqr(a0));
    }
  };

  template<>
  struct pow_expander<0ULL, 0ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const&) BOOST_NOEXCEPT
    {
      return One<A0>();
    }
  };

  template<>
  struct pow_expander<0ULL, 1ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const&) BOOST_NOEXCEPT
    {
      return One<A0>();
    }
  };
} } }

#endif


/*
 * dcdc.cc
 *
 *  created on: 18.04.2016
 *      author: rungger
 */

/*
 * demonstration of the boost ode solver usage
 *
 */

#include <array>
#include <iostream>

#include "cuddObj.hh"

#include "SymbolicSet.hh"
#include "SymbolicModelGrowthBound.hh"

#include "TicToc.hh"
#include <boost/numeric/odeint.hpp>

/* state space dim */
#define sDIM 2
#define iDIM 1

/* data types for the ode solver */
typedef std::array<double,2> state_type;
typedef std::array<double,1> input_type;

typedef boost::numeric::odeint::runge_kutta_dopri5< state_type > stepper_type;

/* we integrate the dcdc ode by 0.5 sec (the result is stored in x)  */
auto  dcdc_post = [](state_type &x, input_type &u) -> void {

  /* the ode describing the system */
  auto  system_ode = [&](const state_type &x, state_type &dxdt, const double) -> void {

    const double r0=1.0 ;
    const double vs = 1.0 ;
    const double rl = 0.05 ;
    const double rc = rl / 10 ;
    const double xl = 3.0 ;
    const double xc = 70.0 ;

    const double b[2]={vs/xl, 0};

    double a[2][2];
    if(u[0]==1) {
      a[0][0] = -rl / xl;
      a[0][1] = 0;
      a[1][0] = 0;
      a[1][1] = (-1 / xc) * (1 / (r0 + rc));
    } else {
      a[0][0] = (-1 / xl) * (rl + ((r0 * rc) / (r0 + rc))) ;
      a[0][1] =  ((-1 / xl) * (r0 / (r0 + rc))) / 5 ;
      a[1][0] = 5 * (r0 / (r0 + rc)) * (1 / xc);
      a[1][1] =(-1 / xc) * (1 / (r0 + rc)) ;
    }

    dxdt[0] = a[0][0]*x[0]+a[0][1]*x[1] + b[0];
    dxdt[1] = a[1][0]*x[0]+a[1][1]*x[1] + b[1];

  };
  boost::numeric::odeint::integrate_adaptive(make_controlled(1E-12 ,1E-12,stepper_type()),system_ode,x,0.0,0.5,0.5);
};

/* computation of the growth bound (the result is stored in r)  */
auto radius_post = [](state_type &r, input_type &u) -> void {

  /* the ode to determine the radius of the cell which over-approximates the
   * attainable set see: http://arxiv.org/abs/1503.03715v1 */
  auto growth_bound_ode = [&](const state_type &r, state_type &drdt, const double t) {
    /* for the dcdc boost converter the growth bound is simply given by the metzler matrix of the system matrices */
    const double r0=1.0 ;
    const double rl = 0.05 ;
    const double rc = rl / 10 ;
    const double xl = 3.0 ;
    const double xc = 70.0 ;

    double a[2][2];
    if(u[0]==1) {
      a[0][0] = -rl / xl;
      a[0][1] = 0;
      a[1][0] = 0;
      a[1][1] = (-1 / xc) * (1 / (r0 + rc));
    } else {
      a[0][0] = (-1 / xl) * (rl + ((r0 * rc) / (r0 + rc))) ;
      a[0][1] =  ((1 / xl) * (r0 / (r0 + rc))) / 5 ;
      a[1][0] = 5 * (r0 / (r0 + rc)) * (1 / xc);
      a[1][1] =(-1 / xc) * (1 / (r0 + rc)) ;
    }

    drdt[0] = a[0][0]*r[0]+a[0][1]*r[1];
    drdt[1] = a[1][0]*r[0]+a[1][1]*r[1];
  };

  boost::numeric::odeint::integrate_adaptive(make_controlled(1E-12,1E-12,stepper_type()),growth_bound_ode,r,0.0,0.5,0.5);
};

int main() {
  /* to measure time */
  TicToc tt;
  /* there is one unique manager to organize the bdd variables */
  Cudd mgr;

  /****************************************************************************/
  /* construct SymbolicSet for the state space */
  /****************************************************************************/
  /* setup the workspace of the synthesis problem and the uniform grid */
  /* lower bounds of the hyper rectangle */
  double lb[sDIM]={1.15,5.45};
  /* upper bounds of the hyper rectangle */
  double ub[sDIM]={1.55,5.85};
  /* grid node distance diameter */
  double eta[sDIM]={2/4e3,2/4e3};
  scots::SymbolicSet ss(mgr,sDIM,lb,ub,eta);
  ss.addGridPoints();

  /****************************************************************************/
  /* construct SymbolicSet for the input space */
  /****************************************************************************/
  double ilb[iDIM]={1};
  double iub[iDIM]={2};
  double ieta[iDIM]={1};
  scots::SymbolicSet is(mgr,iDIM,ilb,iub,ieta);
  is.addGridPoints();

  /****************************************************************************/
  /* setup class for symbolic model computation */
  /****************************************************************************/
  /* create SymbolicSet for the post domain postX in preX x U x postX
   * by coping preX and assigning new BDD IDs */
  scots::SymbolicSet sspost(ss,1);
  scots::SymbolicModelGrowthBound<state_type,input_type> abs(&ss, &is, &sspost);
  /* compute the transition relation */
  tt.tic();
  abs.computeTransitionRelation(dcdc_post, radius_post);
  std::cout << std::endl;
  tt.toc();
  /* get the number of elements in the transition relation */
  std::cout << std::endl << "Number of elements in the transition relation: " << abs.getSize() << std::endl;

  /* get SymbolicSet containing the transition relation with domain X x U x X */
  scots::SymbolicSet tr=abs.getTransitionRelation();
  /* write SymbolicSet to file */
  tr.writeToFile("dcdc_abs.bdd");

  /* read SymbolicSet containing the transition relation from file */
  scots::SymbolicSet trset(mgr,"dcdc_abs.bdd");

  return 1;
}


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> G;

  //====================

  // Get number of edges: codim = 1
  // Get number of nodes: codim = 0
  const lf::mesh::Mesh::size_type numEdges = mesh.NumEntities(1),
                                  numNodes = mesh.NumEntities(2);

  // We know, G has exactly 2 non-zero entries per row.
  G = Eigen::SparseMatrix<int, Eigen::RowMajor> (numEdges, numNodes);
  G.reserve(Eigen::VectorXi::Constant(numEdges, 2)); //size=numEdges, value=2

  // 1. Iterate over alledges
  // 2. Check index of nodes which are endpoints of the edges
  // ! We cannot iterate ver vertices. LehrFem++ does not allow to visit !
  // ! edges adjacent to a vertex.                                       !

  for ( const lf::mesh::Entity *edge : mesh.Entities(1) ) {
    // Get index of this edge
    lf::mesh::Mesh::size_type edgeIdx = mesh.Index(*edge);

    // Get nodes and their indices.
    // ! Now codim(nodes)=1 - because it's a relative codim !
    // ! Seen from the edge, a node has codim 1 !

    auto nodes = edge->SubEntities(1); // ! Relativ Codim !
    lf::mesh::Mesh::size_type firstNodeIdx = mesh.Index(*nodes[0]);
    lf::mesh::Mesh::size_type lastNodeIdx = mesh.Index(*nodes[1]);

    // Add matrix entries to G
    G.coeffRef(edgeIdx, firstNodeIdx) = 1.0;
    G.coeffRef(edgeIdx, lastNodeIdx) = -1.0;
  }

  //====================

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store cell-edge incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> D;

  //====================

  // Get number of cells and edges
  const lf::mesh::Mesh::size_type numCells = mesh.NumEntities(0),
                                  numEdges = mesh.NumEntities(1);

  // Sparse init. of D. D has at most 4 nnz-entries per row.
  D = Eigen::SparseMatrix<int, Eigen::RowMajor> (numCells, numEdges);
  D.reserve( Eigen::VectorXi::Constant(numCells, 4) );

  // Loop over all cells.
  for ( const lf::mesh::Entity *cell : mesh.Entities(0) ) {
    // Get cell index
    lf::mesh::Mesh::size_type cellIdx = mesh.Index(*cell);

    // Get edges of current cell
    auto edges = cell->SubEntities(1);

    // Check orientation of all cells
    auto edgeOrientations = cell->RelativeOrientations();

    // Iterate over both and add to D
    auto edgeIt = edges.begin();
    auto orntIt = edgeOrientations.begin();

    // Fill D by comparing the orientation of the edges of the current cell
    for(; edgeIt != edges.end() && orntIt != edgeOrientations.end();
        ++edgeIt, ++orntIt) {
      lf::mesh::Mesh::size_type edgeIdx = mesh.Index(**edgeIt);
      D.coeffRef(cellIdx, edgeIdx) += lf::mesh::to_sign(*orntIt);
    }
  }

  //====================

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  bool isZero = false;

  //====================
  Eigen::SparseMatrix<int> G = computeEdgeVertexIncidenceMatrix(mesh);
  Eigen::SparseMatrix<int> D = computeCellEdgeIncidenceMatrix(mesh);

  isZero = ( (D*G).norm() == 0);

  //====================
  return isZero;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP
#define BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/extensions/triangulation/strategies/cartesian/detail/precise_math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Adaptive precision predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation (numeric_limits<ct>::epsilon() and numeric_limits<ct>::digits must be supported for calculation type ct)
\tparam robustness Number that determines maximum precision. Values from 0 to 2 may make the calculation terminate faster for inputs that may require higher precision to ensure correctness.
\details This predicate determines at which side of a segment a point lies using an algorithm that is adapted from orient2d as described in "Adaptive Precision Floating-Point Arithmetic and Fast Robust Geometric Predicates" by Jonathan Richard Shewchuk ( https://dl.acm.org/citation.cfm?doid=237218.237337 ). More information and copies of the paper can also be found at https://www.cs.cmu.edu/~quake/robust.html . It is designed to be adaptive in the sense that it should be fast for inputs that lead to correct results with plain float operations but robust for inputs that require higher precision arithmetics.
 */
template
<
    typename CalculationType = void,
    int robustness = 3
>
struct side_robust
{
public:
    //! \brief Computes double the signed area of the CCW triangle p1, p2, p
    template
    <
        typename CoordinateType,
        typename PromotedType,
        typename P1,
        typename P2,
        typename P
    >
    static inline PromotedType side_value(P1 const& p1, P2 const& p2,
        P const& p)
    {
        std::array<PromotedType, 2> pa {{ get<0>(p1), get<1>(p1) }};
        std::array<PromotedType, 2> pb {{ get<0>(p2), get<1>(p2) }};
        std::array<PromotedType, 2> pc {{ get<0>(p), get<1>(p) }};
        return ::boost::geometry::detail::precise_math::orient2d
            <PromotedType, robustness>(pa, pb, pc);
    }

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename coordinate_type<P1>::type coordinate_type1;
        typedef typename coordinate_type<P2>::type coordinate_type2;
        typedef typename coordinate_type<P>::type coordinate_type3;

        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                coordinate_type1, coordinate_type2
                            >::type,
                        coordinate_type3
                    >::type,
                CalculationType
            >::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        promoted_type sv =
            side_value<coordinate_type, promoted_type>(p1, p2, p);
        return sv > 0 ? 1
            : sv < 0 ? -1
            : 0;
    }
#endif

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP


// This file is part of the dune-xt-common project:
//   https://github.com/dune-community/dune-xt-common
// Copyright 2009-2018 dune-xt-common developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   René Fritze    (2018)
//   Tobias Leibner (2018)

#ifndef DUNE_XT_COMMON_COORDINATES_HH
#define DUNE_XT_COMMON_COORDINATES_HH

#include <dune/xt/common/disable_warnings.hh>
#include <boost/geometry.hpp>
#include <dune/xt/common/reenable_warnings.hh>

#include <dune/xt/common/fvector.hh>

namespace Dune {
namespace XT {
namespace Common {

/** Converts from (x, y, z) to (theta, phi) on the unit sphere s.t.
 * (x, y, z) = (sin(theta) cos(phi), sin(theta) sin(phi), cos(theta))
 * with 0 \leq \theta \leq \pi and 0 \leq \varphi < 2\pi. **/
template <class DomainFieldType>
class CoordinateConverter
{
  typedef typename boost::geometry::model::point<DomainFieldType, 3, typename boost::geometry::cs::cartesian>
      BoostCartesianCoordType;
  typedef typename boost::geometry::model::
      point<DomainFieldType, 2, typename boost::geometry::cs::spherical<boost::geometry::radian>>
          BoostSphericalCoordType;

public:
  typedef FieldVector<DomainFieldType, 3> CartesianCoordType;
  typedef FieldVector<DomainFieldType, 2> SphericalCoordType;

  static SphericalCoordType to_spherical(const CartesianCoordType& x)
  {
    BoostCartesianCoordType x_boost(x[0], x[1], x[2]);
    BoostSphericalCoordType x_spherical_boost;
    boost::geometry::transform(x_boost, x_spherical_boost);
    return SphericalCoordType{boost::geometry::get<1>(x_spherical_boost), boost::geometry::get<0>(x_spherical_boost)};
  }

  static CartesianCoordType to_cartesian(const SphericalCoordType& x_spherical, bool first_is_cosine = false)
  {
    // if first_is_cosine, the first coordinate is not theta but rather cos(theta)
    if (first_is_cosine) {
      const auto& mu = x_spherical[0];
      const auto& phi = x_spherical[1];
      return CartesianCoordType{
          std::sqrt(1 - std::pow(mu, 2)) * std::cos(phi), std::sqrt(1 - std::pow(mu, 2)) * std::sin(phi), mu};
    } else {
      BoostSphericalCoordType x_spherical_boost(x_spherical[1], x_spherical[0]);
      BoostCartesianCoordType x_boost;
      boost::geometry::transform(x_spherical_boost, x_boost);
      return CartesianCoordType{
          boost::geometry::get<0>(x_boost), boost::geometry::get<1>(x_boost), boost::geometry::get<2>(x_boost)};
    }
  }
};

} // namespace Common
} // namespace XT
} // namespace Dune

#endif // DUNE_XT_COMMON_COORDINATES_HH


#pragma once

#include <Eigen/Sparse>
#include <vector>

#include "mtao/eigen/shape_checks.hpp"
#include "mtao/quadrature/simpsons.hpp"

//#include "mtao/geometry/mesh/dual_volumes.hpp"

namespace mtao::simulation::hexahedral {
template <typename Scalar, int D>
Eigen::Matrix<Scalar, 1 << D, 1 << D> laplacian_stencil() {
    using Vec = mtao::Vector<Scalar, D>;
    Eigen::Matrix<Scalar, 1 << D, 1 << D> S;
    S.setZero();
    auto eval = [](int elem, const std::array<Scalar, D>& vec) {
        Vec coeffs;
        Vec grad;
        for (size_t idx = 0; idx < D; ++idx) {
            if (elem & (1 << idx)) {
                grad(idx) = -1;
                coeffs(idx) = (Scalar(1) - vec[idx]);
            } else {
                grad(idx) = 1;
                coeffs(idx) = (vec[idx]);
            }
        }
        for (size_t idx = 0; idx < D; ++idx) {
            for (size_t j = 0; j < D; ++j) {
                if (j != idx) {
                    grad(idx) *= coeffs(j);
                }
            }
        }

        return grad;
    };

    auto quad = [&](int i, int j) {
        const int num_samples = 256;
        Scalar val = Scalar(0);

        val = quadrature::multidim_simpsons_rule<D, Scalar>(
            [&](const std::array<Scalar, D>& p) -> Scalar {
                return eval(i, p).dot(eval(j, p));
            },
            0., 1., num_samples);
        return val;
    };

    /*
    std::array<Scalar, D + 1> memo;
    for (int j = 0; j <= D; ++j) {
        memo[j] = quad(0, (1 << (j)) - 1);
        std::cout << memo[j] << std::endl;
    }
    */
    for (int i = 0; i < S.rows(); ++i) {
        S(i, i) = quad(i, i);
        // S(i, i) = memo[0];
        for (int j = i + 1; j < S.cols(); ++j) {
            /*
            int v = i ^ j;
            int count = 0;
            for (int u = 0; u < D; ++u) {
                if (v & (1 << u)) {
                    count++;
                }
            }
            */
            // S(j, i) = S(i, j) = memo[count];
            S(j, i) = S(i, j) = quad(i, j);
        }
    }
    return S;
}

}  // namespace mtao::simulation::hexahedral


/**
 * @file dynkin.hpp
 * @author freol35241
 * @brief A toolkit for 3D dynamics and kinematics of rigid bodies using
 * the YPR euler angle convention.
 * @version 0.3.0
 * @date 2021-01-30
 *
 * @copyright Copyright (c) 2021
 *
 */
#include <cmath>
#include <memory>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <unsupported/Eigen/EulerAngles>

namespace Eigen {
    typedef Eigen::Matrix<double,6,1> Vector6d;
    typedef Eigen::Matrix<double,6,6> Matrix6d;
}

/**
 * @brief `dynkin` namespace
 *
 */
namespace dynkin {

    struct _Frame;
    using Frame = std::shared_ptr<_Frame>;
    struct Transform;
    Transform transform(Frame zeroth, Frame end);

    /**
     * @brief Convert a 3x3 rotation matrix to euler angles using
     * the YPR euler angle convention
     *
     * @param rotation A 3x3 rotation matrix
     * @return Calculated euler angles [roll, pitch, yaw]
     */
    inline Eigen::Vector3d rotation_to_euler(const Eigen::Matrix3d& rotation){
        return rotation.eulerAngles(2,1,0).reverse();
    }

    /**
     * @brief Convert a vector of euler angles to a rotation matrix
     * using the YPR uler angle convention
     *
     * @param attitude A vector of euler angles [roll, pitch, yaw]
     * @return Calculated 3x3 rotation matrix
     */
    inline Eigen::Matrix3d euler_to_rotation(const Eigen::Vector3d& attitude){
        return (
            Eigen::AngleAxisd(attitude(2), Eigen::Vector3d::UnitZ())
            * Eigen::AngleAxisd(attitude(1), Eigen::Vector3d::UnitY())
            * Eigen::AngleAxisd(attitude(0), Eigen::Vector3d::UnitX())
            ).toRotationMatrix();
    }

    /**
     * @brief Representing a transform between two `Frames`
     *
     */
    struct Transform{
        Eigen::Isometry3d HTM;

        Transform(const Eigen::Isometry3d& HTM):HTM(HTM){}

        /**
         * @brief Apply this transformation to a vector
         *
         * @param vector Vector to be transformed
         * @return Transformed vector
         */
        Eigen::Vector3d apply_vector(const Eigen::Vector3d& vector){
            return this->HTM.linear()*vector;
        }

        /**
         * @brief Apply this transformation to a position vector
         *
         * @param position Position vector to be transformed
         * @return Transformed position vector
         */
        Eigen::Vector3d apply_position(const Eigen::Vector3d& position){
            return this->HTM*position;
        }

        /**
         * @brief Apply this transformation to a wrench
         *
         * @param wrench Wrench to be transformed
         * @return Transformed wrench
         */
        Eigen::Vector6d apply_wrench(const Eigen::Vector6d& wrench){
            Eigen::Vector6d out;
            out.head(3) = this->apply_vector(wrench.head(3));
            out.tail(3) = this->apply_vector(wrench.tail(3)) + this->HTM.translation().cross(out.head<3>());
            return out;
        }

        /**
         * @brief Create a Transform which is the inverse of this
         *
         * @return Inverted transform
         */
        Transform inverse(){
            return Transform(this->HTM.inverse());
        }

    };

    /**
     * @brief Create a Frame object
     *
     * @param parent The parent frame of the new frame
     * @return A new Frame
     */
    Frame create_frame(Frame parent = nullptr){
      return std::make_shared<_Frame>(parent);
    }

    /**
     * @brief Representing a coordinate frame
     *
     */
    struct _Frame: public std::enable_shared_from_this<_Frame>{
        Eigen::Isometry3d HTM = Eigen::Isometry3d::Identity();
        Eigen::Vector3d _linear_velocity = Eigen::Vector3d::Zero();
        Eigen::Vector3d _angular_velocity = Eigen::Vector3d::Zero();
        Frame parent;

        _Frame(Frame parent):parent(parent){};

        /**
         * @brief Create a new Frame with this Frame as parent
         *
         * @return Child Frame
         */
        Frame create_child(){
          return create_frame(this->shared_from_this());
        }

        /**
         * @brief Position of this Frame in relation to parent Frame
         *
         * @return Position (3x1)
         */
        auto position(){
            return this->HTM.translation();
        }

        /**
         * @brief Rotation of this Frame in relation to parent Frame
         *
         * @return Rotation matrix 3x3
         */
        auto rotation(){
            return this->HTM.linear();
        }

        /**
         * @brief Linear velocity of this Frame in relation to parent Frame,
         * decomposed in this Frame
         *
         * @return Linear velocity (3x1)
         */
        Eigen::Vector3d& linear_velocity(){
            return this->_linear_velocity;
        }

        /**
         * @brief Angular velocity of this Frame in relation to parent Frame,
         * decomposed in this Frame
         *
         * @return Angular velocity (3x1)
         */
        Eigen::Vector3d& angular_velocity(){
            return this->_angular_velocity;
        }

        /**
         * @brief Get attitude of this Frame in relation to parent Frame
         *
         * @return Attitude (euler angles)(3x1)
         */
        Eigen::Vector3d get_attitude(){
            return rotation_to_euler(this->rotation());
        }

        /**
         * @brief Set attitude of this Frame in relation to parent Frame
         *
         * @param attitude Attitude (euler angles)(3x1)
         */
        void set_attitude(const Eigen::Vector3d& attitude){
            this->rotation() = euler_to_rotation(attitude);
        }

        /**
         * @brief Get pose of this Frame in relation to the inertial Frame
         *
         * @return Pose (6x1)
         */
        Eigen::Vector6d get_pose(){
            Eigen::Vector6d out;
            Transform t = transform(nullptr, this->shared_from_this());
            out.head(3) = t.HTM.translation();
            out.tail(3) = rotation_to_euler(t.HTM.linear());
            return out;
        }

        /**
         * @brief Get twist of this Frame in relation to the inertial Frame
         *
         * @return Twist (6x1)
         */
        Eigen::Vector6d get_twist(){
            Eigen::Vector6d out;

            Eigen::Vector3d v = this->linear_velocity();
            Eigen::Vector3d w = this->angular_velocity();

            if (this->parent != nullptr){
                Eigen::Vector6d twist_p = this->parent->get_twist();
                Transform t = transform(this->shared_from_this(), this->parent);
                v += t.apply_vector(twist_p.head(3)) + twist_p.tail<3>().cross(this->position());
                w += t.apply_vector(twist_p.tail(3));
            }
            out.head(3) = v;
            out.tail(3) = w;
            return out;
        }

    };

    /**
     * @brief Find transform between two frames
     *
     * @param zeroth Zeroth frame of transform
     * @param end End frame of transform
     * @return Transform from zeroth to end
     */
    inline Transform transform(Frame zeroth, Frame end){

        Eigen::Isometry3d HTM = Eigen::Isometry3d::Identity();
        Frame f = end;

        while (f != nullptr){
            if (f == zeroth){
                return Transform(HTM);
            }

            HTM = f->HTM*HTM;
            f = f->parent;

        }

        // If we get to here, the least common base frame is the inertial frame (nullptr)
        Transform T = Transform(HTM);

        if (zeroth == nullptr){
            return T;
        }

        Transform T_ = transform(nullptr, zeroth).inverse();

        return Transform(T_.HTM*T.HTM);
    }

    /**
     * @brief `rigidbody`namespace
     *
     */
    namespace rigidbody{

        /**
         * @brief Create a skew matrix (3x3) from a (3x1) vector
         *
         * @param v Vector to create skew matrix from
         * @return Skew matrix
         */
        inline Eigen::Matrix3d skew(const Eigen::Vector3d& v){
            Eigen::Matrix3d out;
            out <<  0,  -v(2),  v(1),
                    v(2),   0,  -v(0),
                    -v(1),  v(0),   0;
            return out;
        }

        /**
         * @brief Create a Motion Transformation Matrix (6x6)
         *
         * @param position Position vector defining the transformation
         * @return Motion Transformation Matrix
         */
        inline Eigen::Matrix6d motion_transformation_matrix(const Eigen::Vector3d& position){
            Eigen::Matrix6d out = Eigen::Matrix6d::Identity();
            out.block<3,3>(0,3) = skew(position).transpose();
            return out;
        }

        /**
         * @brief Create Generalized Inertia Matrix (6x6)
         *
         * @param mass Rigid body mass
         * @param gyradii Rigid body gyradii (3x1)
         * @return Generalized Inertia Matrix (6x6)
         */
        inline Eigen::Matrix6d generalized_inertia_matrix(const double& mass, const Eigen::Vector3d& gyradii){
            if (mass <= 0.0){
                throw std::invalid_argument("mass must be greater than zero!");
            }
            if ((gyradii.array() <= 0.0).any()){
                throw std::invalid_argument("All gyradii must be greater than zero!");
            }

            Eigen::Matrix6d H = Eigen::Matrix6d::Zero();
            H.block<3,3>(0,0) = Eigen::Matrix3d::Identity() * mass;
            H.block<3,3>(3,3) = (mass*gyradii.array().square()).matrix().asDiagonal();
            return H;
        }

        /**
         * @brief Create an Eulerian Matrix (3x3) relating angular velocity to euler angle derivatives
         *
         * @param attitude Attitude (3x1)
         * @return Eulerian Matrix (3x3)
         */
        inline Eigen::Matrix3d eulerian(const Eigen::Vector3d& attitude){
            double fi = attitude(0);
            double theta = attitude(1);
            Eigen::Matrix3d out;
            out <<  1,      sin(fi)*cos(theta),     cos(fi)*tan(theta),
                    0,      cos(fi),                -sin(fi),
                    0,      sin(fi)/cos(theta),     cos(fi)/cos(theta);
            return out;
        }

        /**
         * @brief Convert angular velocities to euler angle derivatives
         *
         * @param attitude Attitude of rigid body (3x1)
         * @param angular_velocity Angular velocity of rigid body (3x1)
         * @return Euler angle derivatives of rigid body (3x1)
         */
        inline Eigen::Vector3d angular_velocity_to_deuler(
            const Eigen::Vector3d& attitude,
            const Eigen::Vector3d& angular_velocity
        ){
            return eulerian(attitude)*angular_velocity;
        }

        /**
         * @brief Representing an ideal rigid body
         *
         */
        struct RigidBody {
            Eigen::Matrix6d inertia = Eigen::Matrix6d::Identity();
            const Frame origin = create_frame();
            const Frame CoG = create_frame(origin);

            /**
             * @brief Construct a new Rigid Body object
             *
             * @param inertia Generalized Inertia of the rigid body (6x6)
             * @param cog Position vector relating the origin and the CoG of the (3x1)
             * rigid body
             */
            RigidBody(
                const Eigen::Matrix6d inertia,
                const Eigen::Vector3d& cog = Eigen::Vector3d::Zero()
                ): inertia(inertia)
                {
                    this->CoG->position() = cog;
                };

            /**
             * @brief Returns the mass of this rigid body
             *
             * @return Mass
             */
            double mass(){
                return this->inertia(0,0);
            }

            /**
             * @brief Assemble Coriolis-Centripetal Matrix
             *
             * @param inertia Generalized Inertia Matrix of rigid body (6x6)
             * @param twist Twist of rigid body (3x1)
             * @return Coriolis-Centripetal Matrix (6x6)
             */
            Eigen::Matrix6d coriolis_centripetal_matrix(
                const Eigen::Matrix6d inertia,
                const Eigen::Vector6d twist
            ){
                Eigen::Matrix6d C = Eigen::Matrix6d::Zero();
                C.block<3,3>(0,0) = this->mass()*skew(twist.tail(3));
                C.block<3,3>(3,3) = -skew(inertia.block<3,3>(3,3)*twist.tail(3));
                return C;
            }

            /**
             * @brief Generalized coordinates of this rigid body
             * [x, y, z, fi, theta, psi]
             *
             * @return Generalized coordinates (6x1)
             */
            Eigen::Vector6d generalized_coordinates(){
                return this->origin->get_pose();
            }

            /**
             * @brief Generalized velocities of this rigid body
             * [dx, dy, dz, dfi, dtheta, dpsi]
             *
             * @return Generalized velocities (6x1)
             */
            Eigen::Vector6d generalized_velocities(){
                Eigen::Vector6d twist = this->origin->get_twist();
                Transform t = transform(nullptr, this->origin);

                Eigen::Vector6d out = Eigen::Vector6d::Zero();
                out.head(3) = t.apply_vector(twist.head(3));
                out.tail(3) = angular_velocity_to_deuler(
                    this->origin->get_attitude(), twist.tail(3)
                );

                return out;
            }

            /**
             * @brief Returns the resulting acceleration from the given wrench
             *
             * @param wrench Wrench acting on the rigid body (6x1)
             * @param additional_inertia Inertia in addition to the inertia of this
             * rigid body that should be accelerated (6x6)
             * @return Acceleration (6x1)
             */
            Eigen::Vector6d acceleration(
                const Eigen::Vector6d& wrench,
                const Eigen::Matrix6d& additional_inertia = Eigen::Matrix6d::Zero()
            ){
                Eigen::Vector6d twist, f_cc, f;
                Eigen::Matrix6d H, I_cg, C_cg, I_b, C_b;

                f = wrench;
                twist = this->origin->get_twist();

                H = motion_transformation_matrix(this->CoG->position());
                I_cg = (this->inertia.array() + additional_inertia.array()).matrix();
                C_cg = this->coriolis_centripetal_matrix(I_cg, twist);
                I_b = H.transpose() * I_cg * H;
                C_b = H.transpose() * C_cg * H;

                f_cc = C_b * twist;
                f -= f_cc;

                return I_b.lu().solve(f);
            }

            /**
             * @brief Returns the required wrench to obtain the given acceleration
             *
             * @param acceleration Given acceleration (6x1)
             * @param additional_inertia Inertia in addition to the inertia of this
             * rigid body that should be accelerated (6x6)
             * @return Wrench (6x1)
             */
            Eigen::Vector6d wrench(
                const Eigen::Vector6d& acceleration,
                const Eigen::Matrix6d& additional_inertia = Eigen::Matrix6d::Zero()
            ){
                Eigen::Vector6d twist, f_cc, f;
                Eigen::Matrix6d H, I_cg, C_cg, I_b, C_b;

                twist = this->origin->get_twist();

                H = motion_transformation_matrix(this->CoG->position());
                I_cg = (this->inertia.array() + additional_inertia.array()).matrix();
                C_cg = this->coriolis_centripetal_matrix(I_cg, twist);
                I_b = H.transpose() * I_cg * H;
                C_b = H.transpose() * C_cg * H;

                f = I_b * acceleration;
                f_cc = C_b * twist;
                f += f_cc;

                return f;
            }

        };

    }

}