// Copyright (c) 2015-2016 Vittorio Romeo
// License: Academic Free License ("AFL") v. 3.0
// AFL License page: http://opensource.org/licenses/AFL-3.0
// http://vittorioromeo.info | vittorio.romeo@outlook.com

#pragma once

#include <cstddef>
#include <cassert>
#include <cmath>
#include <iostream>
#include <tuple>
#include <vector>
#include <algorithm>
#include <limits>
#include <vrm/core/static_if.hpp>

#undef ARMA_USE_ATLAS
#include <armadillo>

#include "multiples.hpp"
#include "equations.hpp"
#include "folds.hpp"
#include "representations.hpp"
#include "sorted_vector.hpp"

namespace nc
{
    // Tipo vuoto per segnalare al costruttore della classe `matrix` che essa
    // deve essere inizializzata come matrice identità.
    struct init_identity
    {
    };

    // Tipo vuoto per segnalare al costruttore della classe `matrix` che essa
    // non deve essere inizializzata.
    struct dont_init
    {
    };

    // Classe che rappresenta una matrice di tipo `T0` le cui dimensioni sono
    // conosciute a tempo di compilazione.
    template <typename T0, std::size_t TRowCount, std::size_t TColumnCount>
    class matrix
    {
    public:
        using value_type = T0;

        // Restituisce il numero delle righe.
        constexpr static auto row_count() noexcept { return TRowCount; }

        // Restituisce il numero delle colonne.
        constexpr static auto column_count() noexcept { return TColumnCount; }

    private:
        // Dati della matrice: conservati in un `array` unidimensionale
        // standard.
        std::array<T0, TRowCount * TColumnCount> _data;

        // Dati un indice di riga, ed un indice di colonna, calcola
        // l'equivalente indice unidimensionale per accedere all'array dei dati.
        auto calc_index(std::size_t row, std::size_t column) const noexcept
        {
            assert(row < row_count());
            assert(column < column_count());

            return column + column_count() * row;
        }

        // Dato un indice unidimensionale, restituisce una tupla di indici
        // bidimensionali equivalenti all'accesso corretto all'array dei dati.
        auto calc_1d_index(std::size_t i) const noexcept
        {
            assert(i >= 0 && column_count() != 0);

            auto y(i / column_count());
            return std::make_tuple(y, i - y * column_count());
        }

        // Implementazione dell'iterazione sugli indici di una riga della
        // matrice.
        // (Fissa la riga, ed itera su essa).
        template <typename TSelf, typename TF>
        static void impl_for_row_idxs(TSelf&& self, std::size_t i, TF&& f)
        {
            constexpr auto self_column_count(self.column_count());
            for(auto j(0); j < self_column_count; ++j)
            {
                f(i, j);
            }
        }

        // Implementazione dell'iterazione sugli indici di una colonna della
        // matrice.
        // (Fissa la colonna, ed itera su essa).
        template <typename TSelf, typename TF>
        static void impl_for_column_idxs(TSelf&& self, std::size_t j, TF&& f)
        {
            constexpr auto self_row_count(self.row_count());
            for(auto i(0); i < self_row_count; ++i)
            {
                f(i, j);
            }
        }

        // Implementazione dell'iterazione su tutti gli indici di una matrice.
        template <typename TSelf, typename TF>
        static void impl_for_idxs(TSelf&& self, TF&& f)
        {
            constexpr auto self_row_count(self.row_count());
            constexpr auto self_column_count(self.column_count());

            for(auto i(0); i < self_row_count; ++i)
                for(auto j(0); j < self_column_count; ++j)
                {
                    f(i, j);
                }
        }

        template <typename TSelf, typename TTpl>
        static auto& impl_at(TSelf&& self, const TTpl& t)
        {
            return self(std::get<0>(t), std::get<1>(t));
        }

    public:
        // Restituisce `true` se la matrice è quadrata.
        constexpr auto static is_square() noexcept
        {
            return row_count() == column_count();
        }

        // Esegue una funzione `f` su tutti gli indici della matrice.
        template <typename TF>
        void for_idxs(TF&& f)
        {
            impl_for_idxs(*this, FWD(f));
        }
        template <typename TF>
        void for_idxs(TF&& f) const
        {
            impl_for_idxs(*this, FWD(f));
        }

        // Esegue una funzione `f` su tutti gli indici di una riga.
        // (Fissa la riga, ed itera su essa).
        template <typename TF>
        void for_row_idxs(std::size_t i, TF&& f)
        {
            impl_for_row_idxs(*this, i, FWD(f));
        }
        template <typename TF>
        void for_row_idxs(std::size_t i, TF&& f) const
        {
            impl_for_row_idxs(*this, i, FWD(f));
        }

        // Esegue una funzione `f` su tutti gli indici di una colonna.
        // (Fissa la colonna, ed itera su essa).
        template <typename TF>
        void for_column_idxs(std::size_t j, TF&& f)
        {
            impl_for_column_idxs(*this, j, FWD(f));
        }
        template <typename TF>
        void for_column_idxs(std::size_t j, TF&& f) const
        {
            impl_for_column_idxs(*this, j, FWD(f));
        }

        // Pulisce la matrice, settando tutti i valori a `0`.
        void clear()
        {
            for_idxs([this](auto i, auto j)
                {
                    (*this)(i, j) = 0;
                });
        }

        // Pulisce la matrice, settandola come matrice identità. La matrice deve
        // essere quadrata.
        void clear_to_identity()
        {
            static_assert(is_square(), "");

            clear();

            for(auto k(0); k < row_count(); ++k)
            {
                (*this)(k, k) = 1;
            }
        }

        // Construttori.
        matrix(dont_init) {}
        matrix() { clear(); }
        matrix(init_identity) { clear_to_identity(); }

        // Operazioni di copia (default).
        matrix(const matrix& rhs) = default;
        matrix& operator=(const matrix& rhs) = default;

        // Operazioni di spostamento (default).
        matrix(matrix&& rhs) = default;
        matrix& operator=(matrix&& rhs) = default;

        // Restituisce il valore della matrice contenuto negli indici
        // rappresentati da una tupla.
        template <typename TTpl>
        auto& at(const TTpl& t)
        {
            return impl_at(*this, t);
        }
        template <typename TTpl>
        const auto& at(const TTpl& t) const
        {
            return impl_at(*this, t);
        }

        // Restituisce il valore presente negli indici passati.
        auto& operator()(std::size_t row, std::size_t column)
        {
            return _data[calc_index(row, column)];
        }
        const auto& operator()(std::size_t row, std::size_t column) const
        {
            return _data[calc_index(row, column)];
        }

        // Riempie i valori della matrice da un numero variadico di argomenti.
        template <typename... Ts>
        void set_from_variadic(Ts&&... xs)
        {
            // Controlla che il numero di argomenti sia valido.
            static_assert(sizeof...(xs) <= row_count() * column_count(), "");

            // TODO:
            std::vector<T0> vec{xs...};
            for(auto i(0u); i < vec.size(); ++i)
            {
                (*this).at(calc_1d_index(i)) = vec[i];
            }
        }

    private:
        // Implementazione di operazioni semplici (somma, sottrazione) tra due
        // matrici della stessa grandezza.
        template <typename T1, typename TF>
        auto impl_simple_binary_op(
            const matrix<T1, TRowCount, TColumnCount>& rhs, TF&& f) const
        {
            // Tipo in comune tra i valori delle due matrici.
            using common = std::common_type_t<T0, T1>;

            // Alloca una nuova matrice non-inizializzata.
            matrix<common, TRowCount, TColumnCount> result{dont_init{}};

            // Per ogni indice delle matrici, esegue la funzione passata.
            // Converva il risultato nella nuova matrice.
            for_idxs([this, &rhs, &result, &f](auto i, auto j)
                {
                    result(i, j) = f((*this)(i, j), rhs(i, j));
                });

            return result;
        }

    public:
        // Overload dell'operatore `+` per la somma di due matrici.
        template <typename T1>
        auto operator+(const matrix<T1, TRowCount, TColumnCount>& rhs) const
        {
            return impl_simple_binary_op(rhs, [](const auto& lv, const auto& rv)
                {
                    return lv + rv;
                });
        }

        // Overload dell'operatore `-` per la sottrazione di due matrici.
        template <typename T1>
        auto operator-(const matrix<T1, TRowCount, TColumnCount>& rhs) const
        {
            return impl_simple_binary_op(rhs, [](const auto& lv, const auto& rv)
                {
                    return lv - rv;
                });
        }

        // Overload dell'operatore `==` per l'uguaglianza di due matrici.
        template <typename T1>
        auto operator==(const matrix<T1, TRowCount, TColumnCount>& rhs) const
        {
            for(auto i(0); i < row_count(); ++i)
                for(auto j(0); j < column_count(); ++j)
                    if((*this)(i, j) != rhs(i, j))
                    {
                        return false;
                    }

            return true;
        }

        // Overload dell'operatore `!=` per la disuguaglianza di due matrici.
        template <typename T1>
        auto operator!=(const matrix<T1, TRowCount, TColumnCount>& rhs) const
        {
            return !(*this == rhs);
        }

        // Overload dell'operatore `*` per la moltiplicazione di due matrici.
        template <typename T1, std::size_t TRhsColumnCount>
        auto operator*(
            const matrix<T1, TColumnCount, TRhsColumnCount>& rhs) const
        {
            // Tipo in comune tra i valori delle due matrici.
            using common = std::common_type_t<T0, T1>;

            // Alloca una nuova matrice non-inizializzata.
            matrix<common, TRowCount, TRhsColumnCount> result{dont_init{}};

            // Itera su ogni indice della nuova matrice.
            for(std::size_t ri(0); ri < TRowCount; ++ri)
                for(std::size_t rj(0); rj < TRhsColumnCount; ++rj)
                {
                    // Accumulatore per il valore a `(ri, rj)`.
                    common x(0);

                    // Step di accumulazione.
                    for(auto k(0); k < TColumnCount; ++k)
                    {
                        x += ((*this)(ri, k) * rhs(k, rj));
                    }

                    // Setta il valore nella nuova matrice.
                    result(ri, rj) = x;
                }

            return result;
        }

    private:
        // Restituisci la somma dei valore assoluti in una colonna.
        auto sum_abs_value_in_column(std::size_t j) const noexcept
        {
            T0 res(0);
            for(auto i(0); i < TRowCount; ++i)
            {
                res += abs((*this)(i, j));
            }

            return res;
        }

        // Restituisci la somma dei valore assoluti in una riga.
        auto sum_abs_value_in_row(std::size_t i) const noexcept
        {
            T0 res(0);
            for(auto j(0); j < TColumnCount; ++j)
            {
                res += abs((*this)(i, j));
            }

            return res;
        }

    public:
        // Calcolo della norma 1.
        auto norm_1() const noexcept
        {
            T0 curr_max(0);
            for(auto j(0); j < TColumnCount; ++j)
            {
                curr_max = std::max(curr_max, sum_abs_value_in_column(j));
            }

            return curr_max;
        }

        // Calcolo della norma infinita.
        auto norm_inf() const noexcept
        {
            T0 curr_max(0);
            for(auto i(0); i < TRowCount; ++i)
            {
                curr_max = std::max(curr_max, sum_abs_value_in_row(i));
            }

            return curr_max;
        }

        // Calcolo della norma 2.
        // (Utilizza la libreria "armadillo".)
        auto norm_2() const noexcept
        {
            std::vector<double> v;
            for(const auto& x : _data) v.emplace_back(x);

            auto m = arma::mat::fixed<TRowCount, TColumnCount>(v.data());
            return arma::norm(m, 2);
        }

        // Calcolo della norma di Frobenius.
        auto norm_frobenius() const noexcept
        {
            double acc(0);
            for(auto i(0); i < row_count(); ++i)
            {
                for(auto j(0); j < column_count(); ++j)
                {
                    acc += std::abs(std::pow((*this)(i, j), 2));
                }
            }

            return std::sqrt(acc);
        }

        // Restituisce l'ordine della matrice. Se la matrice non è quadrata,
        // restituisce la dimensione minore.
        constexpr auto static order() noexcept
        {
            return column_count() < row_count() ? column_count() : row_count();
        }

        // Calcolo del minore di una matrice (taglia una riga ed una colonna).
        auto calc_minor(
            std::size_t row_to_skip, std::size_t column_to_skip) const noexcept
        {
            // Alloca la matrice minore (ordine inferiore di 1).
            matrix<T0, order() - 1, order() - 1> result{dont_init{}};

            // Prossimi indici da riempire nella matrice nuova.
            std::size_t target_column(0);
            std::size_t target_row(0);

            // Iteriamo sulle nostre righe.
            loop_skipping(0, order(), row_to_skip, [&](auto i)
                {
                    target_column = 0;

                    auto set_inner_column = [&](auto j)
                    {
                        // Settiamo il valore nella matrice nuova.
                        result(target_row, target_column) = (*this)(i, j);
                        ++target_column;
                    };

                    // Iteriamo sulle nostre colonne.
                    // Loop che salta la j-esima colonna.
                    // Setta i valori della matrice `result`.
                    loop_skipping(
                        0, this->order(), column_to_skip, set_inner_column);

                    ++target_row;
                });

            return result;
        }

        // Calcolo del determinante.
        auto determinant() const noexcept
        {
            using namespace vrm::core;

            // Usiamo un `if` a tempo di compilazione per fermare la ricorsione
            // nel caso base in cui il nostro ordine sia uguale a `1`.
            return static_if(bool_v<(order() == 1)>)
                .then([](const auto& x)
                    {
                        // Se l'ordine è uguale a `1`, restitusci il valore in
                        // alto a sinistra.
                        return x(0, 0);
                    })
                .else_([](const auto& x)
                    {
                        // Altrimenti...
                        T0 result(0);

                        // Itera sulle nostre righe.
                        for(int i = 0; i < order(); i++)
                        {
                            // Prendi il minore tagliando la prima riga e
                            // l'i-esima colonna.
                            auto my_minor(x.calc_minor(0, i));

                            // Calcola il segno del prossimo coefficiente.
                            auto sign(i % 2 == 1 ? -1.0 : 1.0);

                            // Aggiungi il coefficiente per il determinante del
                            // minore all'accumulatore. (Ricorsione.)
                            result += sign * x(0, i) * my_minor.determinant();
                        }

                        return result;
                    })(*this);
        }

        // Calcola la matrice inversa.
        auto inverse() const noexcept
        {
            // Alloca matrice vuota della stessa dimensione.
            matrix<T0, TRowCount, TColumnCount> result{dont_init{}};

            using namespace vrm::core;

            // Usiamo un `if` a tempo di compilazione per il caso speciale in
            // cui il nostro ordine è uguale a `1`.
            static_if(bool_v<(order() == 1)>)
                .then([&result](const auto& x)
                    {
                        // Se l'ordine è uguale a `1`, setta il valore in alto a
                        // sinistra della matrice risultato al reciproco di
                        // quello della matrice corrente.
                        result(0, 0) = 1.0 / x(0, 0);
                    })
                .else_([&result](const auto& x)
                    {
                        // Altrimenti, calcola il reciproco del determinante.
                        auto rec_det(1.0 / x.determinant());

                        // Itera sulle colonne.
                        for(int j = 0; j < x.order(); j++)
                        {
                            // Itera sulle righe.
                            for(int i = 0; i < x.order(); i++)
                            {
                                // Calcola il minore tagliando la riga j e la
                                // colonna i.
                                auto my_minor(x.calc_minor(j, i));

                                // Calcola il segno del prossimo coefficiente.
                                auto sign((i + j) % 2 == 1 ? -1.0 : 1.0);

                                // Setta il valore della matrice risultato.
                                result(i, j) =
                                    sign * rec_det * my_minor.determinant();
                            }
                        }
                    })(*this);

            return result;
        }

        // Calcola l'indice di perturbazione.
        auto perturbation_index() const noexcept
        {
            auto result(norm_2() * inverse().norm_2());

            assert(result >= 1);
            return result;
        }

        // Risolve un sistema lineare usando il metodo di eliminazione di Gauss.
        auto solve_gauss() const noexcept
        {
            // La matrice deve avere una colonna in più delle righe.
            // (La colonna dei termini noti.)
            static_assert(column_count() == row_count() + 1, "");
            constexpr auto n(row_count());

            // Creiamo una copia della matrice corrente.
            auto a(*this);

            // Itera sull'ordine.
            for(std::size_t i(0); i < n; i++)
            {
                // Cerca l'indice del valore assoluto massimo nella colonna i.
                auto max_element(abs(a(i, i)));
                auto max_row_index(i);

                // Itera sulle righe (iteratore `k`).
                for(auto k(i + 1); k < n; k++)
                {
                    auto curr_element(abs(a(k, i)));
                    if(curr_element > max_element)
                    {
                        // Aggiorna il pivot.
                        max_element = curr_element;
                        max_row_index = k;
                    }
                }

                // Scambia riga dell'elemento massimo con la riga corrente.
                // (Colonna per colonna.)
                for(auto k(i); k < n + 1; k++)
                {
                    std::swap(a(max_row_index, k), a(i, k));
                }

                // Rende `0` tutti gli elementi sotto il pivot corrente.
                for(auto k(i + 1); k < n; k++)
                {
                    // Calcoliamo il fattore di annullamento dell'elemento `(k,
                    // i)` sotto il pivot.
                    auto c(-a(k, i) / a(i, i));

                    // Itera sui valori sotto il pivot.
                    for(auto j(i); j < n + 1; j++)
                    {
                        if(i == j)
                        {
                            // Se ci troviamo sulla diagonale, possiamo settare
                            // il valore sotto il pivot direttamente a zero.
                            a(k, j) = 0;
                        }
                        else
                        {
                            // Altrimenti, usiamo il fattore di annullamento per
                            // azzerare i valori sotto il pivot.
                            a(k, j) += c * a(i, j);
                        }
                    }
                }
            }

            // Alloca un vettore per i risultati.
            matrix<T0, n, 1> x;

            // Risolvi l'equazione `Ax=b`.
            // La matrice `a` adesso è triangolare superiore.
            for(int i(n - 1); i >= 0; i--)
            {
                // Setta `i`-esimo risultato.
                // `a(i, n)` è inizialmente un termine noto.
                // `a(i, i)` è inizialmente il coefficiente di un'incognita.
                x(i, 0) = a(i, n) / a(i, i);

                // Backward substitution.
                for(int k(i - 1); k >= 0; k--)
                {
                    a(k, n) -= a(k, i) * x(i, 0);
                }
            }

            return x;
        }

        // Risolvi un sistema lineare tramite il metodo di Jacobi.
        template <typename TF>
        auto solve_jacobi(std::size_t max_iterations, accuracy_type accuracy,
            TF loop_fn) const noexcept
        {
            // La matrice deve avere una colonna in più delle righe.
            // (La colonna dei termini noti.)
            static_assert(column_count() == row_count() + 1, "");
            constexpr auto n(row_count());

            // Inizializza i vettori delle soluzioni.
            using vector_type = matrix<accuracy_type, n, 1>;
            vector_type temp_solution;
            vector_type solution = temp_solution;

            divergence_loop(max_iterations, accuracy,
                [this, &solution, &temp_solution, n, loop_fn](
                                auto& max_divergence)
                {
                    // Itera sulle righe.
                    for(std::size_t i(0); i < TRowCount; i++)
                    {
                        // Prende l'i-esimo valore della riga.
                        auto x((*this)(i, n));

                        // Itera sulle colonne, saltando `i == j`.
                        loop_skipping(0, n, i, [&](auto j)
                            {
                                // Formula della sommatoria.
                                x -= (*this)(i, j) * solution(j, 0);
                            });

                        // Parte finale della formula.
                        x /= (*this)(i, i);

                        // Setta la `i`-esima soluzione a `x`.
                        temp_solution(i, 0) = x;

                        // Aggiorna la divergenza massima se necessario.
                        update_max_divergence(
                            i, temp_solution, solution, max_divergence);
                    }

                    // Sostituisce la soluzione precendente con quella corrente.
                    solution = temp_solution;
                    loop_fn(solution);
                });

            return solution;
        }

        auto solve_jacobi(std::size_t max_iterations = 100000,
            accuracy_type accuracy = 0.0001) const noexcept
        {
            return solve_jacobi(max_iterations, accuracy, [](const auto&)
                {
                });
        }

        template <typename TSolutions, typename TF>
        auto solve_gauss_seidel(TSolutions phi, std::size_t max_iterations,
            accuracy_type accuracy, TF loop_fn)
        {
            // La matrice deve avere una colonna in più delle righe.
            // (La colonna dei termini noti.)
            static_assert(column_count() == row_count() + 1, "");
            constexpr auto n(row_count());

            // Inizializza i vettori delle soluzioni.
            using vector_type = TSolutions;
            TSolutions temp_phi = phi;
            double sigma;

            divergence_loop(max_iterations, accuracy,
                [this, &phi, &temp_phi, &sigma, n, loop_fn](
                                auto& max_divergence)
                {
                    // Itera sulle righe.
                    for(int i = 0; i < n; ++i)
                    {
                        // Accumulatore sommatoria.
                        sigma = 0.0;

                        // Itera sulle colonne.
                        loop_skipping(0, n, i, [&](auto j)
                            {
                                // Formula della sommatoria.
                                sigma += (*this)(i, j) * phi(j, 0);
                            });

                        phi(i, 0) = ((*this)(i, n) - sigma) / (*this)(i, i);

                        // Aggiorna la divergenza massima se necessario.
                        update_max_divergence(i, temp_phi, phi, max_divergence);
                    }

                    // Sostituisce la soluzione precendente con quella corrente.
                    temp_phi = phi;
                    loop_fn(phi);
                });

            return phi;
        }

        template <typename TSolutions>
        auto solve_gauss_seidel(TSolutions phi,
            std::size_t max_iterations = 100000,
            accuracy_type accuracy = 0.0001)
        {
            return solve_gauss_seidel(phi, max_iterations, accuracy,
                [](const auto&)
                {
                });
        }

        auto solve_gauss_seidel(std::size_t max_iterations = 100000,
            accuracy_type accuracy = 0.0001)
        {
            constexpr auto n(row_count());

            // Inizializza i vettori delle soluzioni.
            using vector_type = matrix<accuracy_type, n, 1>;
            vector_type phi;

            return solve_gauss_seidel(phi, max_iterations, accuracy);
        }
    };
}


/**
 * \file boost/numeric/ublasx/operation/sum.hpp
 *
 * \brief The \c sum operation.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_SUM_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SUM_HPP

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublasx/operation/begin.hpp>
#include <boost/numeric/ublasx/operation/end.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

//@{ Declarations

//XXX: already implemented in vector_expression.hpp
///**
// * \brief Compute the sum of the elements of the given vector expression.
// * \tparam VectorExprT The type of the vector expression.
// * \param ve The vector expression whose elements are summed up.
// * \return The sum of the elements of the vector expression.
// *
// * \author Marco Guazzone, <marco.guazzone@gmail.com>
// */
//template <typename VectorExprT>
//typename vector_traits<VectorExprT>::value_type sum(vector_expression<VectorExprT> const& ve);
using ::boost::numeric::ublas::sum;

/**
 * \brief Compute the sum of the elements of the given matrix expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param ve The matrix expression whose elements are summed up.
 * \return The sum of the elements of the matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
typename matrix_traits<MatrixExprT>::value_type sum_all(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Compute the sum of the elements over each column of the given matrix
 *  expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression whose elements are summed up by row.
 * \return A vector containing the sum of the elements over each column in the
 *  given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> sum(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Compute the sum of the elements over each column in the given matrix
 *  expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression whose elements are summed up by row.
 * \return A vector containing the sum of the elements over each column in the
 *  given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> sum_rows(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Compute the sum of the elements over each row in the given matrix
 *  expression.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression whose elements are summed up by column.
 * \return A vector containing the sum of the elements over each row in the
 *  given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> sum_columns(matrix_expression<MatrixExprT> const& me);

/**
 * \brief Compute the sum of the elements of the given matrix expression along
 *  the given dimension tag.
 * \tparam MatrixExprT The type of the matrix expression.
 * \param me The matrix expression whose elements are summed up by the given
 *  dimension.
 * \return A vector containing the sum of the elements along the given dimension
 *  in the given matrix expression.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail.com>
 */
template <typename TagT, typename MatrixExprT>
vector<typename matrix_traits<MatrixExprT>::value_type> sum_by_tag(matrix_expression<MatrixExprT> const& me);

//@} Declarations

namespace detail { namespace /*<unnamed>*/ {

//@{ Declarations

/**
 * \brief Auxiliary class for computing the sum of the elements along the given
 *  dimension for a container of the given category.
 * \tparam Dim The dimension number (starting from 1).
 * \tparam CategoryT The category type (e.g., vector_tag).
 */
template < ::std::size_t Dim, typename CategoryT>
struct sum_by_dim_impl;

/**
 * \brief Auxiliary class for computing the sum of the elements along the given
 *  dimension tag for a container of the given category.
 * \tparam TagT The dimension tag type (e.g., tag::major).
 * \tparam CategoryT The category type (e.g., vector_tag).
 * \tparam OrientationT The orientation category type (e.g., row_major_tag).
 */
template <typename TagT, typename CategoryT, typename OrientationT>
struct sum_by_tag_impl;

//@} Declarations

//@{ Definitions

template <>
struct sum_by_dim_impl<1, vector_tag>
{
	template <typename VectorExprT>
	BOOST_UBLAS_INLINE
	static vector<typename vector_traits<VectorExprT>::value_type> apply(vector_expression<VectorExprT> const& ve)
	{
		typedef typename vector_traits<VectorExprT>::value_type value_type;

		vector<value_type> res(1);

		res(0) = sum(ve);

		return res;
	}
};

template <>
struct sum_by_dim_impl<1, matrix_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_rows(me);
	}
};

template <>
struct sum_by_dim_impl<2, matrix_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_columns(me);
	}
};

template <>
struct sum_by_tag_impl<tag::major, matrix_tag, row_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_rows(me);
	}
};

template <>
struct sum_by_tag_impl<tag::minor, matrix_tag, row_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_columns(me);
	}
};

template <>
struct sum_by_tag_impl<tag::leading, matrix_tag, row_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_columns(me);
	}
};

template <>
struct sum_by_tag_impl<tag::major, matrix_tag, column_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_columns(me);
	}
};

template <>
struct sum_by_tag_impl<tag::minor, matrix_tag, column_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_rows(me);
	}
};

template <>
struct sum_by_tag_impl<tag::leading, matrix_tag, column_major_tag>
{
	template <typename MatrixExprT>
	BOOST_UBLAS_INLINE
	static vector<typename matrix_traits<MatrixExprT>::value_type> apply(matrix_expression<MatrixExprT> const& me)
	{
		return sum_rows(me);
	}
};

template <typename TagT>
struct sum_by_tag_impl<TagT, matrix_tag, unknown_orientation_tag>: sum_by_tag_impl<TagT, matrix_tag, row_major_tag>
{
	// Empty
};

//@} Definitions

}} // Namespace detail::<unnamed>

//@{ Definitions

//XXX: already implemented in vector_expression.hpp
//template <typename VectorExprT>
//BOOST_UBLAS_INLINE
//typename vector_traits<VectorExprT>::value_type sum(vector_expression<VectorExprT> const& ve)
//{
//	typedef typename vector_traits<VectorExprT>::const_iterator iterator_type;;
//	typedef typename vector_traits<VectorExprT>::value_type value_type;
//
//	iterator_type it_end = end(ve);
//	value_type s = 0;
//
//	for (iterator_type it = begin(ve); it != it_end; ++it)
//	{
//		s += *it;
//	}
//
//	return s;
//}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::value_type sum_all(matrix_expression<MatrixExprT> const& me)
{
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	size_type nr = num_rows(me);
	size_type nc = num_columns(me);

	value_type s = 0;
	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			s += me()(r,c);
		}
	}

	return s;
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> sum(matrix_expression<MatrixExprT> const& me)
{
	return sum_rows(me);
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> sum_rows(matrix_expression<MatrixExprT> const& me)
{
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	size_type nr = num_rows(me);
	size_type nc = num_columns(me);

	vector<value_type> s(nc);
	size_type j = 0;
	for (size_type c = 0; c < nc; ++c)
	{
		//s(j++) = sum(column(me, c)); //FIXME: don't work

		value_type cs = 0;
		for (size_type r = 0; r < nr; ++r)
		{
			cs += me()(r,c);
		}
		s(j++) = cs;
	}

	return s;
}

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> sum_columns(matrix_expression<MatrixExprT> const& me)
{
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	size_type nr = num_rows(me);
	size_type nc = num_columns(me);

	vector<value_type> s(nr);
	size_type j = 0;
	for (size_type r = 0; r < nr; ++r)
	{
		//s(j++) = sum(row(me, r)); // FIXME don't work

		value_type rs = 0;
		for (size_type c = 0; c < nc; ++c)
		{
			rs += me()(r,c);
		}
		s(j++) = rs;
	}

	return s;
}

template <size_t Dim, typename VectorExprT>
BOOST_UBLAS_INLINE
vector<typename vector_traits<VectorExprT>::value_type> sum(vector_expression<VectorExprT> const& ve)
{
	return detail::sum_by_dim_impl<Dim, vector_tag>::template apply(ve);
}

template <size_t Dim, typename MatrixExprT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> sum(matrix_expression<MatrixExprT> const& me)
{
	return detail::sum_by_dim_impl<Dim, matrix_tag>::template apply(me);
}

template <typename TagT, typename MatrixExprT>
//template <typename MatrixExprT, typename TagT>
BOOST_UBLAS_INLINE
vector<typename matrix_traits<MatrixExprT>::value_type> sum_by_tag(matrix_expression<MatrixExprT> const& me)
{
	return detail::sum_by_tag_impl<TagT, matrix_tag, typename matrix_traits<MatrixExprT>::orientation_category>::template apply(me);
}

//@} Definitions

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SUM_HPP


//  (C) Copyright Nick Thompson 2017.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_SPECIAL_CHEBYSHEV_HPP
#define BOOST_MATH_SPECIAL_CHEBYSHEV_HPP
#include <cmath>
#include <boost/math/constants/constants.hpp>

#if (__cplusplus > 201103) || (defined(_CPPLIB_VER) && (_CPPLIB_VER >= 610))
#  define BOOST_MATH_CHEB_USE_STD_ACOSH
#endif

#ifndef BOOST_MATH_CHEB_USE_STD_ACOSH
#  include <boost/math/special_functions/acosh.hpp>
#endif

namespace boost { namespace math {

template<class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type chebyshev_next(T1 const & x, T2 const & Tn, T3 const & Tn_1)
{
    return 2*x*Tn - Tn_1;
}

namespace detail {

template<class Real, bool second>
inline Real chebyshev_imp(unsigned n, Real const & x)
{
#ifdef BOOST_MATH_CHEB_USE_STD_ACOSH
    using std::acosh;
#else
   using boost::math::acosh;
#endif
    using std::cosh;
    using std::pow;
    using std::sqrt;
    Real T0 = 1;
    Real T1;
    if (second)
    {
        if (x > 1 || x < -1)
        {
            Real t = sqrt(x*x -1);
            return static_cast<Real>((pow(x+t, (int)(n+1)) - pow(x-t, (int)(n+1)))/(2*t));
        }
        T1 = 2*x;
    }
    else
    {
        if (x > 1)
        {
            return cosh(n*acosh(x));
        }
        if (x < -1)
        {
            if (n & 1)
            {
                return -cosh(n*acosh(-x));
            }
            else
            {
                return cosh(n*acosh(-x));
            }
        }
        T1 = x;
    }

    if (n == 0)
    {
        return T0;
    }

    unsigned l = 1;
    while(l < n)
    {
       std::swap(T0, T1);
       T1 = boost::math::chebyshev_next(x, T0, T1);
       ++l;
    }
    return T1;
}
} // namespace detail

template <class Real, class Policy>
inline typename tools::promote_args<Real>::type
chebyshev_t(unsigned n, Real const & x, const Policy&)
{
   typedef typename tools::promote_args<Real>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::chebyshev_imp<value_type, false>(n, static_cast<value_type>(x)), "boost::math::chebyshev_t<%1%>(unsigned, %1%)");
}

template<class Real>
inline typename tools::promote_args<Real>::type chebyshev_t(unsigned n, Real const & x)
{
    return chebyshev_t(n, x, policies::policy<>());
}

template <class Real, class Policy>
inline typename tools::promote_args<Real>::type
chebyshev_u(unsigned n, Real const & x, const Policy&)
{
   typedef typename tools::promote_args<Real>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::chebyshev_imp<value_type, true>(n, static_cast<value_type>(x)), "boost::math::chebyshev_u<%1%>(unsigned, %1%)");
}

template<class Real>
inline typename tools::promote_args<Real>::type chebyshev_u(unsigned n, Real const & x)
{
    return chebyshev_u(n, x, policies::policy<>());
}

template <class Real, class Policy>
inline typename tools::promote_args<Real>::type
chebyshev_t_prime(unsigned n, Real const & x, const Policy&)
{
   typedef typename tools::promote_args<Real>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   if (n == 0)
   {
      return result_type(0);
   }
   return policies::checked_narrowing_cast<result_type, Policy>(n * detail::chebyshev_imp<value_type, true>(n - 1, static_cast<value_type>(x)), "boost::math::chebyshev_t_prime<%1%>(unsigned, %1%)");
}

template<class Real>
inline typename tools::promote_args<Real>::type chebyshev_t_prime(unsigned n, Real const & x)
{
   return chebyshev_t_prime(n, x, policies::policy<>());
}

/*
 * This is Algorithm 3.1 of
 * Gil, Amparo, Javier Segura, and Nico M. Temme.
 * Numerical methods for special functions.
 * Society for Industrial and Applied Mathematics, 2007.
 * https://www.siam.org/books/ot99/OT99SampleChapter.pdf
 * However, our definition of c0 differs by a factor of 1/2, as stated in the docs. . .
 */
template<class Real, class T2>
inline Real chebyshev_clenshaw_recurrence(const Real* const c, size_t length, const T2& x)
{
    using boost::math::constants::half;
    if (length < 2)
    {
        if (length == 0)
        {
            return 0;
        }
        return c[0]/2;
    }
    Real b2 = 0;
    Real b1 = c[length -1];
    for(size_t j = length - 2; j >= 1; --j)
    {
        Real tmp = 2*x*b1 - b2 + c[j];
        b2 = b1;
        b1 = tmp;
    }
    return x*b1 - b2 + half<Real>()*c[0];
}

}}
#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
   /*!
     @brief Radindeg generic tag

     Represents the Radindeg constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Radindeg, double
                                , 57, 0x42652ee1
                                , 0x404ca5dc1a63c1f8ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Radindeg, Site> dispatching_Radindeg(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Radindeg, Site>();
   }
   template<class... Args>
   struct impl_Radindeg;
  }
  /*!
    Constant Radindeg : Degree in Radian multiplier, \f$\frac{180}\pi\f$.

    @par Semantic:

    For type T0:

    @code
    T0 r = Radindeg<T0>();
    @endcode

    is similar to:

    @code
    T0 r = _180<T0>()/Pi<T0>();
    @endcode

    @see  @funcref{inrad}, @funcref{indeg}, @funcref{Radindegr}, @funcref{Deginrad}
    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Radindeg, Radindeg);
}

namespace nt2
{
  /// INTERNAL ONLY
}

#endif


#include "pinocchio/math/multiprecision.hpp"

#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea.hpp"

#include <boost/multiprecision/cpp_dec_float.hpp>

#include <iostream>

// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif

int main(int argc, char ** argv)
{
  using namespace pinocchio;

  // You should change here to set up your own URDF file or just pass it as an argument of this example.
  const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];

  // Load the URDF model
  Model model;
  pinocchio::urdf::buildModel(urdf_filename,model);

  // Build a data related to model
  Data data(model);

  // Define Model and Data for multiprecision types
  typedef boost::multiprecision::cpp_dec_float_100 float_100;
  typedef ModelTpl<float_100> ModelMulti;
  typedef DataTpl<float_100> DataMulti;

  ModelMulti model_multi = model.cast<float_100>();
  DataMulti data_multi(model_multi);

  // Sample a random joint configuration as well as random joint velocity and acceleration
  ModelMulti::ConfigVectorType q_multi = randomConfiguration(model_multi);
  ModelMulti::TangentVectorType v_multi = ModelMulti::TangentVectorType::Random(model.nv);
  ModelMulti::TangentVectorType a_multi = ModelMulti::TangentVectorType::Random(model.nv);

  Model::ConfigVectorType q = q_multi.cast<double>();
  Model::TangentVectorType v = v_multi.cast<double>();
  Model::TangentVectorType a = a_multi.cast<double>();

  // Computes the inverse dynamics (aka RNEA)
  rnea(model, data, q, v, a);
  rnea(model_multi , data_multi , q_multi , v_multi , a_multi);

  // Get access to the joint torque with standard or multiprecision arithmetic and print sufficient decimals for both precisions
  std::cout << "Joint torque standard arithmetic:\n" << std::setprecision(std::numeric_limits<float_100>::max_digits10) << data.tau << std::endl;
  std::cout << "Joint torque multiprecision arithmetic:\n" << std::setprecision(std::numeric_limits<float_100>::max_digits10) << data_multi.tau << std::endl;
}


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

#ifndef SMOOTH__INTERNAL__BUNDLE_HPP_
#define SMOOTH__INTERNAL__BUNDLE_HPP_

#include <array>

#include <Eigen/Core>

#include "common.hpp"
#include "smooth/internal/utils.hpp"

/**
 * @brief Bundle Lie group
 *
 * Represents the direct product
 *  G1 x G2 x ... x Gk
 * for Lie groups G1 ... Gk.
 */
namespace smooth {

using std::get;

template<typename... GsImpl>
struct BundleImpl
{
  using Scalar = std::common_type_t<typename GsImpl::Scalar...>;

  static_assert(
    (std::is_same_v<Scalar, typename GsImpl::Scalar> && ...),
    "Implementation Scalar types must be the same");

  static constexpr std::array<Eigen::Index, sizeof...(GsImpl)> RepSizes{GsImpl::RepSize...};
  static constexpr std::array<Eigen::Index, sizeof...(GsImpl)> Dofs{GsImpl::Dof...};
  static constexpr std::array<Eigen::Index, sizeof...(GsImpl)> Dims{GsImpl::Dim...};

  static constexpr auto RepSizesPsum = smooth::utils::array_psum(RepSizes);
  static constexpr auto DofsPsum     = smooth::utils::array_psum(Dofs);
  static constexpr auto DimsPsum     = smooth::utils::array_psum(Dims);

  template<std::size_t Idx>
  using PartImpl = std::tuple_element_t<Idx, std::tuple<GsImpl...>>;

  static constexpr Eigen::Index RepSize = RepSizesPsum.back();
  static constexpr Eigen::Index Dof     = DofsPsum.back();
  static constexpr Eigen::Index Dim     = DimsPsum.back();

  SMOOTH_DEFINE_REFS;

  // clang-format off

  static void setIdentity(GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::setIdentity(
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void setRandom(GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::setRandom(
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::matrix(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        m_out.template block<get<i>(Dims), get<i>(Dims)>(get<i>(DimsPsum), get<i>(DimsPsum))
      );
    });
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::composition(
        g_in1.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        g_in2.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::inverse(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::log(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        a_out.template segment<get<i>(Dofs)>(get<i>(DofsPsum))
      );
    });
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::Ad(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::exp(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::hat(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dims), get<i>(Dims)>(get<i>(DimsPsum), get<i>(DimsPsum))
      );
    });
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::vee(
        A_in.template block<get<i>(Dims), get<i>(Dims)>(get<i>(DimsPsum), get<i>(DimsPsum)),
        a_out.template segment<get<i>(Dofs)>(get<i>(DofsPsum))
      );
    });
  }

  static void ad(TRefIn a_in, TMapRefOut A_out) {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::ad(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out) {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::dr_exp(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out) {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::dr_expinv(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  // clang-format on
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__BUNDLE_HPP_


/*
 * $Revision: 558 $ $Date: 2010-11-17 03:20:58 -0800 (Wed, 17 Nov 2010) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "StarCatalog.h"
#include "Spectrum.h"
#include "Debug.h"
#include <Eigen/Core>
#include <cmath>
#include <algorithm>

using namespace vesta;
using namespace Eigen;
using namespace std;

/** Create an empty star catalog.
  */
StarCatalog::StarCatalog()
{
}

StarCatalog::~StarCatalog()
{
}

// Convert a Johnson B-V color index to the effective surface temperature. Uses the
// relation from Sekiguchi and Fukugita, "A Study of the B-V Color-Temperature Relation."
// (Astronomical Journal, Aug 2000).
// http://iopscience.iop.org/1538-3881/120/2/1072/pdf/1538-3881_120_2_1072.pdf
//
// bv is the Johnson bv color index
// metallicity is Fe/H
static float BVColorIndexToTeff(float bv, float metallicity = 0.0f, float logG = 0.0f)
{
    const float c0 = 3.939654;
    const float c1 = -0.395361;
    const float c2 = 0.2082113;
    const float c3 = -0.0604097;
    const float f1 = 0.027153;
    const float f2 = 0.005036;
    const float g1 = 0.007367;
    const float h1 = -0.01069;

    float logT = c0 + c1 * bv + c2 * bv * bv + c3 * bv * bv * bv +
                 f1 * metallicity + f2 * metallicity * metallicity +
                 g1 * logG + h1 * bv * logG;

    return pow(10.0f, logT);
}

// Convert the CIE chromaticity coordinates for a black body of the specified
// temperature. The calculation uses a piecewise cubic approximation that is
// valid for temperatures above 1667 K. For cooler temperatures, we simply clamp
// them to 1667 K. This is adequate for our use the function to compute star
// colors as only brown dwarf stars are cooler, and these are so faint that
// they don't need to be represented in VESTA.
static Vector2f planckianLocus(float T)
{
    // Clamp to a valid range
    T = std::max(T, 1667.0f);

    float t = 1000.0f / T;
    float t2 = t * t;
    float t3 = t2 * t;

    float x;
    if (T < 4000.0f)
    {
        x = -0.266162 * t3 - 0.2343580f * t2 + 0.8776956f * t + 0.179910f;
    }
    else
    {
        // Valid from 4000K - 25000K
        x = -3.0258469f * t3 + 2.1070379f * t2 + 0.2226347f * t + 0.24039f;
    }

    float x2 = x * x;
    float x3 = x2 * x;
    float y;
    if (T < 2222)
    {
        // Valid from 1667K - 2222K
        y = -1.1063814f * x3 - 1.3481102 * x2  + 2.18555832f * x - 0.20219683f;
    }
    else if (T < 4000)
    {
        // Valid from 2222K - 4000K
        y = -0.9549976f * x3 - 1.3741859f * x2 + 2.0913702f * x - 0.16748867f;
    }
    else
    {
        // Valid from 4000K - 25000K
        y = 3.0817580f * x3 - 5.8338670f * x2 + 3.75112997f * x - 0.37001483f;
    }

    return Vector2f(x, y);
}

static Vector3f xyToXYZ(const Vector2f& xy)
{
    return Vector3f(xy.x() / xy.y(), 1.0f, (1.0f - xy.x() - xy.y()) / xy.y());
}

static Spectrum linearSrgbStarColor(float bv)
{
    float Teff = BVColorIndexToTeff(bv);
    Vector2f ciexy = planckianLocus(Teff);
    Vector3f cieXYZ = xyToXYZ(ciexy);

    Spectrum srgb = Spectrum::XYZtoLinearSRGB(Spectrum(cieXYZ.x(), cieXYZ.y(), cieXYZ.z()));
    srgb.normalize();
    return srgb;
}

/** Add a new star to the catalog.
  * @param ra the right ascension (in radians)
  * @param dec the declination (in radians)
  * @param vmag the apparent V magnitude in the Johnson photometric system (mean wavelength 540nm)
  * @param bv the value of B-V color index in the Johnson photometric system
  */
void
StarCatalog::addStar(v_uint32 identifier, double ra, double dec, double vmag, double bv)
{
    StarRecord star;
    star.identifier = identifier;
    star.RA = float(ra);
    star.declination = float(dec);
    star.apparentMagnitude = float(vmag);
    star.bvColorIndex = float(bv);

    m_starData.push_back(star);
}

/** Compute the approximate color of a star from it's Johnson B-V color index. The
  * color returned is in the CIE XYZ color space.
  */
Spectrum StarCatalog::StarColor(float bv)
{
    float Teff = BVColorIndexToTeff(bv);
    Vector2f ciexy = planckianLocus(Teff);
    Vector3f cieXYZ = xyToXYZ(ciexy);

    return Spectrum(cieXYZ.x(), cieXYZ.y(), cieXYZ.z());
}

class StarIdPredicate
{
public:
    StarIdPredicate() {}
    bool operator()(const StarCatalog::StarRecord& star0, const StarCatalog::StarRecord& star1) const
    {
        return star0.identifier < star1.identifier;
    }
};

/** Index the star catalog by identifier. This method must be called before star lookups by identifier
  * will work.
  */
void
StarCatalog::buildCatalogIndex()
{
    sort(m_starData.begin(), m_starData.end(), StarIdPredicate());
}

/** Lookup a star by its identifier. Returns null if the star isn't present in the
  * catalog. buildCatalogIndex() must be called once before findStarIdentifier will
  * work.
  */
const StarCatalog::StarRecord*
StarCatalog::findStarIdentifier(v_uint32 id)
{
    StarRecord match;
    match.identifier = id;

    vector<StarRecord>::const_iterator pos = lower_bound(m_starData.begin(), m_starData.end(), match, StarIdPredicate());
    if (pos == m_starData.end())
    {
        return NULL;
    }
    else
    {
        return &(*pos);
    }
}


#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>

#include "geometrycentral/surface/halfedge_mesh.h"
#include "geometrycentral/surface/halfedge_factories.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/heat_method_distance.h"
#include "geometrycentral/surface/fast_marching_method.h"
#include "geometrycentral/surface/vertex_position_geometry.h"
#include "exact_geodesic.h"

using namespace std;
using namespace geometrycentral;
using namespace geometrycentral::surface;
namespace py = pybind11;

// Geometry-central data
std::unique_ptr<HalfedgeMesh> mesh;
std::unique_ptr<VertexPositionGeometry> geometry;

// Loads a mesh from a NumPy array
// source: https://github.com/rubenwiersma/hsn/blob/master/vectorheat/src/main.cpp
std::tuple<std::unique_ptr<HalfedgeMesh>, std::unique_ptr<VertexPositionGeometry>>
loadMesh_np(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces) {

  // Set vertex positions
  std::vector<Vector3> vertexPositions(pos.rows());
  for (size_t i = 0; i < pos.rows(); i++) {
    vertexPositions[i][0] = pos(i, 0);
    vertexPositions[i][1] = pos(i, 1);
    vertexPositions[i][2] = pos(i, 2);
  }

  // Get face list
  std::vector<std::vector<size_t>> faceIndices(faces.rows());
  for (size_t i = 0; i < faces.rows(); i++) {
    faceIndices[i] = {faces(i, 0), faces(i, 1), faces(i, 2)};
  }

  return makeHalfedgeAndGeometry(faceIndices, vertexPositions);
}

// compute geodesic distance using the heat method
Eigen::MatrixXd get_heat_geodesics(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces){
    // Load mesh
    std::tie(mesh, geometry) = loadMesh_np(pos, faces);
    int nverts = mesh->nVertices();

    // Create the Heat Method solver
    HeatMethodDistanceSolver heatSolver(*geometry);

    // create output matrix
    Eigen::MatrixXd geodesic_matrix(nverts, nverts);

    // compute geodesic distances
    int i = 0;
    for (Vertex v : mesh->vertices()) {
        VertexData<double> distToSource = heatSolver.computeDistance(v);
        for (int j = 0; j < nverts; j++){
            geodesic_matrix(i, j) = distToSource[j];
        }
        i++;
    }

    return geodesic_matrix;
}

// compute geodesic distance using the fast marching method
Eigen::MatrixXd get_fmm_geodesics(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces){
    // Load mesh
    std::tie(mesh, geometry) = loadMesh_np(pos, faces);
    int nverts = mesh->nVertices();

    // create output matrix
    Eigen::MatrixXd geodesic_matrix(nverts, nverts);
    double initial_dist = 0.;

    // compute geodesic distances
    int i = 0;
    for (Vertex v : mesh->vertices()) {
        std::vector<std::pair<Vertex, double>> input{std::make_pair(v, initial_dist)};
        VertexData<double> distToSource = FMMDistance(*geometry, input);
        for (int j = 0; j < nverts; j++)
            geodesic_matrix(i, j) = distToSource[j];

        i++;
    }

    return geodesic_matrix;
}

// compute geodesic distance using the Exact geodesic algorithm by Mitchell, Mount and Papadimitriou in 1987
Eigen::MatrixXd get_exact_geodesics(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces){
    // utils vars
    int nverts = pos.rows();
    Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic> FS, FT;
    Eigen::MatrixXi target(nverts, 1);
    for (int i = 0; i < nverts; i++)
        target(i, 0) = i;

    // create output matrix
    Eigen::MatrixXd geodesic_matrix(nverts, nverts);

    // compute geodesic distances
    for (int i = 0; i < nverts; i++){
        Eigen::MatrixXi source(1, 1);
        source(0, 0) = i;
        Eigen::MatrixXd distToSource(nverts, 1);
        igl::exact_geodesic(pos, faces, source, FS, target, FT, distToSource);

        for (int j = 0; j < nverts; j++)
            geodesic_matrix(i, j) = distToSource(j);
    }

    return geodesic_matrix;
}

PYBIND11_MODULE(tridesic, m) {
    m.def("get_heat_geodesics", &get_heat_geodesics, py::return_value_policy::copy,
          "Compute the geodesic matrix using the heat method");
    m.def("get_fmm_geodesics", &get_fmm_geodesics, py::return_value_policy::copy,
          "Compute the geodesic matrix using the fast marching method");
    m.def("get_exact_geodesics", &get_exact_geodesics, py::return_value_policy::copy,
          "Compute the geodesic matrix using the Exact geodesic algorithm by Mitchell et al.");
}


#include <SBGATPolyhedronGravityModel.hpp>
#include <SBGATPolyhedronGravityModelUQ.hpp>

#include <vtkCleanPolyData.h>
#include <vtkOBJReader.h>

#include <json.hpp>
#include <boost/progress.hpp>

int main(){

	std::ifstream i("input_file.json");
	nlohmann::json input_data;
	i >> input_data;

	std::string PATH_SHAPE = input_data["PATH_SHAPE"];
	double CORRELATION_DISTANCE =  input_data["CORRELATION_DISTANCE"];

	double ERROR_STANDARD_DEV  = input_data["ERROR_STANDARD_DEV"];
	double DENSITY  = input_data["DENSITY"];
	double STEP_SIZE  = input_data["STEP_SIZE"];

	bool UNIT_IN_METERS  = input_data["UNIT_IN_METERS"];
	bool HOLD_MASS_CONSTANT  = input_data["HOLD_MASS_CONSTANT"];

	int PROJECTION_AXIS = input_data["PROJECTION_AXIS"];
	int N_MONTE_CARLO = input_data["N_MONTE_CARLO"];

	std::string OUTPUT_DIR = input_data["OUTPUT_DIR"];

	std::cout << "- Path to shape: " << PATH_SHAPE << std::endl;
	std::cout << "- Standard deviation on point coordinates (m) : " << ERROR_STANDARD_DEV << std::endl;
	std::cout << "- Correlation distance (m) : " << CORRELATION_DISTANCE << std::endl;
	std::cout << "- Density (kg/m^3) : " << DENSITY << std::endl;
	std::cout << "- Monte-Carlo draws : " << N_MONTE_CARLO << std::endl;
	std::cout << "- Step size : " << STEP_SIZE << std::endl;
	std::cout << "- Projection axis : " << PROJECTION_AXIS << std::endl;

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(PATH_SHAPE.c_str());
	reader -> Update();

	// An instance of SBGATPolyhedronGravityModel is created to evaluate the PGM of
	// the considered polytdata
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(reader -> GetOutputPort());
	pgm_filter -> SetDensity(DENSITY);

	if(UNIT_IN_METERS){
		pgm_filter -> SetScaleMeters();
	} else{
		pgm_filter -> SetScaleKiloMeters();
	}
	pgm_filter -> Update();

	// An instance of SBGATPolyhedronGravityModelUQ is created to perform
	// uncertainty quantification from the PGM associated to the shape
	SBGATPolyhedronGravityModelUQ pgm_uq;
	pgm_uq.SetModel(pgm_filter);
	pgm_uq.PrecomputeMassPropertiesPartials();

	// Populate the shape vertices covariance
	pgm_uq.ComputeVerticesCovarianceGlobal(ERROR_STANDARD_DEV,CORRELATION_DISTANCE);
	// Regularizing the covariance
	int regularized_eigen_values = pgm_uq.RegularizeCovariance();

	std::cout << regularized_eigen_values << " eigenvalues were regularized\n";

	arma::mat C_CC = pgm_uq.GetCovarianceSquareRoot();
	arma::mat P_CC = pgm_uq.GetVerticesCovariance();
	std::cout << "Maximum absolute error in covariance square root: " << arma::abs(P_CC - C_CC * C_CC.t()).max() << std::endl;

	std::cout << "Saving shape covariance ...\n";
	P_CC.save(OUTPUT_DIR + "full_covariance.txt",arma::raw_ascii);

	// Saving baseline slices
	pgm_uq.TakeAndSaveSlice(0,OUTPUT_DIR + "baseline_slice_x.txt",1e-6);
	pgm_uq.TakeAndSaveSlice(1,OUTPUT_DIR + "baseline_slice_y.txt",1e-6);
	pgm_uq.TakeAndSaveSlice(2,OUTPUT_DIR + "baseline_slice_z.txt",1e-6);

	// First, create the grid from the bounding box
	std::vector<arma::vec::fixed<3> > grid;
	double xmin,xmax,ymin,ymax,zmin,zmax;
	pgm_filter -> GetBoundingBox(xmin,xmax,ymin,ymax,zmin,zmax);

	// Inflate
	xmin *= 2.5;
	xmax *= 2.5;
	ymin *= 2.5;
	ymax *= 2.5;
	zmin *= 2.5;
	zmax *= 2.5;

	double min_dim = std::min(xmin,std::min(ymin,zmin));
	double max_dim = std::max(xmax,std::max(ymax,zmax));

	xmax = max_dim;
	ymax = max_dim;
	zmax = max_dim;

	xmin = min_dim;
	ymin = min_dim;
	zmin = min_dim;

	// Define grid indices
	int i_max,j_max;

	if (PROJECTION_AXIS == 0){
		i_max = 1./STEP_SIZE * (ymax - ymin);
		j_max = 1./STEP_SIZE * (zmax - zmin);

	}
	else if (PROJECTION_AXIS == 1){

		i_max = 1./STEP_SIZE * (xmax - xmin);
		j_max = 1./STEP_SIZE * (zmax - zmin);

	}
	else if (PROJECTION_AXIS == 2){

		i_max = 1./STEP_SIZE * (xmax - xmin);
		j_max = 1./STEP_SIZE * (ymax - ymin);

	}
	else{
		throw(std::runtime_error("PROJECTION_AXIS has to be either 0, 1 or 2. It can't be equal to " + std::to_string(PROJECTION_AXIS)));
	}

	// Create the grid containers
	std::vector<std::vector<int> > indices;
	arma::mat trace_sqrt_cov(i_max,j_max);
	arma::mat reference_acceleration(i_max,j_max);
	arma::mat uncertainty_over_reference_acc_percentage(i_max,j_max);
	arma::mat inside_outside(i_max,j_max);

	// Construct the grid (constant, uniform step size in both directions of space)
	for (int i = 0; i < i_max; ++i){
		for (int j = 0; j < j_max; ++j){

			arma::vec::fixed<3> point;
			if (PROJECTION_AXIS == 0){

				double y = ymin + i * STEP_SIZE;
				double z = zmin + j * STEP_SIZE;

				point(0) = 0;
				point(1) = y;
				point(2) = z;
			}
			else if (PROJECTION_AXIS == 1){

				double x = xmin + i * STEP_SIZE;
				double z = zmin + j * STEP_SIZE;

				point(0) = x;
				point(1) = 0;
				point(2) = z;
			}
			else{

				double x = xmin + i * STEP_SIZE;
				double y = ymin + j * STEP_SIZE;

				point(0) = x;
				point(1) = y;
				point(2) = 0;
			}

			grid.push_back(point);
			indices.push_back(std::vector<int>({i,j}));

			if (pgm_filter -> Contains(point)){
				inside_outside(i,j) = 1;
			}
			else{
				inside_outside(i,j) = 0;
			}
		}
	}
	std::cout << "- Grid size: " << grid.size() << std::endl;

	// Saving the grid to a file
	arma::mat grid_arma(3,grid.size());
	for (unsigned int p = 0; p < grid.size(); ++p){
		grid_arma.col(p) = grid[p];
	}
	grid_arma.save(OUTPUT_DIR + "grid.txt",arma::raw_ascii);

	// Running a Monte Carlo on a subset of the grid
	std::vector<std::vector<arma::vec::fixed<3> > > all_accelerations;
	std::vector<std::vector<double > > all_potentials;
	std::vector<arma::vec> deviations;
	std::vector<double> densities;

	arma::vec::fixed<3> e0 = {1,0,0};
	arma::vec::fixed<3> e1 = {0,1,0};
	arma::vec::fixed<3> e2 = {0,0,1};
	arma::vec::fixed<3> e3 = arma::normalise(arma::vec({1,1,0}));
	arma::vec::fixed<3> e4 = arma::normalise(arma::vec({0,1,1}));
	arma::vec::fixed<3> e5 = arma::normalise(arma::vec({1,0,1}));
	arma::vec::fixed<3> e6 = arma::normalise(arma::vec({-1,1,0}));
	arma::vec::fixed<3> e7 = arma::normalise(arma::vec({0,-1,1}));
	arma::vec::fixed<3> e8 = arma::normalise(arma::vec({1,0,-1}));

	std::vector<arma::vec::fixed<3> > all_positions;
	std::vector<double> distances = {200,300,400,500,600};
	for (auto dist : distances){
		all_positions.push_back(dist * e0);
		all_positions.push_back(dist * e1);
		all_positions.push_back(dist * e2);
		all_positions.push_back(dist * e3);
		all_positions.push_back(dist * e4);
		all_positions.push_back(dist * e5);
		all_positions.push_back(dist * e6);
		all_positions.push_back(dist * e7);
		all_positions.push_back(dist * e8);
		all_positions.push_back(- dist * e0);
		all_positions.push_back(- dist * e1);
		all_positions.push_back(- dist * e2);
		all_positions.push_back(- dist * e3);
		all_positions.push_back(- dist * e4);
		all_positions.push_back(- dist * e5);
		all_positions.push_back(- dist * e6);
		all_positions.push_back(- dist * e7);
		all_positions.push_back(- dist * e8);
	}

	std::cout << "Running MC ... ";

	auto start = std::chrono::system_clock::now();
	SBGATPolyhedronGravityModelUQ::RunMCUQAccelerationInertial(PATH_SHAPE,DENSITY,
		UNIT_IN_METERS,
		HOLD_MASS_CONSTANT,
		C_CC,
		N_MONTE_CARLO,
		all_positions,
		OUTPUT_DIR,
		std::min(30,N_MONTE_CARLO),
		deviations,
		densities,
		all_accelerations);
	auto end = std::chrono::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end-start;

	std::cout << "Done running MC in " << elapsed_seconds.count() << " s\n";

// Computing MC Dispersions
	arma::vec KL_divergence_analytical_vs_mc(all_positions.size());
	arma::vec abs_value_cov_difference_analytical_vs_mc(all_positions.size());
	arma::vec rel_value_cov_difference_analytical_vs_mc(all_positions.size());
	arma::mat all_positions_arma(3,all_positions.size());

#pragma omp parallel for
	for (int e = 0; e < all_positions.size(); ++e){

		arma::mat accelerations_mc(3,N_MONTE_CARLO);

		for (int sample = 0; sample < N_MONTE_CARLO; ++sample){
			accelerations_mc.col(sample) = all_accelerations[sample][e];
		}

		arma::mat mc_covariances_acc = arma::cov(accelerations_mc.t());

		arma::vec mc_mean_acc = arma::mean(accelerations_mc,1);
		arma::vec reference_acc = pgm_filter -> GetAcceleration(all_positions[e]);

		arma::mat cov_analytical = pgm_uq.GetCovarianceAcceleration(all_positions[e],HOLD_MASS_CONSTANT);
		KL_divergence_analytical_vs_mc(e) = SBGATFilterUQ::KLDivergence(reference_acc,mc_mean_acc,	cov_analytical,mc_covariances_acc);

		abs_value_cov_difference_analytical_vs_mc(e) = arma::abs(arma::vectorise(cov_analytical - mc_covariances_acc)).max();
		rel_value_cov_difference_analytical_vs_mc(e) = arma::norm(cov_analytical - mc_covariances_acc)/arma::trace(mc_covariances_acc) * 100;
		all_positions_arma.col(e) = all_positions[e];
	}

	std::cout << "- Evaluating analytical uncertainties over grid ...\n";

	start = std::chrono::system_clock::now();

	// For every point in the grid, evaluate the analytical acceleration covariance and
	// run a monte carlo to get a statistical covariance to compare against
	boost::progress_display progress(grid.size());

	#pragma omp parallel for
	for (int p = 0; p < grid.size(); ++p){

		int i = indices[p][0];
		int j = indices[p][1];

		const arma::vec::fixed<3> & grid_point = grid[p];

		arma::vec::fixed<3> reference_acceleration_vector = pgm_filter -> GetAcceleration(grid_point);
		arma::mat::fixed<3,3> covariance_acceleration_analytical = pgm_uq.GetCovarianceAcceleration(grid_point,HOLD_MASS_CONSTANT);

		reference_acceleration(i,j) = arma::norm(reference_acceleration_vector);
		trace_sqrt_cov(i,j) = std::sqrt(arma::trace(covariance_acceleration_analytical)) ;
		uncertainty_over_reference_acc_percentage(i,j) = trace_sqrt_cov(i,j) / reference_acceleration(i,j) * 100;

		++progress;
	}

	end = std::chrono::system_clock::now();
	elapsed_seconds = end-start;
	std::cout << "\n-- Done evaluating over grid in " << elapsed_seconds.count() << " s\n";

	trace_sqrt_cov.save(OUTPUT_DIR + "trace_sqrt_cov.txt",arma::raw_ascii);
	reference_acceleration.save(OUTPUT_DIR + "reference_acceleration.txt",arma::raw_ascii);
	inside_outside.save(OUTPUT_DIR + "inside_outside.txt",arma::raw_ascii);
	uncertainty_over_reference_acc_percentage.save(OUTPUT_DIR + "uncertainty_over_reference_acc_percentage.txt",arma::raw_ascii);

	abs_value_cov_difference_analytical_vs_mc.save(OUTPUT_DIR + "abs_value_cov_difference_analytical_vs_mc.txt",arma::raw_ascii);
	rel_value_cov_difference_analytical_vs_mc.save(OUTPUT_DIR + "rel_value_cov_difference_analytical_vs_mc.txt",arma::raw_ascii);
	KL_divergence_analytical_vs_mc.save(OUTPUT_DIR + "KL_divergence_analytical_vs_mc.txt",arma::raw_ascii);
	all_positions_arma.save(OUTPUT_DIR + "all_positions_arma.txt",arma::raw_ascii);

	return 0;
}


#include <iostream>
#include <list>
#include <vector>
#include <algorithm>
#include <set>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/assign/std/vector.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/assign.hpp>

#include <boost/foreach.hpp>

#include <stdlib.h>
#include <bits/stdc++.h>

namespace bg = boost::geometry;

// use the following two commands to compile:
//
//WARNING: DO NOT USE O2. IT DESTROYS bg::correct FUNCTIONALITY
// g++ -lboost_system -c -fPIC polygoncalc.cpp -o polygoncalc.o -O2
// g++ -lboost_system -shared -Wl,-soname,libpolygoncalc.so -o libpolygoncalc.so polygoncalc.o -O2
// if it does not work, add -std=c++14 and add -I and -L directories

typedef boost::geometry::model::d2::point_xy<double> point_type;

typedef bg::model::polygon<point_type> polygon_type;

void dump( const std::string & label, const std::list< std::set< unsigned long > > & values )
{
    std::cout << label << std::endl;
    for( auto iter : values )
    {
        std::cout << "{ ";
        for( auto val : iter )
            std::cout << val << ", ";
        std::cout << "}, ";
    }
    std::cout << std::endl;
}

void combine( std::list< std::set< unsigned long > > & values )
{
    for( std::list< std::set< unsigned long > >::iterator iter = values.begin(); iter != values.end(); ++iter )
        for( std::list< std::set< unsigned long > >::iterator niter( iter ); ++niter != values.end(); )
            if( std::find_first_of( iter->begin(), iter->end(), niter->begin(), niter->end() ) != iter->end() )
            {
                iter->insert( niter->begin(), niter->end() );
                values.erase( niter );
                niter = iter;
            }
}

unsigned long merge(unsigned long* parent, unsigned long x)
{
    if (parent[x] == x)
        return x;
    return merge(parent, parent[x]);
}

void connectedcomponents(unsigned long n, std::vector<std::vector<unsigned long> >& edges, std::list<std::list<unsigned long>>& res)
{

    unsigned long parent[n];
    for (unsigned long i = 0; i < n; i++) {
        parent[i] = i;
    }
    for (auto x : edges) {
        parent[merge(parent, x[0])] = merge(parent, x[1]);
    }

    for (unsigned long i = 0; i < n; i++) {
        parent[i] = merge(parent, parent[i]);
    }
    std::map<unsigned long, std::list<unsigned long> > m;
    for (unsigned long i = 0; i < n; i++) {
        m[parent[i]].push_back(i);
    }
    for (auto it = m.begin(); it != m.end(); it++) {
        std::list<unsigned long> l = it->second;

        res.push_back(l);
    }
}

class PolygonCalc{

    public:
        const double helloworld(){
            return 0.123;
        }

        double test_calc(){

            polygon_type poly1 {{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}}};
            polygon_type poly2 {{{5.5, 0.5}, {6.5, 0.5}, {6.5, 1.5}, {5.5, 1.5}}};

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {

                totalArea += bg::area(p);
            }

            double d = bg::distance(poly1, poly2);

            std::cout << d << std::endl;

            return totalArea;

        }

        int test_nparray(double *A, int n){

            int i;
            double sum = 0.0;

            for (i=0; i<n; i++) {

                sum += A[i];

            }

            std::cout << "n: " << n << std::endl;

            std::cout << sum / n << std::endl;

            return n;

        }

        double min_poly_distance(double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

            int i;

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            for (i=0; i<n; i++) {
                points2.push_back(point_type(poly2x[i], poly2y[i]));
            }

            polygon_type poly1;
            polygon_type poly2;

            bg::assign_points(poly1, points1);
            bg::assign_points(poly2, points2);

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {
                totalArea += bg::area(p);
            }

            if (totalArea > 0.0){
                return 0.0;
            }

            return bg::distance(poly1, poly2);

        }

        double poly_area(double *poly1x, double *poly1y, int m){

            int i;

            std::vector<point_type> points1;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            polygon_type poly1;

            bg::assign_points(poly1, points1);

            bg::correct(poly1);

            double totalArea = bg::area(poly1);

            return totalArea;

        }

        double poly_intersection_area(double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

            int i;

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            for (i=0; i<n; i++) {
                points2.push_back(point_type(poly2x[i], poly2y[i]));
            }

            polygon_type poly1;
            polygon_type poly2;

            bg::assign_points(poly1, points1);
            bg::assign_points(poly2, points2);

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {
                totalArea += bg::area(p);
            }

            return totalArea;

        }

        double poly_intersection_area_ratio(double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

            int i;

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            for (i=0; i<m; i++) {
                points1.push_back(point_type(poly1x[i], poly1y[i]));
            }

            for (i=0; i<n; i++) {
                points2.push_back(point_type(poly2x[i], poly2y[i]));
            }

            polygon_type poly1;
            polygon_type poly2;

            bg::assign_points(poly1, points1);
            bg::assign_points(poly2, points2);

            bg::correct(poly1);
            bg::correct(poly2);

            std::deque<polygon_type> output;
            bg::intersection(poly1, poly2, output);

            double totalArea = 0.0;

            BOOST_FOREACH(polygon_type const& p, output)
            {
                totalArea += bg::area(p);
            }

            double area1 = bg::area(poly1);
            double area2 = bg::area(poly2);

            if (area1 > area2) {

                return totalArea / area2;

            } else {

                return totalArea / area1;

            }

            return totalArea;

        }

        unsigned long* old_group_elements(unsigned long *a, unsigned long *b,
            unsigned long *c, unsigned long *d, unsigned long n,
            double threshold_dist)
        {

            unsigned long *element_groups = new unsigned long[n];

            unsigned long i;

            std::vector<bool> v(n);
            std::fill(v.begin(), v.begin() + 2, true);

            std::vector<unsigned long> w(2);

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            double height;

            double dist;

            double totalArea;

            std::vector< std::vector<unsigned long> > to_process = {};

            std::list< std::list<unsigned long> > res = {};

            std::cout << "threshold_dist: " << threshold_dist << std::endl;

            std::cout << "Computing polygon distances..." << std::endl;

            for (i = 0; i < n; i++) {

                to_process.push_back({i, i});

            }

            do {

                w = {};

                points1 = {};
                points2 = {};

                for (i = 0; i < n; i++) {

                    if (v[i]) {
                        w.push_back(i);
                    }

                }

                points1.push_back(point_type(a[w[0]], b[w[0]]));
                points1.push_back(point_type(c[w[0]], b[w[0]]));
                points1.push_back(point_type(a[w[0]], d[w[0]]));
                points1.push_back(point_type(c[w[0]], d[w[0]]));

                points2.push_back(point_type(a[w[1]], b[w[1]]));
                points2.push_back(point_type(c[w[1]], b[w[1]]));
                points2.push_back(point_type(a[w[1]], d[w[1]]));
                points2.push_back(point_type(c[w[1]], d[w[1]]));

                polygon_type poly1;
                polygon_type poly2;

                bg::assign_points(poly1, points1);
                bg::assign_points(poly2, points2);

                bg::correct(poly1);
                bg::correct(poly2);

                height = std::min(abs(double(d[w[0]] - b[w[0]])), abs(double(d[w[1]] - b[w[1]])));

                std::deque<polygon_type> output;
                bg::intersection(poly1, poly2, output);

                totalArea = 0.0;

                BOOST_FOREACH(polygon_type const& p, output)
                {
                    totalArea += bg::area(p);
                }

                if (totalArea > 0.0){
                    dist = 0.0;
                } else {
                    dist = bg::distance(poly1, poly2);
                }

                if (dist <= threshold_dist * height) {
                    to_process.push_back(w);
                }

            } while (std::prev_permutation(v.begin(), v.end()));

            std::cout << "Combining to nested list..." << std::endl;

            connectedcomponents(n, to_process, res);

            i = 0;

            for (auto const& el: res) {

                for (auto const& itm: el) {
                    element_groups[itm] = i;
                }

                i++;

            }

            return element_groups;

        }

        unsigned long* group_elements(unsigned long *a, unsigned long *b,
            unsigned long *c, unsigned long *d, unsigned long n,
            double threshold_dist, double slov_ratio)
        {

            unsigned long *element_groups = new unsigned long[n];

            unsigned long i;

            std::vector<bool> v(n);
            std::fill(v.begin(), v.begin() + 2, true);

            std::vector<unsigned long> w(2);

            std::vector<point_type> points1;
            std::vector<point_type> points2;

            double height, width, size_metric, min_height;

            double min_x_dist, min_y_dist, min_normal_dist;

            double pre_threshold_dist = std::max(5.0, threshold_dist);

            double dist;

            double totalArea;

            unsigned long y1, y2;

            std::vector< std::vector<unsigned long> > to_process = {};

            std::list< std::list<unsigned long> > res = {};

            std::cout << "threshold_dist: " << threshold_dist << std::endl;

            std::cout << "Computing polygon distances..." << std::endl;

            for (i = 0; i < n; i++) {

                to_process.push_back({i, i});

            }

            do {

                w = {};

                points1 = {};
                points2 = {};

                for (i = 0; i < n; i++) {

                    if (v[i]) {
                        w.push_back(i);
                    }

                }

                height = std::max(abs(double(d[w[0]] - b[w[0]])), abs(double(d[w[1]] - b[w[1]])));

                min_height = std::min(abs(double(d[w[0]] - b[w[0]])), abs(double(d[w[1]] - b[w[1]])));

                width = std::max(abs(double(c[w[0]] - a[w[0]])), abs(double(c[w[1]] - a[w[1]])));

                size_metric = std::max(height, width);

                min_x_dist = std::min({abs(double(a[w[0]] - c[w[1]])), abs(double(c[w[0]] - a[w[1]])), abs(double(a[w[0]] - a[w[1]])), abs(double(c[w[0]] - c[w[1]]))});

                min_y_dist = std::min({abs(double(b[w[0] - d[w[1]]])), abs(double(d[w[0]] - b[w[1]])), abs(double(b[w[0] - b[w[1]]])), abs(double(d[w[0]] - d[w[1]]))});

                min_normal_dist = std::min(min_x_dist, min_y_dist);

                if (min_normal_dist > pre_threshold_dist * size_metric) {
                    continue;
                }

                points1.push_back(point_type(a[w[0]], b[w[0]]));
                points1.push_back(point_type(c[w[0]], b[w[0]]));
                points1.push_back(point_type(a[w[0]], d[w[0]]));
                points1.push_back(point_type(c[w[0]], d[w[0]]));

                points2.push_back(point_type(a[w[1]], b[w[1]]));
                points2.push_back(point_type(c[w[1]], b[w[1]]));
                points2.push_back(point_type(a[w[1]], d[w[1]]));
                points2.push_back(point_type(c[w[1]], d[w[1]]));

                polygon_type poly1;
                polygon_type poly2;

                bg::assign_points(poly1, points1);
                bg::assign_points(poly2, points2);

                bg::correct(poly1);
                bg::correct(poly2);

                std::deque<polygon_type> output;
                bg::intersection(poly1, poly2, output);

                totalArea = 0.0;

                BOOST_FOREACH(polygon_type const& p, output)
                {
                    totalArea += bg::area(p);
                }

                if (totalArea > 0.0){
                    dist = 0.0;
                } else {
                    dist = bg::distance(poly1, poly2);
                }

                if (dist == 0) {
                    to_process.push_back(w);
                    continue;
                }

                if (b[w[0]] < d[w[0]]) {
                    y1 = b[w[0]];
                    y2 = d[w[0]];
                } else {
                    y1 = d[w[0]];
                    y2 = b[w[0]];
                }

                if (slov_ratio >= 0) {
                    if (! (((y1 <= b[w[1]] && b[w[1]] <= y2) && (y2 - b[w[1]]) / min_height >= slov_ratio ) || ((y1 <= d[w[1]] && d[w[1]] <= y2) && (d[w[1]] - y1) / min_height >= slov_ratio) || ((y1 <= b[w[1]]) && (y1 <= d[w[1]]) && (y2 >= b[w[1]]) && (y2 >= d[w[1]])))) {
                        continue;
                    }
                }

                if (dist <= threshold_dist * size_metric) {
                    to_process.push_back(w);
                }

            } while (std::prev_permutation(v.begin(), v.end()));

            std::cout << "Combining to nested list..." << std::endl;

            connectedcomponents(n, to_process, res);

            i = 0;

            for (auto const& el: res) {

                for (auto const& itm: el) {
                    element_groups[itm] = i;
                }

                i++;

            }

            return element_groups;

        }

};

extern "C" {
    PolygonCalc* PolygonCalc_new(){ return new PolygonCalc; }
    void PolygonCalc_delete(PolygonCalc *polygoncalc){ delete polygoncalc; }
    const double PolygonCalc_helloworld(PolygonCalc* polygoncalc){ return polygoncalc->helloworld(); }
    double PolygonCalc_test_calc(PolygonCalc* polygoncalc){ return polygoncalc->test_calc(); }
    int PolygonCalc_test_nparray(double *A, int n, PolygonCalc* polygoncalc){ return polygoncalc->test_nparray(A, n); }
    double PolygonCalc_min_poly_distance(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

        return polygoncalc->min_poly_distance(poly1x, poly1y, poly2x, poly2y, m, n);

    }
    double PolygonCalc_poly_area(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, int m){ return polygoncalc->poly_area(poly1x, poly1y, m); }
    double PolygonCalc_poly_intersection_area(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

        return polygoncalc->poly_intersection_area(poly1x, poly1y, poly2x, poly2y, m, n);

    }
    double PolygonCalc_poly_intersection_area_ratio(PolygonCalc* polygoncalc, double *poly1x, double *poly1y, double *poly2x, double *poly2y, int m, int n){

        return polygoncalc->poly_intersection_area_ratio(poly1x, poly1y, poly2x, poly2y, m, n);

    }

    unsigned long* PolygonCalc_group_elements(PolygonCalc* polygoncalc, unsigned long *a, unsigned long *b, unsigned long *c, unsigned long *d, unsigned long n, double threshold_dist, double slov_ratio) {

        return polygoncalc->group_elements(a, b, c, d, n, threshold_dist, slov_ratio);

    }

    void free_long_array(unsigned long* pointer){

        delete[] pointer;

    }

}