/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * 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.
 */

///
/// \file approx_power.hpp
/// \brief Find exponential sum approximations of power function
///
#ifndef MXPFIT_APPROX_POWER_HPP
#define MXPFIT_APPROX_POWER_HPP

#include <algorithm>
#include <iosfwd>
#include <iterator>
#include <stdexcept>

#include <boost/math/special_functions/gamma.hpp>

#include <mxpfit/exponential_sum.hpp>
#include <mxpfit/modified_prony_reduction.hpp>

namespace mxpfit
{

namespace detail
{

/// \internal
///
/// ### newton
///
/// Solve an equation \f$f(x)=0\f$ by Newton's method
///
/// \tparam T value type for function argument \f$x\f$
/// \tparam NewtonFunctor a unwary function that takes \f$x\f$ as an argument
/// and
///         returns pair of function value \f$f(x)\f$ and its first derivative
///         \f$f^{\prime}(x).\f$
///
/// \param[in] guess     initial guess of the solution
/// \param[in] tol       tolerance for convergence
/// \param[in] fn        an instance of `NewtonFunctor`
/// \param[in] max_iter  maximum number of iterations
///
template <typename T, typename NewtonFunctor>
T newton(T guess, T tol, NewtonFunctor fn, std::size_t max_iter = 1000)
{
    using std::abs;
    auto counter = max_iter;
    auto x       = guess;

    while (counter--)
    {
        // We assume df(x) never to be too small.
        const auto f_and_df = fn(x);
        const auto delta    = std::get<0>(f_and_df) / std::get<1>(f_and_df);
        // std::cout << "(newton): " << x << '\t'     // t
        //           << std::get<0>(f_and_df) << '\t' // f(t)
        //           << std::get<1>(f_and_df) << '\t' // f'(t)
        //           << delta << '\n';
        x -= delta;
        if (abs(delta) < abs(x) * tol)
        {
            break;
        }
    }

    return x;
}

} // namespace detail

///
/// ### ApproxPowerFunction
///
/// \brief Compute parameters for the exponential sum approximation of power
/// funciton, \f$r^{-\beta}\,(\beta>0).\f$
///
/// \tparam T Real scalar type for the parameters of the exponential sum
///
/// This funtion computes parameters to approximate the power functions
/// \f$r^{-\beta}\,(\beta>0)\f$ with a linear combination of exponential
/// functions,
///
/// \f[
///   \|r^{-\beta}-sum_{m=1}^{M} w_{m} e^{-a_{m} x} \| < r^{-\beta}\epsilon
/// \f]
///
/// for any given accuracy \f$\epsilon > 0\f$ and distance to singularity
/// \f$\delta>0\f$ real axis `(x)`.
///
/// The multi-exponential function is obtained by discretize the integral
/// representation of spherical Bessel function
///
/// #### References
///
/// 1. G. Beylkin and L. Monz\'{o}n, "Approximation by exponential sums
///    revisited", Appl. Comput. Harmon. Anal. 28 (2010) 131-149.
///    [DOI: https://doi.org/10.1016/j.acha.2009.08.011]
/// 2. W. McLean, "Exponential sum approximations for \f$t^{-\beta}\f$",
///    arXiv:1606.00123 [math]
///
template <typename T>
class ApproxPowerFunction
{
public:
    using Index      = Eigen::Index;
    using Real       = T;
    using ResultType = ExponentialSum<T, T>;

    ApproxPowerFunction()                           = default;
    ApproxPowerFunction(const ApproxPowerFunction&) = default;
    ApproxPowerFunction(ApproxPowerFunction&&)      = default;

    ///
    /// Create an instance with arguments.
    ///
    /// \param[in] beta the power factor \f$\beta > 0\f$
    /// \param[in] eps  the target accuracy of the exponential sum approximation
    /// \param[in] rmin the lower bound of the interval
    /// \param[in] rmax the upper bound of the interval
    /// \pre `beta > 0 && eps > 0 && rmin > 0 && rmax > rmin`
    ///
    ApproxPowerFunction(Real beta, Real eps, Real rmin, Real rmax)
        : m_beta(beta), m_eps(eps), m_rmin(rmin), m_rmax(rmax)
    {
        check_args(beta, eps, rmin, rmax);
        compute_extra_params();
    }

    ~ApproxPowerFunction() = default;

    ApproxPowerFunction& operator=(const ApproxPowerFunction&) = default;
    ApproxPowerFunction& operator=(ApproxPowerFunction&&) = default;

    /// Get the value of power factor \f$ \beta \f$
    Real power_factor() const
    {
        return m_beta;
    }

    /// Get the target accuracy \f$ \epsilon \f$
    Real tolerance() const
    {
        return m_eps;
    }

    ///
    /// Get the lower bound of the interval in which the exponential sum
    /// approximation is constructed.
    ///
    Real rmin() const
    {
        return m_rmin;
    }
    ///
    /// Get the upper bound of the interval in which the exponential sum
    /// approximation is constructed.
    ///
    Real rmax() const
    {
        return m_rmax;
    }
    ///
    /// Set parameters
    ///
    void set_params(Real beta, Real eps, Real rmin, Real rmax)
    {
        check_args(beta, eps, rmin, rmax);
        m_beta = beta;
        m_eps  = eps;
        m_rmin = rmin;
        m_rmax = rmax;
        compute_extra_params();
    }

    ///
    /// Compute the exponential sum approximation with the parameters set in
    /// advance via constructor or `set_params` method.
    ///
    ResultType compute() const;

    ///
    /// Compute the exponential sum approximation with the given parameters.
    ///
    ResultType compute(Real beta, Real eps, Real rmin, Real rmax)
    {
        set_params(beta, eps, rmin, rmax);
        return compute();
    }
    ///
    /// Print parameters to the given ostream
    ///
    template <typename Ch, typename Tr>
    void print(std::basic_ostream<Ch, Tr>& os) const;

private:
    constexpr static const Real safety = Real(10);

    static void check_args(Real beta, Real eps, Real rmin, Real rmax);
    void compute_extra_params();

    Real m_beta;
    Real m_eps;
    Real m_rmin;
    Real m_rmax;

    Real m_h;        // step size for discretization
    Index m_n_minus; // number of terms for trapezoidal rule
    Index m_n_plus;  // number of terms for trapezoidal rule
};

// ostream operator
template <typename Ch, typename Tr, typename T>
std::basic_ostream<Ch, Tr>& operator<<(std::basic_ostream<Ch, Tr>& os,
                                       const ApproxPowerFunction<T>& es)
{
    es.print(os);
    return os;
}

//
// --- Implementations of member functions
//

template <typename T>
typename ApproxPowerFunction<T>::ResultType
ApproxPowerFunction<T>::compute() const
{
    ResultType es(m_n_minus + m_n_plus + 1);

    const Real scale = m_h / std::tgamma(m_beta);

    for (Index n = -m_n_minus; n <= m_n_plus; ++n)
    {
        const Real tn              = Real(n) * m_h;
        const Real en              = std::exp(-tn);
        es.exponent(n + m_n_minus) = std::exp(tn - en);
        es.weight(n + m_n_minus) =
            scale * (Real(1) + en) * std::exp(m_beta * (tn - en));
    }

    //
    // Reduce number of terms with small exponents (a[i] < 1) via the modified
    // Prony method.
    //
    ModifiedPronyReduction<Real> reduction;
    const Index n_target = static_cast<Index>(std::distance(
        es.exponents().data(),
        std::upper_bound(es.exponents().data(),
                         es.exponents().data() + es.exponents().size(),
                         Real(1))));

    ResultType trunc = reduction.compute(es, n_target, m_eps);

    const Real rmax_tmp = m_rmax * safety;
    trunc.exponents() /= rmax_tmp;
    trunc.weights() *= std::pow(rmax_tmp, -m_beta);

    return trunc;
}

template <typename T>
template <typename Ch, typename Tr>
void ApproxPowerFunction<T>::print(std::basic_ostream<Ch, Tr>& os) const
{
    os << "# Exponential sum approximation of power function, r^{-beta}\n"
       << "#   beta    : " << m_beta << '\n'                    //
       << "#   interval: [" << m_rmin << ',' << m_rmax << "]\n" //
       << "#   relative accuracy: " << m_eps << '\n'            //
       << "#   step size: " << m_h << '\n'                      //
       << "#   number of terms for discritization: [" << -m_n_minus << ", "
       << m_n_plus << "]\n";
}

template <typename T>
void ApproxPowerFunction<T>::check_args(Real beta, Real eps, Real rmin,
                                        Real rmax)
{
    if (!(beta > Real()))
    {
        std::ostringstream msg;
        msg << "Invalid value for the argument `beta': "
               "beta > 0 expected, but beta = "
            << beta << " is given";
        throw std::invalid_argument(msg.str());
    }

    if (!(Real() < eps && eps < std::exp(Real(-1))))
    {
        std::ostringstream msg;
        msg << "Invalid value for the argument `eps': "
               "0 < eps < 1/e expected, but "
            << eps << " is given";
        throw std::invalid_argument(msg.str());
    }

    if (!(Real() < rmin && rmin < rmax))
    {
        std::ostringstream msg;
        msg << "Invalid value for the argument `rmin/rmax': "
               "0 < rmin < rmax expected, but rmin = "
            << rmin << " and rmax = " << rmax << " are given";
        throw std::invalid_argument(msg.str());
    }
}

template <typename T>
void ApproxPowerFunction<T>::compute_extra_params()
{
    constexpr const T pi = boost::math::constants::pi<T>();
    //
    // Construct approximation for interval with extended rmax: otherwise, the
    // relative error of Gaussian sum approximation for `r` close to `rmax`
    // exceeds the prescribed accuracy.
    //
    const Real rmax = m_rmax * safety;

    //
    // Some constants related to given arguments
    //
    const T eps_d = m_eps / T(3); // upper bound of discretization error
    const T eps_t = m_eps / T(3); // upper bound of truncation error
    const T delta = m_rmin / rmax;

    //
    // ----- Spacing of discritization (See eq. (14) in [Beylkin2010])
    //
    // The step size h is determined to satisfy
    //
    //   \sum_{n=1}^{\infty}
    //     \frac{2|\Gamma(\beta + 2 \pi i n / h)|}{\Gamma(\beta)} < \epsilon_d.
    //
    // For \beta = 1/2, the equation above can be written as,
    //
    //   \sum_{n=1}^{\infty} 2 / sqrt(cosh(2 \pi^2 n / h)) < \epsilon_d
    //
    // In practice, \f$|\Gamma(\beta + 2 \pi i n / h)|\f$ decay so rapidly with
    // \f$n\f$ that only the first term in the left-hand-side is significant.
    //
    if (m_beta == T(0.5))
    {
        // solve  2 / sqrt(cosh(2 * pi**2 / h)) == eps_d  with h
        m_h = T(2) * pi * pi / std::acosh(T(4) / (eps_d * eps_d));
    }
    else
    {
        //
        // |Gamma(beta + yi)| <= 1+(y/beta)**2 * exp(-y * atan(y/beta))
        // so we solve (log(r.h.s) == log(eps_d)) with y = 2 * pi / h
        //

        //
        // Initial guess of step size (See eq. (15) in [Beylkin2010])
        //
        // This yields a lower bound of step size h, which is not optimal.
        //
        m_h = T(2) * pi /
              (std::log(T(3)) - m_beta * std::log(std::cos(T(1))) -
               std::log(eps_d));

        const T beta_half = m_beta / T(2);
        const T log_eps_d = std::log(eps_d);
        const T y         = detail::newton(
            // Initial guess of y
            T(2) * pi / m_h,
            // relative accuracy
            eps_d,
            // function value and its derivative
            [=](T t) {
                const T x      = t / m_beta;
                const T atan_x = std::atan2(t, m_beta);
                const T f =
                    beta_half * std::log1p(x * x) - t * atan_x - log_eps_d;
                const T df = (beta_half - x) / (T(1) + x * x) - atan_x;
                return std::make_pair(f, df);
            });

        m_h = T(2) * pi / y;
    }

    //
    // Find lower bound `t_lower` such that the truncation error of integral is
    // bounded up to `eps_t`
    //
    // `t_lower` satisfies
    //
    //   C_2 \exp(-\beta e^{t}) < \epsilon_t
    //
    // with \f$ C_2 = 2e/\Gamma(\beta)\f$ (See eq. (25) in [McLean2017]).
    //
    const T c2 =
        T(2) * boost::math::constants::e<T>() / std::tgamma(m_beta + 1);
    const T t_lower = -std::log(-std::log(eps_t / c2) / m_beta);

    //
    // Find upper bound `t_upper` such that the truncation error of integral is
    // bounded up to `eps_t`
    //
    // This is obtained by solving (see eq.(32) in [Beylkin2010])
    //
    //   \Gamma(beta, \delta \exp(t)) = \epsilon_t
    //
    // For beta = 1/2, the equation becomes
    //
    //   \erfc(\delta exp(t/2)) = \epsilon_t
    //
    const T t_upper =
        std::log(boost::math::gamma_q_inv(m_beta, eps_t)) - std::log(delta);

    //
    // Set the step size and number of terms for trapezoidal rule
    //
    m_n_minus = static_cast<Index>(std::floor((-t_lower) / m_h));
    m_n_plus  = static_cast<Index>(std::ceil((t_upper) / m_h));
}

///
/// ### approx_power
///
template <typename T>
ExponentialSum<T, T> approx_power(T beta, T eps, T rmin, T rmax)
{
    ApproxPowerFunction<T> pow;
    return pow.compute(beta, eps, rmin, rmax);
}

} // namespace mxpfit

#endif /* MXPFIT_APPROX_POWER_HPP */


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Array4i v = Array4i::Random().abs();
cout << "Here is the initial vector v:\n" << v.transpose() << "\n";
std::sort(v.begin(), v.end());
cout << "Here is the sorted vector v:\n" << v.transpose() << "\n";

}
  return 0;
}


/**
 * Authors:
 *      Joao Quintas (jquintas@gmail.com)
 *      Joao Cruz (joao.pedro.cruz@tecnico.ulisboa.pt)
 * 		Marcelo Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
 * Maintained by: Marcelo Fialho Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
 * Last Update: 14/12/2021
 * License: MIT
 * File: spherical_coordinates.hpp
 * Brief: Defines all functions related to spherical coordinates conversions
 */
#pragma once
#include <Eigen/Dense>

namespace DSOR {

    /**
	 * @brief Convert from spherical to cartesian coordinates. Used mainly with usbl fixes
	 *
	 * @param bearing Horizontal angle between the direction of an object and another object or between it and the true north direction in degrees.
	 * @param elevation Angle measured between the horizontal and the vehicle line of sight to the object
	 * @param range Distance to the object
	 * @return Eigen Vector with cartesian coordinates
	 */
    template <typename T>
	Eigen::Matrix<T, 3, 1> spherical_to_cartesian(T bearing, T elevation, T range);
}


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/protect.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/list.hpp>

#include <cmath>
#include <boost/safe_float.hpp>
#include <boost/safe_float/convenience.hpp>
#include <boost/safe_float/policy/check_multiplication_overflow.hpp>
#include <boost/safe_float/policy/check_multiplication_underflow.hpp>
#include <boost/safe_float/policy/check_multiplication_inexact.hpp>
#include <boost/safe_float/policy/check_multiplication_invalid_result.hpp>

//types to be tested
using test_types=boost::mpl::list<
    float, double, long double
>;

using namespace boost::safe_float;

/**
  This test suite checks different policies on multiplication operations using default parameters for the other policies.
  */
BOOST_AUTO_TEST_SUITE( safe_float_multiplication_test_suite )

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_multiplication_throws_on_overflow, FPT, test_types){
    // define two FPT numbers suppose to positive overflow
    FPT a = std::numeric_limits<FPT>::max();
    FPT b = 2;
    // check FPT overflows to inf after add
    BOOST_CHECK(std::isinf(a*b));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_multiplication_overflow> c(std::numeric_limits<FPT>::max());
    safe_float<FPT, policy::check_multiplication_overflow> d(FPT(2));

    // check the multiplication throws
    BOOST_CHECK_THROW(c*d, std::exception);

    // define two FPT numbers suppose to negative overflow
    FPT e = std::numeric_limits<FPT>::lowest();
    FPT f = 2;
    // check FPT overflows to inf after add
    BOOST_CHECK(std::isinf(e*f));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_multiplication_overflow> g(std::numeric_limits<FPT>::lowest());
    safe_float<FPT, policy::check_multiplication_overflow> h(FPT(2));

    // check the multiplication throws
    BOOST_CHECK_THROW(g*h, std::exception);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_multiplication_inexact_rounding, FPT, test_types){
    // define two FPT numbers suppose to produce inexact rounding
    FPT a, b;

    if(std::is_same<FPT, float>()) {
        a = 1.64005529880523681640625f;
        b = 3.1559422016143798828125f;
    } else if(std::is_same<FPT, double>()){
        a = 1.200941392190915113502569511183537542819976806640625;
        b = 1.7035518365272823704259508303948678076267242431640625;
    } else if(std::is_same<FPT, long double>()) {
        a = 1.48057361058650153290937312444697226965217851102352142333984375L;
        b = 1.8352666822131742060432435525996197611675597727298736572265625L;
    } else {
        BOOST_ERROR("Test implemented only for float, double and long double");
    }

    // check multiplying and dividing gives the same number back.
    BOOST_CHECK((a*b)/b != a);

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_multiplication_inexact> c(a);
    safe_float<FPT, policy::check_multiplication_inexact> d(b);

    // check the multiplication throws
    BOOST_CHECK_THROW(c*d, std::exception);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_multiplication_underflow, FPT, test_types){
    // define two FPT numbers suppose to produce underflow
    FPT a = std::numeric_limits<FPT>::min();
    FPT b = 0.5;

    // check FPT underflow to denormalized result after multiply
    BOOST_CHECK(std::fpclassify(a*b) == FP_SUBNORMAL);

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_multiplication_underflow> c(a);
    safe_float<FPT, policy::check_multiplication_underflow> d(b);

    // check the multiplication throws
    BOOST_CHECK_THROW(c*d, std::exception);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_multiplication_invalid_result, FPT, test_types){
    // define two FPT numbers suppose to produce a NAN
    FPT a = std::numeric_limits<FPT>::infinity();
    FPT b = 0;

    // check adding produced NaN
    BOOST_CHECK(std::isnan(a*b));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_multiplication_invalid_result> c(std::numeric_limits<FPT>::infinity());
    safe_float<FPT, policy::check_multiplication_invalid_result> d(FPT(0));

    // check the multiplication throws
    BOOST_CHECK_THROW(c*d, std::exception);
}

BOOST_AUTO_TEST_SUITE_END()


/*
Copyright 2012, 2013 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Test std::numeric_limits <log_float <...>>.
*/

#define BOOST_TEST_MODULE log_float_limits
#include "../boost_unit_test.hpp"

#include "math/log-float.hpp"

#include <limits>

#include <boost/math/special_functions/fpclassify.hpp>

BOOST_AUTO_TEST_SUITE(test_suite_log_float_limits)

template <class LogFloat, class Underlying> void check_limits() {
    typedef std::numeric_limits <LogFloat> limits;
    typedef std::numeric_limits <Underlying> limits_underlying;

    /* Static properties. */
    static_assert (limits::is_specialized, "");
    static_assert (!limits::is_integer, "");
    static_assert (!limits::is_exact, "");
    static_assert (limits::has_infinity, "");

    static_assert (limits::has_quiet_NaN, "");
    static_assert (limits::has_signaling_NaN, "");
    static_assert (limits::has_denorm == std::denorm_absent, "");
    static_assert (!limits::has_denorm_loss, "");
    static_assert (limits::round_style == std::round_to_nearest, "");

    static_assert (!limits::is_iec559, "");
    static_assert (limits::is_bounded, "");
    static_assert (!limits::is_modulo, "");

    static_assert (!limits::traps, "");
    static_assert (!limits::tinyness_before, "");

    /* Functions. */
    // min.
    {
        auto min = limits::min();
        BOOST_CHECK (min > 0);
        BOOST_CHECK_EQUAL (min.exponent(), limits_underlying::lowest());
    }
    {
        auto max = limits::max();
        BOOST_CHECK (max > 0);
        BOOST_CHECK_EQUAL (max.exponent(), limits_underlying::max());
    }
    {
        // 1+epsilon must have the smallest representable underlying float as
        // its exponent.
        auto epsilon = limits::epsilon();
        decltype (epsilon) one = 1;
        auto one_plus_epsilon = one + epsilon;
        BOOST_CHECK_EQUAL (one_plus_epsilon.exponent(),
            limits_underlying::denorm_min());
    }
    {
        auto infinity = limits::infinity();
        BOOST_CHECK (infinity > 0);
        BOOST_CHECK_EQUAL (infinity.exponent(), limits_underlying::infinity());
    }
    {
        auto quiet_NaN = limits::quiet_NaN();
        BOOST_CHECK (!(quiet_NaN == quiet_NaN));
        using namespace boost::math;
        BOOST_CHECK (isnan (quiet_NaN.exponent()));
    }
    {
        auto signaling_NaN = limits::signaling_NaN();
        BOOST_CHECK (!(signaling_NaN == signaling_NaN));
        using namespace boost::math;
        BOOST_CHECK (isnan (signaling_NaN.exponent()));
    }

    /*
    Not tested because they are not meaningfully implemented:
    digits, digits10, max_digits10, radix
    min_exponent, min_exponent10, max_exponent, max_exponent10
    round_error(), denorm_min()
    */
}

template <class LogFloat, class Underlying> void check_limits_unsigned() {
    check_limits <LogFloat, Underlying>();

    typedef std::numeric_limits <LogFloat> limits;

    static_assert (!limits::is_signed, "");

    BOOST_CHECK_EQUAL (limits::lowest(), limits::min());
}

template <class LogFloat, class Underlying> void check_limits_signed() {
    check_limits <LogFloat, Underlying>();

    typedef std::numeric_limits <LogFloat> limits;

    static_assert (limits::is_signed, "");

    BOOST_CHECK_EQUAL (limits::lowest(), -limits::max());
}

BOOST_AUTO_TEST_CASE (test_log_float_limits) {

    check_limits_unsigned <math::log_float <float>, float>();
    check_limits_unsigned <math::log_float <double> const, double>();
    check_limits_unsigned <math::log_float <double> &, double>();
    check_limits_unsigned <math::log_float <double> const &, double>();

    check_limits_signed <math::signed_log_float <float>, float>();
    check_limits_signed <math::signed_log_float <double> const &, double>();
    check_limits_signed <math::signed_log_float <double> &&, double>();

    // long double does not work under Valgrind.
    // check_limits_signed <
    //     math::signed_log_float <long double>, long double>();
    // check_limits_signed <
    //     math::signed_log_float <long double> const &, long double>();

    using namespace boost::math::policies;
    typedef policy <domain_error <errno_on_error>,
        overflow_error <errno_on_error>> other_policy;

    check_limits_unsigned <math::log_float <float, other_policy>, float>();
    check_limits_unsigned <math::log_float <double, other_policy> &, double>();

    check_limits_signed <math::signed_log_float <float, other_policy>, float>();
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE example

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/test/included/unit_test.hpp>
#include <cmath>

#include "random_tree.h"
#include "test_common.h"

using namespace curfil;
using namespace cuv;

BOOST_AUTO_TEST_SUITE(RandomTreeTest)

BOOST_AUTO_TEST_CASE(testNormalizeHistogramEqualPriorDistribution) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 850;
    histogram[1] = 50;
    histogram[2] = 100;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    for (LabelType label = 0; label < NUM_LABELS; label++) {
        priorDistribution[label] = 100;
    }

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.0);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.850 / 3.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.050 / 3.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.100 / 3.0, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramEqualHistogramUnequalPriorDistribution) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 50;
    histogram[1] = 50;
    histogram[2] = 50;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 80;
    priorDistribution[1] = 10;
    priorDistribution[2] = 10;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.0);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), (1 / 3.0) * 0.8, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), (1 / 3.0) * 0.1, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), (1 / 3.0) * 0.1, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramHighBias) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 50;
    histogram[1] = 50;
    histogram[2] = 50;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 80;
    priorDistribution[1] = 10;
    priorDistribution[2] = 10;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.5);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.0, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramMediumBias) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 60;
    histogram[1] = 20;
    histogram[2] = 20;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 50;
    priorDistribution[1] = 25;
    priorDistribution[2] = 25;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.5);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.5, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.0, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramLowBias) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 20;
    histogram[1] = 40;
    histogram[2] = 40;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 20;
    priorDistribution[1] = 10;
    priorDistribution[2] = 10;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.2);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.5 * 0.25, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.5 * 0.25, 1e-15);
}

BOOST_AUTO_TEST_CASE(testReservoirSampler) {

    size_t sampleSize = 1000;
    const int MAX = 100000;

    RandomSource randomSource(4711);
    Sampler sampler = randomSource.uniformSampler(0, 10 * MAX);

    ReservoirSampler<int> reservoirSampler(sampleSize);
    for (int i = 0; i < MAX; i++) {
        reservoirSampler.sample(sampler, i);
    }

    const auto& reservoir = reservoirSampler.getReservoir();
    BOOST_REQUIRE_EQUAL(reservoir.size(), sampleSize);

    boost::accumulators::accumulator_set<int,
            boost::accumulators::features<
                    boost::accumulators::tag::min,
                    boost::accumulators::tag::max,
                    boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance> > acc;

    acc = std::for_each(reservoir.begin(), reservoir.end(), acc);

    double min = boost::accumulators::min(acc);
    double max = boost::accumulators::max(acc);
    double mean = boost::accumulators::mean(acc);
    double stddev = std::sqrt(static_cast<double>(boost::accumulators::variance(acc)));

    /*
     * values are empirically determined with python and numpy
     *
     * >>> import random, numpy as np
     * >>> a = np.arange(100000)
     * >>> random.shuffle(a)
     * >>> b = a[:10000]
     * >>> b.mean()
     * 50271.870999999999
     *
     * >>> np.sqrt(b.var())
     * 28804.962206705768
     *
     * >>> b.min()
     * 12.0
     *
     * >>> b.max()
     * 99996.0
     */
    BOOST_REQUIRE_LT(min, 0.01 * MAX);
    BOOST_REQUIRE_GT(max, 0.99 * MAX);
    BOOST_REQUIRE_CLOSE(mean, 50000.0, 0.05);
    BOOST_REQUIRE_CLOSE(stddev, 28000.0, 2.0);
}
BOOST_AUTO_TEST_SUITE_END()


#include <util_tensors.h>
#include <util_coo_matrix.h>
#include <glue_triplet.h>
#include <glue_matrix_assembly.h>

#include <vector>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

/**
 * This assembles a test problem on a small example mesh. The mesh consist
 * of four vertices and three triangles. The element matrices is chosen such
 * that it is quite simple to verify if the assembled matrix is correct or not.
 */
inline util::COOMatrix<double> do_assembly_of_tensor2_test_problem( bool const & interlaced )
{
  std::vector<glue::Triplet> triangles;

  triangles.push_back( glue::make_triplet(0u, 1u, 2u) );
  triangles.push_back( glue::make_triplet(0u, 2u, 3u) );
  triangles.push_back( glue::make_triplet(0u, 3u, 1u) );

  unsigned int const V = 4u;  // We got 4 vertices

  std::vector<util::Block3x3Tensor2> Ae;

  Ae.resize(3u); // We got 3 triangles

  util::Block3x3Tensor2 E;

  // Element matrix is the same for all elements
  //
  //         1 0 2 0 4 0
  //         0 1 0 2 0 4
  //         2 0 1 0 3 0
  //   E =   0 2 0 1 0 3
  //         4 0 3 0 1 0
  //         0 4 0 3 0 1
  //
  E.m_block[0][0] = util::identity();
  E.m_block[1][1] = util::identity();
  E.m_block[2][2] = util::identity();
  E.m_block[0][1] = util::mul( 2.0, util::identity() );
  E.m_block[0][2] = util::mul( 4.0, util::identity() );
  E.m_block[1][2] = util::mul( 3.0, util::identity() );
  E.m_block[1][0] = E.m_block[0][1];
  E.m_block[2][0] = E.m_block[0][2];
  E.m_block[2][1] = E.m_block[1][2];

  Ae[0] = E;
  Ae[1] = E;
  Ae[2] = E;

  // Do the assembley, the "interlaced" results should be
  //
  //  3     0     6     0     6     0     6     0
  //  0     3     0     6     0     6     0     6
  //  6     0     2     0     3     0     3     0
  //  0     6     0     2     0     3     0     3
  //  6     0     3     0     2     0     3     0
  //  0     6     0     3     0     2     0     3
  //  6     0     3     0     3     0     2     0
  //  0     6     0     3     0     3     0     2
  //
  // and non-interlaced should be
  //
  //  3     6     6     6     0     0     0     0
  //  6     2     3     3     0     0     0     0
  //  6     3     2     3     0     0     0     0
  //  6     3     3     2     0     0     0     0
  //  0     0     0     0     3     6     6     6
  //  0     0     0     0     6     2     3     3
  //  0     0     0     0     6     3     2     3
  //  0     0     0     0     6     3     3     2

  util::COOMatrix<double> A = glue::matrix_assembly<double>( triangles, V, Ae, interlaced );

  std::cout << util::to_string(A) << std::endl;

  return A;
}

/**
 * Specific for tensor1 data using cell-centered variables
 */
inline util::COOMatrix<double> do_assembly_of_tensor1_test_problem( bool const & interlaced )
{
  std::vector<glue::Triplet> triangles;

  triangles.push_back( glue::make_triplet(0u, 1u, 2u) );
  triangles.push_back( glue::make_triplet(0u, 2u, 3u) );
  triangles.push_back( glue::make_triplet(0u, 3u, 1u) );

  unsigned int const V = 4u;  // We got 4 vertices

  std::vector<util::Block3x1Tensor1> Ae;

  Ae.resize(3u); // We got 3 triangles

  util::Block3x1Tensor1 E;

  E.m_block[0] = util::make(1.0, 1.0);
  E.m_block[1] = util::make(1.0, 1.0);
  E.m_block[2] = util::make(1.0, 1.0);

  Ae[0] = E;
  Ae[1] = E;
  Ae[2] = E;

  // Do the assembley, the "interlaced" results should be
  //
  //  1     1     1
  //  1     1     1
  //  1     0     1
  //  1     0     1
  //  1     1     0
  //  1     1     0
  //  0     1     1
  //  0     1     1
  //
  // and non-interlaced should be
  //
  //  1     1     1
  //  1     0     1
  //  1     1     0
  //  0     1     1
  //  1     1     1
  //  1     0     1
  //  1     1     0
  //  0     1     1

  util::COOMatrix<double> A = glue::matrix_assembly<double>( triangles, V, Ae, interlaced );

  std::cout << util::to_string(A) << std::endl;

  return A;
}

/**
 * Specific for tensor0 data using vertex variables
 */
inline util::COOMatrix<double> do_assembly_of_tensor0_test_problem( )
{
  std::vector<glue::Triplet> triangles;

  triangles.push_back( glue::make_triplet(0u, 1u, 2u) );
  triangles.push_back( glue::make_triplet(0u, 2u, 3u) );
  triangles.push_back( glue::make_triplet(0u, 3u, 1u) );

  unsigned int const V = 4u;  // We got 4 vertices

  std::vector<util::Block3x3Tensor0> Ae;

  Ae.resize(3u); // We got 3 triangles

  util::Block3x3Tensor0 E;

  // Element matrix is the same for all elements
  //
  //         1 2 4
  //         2 1 3
  //   E =   4 3 1
  //
  E.m_block[0][0] = 1.0;
  E.m_block[1][1] = 1.0;
  E.m_block[2][2] = 1.0;
  E.m_block[0][1] = 2.0;
  E.m_block[0][2] = 4.0;
  E.m_block[1][2] = 3.0;
  E.m_block[1][0] = E.m_block[0][1];
  E.m_block[2][0] = E.m_block[0][2];
  E.m_block[2][1] = E.m_block[1][2];

  Ae[0] = E;
  Ae[1] = E;
  Ae[2] = E;

  // Do the assembley, the results should be
  //
  //
  //  3     6     6     6
  //  6     2     3     3
  //  6     3     2     3
  //  6     3     3     2

  util::COOMatrix<double> A = glue::matrix_assembly<double>( triangles, V, Ae );

  std::cout << util::to_string(A) << std::endl;

  return A;
}

BOOST_AUTO_TEST_SUITE(coo_matrix);

BOOST_AUTO_TEST_CASE(assemble_tensor2_matrix_interlaced)
{
  util::COOMatrix<double> const A = do_assembly_of_tensor2_test_problem( true );

  BOOST_CHECK_EQUAL(A.row_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[1], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[2], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[3], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[4], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[5], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[6], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[7], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[8], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[9], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[10], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[11], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[12], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[13], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[14], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[15], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[16], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[17], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[18], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[19], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[20], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[21], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[22], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[23], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[24], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[25], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[26], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[27], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[28], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[29], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[30], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[31], 7u);

  BOOST_CHECK_EQUAL(A.column_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[1], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[2], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[3], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[4], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[5], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[6], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[7], 7u);
  BOOST_CHECK_EQUAL(A.column_indices()[8], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[9], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[10], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[11], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[12], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[13], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[14], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[15], 7u);
  BOOST_CHECK_EQUAL(A.column_indices()[16], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[17], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[18], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[19], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[20], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[21], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[22], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[23], 7u);
  BOOST_CHECK_EQUAL(A.column_indices()[24], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[25], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[26], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[27], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[28], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[29], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[30], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[31], 7u);

  BOOST_CHECK_CLOSE(A.values()[0], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[1], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[2], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[3], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[4], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[5], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[6], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[7], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[8], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[9], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[10], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[11], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[12], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[13], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[14], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[15], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[16], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[17], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[18], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[19], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[20], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[21], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[22], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[23], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[24], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[25], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[26], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[27], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[28], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[29], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[30], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[31], 2.0, 0.01);

}

BOOST_AUTO_TEST_CASE(assemble_tensor2_matrix_non_interlaced)
{
  util::COOMatrix<double> const A = do_assembly_of_tensor2_test_problem( false );

  BOOST_CHECK_EQUAL(A.row_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[1], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[2], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[3], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[4], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[5], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[6], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[7], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[8], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[9], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[10], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[11], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[12], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[13], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[14], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[15], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[16], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[17], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[18], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[19], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[20], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[21], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[22], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[23], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[24], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[25], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[26], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[27], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[28], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[29], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[30], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[31], 7u);

  BOOST_CHECK_EQUAL(A.column_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[1], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[2], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[3], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[4], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[5], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[6], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[7], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[8], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[9], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[10], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[11], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[12], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[13], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[14], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[15], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[16], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[17], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[18], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[19], 7u);
  BOOST_CHECK_EQUAL(A.column_indices()[20], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[21], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[22], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[23], 7u);
  BOOST_CHECK_EQUAL(A.column_indices()[24], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[25], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[26], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[27], 7u);
  BOOST_CHECK_EQUAL(A.column_indices()[28], 4u);
  BOOST_CHECK_EQUAL(A.column_indices()[29], 5u);
  BOOST_CHECK_EQUAL(A.column_indices()[30], 6u);
  BOOST_CHECK_EQUAL(A.column_indices()[31], 7u);

  BOOST_CHECK_CLOSE(A.values()[0], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[1], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[2], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[3], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[4], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[5], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[6], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[7], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[8], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[9], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[10], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[11], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[12], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[13], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[14], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[15], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[16], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[17], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[18], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[19], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[20], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[21], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[22], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[23], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[24], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[25], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[26], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[27], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[28], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[29], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[30], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[31], 2.0, 0.01);

}

BOOST_AUTO_TEST_CASE(assemble_tensor0_matrix)
{
  util::COOMatrix<double> const A = do_assembly_of_tensor0_test_problem();

  BOOST_CHECK_EQUAL(A.row_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[1], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[2], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[3], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[4], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[5], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[6], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[7], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[8], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[9], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[10], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[11], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[12], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[13], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[14], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[15], 3u);

  BOOST_CHECK_EQUAL(A.column_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[1], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[2], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[3], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[4], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[5], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[6], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[7], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[8], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[9], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[10], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[11], 3u);
  BOOST_CHECK_EQUAL(A.column_indices()[12], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[13], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[14], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[15], 3u);

  BOOST_CHECK_CLOSE(A.values()[0], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[1], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[2], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[3], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[4], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[5], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[6], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[7], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[8], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[9], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[10], 2.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[11], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[12], 6.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[13], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[14], 3.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[15], 2.0, 0.01);

}

BOOST_AUTO_TEST_CASE(assemble_tensor1_matrix_interlaced)
{
  util::COOMatrix<double> const A = do_assembly_of_tensor1_test_problem( true );

  BOOST_CHECK_EQUAL(A.row_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[1], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[2], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[3], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[4], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[5], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[6], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[7], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[8], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[9], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[10], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[11], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[12], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[13], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[14], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[15], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[16], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[17], 7u);

  BOOST_CHECK_EQUAL(A.column_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[1], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[2], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[3], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[4], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[5], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[6], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[7], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[8], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[9], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[10], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[11], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[12], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[13], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[14], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[15], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[16], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[17], 2u);

  BOOST_CHECK_CLOSE(A.values()[0], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[1], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[2], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[3], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[4], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[5], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[6], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[7], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[8], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[9], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[10], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[11], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[12], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[13], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[14], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[15], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[16], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[17], 1.0, 0.01);

}

BOOST_AUTO_TEST_CASE(assemble_tensor1_matrix_non_interlaced)
{
  util::COOMatrix<double> const A = do_assembly_of_tensor1_test_problem( false );

  BOOST_CHECK_EQUAL(A.row_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[1], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[2], 0u);
  BOOST_CHECK_EQUAL(A.row_indices()[3], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[4], 1u);
  BOOST_CHECK_EQUAL(A.row_indices()[5], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[6], 2u);
  BOOST_CHECK_EQUAL(A.row_indices()[7], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[8], 3u);
  BOOST_CHECK_EQUAL(A.row_indices()[9], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[10], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[11], 4u);
  BOOST_CHECK_EQUAL(A.row_indices()[12], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[13], 5u);
  BOOST_CHECK_EQUAL(A.row_indices()[14], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[15], 6u);
  BOOST_CHECK_EQUAL(A.row_indices()[16], 7u);
  BOOST_CHECK_EQUAL(A.row_indices()[17], 7u);

  BOOST_CHECK_EQUAL(A.column_indices()[0], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[1], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[2], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[3], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[4], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[5], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[6], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[7], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[8], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[9], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[10], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[11], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[12], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[13], 2u);
  BOOST_CHECK_EQUAL(A.column_indices()[14], 0u);
  BOOST_CHECK_EQUAL(A.column_indices()[15], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[16], 1u);
  BOOST_CHECK_EQUAL(A.column_indices()[17], 2u);

  BOOST_CHECK_CLOSE(A.values()[0], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[1], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[2], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[3], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[4], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[5], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[6], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[7], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[8], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[9], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[10], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[11], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[12], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[13], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[14], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[15], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[16], 1.0, 0.01);
  BOOST_CHECK_CLOSE(A.values()[17], 1.0, 0.01);

}

BOOST_AUTO_TEST_SUITE_END();


#include "stdafx.h"

#include "problem.hpp"

#include <fstream>

#include <boost/algorithm/string.hpp>

struct advent_2017_2 : problem
{
	advent_2017_2() noexcept : problem(2017, 2) {
	}

	using row         = std::vector<std::size_t>;
	using spreadsheet = std::vector<row>;

protected:
	spreadsheet ss;

	void prepare_input(std::ifstream& fin) override {
		//auto const ss = spreadsheet(
		//        input | ranges::view::split(is_char('\n'))
		//              | ranges::view::transform([](const std::string& line) -> row {
		//                return line | ranges::view::split(is_char('\t'))
		//                            | ranges::view::transform(ranges::convert_to<std::size_t>{});
		//                })
		//);

		for(std::string line; std::getline(fin, line);) {
			std::vector<std::string> cells;
			boost::split(cells, line, [](char c) { return c == '\t'; });
			row r;
			std::transform(begin(cells), end(cells), std::back_inserter(r), [](const std::string& s) { return std::stoull(s); });
			ss.push_back(r);
		}
	}

	std::string part_1() override {
		std::size_t running_checksum = 0;
		for(const row& r : ss) {
			const auto mm = std::minmax_element(begin(r), end(r));
			running_checksum += *mm.second - *mm.first;
		}
		return std::to_string(running_checksum);
	}

	std::string part_2() override {
		std::size_t running_sum = 0;
		for(const row& r : ss) {
			const std::size_t end = r.size();
			for(size_t i = 0; i != end; ++i) {
				for(size_t j = 0; j != end; ++j) {
					if(i == j) {
						continue;
					}
					if(r[i] % r[j] == 0) {
						running_sum += r[i] / r[j];
					}
				}
			}
		}
		return std::to_string(running_sum);
	}
};

REGISTER_SOLVER(2017, 2);


//
//=======================================================================
// Copyright 2002 Marc Wintermantel (wintermantel@imes.mavt.ethz.ch)
// ETH Zurich, Center of Structure Technologies (www.imes.ethz.ch/st)
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//

#include <boost/config.hpp>
#include <vector>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/sloan_ordering.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/bandwidth.hpp>
#include <boost/graph/profile.hpp>
#include <boost/graph/wavefront.hpp>

using std::cout;
using std::endl;

/*
  Sample Output
  #####################################
  ### First light of sloan-ordering ###
  #####################################

  original bandwidth: 8
  original profile: 42
  original max_wavefront: 7
  original aver_wavefront: 4.2
  original rms_wavefront: 4.58258

  Starting vertex: 0
  Pseudoperipheral vertex: 9
  Pseudoperipheral radius: 4

  Sloan ordering starting at: 0
    0 8 3 7 5 2 4 6 1 9
    bandwidth: 4
    profile: 28
    max_wavefront: 4
    aver_wavefront: 2.8
    rms_wavefront: 2.93258

  Sloan ordering without a start-vertex:
    8 0 3 7 5 2 4 6 1 9
    bandwidth: 4
    profile: 27
    max_wavefront: 4
    aver_wavefront: 2.7
    rms_wavefront: 2.84605

  ###############################
  ### sloan-ordering finished ###
  ###############################
*/

int main(int , char* [])
{
  cout << endl;
  cout << "#####################################" << endl;
  cout << "### First light of sloan-ordering ###" << endl;
  cout << "#####################################" << endl << endl;

  using namespace boost;
  using namespace std;

  //Defining the graph type
  typedef adjacency_list<
    setS,
    vecS,
    undirectedS,
    property<
    vertex_color_t,
    default_color_type,
    property<
    vertex_degree_t,
    int,
    property<
    vertex_priority_t,
    double > > > > Graph;

  typedef graph_traits<Graph>::vertex_descriptor Vertex;
  typedef graph_traits<Graph>::vertices_size_type size_type;

  typedef std::pair<std::size_t, std::size_t> Pair;

  Pair edges[14] = { Pair(0,3), //a-d
                     Pair(0,5),  //a-f
                     Pair(1,2),  //b-c
                     Pair(1,4),  //b-e
                     Pair(1,6),  //b-g
                     Pair(1,9),  //b-j
                     Pair(2,3),  //c-d
                     Pair(2,4),  //c-e
                     Pair(3,5),  //d-f
                     Pair(3,8),  //d-i
                     Pair(4,6),  //e-g
                     Pair(5,6),  //f-g
                     Pair(5,7),  //f-h
                     Pair(6,7) }; //g-h

  //Creating a graph and adding the edges from above into it
  Graph G(10);
  for (int i = 0; i < 14; ++i)
    add_edge(edges[i].first, edges[i].second, G);

  //Creating two iterators over the vertices
  graph_traits<Graph>::vertex_iterator ui, ui_end;

  //Creating a property_map with the degrees of the degrees of each vertex
  property_map<Graph,vertex_degree_t>::type deg = get(vertex_degree, G);
  for (boost::tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui)
    deg[*ui] = degree(*ui, G);

  //Creating a property_map for the indices of a vertex
  property_map<Graph, vertex_index_t>::type index_map = get(vertex_index, G);

  std::cout << "original bandwidth: " << bandwidth(G) << std::endl;
  std::cout << "original profile: " << profile(G) << std::endl;
  std::cout << "original max_wavefront: " << max_wavefront(G) << std::endl;
  std::cout << "original aver_wavefront: " << aver_wavefront(G) << std::endl;
  std::cout << "original rms_wavefront: " << rms_wavefront(G) << std::endl;

  //Creating a vector of vertices
  std::vector<Vertex> sloan_order(num_vertices(G));
  //Creating a vector of size_type
  std::vector<size_type> perm(num_vertices(G));

  {

    //Setting the start node
    Vertex s = vertex(0, G);
    int ecc;   //defining a variable for the pseudoperipheral radius

    //Calculating the pseudoeperipheral node and radius
    Vertex e = pseudo_peripheral_pair(G, s, ecc, get(vertex_color, G), get(vertex_degree, G) );

    cout << endl;
    cout << "Starting vertex: " << s << endl;
    cout << "Pseudoperipheral vertex: " << e << endl;
    cout << "Pseudoperipheral radius: " << ecc << endl << endl;

    //Sloan ordering
    sloan_ordering(G, s, e, sloan_order.begin(), get(vertex_color, G),
                           get(vertex_degree, G), get(vertex_priority, G));

    cout << "Sloan ordering starting at: " << s << endl;
    cout << "  ";

    for (std::vector<Vertex>::const_iterator i = sloan_order.begin();
         i != sloan_order.end(); ++i)
      cout << index_map[*i] << " ";
    cout << endl;

    for (size_type c = 0; c != sloan_order.size(); ++c)
      perm[index_map[sloan_order[c]]] = c;
    std::cout << "  bandwidth: "
              << bandwidth(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  profile: "
              << profile(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  max_wavefront: "
              << max_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  aver_wavefront: "
              << aver_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
    std::cout << "  rms_wavefront: "
              << rms_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
              << std::endl;
  }

    /////////////////////////////////////////////////
    //Version including finding a good starting point
    /////////////////////////////////////////////////

    {
      //sloan_ordering
      sloan_ordering(G, sloan_order.begin(),
                        get(vertex_color, G),
                        make_degree_map(G),
                        get(vertex_priority, G) );

      cout << endl << "Sloan ordering without a start-vertex:" << endl;
      cout << "  ";
      for (std::vector<Vertex>::const_iterator i=sloan_order.begin();
           i != sloan_order.end(); ++i)
        cout << index_map[*i] << " ";
      cout << endl;

      for (size_type c = 0; c != sloan_order.size(); ++c)
        perm[index_map[sloan_order[c]]] = c;
      std::cout << "  bandwidth: "
                << bandwidth(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  profile: "
                << profile(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  max_wavefront: "
                << max_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  aver_wavefront: "
                << aver_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
      std::cout << "  rms_wavefront: "
                << rms_wavefront(G, make_iterator_property_map(&perm[0], index_map, perm[0]))
                << std::endl;
    }

  cout << endl;
  cout << "###############################" << endl;
  cout << "### sloan-ordering finished ###" << endl;
  cout << "###############################" << endl << endl;
  return 0;

}


/**
 * @file orientation.cc
 */
#include <usml/sensors/orientation.h>
#include <boost/numeric/ublas/io.hpp>

using namespace usml::sensors ;

/**
 * Default constructor
 */
orientation::orientation()
    : _heading(0.0), _pitch(0.0), _roll(0.0),
      _axis(3,0)
{

}

/**
 * Pitch, heading, roll constructor
 */
orientation::orientation(
    double heading, double pitch, double roll,
    vector<double> ref_axis )
    : _heading(-pitch*M_PI/180.0),
      _pitch(-heading*M_PI/180.0),
      _roll(roll*M_PI/180.0),
      _axis(ref_axis)
{
    apply_rotation() ;
}

/**
 * Tilt angle/direction constructor
 */
orientation::orientation( double angle, double direction )
    : _heading(0.0), _pitch(0.0), _roll(0.0),
      _axis(3,0)
{
    compute_orientation(angle,direction) ;
}

/**
 * Destructor
 */
orientation::~orientation()
{

}

/**
 * Computes pitch, heading, and roll as a function angle/direction from
 * the vertical reference axis
 */
void orientation::compute_orientation(double angle, double direction)
{
    if( angle <= M_PI_2 ) {
        double sin_theta2 = sin(angle)*sin(angle) ;
        double sin_phi2 = sin(direction)*sin(direction) ;
        double sqrt_theta_phi = std::sqrt(1 - sin_theta2*sin_phi2) ;
        _roll = std::atan2( (cos(angle) / sqrt_theta_phi) , (cos(direction)*sin(angle) / sqrt_theta_phi) ) ;
        _pitch = std::atan2( sqrt_theta_phi, (-sin(angle)*sin(direction)) ) ;
        _heading = 0.0 ;
    } else {
        _pitch = angle ;
        _heading = direction ;
        _roll = 0.0 ;
    }
}

/**
 * Applies a rotation from one coordinate system to the
 * current rotated coordinates for asymmetric systems.
 */
void orientation::apply_rotation(
    double& de, double& az,
    double* de_prime, double* az_prime )
{
    _theta = M_PI_2 - de ;
    _phi = az ;
    _axis(0) = sin(_theta) * cos(_phi) ;
    _axis(1) = sin(_theta) * sin(_phi) ;
    _axis(2) = cos(_theta) ;
    apply_rotation() ;
    *de_prime = M_PI_2 - _theta ;
    *az_prime = _phi ;
}

/**
 * Updates the heading, pitch, and roll
 */
void orientation::update_orientation( double h, double p, double r )
{
   _heading = -h*M_PI/180.0 ;
   _pitch = -p*M_PI/180.0 ;
   _roll = r*M_PI/180.0 ;
   apply_rotation() ;
}

/**
* Updates the tilt angle and direction.
*/
void orientation::update_orientation(double angle, double direction)
{
    compute_orientation(angle,direction) ;
}

/**
 * Applies a rotation from one coordinate system to the
 * current rotated coordinates for symmetric systems.
 */
void orientation::apply_rotation()
{
    _x = _axis(0)*cos(_heading)*cos(_roll) +
         _axis(2)*( sin(_heading)*sin(_pitch) + cos(_heading)*cos(_pitch)*sin(_roll) ) +
         _axis(1)*( -cos(_pitch)*sin(_heading) + cos(_heading)*sin(_pitch)*sin(_roll) ) ;
    _y = _axis(0)*cos(_roll)*sin(_heading) +
         _axis(2)*( -cos(_heading)*sin(_pitch) + cos(_pitch)*sin(_heading)*sin(_roll) ) +
         _axis(1)*( cos(_heading)*cos(_pitch) + sin(_heading)*sin(_pitch)*sin(_roll) ) ;
    _z = _axis(2)*cos(_pitch)*cos(_roll) +
         _axis(1)*cos(_roll)*sin(_pitch) -
         _axis(0)*sin(_roll) ;
    convert_to_spherical() ;
}

/**
 * Converts the store vector in spherical coordinates to
 * cartesian coordinates.
 */
void orientation::convert_to_cartesian()
{
    _x = sin(_theta) * cos(_phi) ;
    _y = sin(_theta) * sin(_phi) ;
    _z = cos(_theta) ;
}

/**
 * Converts the stored vector in cartesian coordinates to
 * spherical coordinates.
 */
void orientation::convert_to_spherical()
{
//    double rho = std::sqrt( _x*_x + _y*_y + _z*_z ) ;
    _theta = std::acos( _z - 1e-10 ) ;
    _theta = std::fmod( _theta, M_PI ) ;
    _phi = std::atan2( _y, _x ) ;
    _phi = std::fmod( _phi, 2.0*M_PI ) ;
}


/**
 * Copyright 2020, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Jingnan Shi, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 */

#include "gtest/gtest.h"

#include <iostream>
#include <iomanip>
#include <fstream>
#include <chrono>
#include <random>

#include <Eigen/Eigenvalues>

#include "teaser/registration.h"
#include "teaser/macros.h"
#include "test_utils.h"

TEST(ScaleSolverTest, UnknownScale) {
  double ACCEPTABLE_ERROR = 1e-5;

  // Read in data
  std::ifstream objectFile("./data/registration_test/objectIn.csv");
  auto object_points = teaser::test::readFileToEigenMatrix<double, 3, Eigen::Dynamic>(objectFile);

  // Problem 1: No scaling
  {
    // Prepare parameters & solver
    double noise_bound = 1; // arbitrary
    int cbar2 = 1;          // arbitrary
    teaser::TLSScaleSolver solver(noise_bound, cbar2);

    // Solve for scale
    double actual_scale = 0;
    double expected_scale = 1;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    solver.solveForScale(object_points, object_points, &actual_scale, &actual_inliers);

    // Compare with expected values
    EXPECT_NEAR(expected_scale, actual_scale, ACCEPTABLE_ERROR);
  }
  // Problem 2: Random scaling
  {
    // Prepare parameters & solver
    double noise_bound = 1; // arbitrary
    int cbar2 = 1;          // arbitrary
    teaser::TLSScaleSolver solver(noise_bound, cbar2);

    // Scaling input points by a random value
    std::uniform_real_distribution<double> unif(0, 5);
    std::default_random_engine re;
    double expected_scale = unif(re);
    Eigen::Matrix<double, 3, Eigen::Dynamic> scaled_points = object_points.array() * expected_scale;

    // Solve for scale
    double actual_scale = 0;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    solver.solveForScale(object_points, scaled_points, &actual_scale, &actual_inliers);

    // Compare with expected values
    EXPECT_NEAR(expected_scale, actual_scale, ACCEPTABLE_ERROR);
  }
}

TEST(ScaleSolverTest, FixedScale) {
  // Read in data
  std::ifstream objectFile("./data/registration_test/objectIn.csv");
  auto object_points = teaser::test::readFileToEigenMatrix<double, 3, Eigen::Dynamic>(objectFile);

  // Problem 1: No outliers
  {
    double noise_bound = 1; // arbitrary
    int cbar2 = 1;          // arbitrary
    teaser::ScaleInliersSelector solver(noise_bound, cbar2);

    double actual_scale = 0;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    solver.solveForScale(object_points, object_points, &actual_scale, &actual_inliers);

    EXPECT_EQ(actual_scale, 1);
    for (size_t i = 0; i < actual_inliers.cols(); ++i) {
      EXPECT_TRUE(actual_inliers(0, i));
    }
  }
  // Problem 2: All outliers
  {
    double noise_bound = 1; // arbitrary
    int cbar2 = 1;          // arbitrary
    // shift & scale the points so all points will be outliers
    Eigen::Matrix<double, 3, Eigen::Dynamic> shifted_object = object_points.array() * 3 + 10;
    teaser::ScaleInliersSelector solver(noise_bound, cbar2);

    double actual_scale = 0;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    solver.solveForScale(object_points, shifted_object, &actual_scale, &actual_inliers);

    EXPECT_EQ(actual_scale, 1);
    for (size_t i = 0; i < actual_inliers.cols(); ++i) {
      EXPECT_FALSE(actual_inliers(0, i));
    }
  }
  // Problem 3: One outlier
  {
    double noise_bound = 1; // arbitrary
    int cbar2 = 1;          // arbitrary
    // shift & scale the points so all points will be outliers
    Eigen::Matrix<double, 3, Eigen::Dynamic> shifted_object = object_points.array();
    shifted_object.col(0).array() *= 10;
    teaser::ScaleInliersSelector solver(noise_bound, cbar2);

    double actual_scale = 0;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    solver.solveForScale(object_points, shifted_object, &actual_scale, &actual_inliers);

    EXPECT_EQ(actual_scale, 1);
    EXPECT_FALSE(actual_inliers(0, 0));
    for (size_t i = 1; i < actual_inliers.cols(); ++i) {
      EXPECT_TRUE(actual_inliers(0, i));
    }
  }
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "EntropyRegularizedBinaryPhrase"

#include <cmath>
#include <algorithm>
#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;

#include "diffdp/algorithm/binary_phrase.h"

// using boost test with intolerance fails (too precise),
// so let's just use the same test as in Dynet.
bool check_grad(float g, float g_act)
{
    float f = std::fabs(g - g_act);
    float m = std::max(std::fabs(g), std::fabs(g_act));
    if (f > 0.01 && m > 0.f)
        f /= m;

    if (f > 0.01 || std::isnan(f))
        return false;
    else
        return true;
}

BOOST_AUTO_TEST_CASE(first_order_gradient, * utf::tolerance(1e-2f))
{
    const unsigned size = 10;
    const float sensitivity = 1e-2;

    std::vector<float> weights(size * size);
    for (unsigned i = 0 ; i < size ; ++i)
        weights.at(i) = size;

    diffdp::EntropyRegularizedBinaryPhraseStructure parser(size);
        parser.forward(
        [&] (unsigned left, unsigned right) -> float
        {
            return weights.at(left + right * size);
        }
    );

    for (unsigned left = 0 ; left < size ; ++left)
    {
        for (unsigned right = left + 1; right < size ; ++right)
        {
            const float computed_arc = parser.output(left, right);

            // estimate the gradient
            const float original_weights = weights.at(left + right * size);

            weights.at(left + right * size) = original_weights + sensitivity;
            diffdp::EntropyRegularizedBinaryPhraseStructure parser2(size);
                parser2.forward(
                [&] (const unsigned left, const unsigned right) -> float
                {
                    return weights.at(left + right * size);
                }
            );
            const float output_a = parser2.chart_forward->weight(0, size-1);

            weights.at(left + right * size) = original_weights - sensitivity;
            parser2.forward(
            [&] (const unsigned left, const unsigned right) -> float
                {
                    return weights.at(left + right * size);
                }
            );
            const float output_b = parser2.chart_forward->weight(0, size-1);

            // restore
            weights.at(left + right * size) = original_weights;

            const float estimated_arc = (output_a - output_b) / (2.f * sensitivity);

            BOOST_CHECK(check_grad(computed_arc, estimated_arc));
        }
    }
}

BOOST_AUTO_TEST_CASE(second_order_gradient, * utf::tolerance(1e-2f))
{
    const unsigned size = 10;
    const float sensitivity = 1e-5;

    std::vector<float> weights(size * size);
    for (unsigned i = 0 ; i < size ; ++i)
    weights.at(i) = size;

    diffdp::EntropyRegularizedBinaryPhraseStructure parser(size);
    parser.forward(
        [&] (unsigned left, unsigned right) -> float
        {
            return weights.at(left + right * size);
        }
    );

    for (unsigned input_left = 0 ; input_left < size ; ++input_left)
    {
        for (unsigned input_right = input_left + 1; input_right < size ; ++input_right)
        {
            for (unsigned output_left = 0 ; output_left < size ; ++output_left)
            {
                for (unsigned output_right = output_left + 1; output_right < size; ++output_right)
                {
                    parser.backward(
                        [&](const unsigned left, const unsigned right)
                        {
                            if (left == output_left && right == output_right)
                                return 1.f;
                            else
                                return 0.f;
                        }
                    );
                    const float computed_gradient = parser.gradient(input_left, input_right);

                    // estimate the gradient
                    const float sensitivity = 1e-3;
                    const float original_weights = weights.at(input_left + input_right * size);

                    weights.at(input_left + input_right * size) = original_weights + sensitivity;
                        diffdp::EntropyRegularizedBinaryPhraseStructure parser2(size);
                        parser2.forward(
                        [&](const unsigned left, const unsigned right) -> float
                        {
                            return weights.at(left + right * size);
                        }
                    );
                    const float output_a = parser2.output(output_left, output_right);

                    weights.at(input_left + input_right * size) = original_weights - sensitivity;
                        parser2.forward(
                        [&](const unsigned left, const unsigned right) -> float
                        {
                            return weights.at(left + right * size);
                        }
                    );
                    const float output_b = parser2.output(output_left, output_right);

                    // restore
                    weights.at(input_left + input_right * size) = original_weights;

                    const double estimated_gradient = (output_a - output_b) / (2.f * sensitivity);
                    BOOST_CHECK(check_grad(computed_gradient, estimated_gradient));
                }
            }
        }
    }
}