// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <boost/rational.hpp>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <random>
#include <set>
#include <vector>

#include "ErrorHandling/Assert.hpp"
#include "Framework/TestHelpers.hpp"
#include "Utilities/FractionUtilities.hpp"

namespace {
template <typename Source>
std::vector<typename Source::value_type> collect(
    Source source, const size_t limit = std::numeric_limits<size_t>::max()) {
  std::vector<typename Source::value_type> result;
  for (size_t count = 0; count < limit and source; ++count, ++source) {
    result.push_back(*source);
  }
  return result;
}
}  // namespace

SPECTRE_TEST_CASE("Unit.Utilities.FractionUtilities.ContinuedFraction",
                  "[Utilities][Unit]") {
  using Rational = boost::rational<int64_t>;

  CHECK((std::vector<int64_t>{1, 2, 4, 2}) ==
        collect(ContinuedFraction<Rational>(Rational(29, 20))));
  CHECK((std::vector<int64_t>{0, 8}) ==
        collect(ContinuedFraction<double>(0.125)));
  CHECK((std::vector<int64_t>{-1, 1, 7}) ==
        collect(ContinuedFraction<double>(-0.125)));
  CHECK(std::vector<int64_t>(20, 1) ==
        collect(ContinuedFraction<double>(0.5 * (1. + sqrt(5.))), 20));
  CHECK((std::vector<int64_t>{0}) ==
        collect(ContinuedFraction<Rational>(Rational(0))));
  CHECK((std::vector<int64_t>{0}) == collect(ContinuedFraction<double>(0.)));

  // Check that the iterator terminates because of precision loss.
  CHECK(collect(ContinuedFraction<double>(0.5 * (1. + sqrt(5.))), 100).size() <
        100);

  // Check that the iterator doesn't terminate prematurely
  // because of precision loss.
  const int64_t two_to_the_fourty = 1099511627776;
  CHECK(std::vector<int64_t>{0, two_to_the_fourty} ==
        collect(ContinuedFraction<double>(1. / two_to_the_fourty)));
  CHECK(std::vector<int64_t>{1, two_to_the_fourty} ==
        collect(ContinuedFraction<double>(1 + 1. / two_to_the_fourty)));
  CHECK(std::vector<int64_t>{0, two_to_the_fourty, 2} ==
        collect(ContinuedFraction<double>(1. / (two_to_the_fourty + 0.5))));

  MAKE_GENERATOR(gen);
  {
    std::uniform_real_distribution<> dist(-10., 10.);
    const double value = dist(gen);
    // Set the scale because the fractional part of a negative number
    // (defined as `x - floor(x)`) can be larger than the number.
    auto approx_value = approx.scale(std::abs(std::floor(value)))(value);
    ContinuedFractionSummer<Rational> summer;
    bool should_be_smaller = true;
    std::vector<int64_t> terms{};  // Only for output
    std::vector<double> convergents{};  // Only for output
    for (ContinuedFraction<double> source(value); source; ++source) {
      summer.insert(*source);
      terms.push_back(*source);
      convergents.push_back(boost::rational_cast<double>(summer.value()));
      CAPTURE(terms);
      CAPTURE(convergents);

      // Convergents to a continued fraction always alternate between
      // over- and underestimates.
      if (convergents.back() != approx_value) {
        if (should_be_smaller) {
          CHECK(convergents.back() <= value);
        } else {
          CHECK(convergents.back() >= value);
        }
      }
      should_be_smaller = !should_be_smaller;
    }
    CAPTURE(terms);
    CAPTURE(convergents);
    CHECK(convergents.back() == approx_value);
  }
}

SPECTRE_TEST_CASE("Unit.Utilities.FractionUtilities.ContinuedFractionSummer",
                  "[Utilities][Unit]") {
  using Rational = boost::rational<int>;

  const auto check = [](const int num, const int denom) {
    const Rational value(num, denom);
    ContinuedFractionSummer<Rational> summer;
    for (ContinuedFraction<Rational> source(value); source; ++source) {
      summer.insert(*source);
    }
    CHECK(summer.value() == value);
  };

  check(0, 1);
  check(1, 1);
  check(2, 1);
  check(1, 2);
  check(2, 3);
  check(29, 20);
}

SPECTRE_TEST_CASE(
    "Unit.Utilities.FractionUtilities.simplest_fraction_in_interval",
    "[Utilities][Unit]") {
  using Rational = boost::rational<int>;

  const int denom_max = 20;

  std::set<Rational> fractions;
  for (int d = 1; d <= denom_max; ++d) {
    for (int n = 0; n <= d; ++n) {
      fractions.emplace(n, d);
    }
  }

  for (auto end1 = fractions.begin(); end1 != fractions.end(); ++end1) {
    // Worse than any considered value so will be immediately replaced.
    Rational simplest(1, denom_max + 1);
    for (auto end2 = end1; end2 != fractions.end(); ++end2) {
      if (*end1 == 0 and *end2 == 1) {
        // Correct answer not clear for the entire interval
        continue;
      }
      ASSERT(end2->denominator() != simplest.denominator(),
             "Answer is not unique");
      if (end2->denominator() < simplest.denominator()) {
        simplest = *end2;
      }
      CHECK(simplest_fraction_in_interval<Rational>(*end1, *end2) == simplest);
      CHECK(simplest_fraction_in_interval<Rational>(*end2, *end1) == simplest);
    }
  }

  // Quick check of non-exact input
  CHECK(simplest_fraction_in_interval<Rational>(0.6, 0.9) == Rational(2, 3));
}


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


#include <iostream>
#include <set>
#include <stack>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/assignment.hpp>

namespace linkedListAlgorithms
{
      using namespace std;

      template <typename keyType,typename valueType>
      struct Node
      {
            Node<keyType,valueType> *right;
            Node<keyType,valueType> *left;
            keyType key;
            valueType value;
      };

      template <typename keyType,typename valueType>
      void printLinkedList(Node<keyType,valueType> &root)
      {
            Node<keyType,valueType> *runner = &root;
            while (runner!=0)
            {
                  cout << runner->key << "->";
                  runner=runner->right;
            }
            cout << endl;
      };

      // 2.1
      template <typename keyType,typename valueType>
      void removeDuplicate(Node<keyType,valueType> &root)
      {
            set<keyType> keySet;
            keySet.insert(root.key);
            Node<keyType,valueType> *runner;
            runner = &root;

            while (runner->right != 0)
            {
                  if (keySet.find(runner->right->key) != keySet.end()) //contains key
                  { runner->right = runner->right->right; }
                  else
                  {
                        keySet.insert(runner->right->key);
                        runner = runner->right;
                  }
            }
      };

      // 2.2
      // k=1 returns the last element
      template <typename keyType,typename valueType>
      Node<keyType,valueType>* returnKthToLast(Node<keyType,valueType> &root,int k)
      {
            Node<keyType,valueType> *runner = &root;
            Node<keyType,valueType> *frontRunner = &root;

            for (int i=0;i<k;i++)
            {
                  if (frontRunner!=0) { frontRunner = frontRunner->right; }
                  else { return NULL; }
            }

            while (frontRunner!=0)
            {
                  frontRunner = frontRunner->right;
                  runner = runner->right;
            }
            return runner;
      };

      template <typename keyType,typename valueType>
      void printKthToLast(Node<keyType,valueType> &root,int k)
      {
            cout << returnKthToLast(root,k)->key << endl;
      };

      //2.3
      // delete a node given only access to it; return true if successful,
      // false if target is last element
      template <typename keyType,typename valueType>
      bool deleteNode(Node<keyType,valueType> &target)
      {
            if (target.right == 0) {return false;}
            target.key = target.right->key;
            target.value = target.right->value;
            target.right = target.right->right;
            return true;
      };

      //2.5
      Node<int,int>* add(Node<int,int> &num1, Node<int,int> &num2)
      {
            int carry = 0;
            int sum = 0;
            Node<int,int> *runner1, *runner2, *runner3, *result, *tail;
            result  = new Node<int,int>();
            runner1 = &num1;
            runner2 = &num2;
            runner3 = result;

            while (true)
            {
                  sum = runner1->key + runner2->key;
                  if ( sum > 9)
                  { runner3->key = (sum % 10)+ carry; carry = 1; }
                  else
                  { runner3->key = sum+carry; carry = 0; }
                  runner2 = runner2->right;
                  runner1 = runner1->right;
                  if (runner1==0 || runner2==0) {break;}
                  else
                  {
                        runner3->right = new Node<int,int>();
                        runner3 = runner3->right;
                  }
            }
            if (runner1==0 && runner2==0 && carry==1)
            {
                  runner3->right = new Node<int,int>();
                  runner3 = runner3->right;
                  runner3->key = carry;
            }

            tail = max(runner1,runner2);
            if (tail!=0)
            { runner3->right = tail; runner3->key += carry; }

            return result;
      };

      //2.7
      // slow runner fast runner
      template <typename keyType,typename valueType>
      bool isPalindrome(Node<keyType,valueType> &head)
      {
            Node<keyType,valueType> *slowRunner,*fastRunner;
            slowRunner = &head;
            fastRunner = &head;
            bool result = true;
            stack<keyType> keyStack;

            while (fastRunner!=0 && fastRunner->right !=0)
            {
                  keyStack.push(slowRunner->key);
                  slowRunner = slowRunner->right;
                  fastRunner = fastRunner->right->right;
            }

            if (fastRunner!=0) // odd number of elements
            {  slowRunner = slowRunner->right; }

            while (slowRunner!=0)
            {
                  result = result && (slowRunner->key == keyStack.top());
                  keyStack.pop();
                  slowRunner = slowRunner->right;
            }

            return result;
      };
};

int main()
{
      using namespace linkedListAlgorithms;
      using namespace boost::numeric::ublas;

      matrix<int> entries(13,1);
      entries <<= 0,1,2,3,4,4,5,4,4,3,2,1,0;

      Node<int,int> root = {0,0,entries(0,0),0};
      Node<int,int> *runner = &root;
      for (int i=1;i<entries.size1();i++)
      {
            runner->right = new Node<int,int>();
            runner->right->key = entries(i,0);
            runner = runner->right;
      }

      Node<int,int> root1 = {0,0,9,0};
      runner = &root1;
      for (int i=2;i<10;i++)
      {
            runner->right = new Node<int,int>();
            runner->right->key = rand()%10;
            runner = runner->right;
      }

      printLinkedList(root);
      //printLinkedList(root1);
      //removeDuplicate(root);
      //printLinkedList(root);
      //printKthToLast(root,3);
      // runner = returnKthToLast(root,8);
      //if (deleteNode(*runner)) { printLinkedList(root); }
      //printLinkedList(*add(root,root1));
      cout << isPalindrome<int>(root) << endl;
      return 0;
}


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

#include <iostream>
#include <cmath>
#include <boost/numeric/mtl/mtl.hpp>

template <typename Matrix, typename Vector>
void test(const char* A_string, const char* v_string, const Matrix& A, const Vector&x)
{
    using mtl::io::tout;
    tout << "\n" << A_string << "ly sized matrix and " << v_string << "ly sized vector\nA is\n" << A;

    // asm("#mat_add begins here!");
    Matrix B(A + A);
    // asm("#mat_add ends here!");
    tout << "A+A = \n" << B;
    MTL_THROW_IF(B[0][0] != 4.0, mtl::runtime_error("wrong result in matrix addition."));

    // asm("#mat_mult begins here!");
    B= A * A;
    // asm("#mat_mult ends here!");
    tout << "A*A = \n" << B;
    MTL_THROW_IF(B[0][0] != 16.0, mtl::runtime_error("wrong result in matrix product."));

    // asm("#vec_add begins here!");
    Vector w(x + x);
    // asm("#vec_add ends here!");
    tout << "x = " << x << "\nw = x+x = " << w << "\n";
    MTL_THROW_IF(w[0] != 6.0, mtl::runtime_error("wrong result in vector addition."));

    // asm("#mat_vec_mult begins here!");
    w= A * x;
    // asm("#mat_vec_mult ends here!");

    tout << "A*x = " << w << "\n";
    MTL_THROW_IF(w[0] != 18.0, mtl::runtime_error("wrong result in matrix vector product."));
}

int main(int , char**)
{
    using namespace mtl;
    typedef mtl::vec::parameters<tag::col_major, mtl::vec::fixed::dimension<2>, true> fvec_para;
    typedef mat::parameters<tag::row_major, mtl::index::c_index, mtl::fixed::dimensions<2, 2>, true> fmat_para;

    float ma[2][2]= {{2., 3.}, {4., 5.}}, va[2]= {3., 4.};

    dense2D<float>                   A_dyn(ma);
    dense2D<float, fmat_para>        A_stat(ma);
    dense_vector<float>              v_dyn(va);
    dense_vector<float, fvec_para>   v_stat(va);

    typedef mtl::vec::parameters<tag::col_major, mtl::vec::fixed::dimension<4>, true> fvec_para4;
    dense_vector<double, fvec_para4>   v_stat4, w_stat4;
    v_stat4= 3;

    w_stat4= v_stat4 + v_stat4;
    io::tout << "w_stat4 is " << w_stat4 << '\n';

    test("dynamic", "dynamic", A_dyn, v_dyn);
    test("dynamic", "static", A_dyn, v_stat);
    test("static", "dynamic", A_stat, v_dyn);
    test("static", "static", A_stat, v_stat);

    return 0;
}


//  (C) Copyright Matt Borland 2021.
//  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)

#include <boost/math/ccmath/modf.hpp>
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   float i_f;
   check_result<float>(boost::math::ccmath::modf(1.0f, &i_f));

   double i_d;
   check_result<double>(boost::math::ccmath::modf(1.0, &i_d));

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   long double i_ld;
   check_result<long double>(boost::math::ccmath::modf(1.0l, &i_ld));
#endif
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <algorithm>
#include <iostream>
#include <vector>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/sort.hpp>
#include <boost/compute/container/vector.hpp>

namespace compute = boost::compute;

int rand_int()
{
    return rand() % 100;
}

// this example demonstrates how to sort a vector of ints on the GPU
int main()
{
    // create vector of random values on the host
    std::vector<int, mi_stl_allocator<int>> host_vector(10);
    std::generate(host_vector.begin(), host_vector.end(), rand_int);

    // print out input vector
    std::cout << "input:  [ ";
    for (size_t i = 0; i < host_vector.size(); i++)
    {
        std::cout << host_vector[i];

        if (i != host_vector.size() - 1)
        {
            std::cout << ", ";
        }
    }
    std::cout << " ]" << std::endl;

    // transfer the values to the device
    compute::vector<int> device_vector = host_vector;

    // sort the values on the device
    compute::sort(device_vector.begin(), device_vector.end());

    // transfer the values back to the host
    compute::copy(device_vector.begin(),
                  device_vector.end(),
                  host_vector.begin());

    // print out the sorted vector
    std::cout << "output: [ ";
    for (size_t i = 0; i < host_vector.size(); i++)
    {
        std::cout << host_vector[i];

        if (i != host_vector.size() - 1)
        {
            std::cout << ", ";
        }
    }
    std::cout << " ]" << std::endl;

    return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <boost/math/special_functions/hermite.hpp>
#include <eve/function/hermite.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>

TTS_CASE_TPL("Check eve::hermite return type", EVE_TYPE)
{
  TTS_EXPR_IS(eve::hermite(0, T(0)), T);
}

TTS_CASE_TPL("Check eve::hermite behavior", EVE_TYPE)
{

  auto eve__hermite =  [](auto n, auto x) { return eve::hermite(n, x); };
  auto boost_hermite =  [](auto n, auto x) { return boost::math::hermite(n, x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__hermite(2u, eve::minf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__hermite(2u, eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__hermite(3u, eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__hermite(3u, eve::inf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__hermite(3u, eve::inf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }

  for(unsigned int i=0; i < 10; ++i)
  {
    TTS_ULP_EQUAL(eve__hermite(i, T(10)), T(boost_hermite(i, 10)), 1);
    TTS_ULP_EQUAL(eve__hermite(i, T(5)), T(boost_hermite(i, 5)), 1);
    TTS_ULP_EQUAL(eve__hermite(i, T(2)), T(boost_hermite(i, 2)), 1);
    TTS_ULP_EQUAL(eve__hermite(i, T(1)), T(boost_hermite(i, 1)), 1);
    TTS_ULP_EQUAL(eve__hermite(i, T(0)), T(boost_hermite(i, 0)), 1);
  }
}


#pragma once

#include <cstdint>
#include <Eigen/Dense>

// todo improve documentation on these
class RobotEstimator {
private:
    // X Y W bot velocities (m/s)
    static constexpr int numStates = 3;

    // Motor duty cycles (% max)
    static constexpr int numInputs = 4;

    // Motor encoders (rad/s), gyro angular vel (rad/s)
    static constexpr int numOutputs = 5;

public:
    /**
     * @param dt_us Expected period of the controller in us
     */
    RobotEstimator(uint32_t dt_us);

    /**
     * Using the previous state and the next input
     * We can guess where we are this time step
     *
     * @param u Last motor command
     */
    void predict(Eigen::Matrix<double, numInputs, 1> u);

    /**
     * Using the next measurements, we can move our prediction
     * closer to the true target
     *
     * @param z Encoders 1-4 then gyro
     */
    void update(Eigen::Matrix<double, numOutputs, 1> z);

    /**
     * @param state Matrix that the current guess will be saved into
     */
    void getState(Eigen::Matrix<double, numStates, 1>& state);

private:
    static constexpr double processNoise = 0.05;
    static constexpr double encoderNoise = 0.04;
    static constexpr double gyroNoise = 0.005;
    static constexpr double initCovariance = 10.0;

    Eigen::Matrix<double, numStates,  numStates>  F;
    Eigen::Matrix<double, numStates,  numInputs>  B;
    Eigen::Matrix<double, numOutputs, numStates>  H;
    Eigen::Matrix<double, numStates,  numStates>  Q;
    Eigen::Matrix<double, numOutputs, numOutputs> R;
    Eigen::Matrix<double, numStates,  numStates>  P;

    // Identity
    Eigen::Matrix<double, numStates, numStates> I;

    // Current estimate
    Eigen::Matrix<double, numStates, 1> x_hat;
};


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


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2010: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example dynamic_interval.cpp \file dynamic_interval.cpp
    \brief Intervals with dynamic interval bounds that can be changed at runtime.

    Intervals types with dynamic interval bounds can represent closed and
    open interval borders. Interval borders are not static or fixed for
    the type but may change due to computations in interval containers.
    Dynamically bounded intervals are the library default for interval
    parameters in interval containers.

    \include dynamic_interval_/dynamic_interval.cpp
*/
//[example_dynamic_interval
#include <iostream>
#include <string>
#include <math.h>
#include <boost/type_traits/is_same.hpp>

#include <boost/icl/interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>
// Dynamically bounded intervals 'discrete_interval' and 'continuous_interval'
// are indirectly included via interval containers as library defaults.
#include "../toytime.hpp"
#include <boost/icl/rational.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

int main()
{
    cout << ">>Interval Container Library: Sample interval.cpp <<\n";
    cout << "----------------------------------------------------\n";

    // Dynamically bounded intervals are the library default for
    // interval parameters in interval containers.
    BOOST_STATIC_ASSERT((
        boost::is_same< interval_set<int>::interval_type
                      , discrete_interval<int> >::value
                      ));

    BOOST_STATIC_ASSERT((
        boost::is_same< interval_set<float>::interval_type
                      , continuous_interval<float> >::value
                      ));

    // As we can see the library default chooses the appropriate
    // class template instance discrete_interval<T> or continuous_interval<T>
    // dependent on the domain_type T. The library default for intervals
    // is also available via the template 'interval':
    BOOST_STATIC_ASSERT((
        boost::is_same< interval<int>::type
                      , discrete_interval<int> >::value
                      ));

    BOOST_STATIC_ASSERT((
        boost::is_same< interval<float>::type
                      , continuous_interval<float> >::value
                      ));

    // template interval also provides static functions for the four border types

    interval<int>::type    int_interval  = interval<int>::closed(3, 7);
    interval<double>::type sqrt_interval = interval<double>::right_open(1/sqrt(2.0), sqrt(2.0));
    interval<string>::type city_interval = interval<string>::left_open("Barcelona", "Boston");
    interval<Time>::type   time_interval = interval<Time>::open(Time(monday,8,30), Time(monday,17,20));

    cout << "----- Dynamically bounded intervals ----------------------------------------\n";
    cout << "  discrete_interval<int>   : " << int_interval  << endl;
    cout << "continuous_interval<double>: " << sqrt_interval << " does "
                                            << string(contains(sqrt_interval, sqrt(2.0))?"":"NOT")
                                            << " contain sqrt(2)" << endl;
    cout << "continuous_interval<string>: " << city_interval << " does "
                                            << string(contains(city_interval,"Barcelona")?"":"NOT")
                                            << " contain 'Barcelona'" << endl;
    cout << "continuous_interval<string>: " << city_interval << " does "
                                            << string(contains(city_interval, "Berlin")?"":"NOT")
                                            << " contain 'Berlin'" << endl;
    cout << "  discrete_interval<Time>  : " << time_interval << "\n\n";

    // Using dynamically bounded intervals allows to apply operations
    // with intervals and also with elements on all interval containers
    // including interval containers of continuous domain types:

    interval<rational<int> >::type unit_interval
        = interval<rational<int> >::right_open(rational<int>(0), rational<int>(1));
    interval_set<rational<int> > unit_set(unit_interval);
    interval_set<rational<int> > ratio_set(unit_set);
    ratio_set -= rational<int>(1,3); // Subtract 1/3 from the set

    cout << "----- Manipulation of single values in continuous sets ---------------------\n";
    cout << "1/3 subtracted from [0..1) : " << ratio_set << endl;
    cout << "The set does " << string(contains(ratio_set, rational<int>(1,3))?"":"NOT")
                                            << " contain '1/3'" << endl;
    ratio_set ^= unit_set;
    cout << "Flipping the holey set     : " << ratio_set << endl;
    cout << "yields the subtracted      :     1/3\n\n";

    // Of course we can use interval types that are different from the
    // library default by explicit instantiation:
    split_interval_set<int, std::less, closed_interval<Time> > intuitive_times;
    // Interval set 'intuitive_times' uses statically bounded closed intervals
    intuitive_times += closed_interval<Time>(Time(monday,  9,00), Time(monday, 10,59));
    intuitive_times += closed_interval<Time>(Time(monday, 10,00), Time(monday, 11,59));
    cout << "----- Here we are NOT using the library default for intervals --------------\n";
    cout << intuitive_times << endl;

    return 0;
}

// Program output:
//>>Interval Container Library: Sample interval.cpp <<
//----------------------------------------------------
//----- Dynamically bounded intervals ----------------------------------------
//  discrete_interval<int>   : [3,7]
//continuous_interval<double>: [0.707107,1.41421) does NOT contain sqrt(2)
//continuous_interval<string>: (Barcelona,Boston] does NOT contain 'Barcelona'
//continuous_interval<string>: (Barcelona,Boston] does  contain 'Berlin'
//  discrete_interval<Time>  : (mon:08:30,mon:17:20)
//
//----- Manipulation of single values in continuous sets ---------------------
//1/3 subtracted from [0..1) : {[0/1,1/3)(1/3,1/1)}
//The set does NOT contain '1/3'
//Flipping the holey set     : {[1/3,1/3]}
//yields the subtracted      :     1/3
//
//----- Here we are NOT using the library default for intervals --------------
//{[mon:09:00,mon:09:59][mon:10:00,mon:10:59][mon:11:00,mon:11:59]}
//]


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestScan
#include <boost/test/unit_test.hpp>

#include <boost/compute/lambda.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/exclusive_scan.hpp>
#include <boost/compute/algorithm/inclusive_scan.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>
#include <boost/compute/iterator/transform_iterator.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(inclusive_scan_int)
{
    int data[] = { 1, 2, 1, 2, 3 };
    bc::vector<int> vector(data, data + 5, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    bc::vector<int> result(5, context);
    BOOST_CHECK_EQUAL(result.size(), size_t(5));

    // inclusive scan
    bc::inclusive_scan(vector.begin(), vector.end(), result.begin());
    CHECK_RANGE_EQUAL(int, 5, result, (1, 3, 4, 6, 9));

    // in-place inclusive scan
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 1, 2, 3));
    bc::inclusive_scan(vector.begin(), vector.end(), vector.begin());
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 3, 4, 6, 9));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_int)
{
    int data[] = { 1, 2, 1, 2, 3 };
    bc::vector<int> vector(data, data + 5, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    bc::vector<int> result(5, context);
    BOOST_CHECK_EQUAL(vector.size(), size_t(5));

    // exclusive scan
    bc::exclusive_scan(vector.begin(), vector.end(), result.begin());
    CHECK_RANGE_EQUAL(int, 5, result, (0, 1, 3, 4, 6));

    // in-place exclusive scan
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 1, 2, 3));
    bc::exclusive_scan(vector.begin(), vector.end(), vector.begin());
    CHECK_RANGE_EQUAL(int, 5, vector, (0, 1, 3, 4, 6));
}

BOOST_AUTO_TEST_CASE(inclusive_scan_int2)
{
    using boost::compute::int2_;

    int data[] = { 1, 2,
                   3, 4,
                   5, 6,
                   7, 8,
                   9, 0 };

    boost::compute::vector<int2_> input(reinterpret_cast<int2_*>(data),
                                        reinterpret_cast<int2_*>(data) + 5);
    BOOST_CHECK_EQUAL(input.size(), size_t(5));

    boost::compute::vector<int2_> output(5);
    boost::compute::inclusive_scan(input.begin(), input.end(), output.begin());
    CHECK_RANGE_EQUAL(
        int2_, 5, output,
        (int2_(1, 2), int2_(4, 6), int2_(9, 12), int2_(16, 20), int2_(25, 20))
    );
}

BOOST_AUTO_TEST_CASE(inclusive_scan_counting_iterator)
{
    bc::vector<int> result(10, context);
    bc::inclusive_scan(bc::make_counting_iterator(1),
                       bc::make_counting_iterator(11),
                       result.begin());
    CHECK_RANGE_EQUAL(int, 10, result, (1, 3, 6, 10, 15, 21, 28, 36, 45, 55));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_counting_iterator)
{
    bc::vector<int> result(10, context);
    bc::exclusive_scan(bc::make_counting_iterator(1),
                       bc::make_counting_iterator(11),
                       result.begin());
    CHECK_RANGE_EQUAL(int, 10, result, (0, 1, 3, 6, 10, 15, 21, 28, 36, 45));
}

BOOST_AUTO_TEST_CASE(inclusive_scan_transform_iterator)
{
    float data[] = { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f };
    bc::vector<float> input(data, data + 5, queue);
    bc::vector<float> output(5, context);

    // normal inclusive scan of the input
    bc::inclusive_scan(input.begin(), input.end(), output.begin());
    bc::system::finish();
    BOOST_CHECK_CLOSE(float(output[0]), 1.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[1]), 3.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[2]), 6.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[3]), 10.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[4]), 15.0f, 1e-4f);

    // inclusive scan of squares of the input
    using ::boost::compute::_1;

    bc::inclusive_scan(bc::make_transform_iterator(input.begin(), pown(_1, 2)),
                       bc::make_transform_iterator(input.end(), pown(_1, 2)),
                       output.begin());
    bc::system::finish();
    BOOST_CHECK_CLOSE(float(output[0]), 1.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[1]), 5.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[2]), 14.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[3]), 30.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(output[4]), 55.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(inclusive_scan_doctest)
{
//! [inclusive_scan_int]
// setup input
int data[] = { 1, 2, 3, 4 };
boost::compute::vector<int> input(data, data + 4, queue);

// setup output
boost::compute::vector<int> output(4, context);

// scan values
boost::compute::inclusive_scan(
    input.begin(), input.end(), output.begin(), queue
);

// output = [ 1, 3, 6, 10 ]
//! [inclusive_scan_int]

    CHECK_RANGE_EQUAL(int, 4, output, (1, 3, 6, 10));
}

BOOST_AUTO_TEST_CASE(exclusive_scan_doctest)
{
//! [exclusive_scan_int]
// setup input
int data[] = { 1, 2, 3, 4 };
boost::compute::vector<int> input(data, data + 4, queue);

// setup output
boost::compute::vector<int> output(4, context);

// scan values
boost::compute::exclusive_scan(
    input.begin(), input.end(), output.begin(), queue
);

// output = [ 0, 1, 3, 6 ]
//! [exclusive_scan_int]

    CHECK_RANGE_EQUAL(int, 4, output, (0, 1, 3, 6));
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * concatenate Eigen matrices
 * by R. Falque
 * 03/07/2019
 */

#ifndef EIGEN_CONCATENATE_HPP
#define EIGEN_CONCATENATE_HPP

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Sparse>

// https://stackoverflow.com/a/21496281/2562693
template <typename T>
inline Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> concatenate(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> in_1,
                                                                    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> in_2,
                                                                    int direction)
{
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> out;
    int cols, rows;
    if (in_1.rows() == 0 & in_1.cols() ==0) {
        rows = in_2.rows();
        cols = in_2.cols();
    } else if (in_2.rows() == 0 & in_2.cols() ==0) {
        rows = in_1.rows();
        cols = in_1.cols();
    } else if (direction == 1 & in_1.cols() == in_2.cols()) {
        rows = in_1.rows()+in_2.rows();
        cols = in_1.cols();
    } else if (direction == 2 & in_1.rows() == in_2.rows()) {
        rows = in_1.rows();
        cols = in_1.cols()+in_2.cols();
    }

    out.resize(rows, cols);
    out << in_1, in_2;

    return out;
};

// https://stackoverflow.com/a/50353398/2562693
template <typename T>
inline Eigen::SparseMatrix< T > concatenate(Eigen::SparseMatrix<T>in_1, Eigen::SparseMatrix<T>in_2, int direction)
{
    typedef typename Eigen::SparseMatrix<T>::InnerIterator SparseIterator;

    // first test if the input size are correct with respect to the direction:
    if (direction == 1)
        if (in_1.cols() != in_2.cols())
        {
            std::cout << "Error: wrong input size, the cols size do not match.\n";
            std::exit(0);
        }
    else if (direction == 2)
        if (in_1.rows() != in_2.rows())
        {
            std::cout << "Error: wrong input size, the rows size do not match.\n";
            std::exit(0);
        }
    else {
        std::cout << "Error: wrong direction (direction should be 1 or 2).\n";
        std::exit(0);
    }

    // start the concatenation process:
    Eigen::SparseMatrix<T> out;

    if (direction == 1) // vertical
        out.resize(in_1.rows() + in_2.rows(), in_1.cols());
    else // horizontal
        out.resize(in_1.rows(), in_1.cols() + in_2.cols());

    out.reserve(in_1.nonZeros() + in_2.nonZeros());

    std::vector< Eigen::Triplet<T, size_t> > triplets;
    triplets.reserve(in_1.nonZeros() + in_2.nonZeros());

    for (int k = 0; k < in_1.outerSize(); ++k) {
        for (SparseIterator it(in_1, k); it; ++it) {
            triplets.emplace_back(it.row(), it.col(), it.value());
        }
    }

    for (int k = 0; k < in_2.outerSize(); ++k) {
        for (SparseIterator it(in_2, k); it; ++it) {
            if (direction == 1)
                triplets.emplace_back(in_1.rows() + it.row(), it.col(), it.value());
            else
                triplets.emplace_back(it.row(), in_1.cols() + it.col(), it.value());
        }
    }

    out.setFromTriplets(triplets.begin(), triplets.end());

    return out;
};

#endif


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


#ifdef STAN_OPENCL

#include <stan/math/opencl/kernel_generator.hpp>
#include <test/unit/math/opencl/kernel_generator/reference_kernel.hpp>
#include <stan/math/opencl/matrix_cl.hpp>
#include <stan/math/opencl/copy.hpp>
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <string>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using stan::math::diagonal;
using stan::math::matrix_cl;

#define EXPECT_MATRIX_NEAR(A, B, DELTA) \
  EXPECT_EQ(A.rows(), B.rows());        \
  EXPECT_EQ(A.cols(), B.cols());        \
  for (int i = 0; i < A.size(); i++)    \
    EXPECT_NEAR(A(i), B(i), DELTA);

TEST(KernelGenerator, diagonal_test) {
  MatrixXd m = MatrixXd::Random(3, 4);

  matrix_cl<double> m_cl(m);

  matrix_cl<double> res_cl = diagonal(m_cl);
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = m.diagonal();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, diagonal_multiple_operations_test) {
  MatrixXd m = MatrixXd::Random(4, 3);

  matrix_cl<double> m_cl(m);

  matrix_cl<double> res_cl = diagonal(m_cl * 2);
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = (2 * m).diagonal();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, diagonal_multiple_operations_accept_lvalue_test) {
  MatrixXd m = MatrixXd::Random(3, 4);

  matrix_cl<double> m_cl(m);

  auto tmp = m_cl * 2;
  matrix_cl<double> res_cl = diagonal(tmp);
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = (2 * m).diagonal();
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, diagonal_lhs_test) {
  MatrixXd m = MatrixXd::Random(3, 4);
  VectorXd v = VectorXd::Random(3);

  matrix_cl<double> m_cl(m);
  matrix_cl<double> v_cl(v);

  diagonal(m_cl) = v_cl;
  MatrixXd res = stan::math::from_matrix_cl(m_cl);

  MatrixXd correct = m;
  correct.diagonal() = v;
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, diagonal_of_a_block_test) {
  MatrixXd m = MatrixXd::Random(4, 4);

  matrix_cl<double> m_cl(m);

  diagonal(block(m_cl, 0, 1, 3, 3)) = diagonal(block(m_cl, 1, 0, 3, 3));
  MatrixXd res = stan::math::from_matrix_cl(m_cl);

  MatrixXd correct = m;
  correct.diagonal(1) = correct.diagonal(-1);
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

#endif