// fixed_point.hpp
//
// Copyright (c) 2016 Piotr K. Semenov (piotr.k.semenov at gmail dot com)
// Distributed under the New BSD License. (See accompanying file LICENSE)

/*!
 \file fixed_point.hpp

 \brief Provides the fixed-point numbers of any format and CORDIC-based
 overloading of STD math as well.
*/

#ifndef INC_LIBQ_FIXED_POINT_HPP_
#define INC_LIBQ_FIXED_POINT_HPP_

#include <boost/integer.hpp>
#include <boost/integer/integer_mask.hpp>

#include <cstdint>
#include <cmath>
#include <limits>
#include <exception>
#include <type_traits>

#include "arithmetics_safety.hpp"
#include "type_promotion.hpp"

#ifndef IMPLICIT_COPY_CTR
#define COPY_CTR_EXPLICIT_SPECIFIER explicit
#else
#define COPY_CTR_EXPLICIT_SPECIFIER
#endif

namespace libq {
namespace details {
    double exp2(double _val) {
#if defined(_MSC_VER)
        return std::exp2(_val);
#elif defined(__GNUC__)
        return std::pow(2.0, _val);
#endif
    }
}  // details

/*!
 \brief Gets the reference to the stored integer behind the fixed-point number
 \param[in] _x the fixed-point number
*/
template<typename T, std::size_t n, std::size_t f, int e, class ... Ps>
T& lift(fixed_point<T, n, f, e, Ps...>& _x) {  // NOLINT
    return _x.m_value;
}

/*!
 \brief Gets the value of the stored integer behind the fixed-point number
 \param[in] _x the fixed-point number
*/
template<typename T, std::size_t n, std::size_t f, int e, class ... Ps>
T const lift(fixed_point<T, n, f, e, Ps...> const& _x) {
    return _x.m_value;
}

/*!
 \brief Implements the fixed-point number arithmetics. Note, this extends the
 Q-formats like UQn.f, Qn.f with fixed pre-scaling factor \f$2^e\f$.
 \tparam value_type Built-in integral type to represent a fixed-point number.
 \tparam n Number of integral bits.
 \tparam f Number of fractional bits.
 \remark Note, \f$n\f$ and \f$f\f$ exclude the sign bit. So if storage_type is
 signed then total number of bits is \f$(n + f + 1)\f$.
 \remark Note, the supremum of \f$(n + f)\f$ is
 std::numeric_limits<std::uintmax_t>::digits in case of the unsigned numbers
 and std::numeric_limits<std::intmax_t>::digits in case of the signed numbers.
 \tparam e Exponent of the pre-scaling factor \f$2^e\f$.
 \tparam op Policy class specifying the actions to do if overflow occurred.
 \tparam up Policy class specifying the actions to do if underflow occurred.

 Usage

 Example 1: Flexible switch between the floating-point calculations and
 fixed precision calculations while algorithm staying the same.
 \code{.cpp}
    #include "fixed_point.hpp"
    #include <iostream>
    #include <cstdlib>

    namespace floating_point {
        using value_type = double;
    };  // namespace floating_point
    namespace fixed_precision {
        using value_type = libq::Q<30, 20>;
    };  // namespace fixed_precision
    using value_type = fixed_precision::value_type;

    int main(int, char**) {
        value_type input{ 0.0 };
        std::cin >> input;

        value_type const result = your_algorithm_here(input);
        std::cout << result << std::endl;

        return EXIT_SUCCESS;
    }
 \endcode

 \note Please, see http://en.wikipedia.org/wiki/Q_(number_format) for details.
*/
template<typename value_type,
         std::size_t n,
         std::size_t f,
         int e,
         class op,
         class up>
class fixed_point {
    static_assert(std::is_integral<value_type>::value,
                  "value_type must be of the built-in integral type");

    using this_class = fixed_point<value_type, n, f, e, op, up>;
    using largest_type = typename std::conditional<
                                    std::numeric_limits<value_type>::is_signed,
                                    std::intmax_t, std::uintmax_t>::type;

 public:
    using type = this_class;
    using overflow_policy = op;
    using underflow_policy = up;

    /*!
     \brief Used type for the stored integer.
    */
    using storage_type = value_type;

    enum: int {
        scaling_factor_exponent = e
    };
    enum: std::size_t {
        /*!
         \brief Total number of significant bits.
        */
        number_of_significant_bits = n + f,

        /*!
         \brief Queried number of bits to represent the fractional part of
         fixed-point number.
        */
        bits_for_fractional = f,

        /*!
         \brief Number of bits to represent the integral part.
        */
        bits_for_integral = n,

        /*!
         \brief This checks if this fixed-point number is signed.
        */
        is_signed = std::numeric_limits<storage_type>::is_signed
    };

    /*!
     \brief Gets the scaling factor for this fixed-point number.
    */
    inline static double scaling_factor() {
        static double const factor = details::exp2(
                    -static_cast<double>(this_class::scaling_factor_exponent));

        return factor;
    }

    static_assert(this_class::number_of_significant_bits <=
                    std::numeric_limits<largest_type>::digits,
                  "too big word size is required");
#define EXP2N(N) (std::uintmax_t(1u) << (N))
    enum : std::uintmax_t {
        /*!
         \brief Scale factor for this fixed-point number.
        */
        scale = EXP2N(this_class::bits_for_fractional),

        /*!
         \brief Binary mask to extract the integral bits only from the stored
         integer.
        */
        // This is tricky to process if n + f = max. possible word size.
        integer_bits_mask = (this_class::bits_for_integral > 0u) ?
            2u * ( EXP2N(this_class::bits_for_fractional + this_class::bits_for_integral - 1u) -  // NOLINT
                   ( (this_class::bits_for_fractional > 0u) ? EXP2N(this_class::bits_for_fractional - 1u) : 0u)) : 0u,  // NOLINT

        /*!
         \brief Binary mask to extract the fractional bits from the stored
         integer.
        */
        fractional_bits_mask = (this_class::bits_for_fractional > 0u) ?
            2u * (EXP2N(this_class::bits_for_fractional - 1u) - 1u) + 1u : 0u
    };
#undef EXP2N

    /*!
     \brief The maximum value of stored integer for this fixed-point format.
    */
    static typename this_class::largest_type const largest_stored_integer =
        boost::low_bits_mask_t<this_class::number_of_significant_bits>::sig_bits;  // NOLINT
    /*!
     \brief Gets the maximum available fixed-point number.
    */
    static this_class largest() {
        return
            this_class::wrap<typename this_class::largest_type>(
                                           this_class::largest_stored_integer);
    }

    /*
     \brief The minimum value of stored integer for this fixed-point format.
    */
    static std::intmax_t const least_stored_integer =
        this_class::is_signed * (-static_cast<std::intmax_t>(this_class::largest_stored_integer) - 1);  // NOLINT
    /*!
     \brief Gets the minimum available fixed-point number.
    */
    static this_class least() {
        return
            this_class::wrap(this_class::least_stored_integer);
    }

    /*!
     \brief Gets the dynamic range (dB) for this fixed-point format
    */
    static double dynamic_range_db() {
        double const max_stored_integer =
            static_cast<double>(this_class::largest_stored_integer);

        return
            20.0 * std::log10(max_stored_integer);
    }

    /*!
     \brief Gets the precision of this fixed-point number.
    */
    static double precision() {
        return
            1.0 / this_class::scale;
    }

    /*!
     \brief Signed version of this fixed-point number type.
    */
    using to_signed_type = fixed_point<
                                 typename std::make_signed<storage_type>::type,
                                 this_class::bits_for_integral,
                                 this_class::bits_for_fractional,
                                 this_class::scaling_factor_exponent,
                                 overflow_policy,
                                 underflow_policy>;

    /*!
     \brief Unsigned version of this fixed-point number type.
    */
    using to_unsigned_type = fixed_point<
                               typename std::make_unsigned<storage_type>::type,
                               this_class::bits_for_integral,
                               this_class::bits_for_fractional,
                               this_class::scaling_factor_exponent,
                               overflow_policy,
                               underflow_policy>;

    /*!
     \brief Wraps the input integer _val as a fixed-point number.

     Usage

     Example 1:
     \code{.cpp}
         #include "fixed_point.hpp"
         #include <cstdint>

         int main(int, char**) {
             using Q = libq::Q<30, 20>;

             std::uint8_t input{ 23u };
             Q const fp = Q::wrap(input);

             return EXIT_SUCCESS;
         }
     \endcode
    */
    template<typename T>
    static this_class wrap(T const& _val) {
        static_assert(std::is_integral<T>::value,
                      "input param must be of the built-in integral type");

        if ((_val < 0 && _val < this_class::least_stored_integer) ||
            (_val > 0 && _val > this_class::largest_stored_integer)) {
            overflow_policy::raise_event();
        }

        this_class x;
        x.m_value = storage_type(_val);

        return x;
    }
    static this_class wrap(float const&) = delete;
    static this_class wrap(double const&) = delete;

    fixed_point() = default;
    COPY_CTR_EXPLICIT_SPECIFIER fixed_point(this_class const& _x) = default;  // NOLINT

    /*!
     \brief Normalizes the input fixed-point number to be accepted by current
     format.
    */
    template<typename T1,
             std::size_t n1,
             std::size_t f1,
             int e1,
             typename op1,
             typename up1>
    COPY_CTR_EXPLICIT_SPECIFIER
        fixed_point(fixed_point<T1, n1, f1, e1, op1, up1> const& _x)
        : m_value(
            this_class::normalize(_x,
                std::integral_constant<bool, (int(f1) + e1 - int(this_class::bits_for_fractional) - this_class::scaling_factor_exponent > 0)>())) { // NOLINT
    }

    /*!
     \brief Creates the fixed-point number from any arithmetic object.
    */
    template<typename T>
    COPY_CTR_EXPLICIT_SPECIFIER fixed_point(T const& _value)
        : m_value(
            this_class::calc_stored_integer_from(_value,
                                                 std::integral_constant<bool, std::is_floating_point<T>::value>())) {  // NOLINT
    }

    /*!
     \brief Assigns the fixed-point number being of different format.
    */
    template<typename T1,
            std::size_t n1,
            std::size_t f1,
            int e1,
            typename op1,
            typename up1>
    this_class& operator =(fixed_point<T1, n1, f1, e1, op1, up1> const& _x) {
        using status_type =
            std::integral_constant<bool,
                            (static_cast<int>(f1) + e1 -
                            static_cast<int>(this_class::bits_for_fractional) -
                            this_class::scaling_factor_exponent > 0)>;  // NOLINT
        return
            this->set_value_to(this_class::normalize(_x, status_type()));
    }
    this_class& operator =(this_class const& _x) = default;

    /*!
     \brief Assigns any arithmetic type.
    */
    template<typename T>
    void operator =(T const& _x) {
        static_assert(std::is_arithmetic<T>::value,
                      "T must be of the arithmetic type");
        using status_type = std::integral_constant<bool,
                                             std::is_floating_point<T>::value>;

        this->m_value =
            this_class::calc_stored_integer_from(_x, status_type());
    }

    /*!
     \brief Converts this fixed-point number to the single-precision
     floating-point number.
    */
    operator float() const {
        return static_cast<float>(to_floating_point());
    }

    /*!
     \brief Converts this fixed-point number to the double-precision
     floating-point number.
    */
    operator double() const {
        return to_floating_point();
    }

    /*!
     \brief Gets the stored integer behind this fixed-point number.
    */
    storage_type value() const {
        return this->m_value;
    }

    // Unfortunately, nobody can use BOOST.Operators here because it cannot
    // handle the template operators
#define COMPARISON_OPERATOR(op)\
    template<typename T>\
    bool operator op(T const& _x) const {\
        return this->value() op this_class(_x).value();\
     }

    COMPARISON_OPERATOR(<);  // NOLINT
    COMPARISON_OPERATOR(<=);  // NOLINT
    COMPARISON_OPERATOR(>);  // NOLINT
    COMPARISON_OPERATOR(>=);  // NOLINT
    COMPARISON_OPERATOR(==);  // NOLINT
    COMPARISON_OPERATOR(!=);  // NOLINT
#undef COMPARISON_OPERATOR

    bool operator !() const {
        return this->value() == 0;
    }

    /*!
     \brief Fixed-point approximation of the widely-used constants.
     \note This uses the following naming convention:
     - CONST_2PI is for \f$2 * \pi\f$.
     - CONST_2_PI is for \f$\frac{2}{\pi}\f$.
     - CONST_PI_2 is for \f$\frac{\pi}{2}\f$.
     \note Weirdly, g++ 5.3.0 needs the full specification of type here.
     Otherwise, it will not interpret lines 705-707 as declaration instead of
     definition.
    */
    static fixed_point<value_type, n, f, e, op, up> const
        CONST_E, CONST_LOG2E, CONST_1_LOG2E, CONST_LOG10E, CONST_LOG102,
        CONST_LN2, CONST_LN10, CONST_2PI, CONST_PI, CONST_PI_2, CONST_PI_4,
        CONST_1_PI, CONST_2_PI, CONST_2_SQRTPI, CONST_SQRT2, CONST_SQRT1_2,
        CONST_2SQRT2;

    /*!
     \brief Calculates the sum of the current fixed-point number and some
     numeric object.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T>
    typename libq::details::sum_traits<this_class>::promoted_type
        operator +(T const& _x) const {
        using sum_type = typename libq::details::sum_traits<this_class>::promoted_type;  // NOLINT
        using word_type = typename sum_type::storage_type;

        this_class const converted(_x);
        if (details::does_add_overflow(*this, converted)) {
            overflow_policy::raise_event();
        }

        word_type const stored_integer = word_type(this->value()) +
            word_type(converted.value());
        return sum_type::wrap(stored_integer);
    }
    template<typename T>
    inline this_class& operator +=(T const& _x) {
        this_class const result(*this + _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Subtracts some numeric object from the current fixed-point number.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T>
    typename libq::details::sum_traits<this_class>::promoted_type
        operator -(T const& _x) const {
        using diff_type = typename libq::details::sum_traits<this_class>::promoted_type;  // NOLINT
        using word_type = typename diff_type::storage_type;

        this_class const converted(_x);
        if (details::does_sub_overflow(*this, converted)) {
            overflow_policy::raise_event();
        }

        word_type const stored_integer = word_type(this->value()) -
            word_type(converted.value());
        return diff_type::wrap(stored_integer);
    }
    template<typename T>
    this_class operator -=(T const& _x) {
        this_class const result(*this - _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Multiplies the current fixed-point number with some numeric object.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T1,
             std::size_t n1,
             std::size_t f1,
             int e1,
             class op1,
             class up1>
    typename libq::details::mult_of<this_class,
                                    libq::fixed_point<T1, n1, f1, e1, op1, up1> >::promoted_type  // NOLINT
        operator *(libq::fixed_point<T1, n1, f1, e1, op1, up1> const& _x)
                                                                        const {
        using operand_type =
            typename libq::fixed_point<T1, n1, f1, e1, op1, up1>;
        using promotion_traits = libq::details::mult_of<this_class,
                                                        operand_type>;
        using result_type = typename promotion_traits::promoted_type;
        using word_type = typename promotion_traits::promoted_storage_type;

        if (details::does_mul_overflow(*this, _x)) {
            overflow_policy::raise_event();
        }

        // do the exact/approximate multiplication of fixed-point numbers
        return result_type::wrap(
            (static_cast<word_type>(this->value()) * static_cast<word_type>(_x.value()))  // NOLINT
                >> (promotion_traits::is_expandable ? 0 : operand_type::bits_for_fractional));  // NOLINT
    }
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class op1, class up1>  // NOLINT
    this_class
        operator *=(libq::fixed_point<T1, n1, f1, e1, op1, up1> const& _x) {
        this_class const result(*this * _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Divides the current fixed-point number with some numeric object.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    typename libq::details::div_of<this_class, libq::fixed_point<T1, n1, f1, e1, Ps...> >::promoted_type  // NOLINT
        operator /(libq::fixed_point<T1, n1, f1, e1, Ps...> const& _x) const {
        using operand_type = typename libq::fixed_point<T1, n1, f1, e1, Ps...>;
        using promotion_traits =
            libq::details::div_of<this_class, operand_type>;
        using result_type = typename promotion_traits::promoted_type;
        using word_type = typename promotion_traits::promoted_storage_type;

        if (details::does_div_overflow(*this, _x)) {
            overflow_policy::raise_event();
        }

        word_type const shifted = static_cast<word_type>(this->value()) << operand_type::number_of_significant_bits;  // NOLINT
        if (!promotion_traits::is_expandable && _x.value() !=
                (shifted >> operand_type::number_of_significant_bits)) {
            overflow_policy::raise_event();
        }

        return
            result_type::wrap(shifted / static_cast<word_type>(_x.value()));
    }
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    this_class
        operator /=(libq::fixed_point<T1, n1, f1, e1, Ps...> const& _x) {
        this_class const result(*this / _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Gets the negative value of the current fixed-point number.
    */
    this_class operator -() const {
        if (details::does_unary_neg_overflow(*this)) {
            overflow_policy::raise_event();
        }

        return this_class::wrap(-this->value());
    }

 private:
    /*!
     \brief Represents some floating-point number as a fixed-point number.
     \note It uses the rounding-to-nearest logics in case of floating-point
     types.
    */
    template<typename T>
    static storage_type
        calc_stored_integer_from(T const& _x, std::true_type) {
        double const scale = static_cast<double>(
                                          this_class::scaling_factor_exponent);
        double const value = static_cast<double>(_x) / details::exp2(scale);
        if (_x > T(0)) {
            storage_type const converted = static_cast<storage_type>(
                                  std::floor(value * this_class::scale + 0.5));
            if (converted < 0) {
                overflow_policy::raise_event();
            }

            return converted;
        }

        return
            static_cast<storage_type>(
                                   std::ceil(value * this_class::scale - 0.5));
    }

    /*!
     \brief Represents some integral number as a fixed-point number.
    */
    template<typename T>
    static storage_type
        calc_stored_integer_from(T const& _x, std::false_type) {
        double const scale = static_cast<double>(
                                          this_class::scaling_factor_exponent);
        double const value = static_cast<double>(_x) / details::exp2(scale);
        return storage_type(value) << this_class::bits_for_fractional;
    }

    /*!
     \brief Normalizes the input fixed-point number to one of the current
     format in case \f$f + e - f1 - e1 > 0\f$.
    */
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    static storage_type normalize(fixed_point<T1, n1, f1, e1, Ps...> const& _x,
                                  std::false_type) {
        static std::size_t const shifts =
            (static_cast<int>(this_class::bits_for_fractional) + this_class::scaling_factor_exponent) -  // NOLINT
            (static_cast<int>(e1) + f1);
        storage_type const normalized = storage_type(_x.value()) << shifts;

        if (_x.value() != (normalized >> shifts)) {
            overflow_policy::raise_event();
        }
        return normalized;
    }

    /*!
     \brief Normalizes the input fixed-point number to one of the current
     format in case \f$f + e - f1 - e1 < 0\f$.
    */
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    static storage_type normalize(fixed_point<T1, n1, f1, e1, Ps...> const& _x,
                                  std::true_type) {
        static std::size_t const shifts =
            (static_cast<int>(e1) + f1) -
            (static_cast<int>(this_class::bits_for_fractional) + this_class::scaling_factor_exponent);  // NOLINT
        storage_type const normalized =
            static_cast<storage_type>(_x.value() >> shifts);

        if (_x.value() && !normalized) {
            underflow_policy::raise_event();
        }
        return normalized;
    }

    /*!
     \brief Converts the fixed-point number to the floating-point number.
    */
    double to_floating_point() const {
        return
            this->scaling_factor() * static_cast<double>(value()) / this_class::scale;  // NOLINT
    }

    storage_type m_value;
    /*!
     \brief This also checks if the stored integer is within the range of
     current fixed-point number.
    */
    this_class& set_value_to(storage_type const _x) {
        if (_x < this_class::least_stored_integer ||
            _x > this_class::largest_stored_integer) {
            overflow_policy::raise_event();
        }

        this->m_value = _x;
        return *this;
    }

    friend storage_type& lift<value_type, n, f, e, overflow_policy, underflow_policy>(this_class&);  // NOLINT
};

/*!
 \brief Short-cut for the signed fixed-point with just 2 template parameters
 n and f.
*/
template<std::size_t n, std::size_t f, int e = 0, class op = libq::ignorance_policy, class up = libq::ignorance_policy>  // NOLINT
using Q = libq::fixed_point<typename boost::int_t<n+1>::least, n-f, f, e, op, up>;  // NOLINT

/*!
 \brief Short-cut for the unsigned fixed-point with just 2 template parameters
 n and f.
*/
template<std::size_t n, std::size_t f, int e = 0, class op = libq::ignorance_policy, class up = libq::ignorance_policy>  // NOLINT
using UQ = libq::fixed_point<typename boost::uint_t<n>::least, n-f, f, e, op, up>;  // NOLINT

#define CONSTANT(name, value)\
    template<class T, std::size_t n, std::size_t f, int e, class op, class up>\
    fixed_point<T, n, f, e, op, up> const fixed_point<T, n, f, e, op, up>::name(value);  // NOLINT

CONSTANT(CONST_E, 2.71828182845904523536)
CONSTANT(CONST_1_LOG2E, 0.6931471805599453)
CONSTANT(CONST_LOG2E, 1.44269504088896340736)
CONSTANT(CONST_LOG10E, 0.434294481903251827651)
CONSTANT(CONST_LOG102, 0.301029995663981195214)
CONSTANT(CONST_LN2, 0.693147180559945309417)
CONSTANT(CONST_LN10, 2.30258509299404568402)
CONSTANT(CONST_2PI, 6.283185307179586)
CONSTANT(CONST_PI, 3.14159265358979323846)
CONSTANT(CONST_PI_2, 1.57079632679489661923)
CONSTANT(CONST_PI_4, 0.785398163397448309616)
CONSTANT(CONST_1_PI, 0.318309886183790671538)
CONSTANT(CONST_2_PI, 0.636619772367581343076)
CONSTANT(CONST_2_SQRTPI, 1.12837916709551257390)
CONSTANT(CONST_SQRT2, 1.41421356237309504880)
CONSTANT(CONST_SQRT1_2, 0.707106781186547524401)
CONSTANT(CONST_2SQRT2, 2.82842712474619009760)

#undef CONSTANT
}  // namespace libq

#include "details/sum_traits.inl"
#include "details/mult_of.inl"
#include "details/div_of.inl"

#include "details/sign.inl"

#include "details/ceil.inl"
#include "details/fabs.inl"
#include "details/floor.inl"
#include "details/round.inl"
#include "details/remainder.inl"
#include "details/fmod.inl"
#include "details/numeric_limits.inl"
#include "details/type_traits.inl"

#include "loop_unroller.hpp"

#include "CORDIC/lut/lut.hpp"

#include "CORDIC/log.inl"
#include "CORDIC/sqrt.inl"

#include "CORDIC/sin.inl"
#include "CORDIC/cos.inl"
#include "CORDIC/tan.inl"

#include "CORDIC/exp.inl"

#include "CORDIC/sinh.inl"
#include "CORDIC/cosh.inl"
#include "CORDIC/tanh.inl"

#include "CORDIC/acos.inl"
#include "CORDIC/asin.inl"
#include "CORDIC/atan.inl"

#include "CORDIC/asinh.inl"
#include "CORDIC/acosh.inl"
#include "CORDIC/atanh.inl"

#endif  // INC_LIBQ_FIXED_POINT_HPP_


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/diag.cpp
 *
 * \brief Test suite for the \c diag operation.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 *  Copyright (c) 2009, 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)
 */

#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/container/generalized_diagonal_matrix.hpp>
#include <boost/numeric/ublasx/operation/diag.hpp>
#include <cmath>
#include <iostream>
#include "libs/numeric/ublasx/test/utils.hpp"

static const double TOL = 1.0e-5; ///< Tolerance for real number comparison.

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

//@{ View //////////////////////////////////////////////////////////////////////

BOOST_UBLASX_TEST_DEF( test_main_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Main Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix,ix) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix,ix), TOL );
    }
}

BOOST_UBLASX_TEST_DEF( test_up1_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST First Upper Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A, 1);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix,ix+1) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix,ix+1), TOL );
    }
}

BOOST_UBLASX_TEST_DEF( test_up2_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Second Upper Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A, 2);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix,ix+2) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix,ix+2), TOL );
    }
}

BOOST_UBLASX_TEST_DEF( test_up3_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Third Upper Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A, 3);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix,ix+3) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix,ix+3), TOL );
    }
}

BOOST_UBLASX_TEST_DEF( test_low1_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST First Lower Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A, -1);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix+1,ix) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix+1,ix), TOL );
    }
}

BOOST_UBLASX_TEST_DEF( test_low2_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Second Lower Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A, -2);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix+2,ix) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix+2,ix), TOL );
    }
}

BOOST_UBLASX_TEST_DEF( test_low3_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Third Lower Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A, -3);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix+3,ix) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix+3,ix), TOL );
    }
}

BOOST_UBLASX_TEST_DEF( test_low4_diagonal_view )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Fourth Lower Diagonal -- View" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublasx::matrix_diagonal<matrix_type> diagonal_type;

    matrix_type A(5,4);

    A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
    A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
    A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;

    diagonal_type D = ublasx::diag(A, -4);

    for (
        diagonal_type::const_iterator it = D.begin();
        it != D.end();
        ++it
    ) {
        matrix_type::size_type ix(it.index());

        BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix << ") = " << *it << " ==> " << A(ix+4,ix) );
        BOOST_UBLASX_TEST_CHECK_CLOSE( *it, A(ix+4,ix), TOL );
    }
}

//@} View //////////////////////////////////////////////////////////////////////

//@{ Creation //////////////////////////////////////////////////////////////////

BOOST_UBLASX_TEST_DEF( test_main_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Main Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << v.size() );
    BOOST_UBLASX_TEST_CHECK( D.size1() == v.size() );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << v.size() );
    BOOST_UBLASX_TEST_CHECK( D.size2() == v.size() );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 0 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 0 );

    // Check elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_up1_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST First Upper Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    //typedef ublas::banded_matrix<value_type> matrix_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 1);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << (v.size()+1) );
    BOOST_UBLASX_TEST_CHECK( D.size1() == (v.size()+1) );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << (v.size()+1) );
    BOOST_UBLASX_TEST_CHECK( D.size2() == (v.size()+1) );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 1 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 1 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << ((row+1) == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, ((row+1) == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_up2_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Second Upper Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    //typedef ublas::banded_matrix<value_type> matrix_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 2);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << (v.size()+2) );
    BOOST_UBLASX_TEST_CHECK( D.size1() == (v.size()+2) );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << (v.size()+2) );
    BOOST_UBLASX_TEST_CHECK( D.size2() == (v.size()+2) );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 2 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 2 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << ((row+2) == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, ((row+2) == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_up3_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Third Upper Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 3);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << (v.size()+3) );
    BOOST_UBLASX_TEST_CHECK( D.size1() == (v.size()+3) );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << (v.size()+3) );
    BOOST_UBLASX_TEST_CHECK( D.size2() == (v.size()+3) );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 3 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 3 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << ((row+3) == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, ((row+3) == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low1_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST First Lower Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, -1);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << (v.size()+1) );
    BOOST_UBLASX_TEST_CHECK( D.size1() == (v.size()+1) );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << (v.size()+1) );
    BOOST_UBLASX_TEST_CHECK( D.size2() == (v.size()+1) );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -1 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -1 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+1) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+1) ? v(col) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low2_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Second Lower Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, -2);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << (v.size()+2) );
    BOOST_UBLASX_TEST_CHECK( D.size1() == (v.size()+2) );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << (v.size()+2) );
    BOOST_UBLASX_TEST_CHECK( D.size2() == (v.size()+2) );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -2 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -2 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+2) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+2) ? v(col) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low3_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Third Lower Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, -3);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << (v.size()+3) );
    BOOST_UBLASX_TEST_CHECK( D.size1() == (v.size()+3) );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << (v.size()+3) );
    BOOST_UBLASX_TEST_CHECK( D.size2() == (v.size()+3) );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -3 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -3 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+3) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+3) ? v(col) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low4_diagonal_create )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Fourth Lower Diagonal -- Create" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, -4);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << (v.size()+4) );
    BOOST_UBLASX_TEST_CHECK( D.size1() == (v.size()+4) );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << (v.size()+4) );
    BOOST_UBLASX_TEST_CHECK( D.size2() == (v.size()+4) );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -4 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -4 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+4) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+4) ? v(col) : value_type(0)), TOL );
        }
    }
}

//@} Creation //////////////////////////////////////////////////////////////////

//@{ Rectangular Creation //////////////////////////////////////////////////////

BOOST_UBLASX_TEST_DEF( test_main_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Main Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, 0);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 0 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 0 );

    // Check elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_up1_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST First Upper Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, 1);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 1 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 1 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << ((row+1) == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, ((row+1) == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_up2_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Second Upper Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, 2);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 2 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 2 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << ((row+2) == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, ((row+2) == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_up3_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Third Upper Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, 3);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << 3 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == 3 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << ((row+3) == col ? v(row) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, ((row+3) == col ? v(row) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low1_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST First Lower Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, -1);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -1 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -1 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+1) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+1) ? v(col) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low2_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Second Lower Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublasx::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, -2);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -2 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -2 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+2) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+2) ? v(col) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low3_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Third Lower Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, -3);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -3 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -3 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+3) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+3) ? v(col) : value_type(0)), TOL );
        }
    }
}

BOOST_UBLASX_TEST_DEF( test_low4_diagonal_create_rect )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Fourth Lower Diagonal -- Create Rectangular" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublasx::generalized_diagonal_matrix<value_type> matrix_type;

    vector_type v(5);

    v(0) = 0.555950;
    v(1) = 0.108929;
    v(2) = 0.948014;
    v(3) = 0.023787;
    v(4) = 1.023787;

    matrix_type D = ublasx::diag(v, 5, 4, -4);

    // Check dimensions
    BOOST_UBLASX_DEBUG_TRACE( "D.size1() = " << D.size1() << " ==> " << 5 );
    BOOST_UBLASX_TEST_CHECK( D.size1() == 5 );
    BOOST_UBLASX_DEBUG_TRACE( "D.size2() = " << D.size2() << " ==> " << 4 );
    BOOST_UBLASX_TEST_CHECK( D.size2() == 4 );
    BOOST_UBLASX_DEBUG_TRACE( "D.offset() = " << D.offset() << " ==> " << -4 );
    BOOST_UBLASX_TEST_CHECK( D.offset() == -4 );

    // Check Elements
    for (
        matrix_type::const_iterator1 row_it = D.begin1();
        row_it != D.end1();
        ++row_it
    ) {
        for (
            matrix_type::const_iterator2 col_it = row_it.begin();
            col_it != row_it.end();
            ++col_it
        ) {
            matrix_type::size_type row(col_it.index1());
            matrix_type::size_type col(col_it.index2());

            BOOST_UBLASX_DEBUG_TRACE( "diag(v)(" << row << "," << col << ") = " << *col_it << " ==> " << (row == (col+4) ? v(col) : 0) );
            BOOST_UBLASX_TEST_CHECK_CLOSE( *col_it, (row == (col+4) ? v(col) : value_type(0)), TOL );
        }
    }
}

//@} Rectangular Creation //////////////////////////////////////////////////////

//@{ Double Creation ///////////////////////////////////////////////////////////

///FIXME: does not work since matrix_diagonal is not a really vector
//BOOST_UBLASX_TEST_DEF( test_main_diagonal_double_create )
//{
//  BOOST_UBLASX_DEBUG_TRACE( "TEST Main Diagonal -- Double Creation" );
//
//  typedef double value_type;
//  typedef ublas::matrix<value_type> in_matrix_type;
//  typedef ublasx::generalized_diagonal_matrix<value_type> out_matrix_type;
//
//  in_matrix_type A(5,4);
//
//  A(0,0) = 0.555950; A(0,1) = 0.274690; A(0,2) = 0.540605; A(0,3) = 0.798938;
//  A(1,0) = 0.108929; A(1,1) = 0.830123; A(1,2) = 0.891726; A(1,3) = 0.895283;
//  A(2,0) = 0.948014; A(2,1) = 0.973234; A(2,2) = 0.216504; A(2,3) = 0.883152;
//  A(3,0) = 0.023787; A(3,1) = 0.675382; A(3,2) = 0.231751; A(3,3) = 0.450332;
//  A(4,0) = 1.023787; A(4,1) = 1.675382; A(4,2) = 1.231751; A(4,3) = 1.450332;
//
//
//  out_matrix_type D = ublasx::diag(ublasx::diag(A));
//
//  for (
//      out_matrix_type::const_iterator1 it1 = D.begin1();
//      it1 != D.end1();
//      ++it1
//  ) {
//      for (
//          out_matrix_type::const_iterator2 it2 = it1.begin();
//          it2 != it1.end();
//          ++it2
//      ) {
//          out_matrix_type::size_type ix1(it2.index1());
//          out_matrix_type::size_type ix2(it2.index2());
//
//          BOOST_UBLASX_DEBUG_TRACE( "diag(A)(" << ix1 << "," << ix2 << ") = " << *it2 << " ==> " << A(ix1,ix1) );
//          BOOST_UBLASX_TEST_CHECK_CLOSE( *it2, A(ix1,ix2), TOL );
//      }
//  }
//}

//@} Double Creation ///////////////////////////////////////////////////////////

int main()
{
    BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'diag' operations");

    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( test_main_diagonal_view );
    BOOST_UBLASX_TEST_DO( test_up1_diagonal_view );
    BOOST_UBLASX_TEST_DO( test_up2_diagonal_view );
    BOOST_UBLASX_TEST_DO( test_up3_diagonal_view );
    BOOST_UBLASX_TEST_DO( test_low1_diagonal_view );
    BOOST_UBLASX_TEST_DO( test_low2_diagonal_view );
    BOOST_UBLASX_TEST_DO( test_low3_diagonal_view );
    BOOST_UBLASX_TEST_DO( test_low4_diagonal_view );

    BOOST_UBLASX_TEST_DO( test_main_diagonal_create );
    BOOST_UBLASX_TEST_DO( test_up1_diagonal_create );
    BOOST_UBLASX_TEST_DO( test_up2_diagonal_create );
    BOOST_UBLASX_TEST_DO( test_up3_diagonal_create );
    BOOST_UBLASX_TEST_DO( test_low1_diagonal_create );
    BOOST_UBLASX_TEST_DO( test_low2_diagonal_create );
    BOOST_UBLASX_TEST_DO( test_low3_diagonal_create );
    BOOST_UBLASX_TEST_DO( test_low4_diagonal_create );

    BOOST_UBLASX_TEST_DO( test_main_diagonal_create_rect );
    BOOST_UBLASX_TEST_DO( test_up1_diagonal_create_rect );
    BOOST_UBLASX_TEST_DO( test_up2_diagonal_create_rect );
    BOOST_UBLASX_TEST_DO( test_up3_diagonal_create_rect );
    BOOST_UBLASX_TEST_DO( test_low1_diagonal_create_rect );
    BOOST_UBLASX_TEST_DO( test_low2_diagonal_create_rect );
    BOOST_UBLASX_TEST_DO( test_low3_diagonal_create_rect );
    BOOST_UBLASX_TEST_DO( test_low4_diagonal_create_rect );

//  BOOST_UBLASX_TEST_DO( test_main_diagonal_double_create );

    BOOST_UBLASX_TEST_END();
}


/*
 * MIT License
 *
 * Copyright (c) 2015 Alexis LE GOADEC
 *
 * 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.
 *
 */

/*
 * Adjacency matrix object
 *
 */

#ifndef MODEL_INCLUDE_ADJACENTMATRIX_HH_
#define MODEL_INCLUDE_ADJACENTMATRIX_HH_

#include "LibType.hh"
#include <boost/tuple/tuple.hpp>

class HyperEdge;

class HyperVertex;

/**
 * Adjacency matrix object
 */
class AdjacentMatrix {

public:

	/**
	 * Constructor
	 */
	AdjacentMatrix();

	/**
	 * Constructor
	 * @param x The abscissa
	 * @param y The ordinate
	 */
	AdjacentMatrix(const unsigned int&, const unsigned int&);

	/**
	 * Matrix resize
	 * @param x The abscissa
	 * @param y The ordinate
	 */
	void resize(const unsigned int&, const unsigned int&);

	/**
	 * Add hyper-vertex
	 * @param HyperVertex Hyper-vertex to add
	 */
	void addHyperVertex(const boost::shared_ptr<HyperVertex>&);

	/**
	 * Add hyper-edge
	 * @param HyperEdge Hyper-edge to add
	 */
	void addHyperEdge(const boost::shared_ptr<HyperEdge>&);

	/**
	 * Check whether the hyper-vertex is contained into the specified hyper-edge
	 * @param Hyper-vertex
	 * @param Hyper-edge
	 * @return True or False
	 */
	bool isVertexInEdge(const boost::shared_ptr<HyperVertex>&, const boost::shared_ptr<HyperEdge>&) const;

	/**
	 * Check whether the hyper-edge contains the specified hyper-vertex
	 * @param Hyper-edge
	 * @param Hyper-vertex
	 * @return True or False
	 */
	bool isEdgeInVertex(const boost::shared_ptr<HyperEdge>&, const boost::shared_ptr<HyperVertex>&) const;

	/**
	 * Check whether the hyper-vertex is contained into the specified hyper-edge
	 * @param Hyper-vertex's Id
	 * @param Hyper-edge's Id
	 * @return True or False
	 */
	bool isVertexInEdge(const int&, const int&) const;

	/**
	 * Check whether the hyper-edge contains the specified hyper-vertex
	 * @param Hyper-edge's Id
	 * @param Hyper-vertex's Id
	 * @return True or False
	 */
	bool isEdgeInVertex(const int&, const int&) const;

	/**
	 * Get the boolean adajency matrix
	 * @return The boolean adajency matrix
	 */
	LibType::AdjacentMatrixContainerBool& getBoolAdjacentMatrix();

	/**
	 * Get the integer adajency matrix
	 * @return The integer adajency matrix
	 */
	LibType::AdjacentMatrixContainerInt& getUIntAdjacentMatrix();

	/**
	 * Get the hyper-vertex's degree
	 * @param Hyper-vertex
	 * @return The hyper-vertex's degree
	 */
	unsigned int getVertexDegree(const boost::shared_ptr<HyperVertex>&) const;

	/**
	 * Get the hyper-edge degree
	 * @param Hyper-edge
	 * @return The hyper-edge degree
	 */
	unsigned int getEdgeSize(const boost::shared_ptr<HyperEdge>&) const;

	/**
	 * Get co-rank
	 * @return Co-rank
	 */
	unsigned int getCoRank() const;

	/**
	 * Get rank
	 * @return Rank
	 */
	unsigned int getRank() const;

	/**
	 * Get the matrix's size
	 * @return Tuple containing the abscissa and the ordinate
	 */
	boost::tuple<unsigned int, unsigned int>&
	getMatrixDimension();

	/**
	 * Displaying the matrix - DO NOT USE
	 */
	void display() const;

protected:

	/**
	 * Abscissa
	 */
	unsigned int _m;

	/**
	 * Ordinate
	 */
	unsigned int _n;

	/**
	 * Boolean matrix
	 */
	LibType::AdjacentMatrixContainerBool _adjacentMatrixBool;

	/**
	 * Integer matrix
	 */
	LibType::AdjacentMatrixContainerInt  _adjacentMatrixUInt;

};

#endif


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2017 by Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#ifndef ROKKO_LAPACK_GETRF_HPP
#define ROKKO_LAPACK_GETRF_HPP

#include <complex>
#include <stdexcept>
#include <boost/type_traits/is_same.hpp>
#include <lapacke.h>
#undef I
#include <rokko/traits/value_t.hpp>
#include "complex_cast.hpp"

namespace rokko {
namespace lapack {

namespace {

template<typename T> struct getrf_dispatch;

template<>
struct getrf_dispatch<float> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int getrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& ipiv) {
    return LAPACKE_sgetrf(matrix_layout, m, n, storage(a), ld(a), storage(ipiv));
  }
};

template<>
struct getrf_dispatch<double> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int getrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& ipiv) {
    return LAPACKE_dgetrf(matrix_layout, m, n, storage(a), ld(a), storage(ipiv));
  }
};

template<>
struct getrf_dispatch<std::complex<float>> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int getrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& ipiv) {
    return LAPACKE_cgetrf(matrix_layout, m, n, complex_cast(storage(a)), ld(a),
                          storage(ipiv));
  }
};

template<>
struct getrf_dispatch<std::complex<double>> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int getrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& ipiv) {
    return LAPACKE_zgetrf(matrix_layout, m, n, complex_cast(storage(a)), ld(a),
                          storage(ipiv));
  }
};

}

template<typename MATRIX, typename VECTOR>
lapack_int getrf(MATRIX& a, VECTOR& ipiv) {
  lapack_int m = rows(a);
  lapack_int n = cols(a);
  static_assert(std::is_same<value_t<VECTOR>, lapack_int>::value, "");
  if (size(ipiv) < std::min(m, n))
    throw std::invalid_argument("vector ipiv size mismatch");
  return getrf_dispatch<value_t<MATRIX>>
    ::getrf((is_col_major(a) ? LAPACK_COL_MAJOR : LAPACK_ROW_MAJOR),
            m, n, a, ipiv);
}

} // end namespace lapack
} // end namespace rokko

#endif // ROKKO_LAPACK_GETRF_HPP


#include <boost/numeric/interval/compare/set.hpp>


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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/graph/properties.hpp>
#include <boost/graph/adjacency_list.hpp>

using namespace boost;

struct vertex_info_t
{
};
struct edge_info_t
{
};
namespace boost
{
BOOST_INSTALL_PROPERTY(vertex, info);
BOOST_INSTALL_PROPERTY(edge, info);
};

typedef property< vertex_info_t, double > vertex_properties;
typedef property< edge_info_t, double > edge_properties;

typedef adjacency_list< vecS, vecS, bidirectionalS, vertex_properties,
    edge_properties >
    graph_t;

double& foo_1(graph_t& x)
{
    property_map< graph_t, vertex_info_t >::type pmap = get(vertex_info_t(), x);
    return pmap[vertex(0, x)];
}

const double& foo_2(graph_t const& x)
{
    property_map< graph_t, vertex_info_t >::const_type pmap
        = get(vertex_info_t(), x);
    return pmap[vertex(0, x)];
}

double& bar_1(graph_t& x)
{
    property_map< graph_t, edge_info_t >::type pmap = get(edge_info_t(), x);
    return pmap[edge(vertex(0, x), vertex(1, x), x).first];
}

const double& bar_2(graph_t const& x)
{
    property_map< graph_t, edge_info_t >::const_type pmap
        = get(edge_info_t(), x);
    return pmap[edge(vertex(0, x), vertex(1, x), x).first];
}

int main() { return 0; }


#include <boost/python.hpp>
#include <GeographicLib/Geoid.hpp>

using namespace boost::python;
using namespace GeographicLib;

double EllipsoidHeight(Geoid& geoid,
                       double lat, double lon, double hmsl) {
  return hmsl + Geoid::GEOIDTOELLIPSOID * geoid(lat, lon);
}

BOOST_PYTHON_MODULE(PyGeographicLib) {

  class_<Geoid, boost::noncopyable>("Geoid", init<std::string>())
    .def("EllipsoidHeight", &EllipsoidHeight,
         "Return geoid height:\n\
    input: lat, lon, height_above_geoid\n\
    output: height_above_ellipsoid")
    ;

}


/* boost random/uniform_01.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_01_HPP
#define BOOST_RANDOM_UNIFORM_01_HPP

#include <iostream>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/ptr_helper.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace lslboost {
namespace random {

#ifdef BOOST_RANDOM_DOXYGEN

/**
 * The distribution function uniform_01 models a \random_distribution.
 * On each invocation, it returns a random floating-point value
 * uniformly distributed in the range [0..1).
 *
 * The template parameter RealType shall denote a float-like value type
 * with support for binary operators +, -, and /.
 *
 * Note: The current implementation is buggy, because it may not fill
 * all of the mantissa with random bits. I'm unsure how to fill a
 * (to-be-invented) @c lslboost::bigfloat class with random bits efficiently.
 * It's probably time for a traits class.
 */
template<class RealType = double>
class uniform_01
{
public:
  typedef RealType input_type;
  typedef RealType result_type;
  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const;
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const;
  void reset();

  template<class Engine>
  result_type operator()(Engine& eng);

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const new_uniform_01&)
  {
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, new_uniform_01&)
  {
    return is;
  }
#endif
};

#else

namespace detail {

template<class RealType>
class new_uniform_01
{
public:
  typedef RealType input_type;
  typedef RealType result_type;
  // compiler-generated copy ctor and copy assignment are fine
  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(0); }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(1); }
  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng) {
    for (;;) {
      typedef typename Engine::result_type base_result;
      result_type factor = result_type(1) /
              (result_type(base_result((eng.max)()-(eng.min)())) +
               result_type(std::numeric_limits<base_result>::is_integer ? 1 : 0));
      result_type result = result_type(base_result(eng() - (eng.min)())) * factor;
      if (result < result_type(1))
        return result;
    }
  }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const new_uniform_01&)
  {
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, new_uniform_01&)
  {
    return is;
  }
#endif
};

template<class UniformRandomNumberGenerator, class RealType>
class backward_compatible_uniform_01
{
  typedef lslboost::random::detail::ptr_helper<UniformRandomNumberGenerator> traits;
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

#if !defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS)
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit backward_compatible_uniform_01(typename traits::rvalue_type rng)
    : _rng(rng),
      _factor(result_type(1) /
              (result_type((base().max)()-(base().min)()) +
               result_type(std::numeric_limits<base_result>::is_integer ? 1 : 0)))
  {
  }
  // compiler-generated copy ctor and copy assignment are fine

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(0); }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(1); }
  typename traits::value_type& base() { return traits::ref(_rng); }
  const typename traits::value_type& base() const { return traits::ref(_rng); }
  void reset() { }

  result_type operator()() {
    for (;;) {
      result_type result = result_type(base()() - (base().min)()) * _factor;
      if (result < result_type(1))
        return result;
    }
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const backward_compatible_uniform_01& u)
  {
    os << u._rng;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, backward_compatible_uniform_01& u)
  {
    is >> u._rng;
    return is;
  }
#endif

private:
  typedef typename traits::value_type::result_type base_result;
  UniformRandomNumberGenerator _rng;
  result_type _factor;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UniformRandomNumberGenerator, class RealType>
const bool backward_compatible_uniform_01<UniformRandomNumberGenerator, RealType>::has_fixed_range;
#endif

template<class UniformRandomNumberGenerator, bool is_number = std::numeric_limits<UniformRandomNumberGenerator>::is_specialized>
struct select_uniform_01
{
  template<class RealType>
  struct apply
  {
    typedef backward_compatible_uniform_01<UniformRandomNumberGenerator, RealType> type;
  };
};

template<class Num>
struct select_uniform_01<Num, true>
{
  template<class RealType>
  struct apply
  {
    typedef new_uniform_01<Num> type;
  };
};

}

// Because it is so commonly used: uniform distribution on the real [0..1)
// range.  This allows for specializations to avoid a costly int -> float
// conversion plus float multiplication
template<class UniformRandomNumberGenerator = double, class RealType = double>
class uniform_01
  : public detail::select_uniform_01<UniformRandomNumberGenerator>::BOOST_NESTED_TEMPLATE apply<RealType>::type
{
  typedef typename detail::select_uniform_01<UniformRandomNumberGenerator>::BOOST_NESTED_TEMPLATE apply<RealType>::type impl_type;
  typedef lslboost::random::detail::ptr_helper<UniformRandomNumberGenerator> traits;
public:

  uniform_01() {}

  explicit uniform_01(typename traits::rvalue_type rng)
    : impl_type(rng)
  {
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_01& u)
  {
    os << static_cast<const impl_type&>(u);
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_01& u)
  {
    is >> static_cast<impl_type&>(u);
    return is;
  }
#endif
};

#endif

} // namespace random

using random::uniform_01;

} // namespace lslboost

#include <boost/random/detail/enable_warnings.hpp>

#endif // BOOST_RANDOM_UNIFORM_01_HPP


//
// ============================================================
//
//                      POINT.C
//
// ============================================================
//
//
//      Copyright (C) 1992,1993,1994,1995,1996
//
//                      Professor Kenneth I. Joy
//                      Computer Science Department
//                      University of California
//                      Davis, CA  95616
//
//      Permission is granted to use at your own risk and
//      distribute this software in source and binary forms
//      provided the above copyright notice and this paragraph
//      are preserved on all copies.  This software is provided
//      "as is" with no express or implied warranty.
//
//
//  ============================================================
//

#include <cstdlib>
#include <cmath>
#include <strings.h>
#include <iostream>
#include <boost/format.hpp>
#include <iomanip>
#include "Point.h"
using namespace std;

#define		TRUE		1
#define		FALSE		0

/*
   ==========================================================

	       CONSTRUCTORS, DESTRUCTOR

   ==========================================================
*/

Point :: Point ()

   {
      _x = 0.0 ;
      _y = 0.0 ;
      _z = 0.0 ;
      color.setRed(255);
      color.setGreen(255);
      color.setBlue(255);
      color.setAlpha(255);
   }

Point :: Point ( const double x, const double y, const double z, const unsigned char r,
		const unsigned char g, const unsigned char b, const unsigned char a )

   {
      _x = x ;
      _y = y ;
      _z = z ;
      color.setRed(r);
      color.setGreen(g);
      color.setBlue(b);
      color.setAlpha(a);
   }

Point :: Point ( const double x, const double y, const double z, const Color256 c )

   {
      _x = x ;
      _y = y ;
      _z = z ;
      color = c;
   }

Point :: Point ( const Point& p )

   {
      _x = p._x ;
      _y = p._y ;
      _z = p._z ;
      color.setRed(p.r());
      color.setGreen(p.g());
      color.setBlue(p.b());
      color.setAlpha(p.a());
   }

Point :: ~Point () { }

/*
   ==========================================================

	       OPERATOR=

   ==========================================================
*/

Point& Point :: operator= ( const Point& p )

   {
      if ( this == &p ) return ( *this ) ;

      _x = p._x ;
      _y = p._y ;
      _z = p._z ;
      color.setRed(p.r());
      color.setGreen(p.g());
      color.setBlue(p.b());
      color.setAlpha(p.a());

      return ( *this ) ;
   }

/*
   ==========================================================

	       OUTPUT

   ==========================================================
*/

ostream& operator<< ( ostream& co, const Point& p )

   {
	co << "(" << setprecision(4)
	<< p._x
	<< ","
	<< p._y
	<< ","
	<< p._z
	<< ",";
	co << p.color << ")";
//	<< ")";

      return co ;
   }

/*
   ==========================================================

	       COMPARISON OPERATIONS

   ==========================================================
*/

int operator== ( const Point& p1, const Point& p2 )

   {
      if ( ( p1._x == p2._x ) &&
           ( p1._y == p2._y ) &&
           ( p1._z == p2._z ) )
	    return ( TRUE ) ;
	 else
	    return ( FALSE ) ;
   }

int operator!= ( const Point& p1, const Point& p2 )

   {
      if ( ( p1._x != p2._x ) ||
           ( p1._y != p2._y ) ||
           ( p1._z != p2._z ) )
	    return ( TRUE ) ;
	 else
	    return ( FALSE ) ;
   }

/*
   ==========================================================

	       ARITHMETIC OPERATIONS

   ==========================================================
*/

Point operator+ ( const Point& p1, const Vector& p2 )

   {
      Point pp(p1);

      pp._x = p1._x + p2.x() ;
      pp._y = p1._y + p2.y() ;
      pp._z = p1._z + p2.z() ;

      return ( pp ) ;
   }

Point operator+ ( const Vector& p2, const Point& p1 )

   {
      Point pp(p1);

      pp._x = p1._x + p2.x() ;
      pp._y = p1._y + p2.y() ;
      pp._z = p1._z + p2.z() ;

      return ( pp ) ;
   }

Vector operator- ( const Point& p1, const Point& p2 )

   {
      return ( Vector ( p1._x - p2._x, p1._y - p2._y, p1._z - p2._z ) ) ;
   }

Point operator- ( const Point& p, const Vector& v )

   {
      return ( Point ( p._x - v.x(), p._y - v.y(), p._z - v.z() ) ) ;
   }

Point operator- ( const Point& p )

   {
      Point pp(p) ;

      pp._x = -p._x ;
      pp._y = -p._y ;
      pp._z = -p._z ;

      return ( pp ) ;
   }

Point& Point :: operator+= ( const Vector& v )

   {
      _x += v.x() ;
      _y += v.y() ;
      _z += v.z() ;

      return ( *this ) ;
   }

Point& Point :: operator-= ( const Vector& v )

   {
      _x -= v.x() ;
      _y -= v.y() ;
      _z -= v.z() ;

      return ( *this ) ;
   }

Point operator+ ( const Point& p1, const Point& p2 ) {
	return Point(p1._x + p2._x, p1._y + p2._y, p1._z + p2._z, Color256(affine(p1.color, p2.color, 0.5, 0.5)));
}
Point operator* ( const double s, const Point& p ) {
	return Point(s * p._x, s * p._y, s* p._z, p.color);
}
Point operator* ( const Point& p, const double s ) {
	return Point(s * p._x, s * p._y, s* p._z, p.color);
}

/*
   ==========================================================

	       AFFINE

   ==========================================================
*/

Point affine ( const Point& p1, const Point& p2, const double t )

   {
      Point p ;

      p._x = p1._x + t * ( p2._x - p1._x ) ;
      p._y = p1._y + t * ( p2._y - p1._y ) ;
      p._z = p1._z + t * ( p2._z - p1._z ) ;

      return ( p ) ;
   }

Point affine ( const Point& p1,
	       const Point& p2,
	       const Point& p3,
	       const double a1,
	       const double a2,
	       const double a3 )

   {
      Point p ;

      p._x = a1 * p1._x + a2 * p2._x + a3 * p3._x ;
      p._y = a1 * p1._y + a2 * p2._y + a3 * p3._y ;
      p._z = a1 * p1._z + a2 * p2._z + a3 * p3._z ;

      return ( p ) ;
   }

Point average ( Point *p, const int n )

   {
      double xsum = 0.0 ;
      double ysum = 0.0 ;
      double zsum = 0.0 ;

      double weight = 1.0 / ( (double) n ) ;

      for ( int i = 0 ; i < n ; i++ ) {
	 xsum += weight * p[i].x() ;
	 ysum += weight * p[i].y() ;
	 zsum += weight * p[i].z() ;
	 }

      return ( Point ( xsum, ysum, zsum ) ) ;
   }

Point midpoint ( const Point& p1, const Point& p2 )

   {
      return ( affine ( p1, p2, 0.5 ) ) ;
   }


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_METROPOLISHAMILTONIAN_HPP
#define NETKET_METROPOLISHAMILTONIAN_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

namespace netket {

// Metropolis sampling generating transitions using the Hamiltonian
template <class H>
class MetropolisHamiltonian : public AbstractSampler {
  H &hamiltonian_;

  // number of visible units
  const int nv_;

  // states of visible units
  Eigen::VectorXd v_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  int mynode_;
  int totalnodes_;

  // Look-up tables
  typename AbstractMachine::LookupType lt_;

  std::vector<std::vector<int>> tochange_;
  std::vector<std::vector<double>> newconfs_;
  std::vector<Complex> mel_;

  std::vector<std::vector<int>> tochange1_;
  std::vector<std::vector<double>> newconfs1_;
  std::vector<Complex> mel1_;

  Eigen::VectorXd v1_;

 public:
  MetropolisHamiltonian(AbstractMachine &psi, H &hamiltonian)
      : AbstractSampler(psi),
        hamiltonian_(hamiltonian),
        nv_(GetHilbert().Size()) {
    Init();
  }

  void Init() {
    v_.resize(nv_);

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    if (!GetHilbert().IsDiscrete()) {
      throw InvalidInputError(
          "Hamiltonian Metropolis sampler works only for discrete "
          "Hilbert spaces");
    }

    accept_.resize(1);
    moves_.resize(1);

    Reset(true);

    InfoMessage() << "Hamiltonian Metropolis sampler is ready " << std::endl;
  }

  void Reset(bool initrandom = false) override {
    if (initrandom) {
      GetHilbert().RandomVals(v_, this->GetRandomEngine());
    }

    GetMachine().InitLookup(v_, lt_);

    accept_ = Eigen::VectorXd::Zero(1);
    moves_ = Eigen::VectorXd::Zero(1);
  }

  void Sweep() override {
    for (int i = 0; i < nv_; i++) {
      hamiltonian_.FindConn(v_, mel_, tochange_, newconfs_);

      const double w1 = tochange_.size();

      std::uniform_int_distribution<int> distrs(0, tochange_.size() - 1);
      std::uniform_real_distribution<double> distu;

      // picking a random state to transit to
      int si = distrs(this->GetRandomEngine());

      // Inverse transition
      v1_ = v_;
      GetHilbert().UpdateConf(v1_, tochange_[si], newconfs_[si]);

      hamiltonian_.FindConn(v1_, mel1_, tochange1_, newconfs1_);

      double w2 = tochange1_.size();

      const auto lvd = GetMachine().LogValDiff(v_, tochange_[si], newconfs_[si], lt_);
      double ratio = this->GetMachineFunc()(std::exp(lvd)) * w1 / w2;

#ifndef NDEBUG
      const auto psival1 = GetMachine().LogVal(v_);
      if (std::abs(std::exp(GetMachine().LogVal(v_) - GetMachine().LogVal(v_, lt_)) - 1.) >
          1.0e-8) {
        std::cerr << GetMachine().LogVal(v_) << "  and LogVal with Lt is "
                  << GetMachine().LogVal(v_, lt_) << std::endl;
        std::abort();
      }
#endif

      // Metropolis acceptance test
      if (ratio > distu(this->GetRandomEngine())) {
        accept_[0] += 1;
        GetMachine().UpdateLookup(v_, tochange_[si], newconfs_[si], lt_);
        v_ = v1_;

#ifndef NDEBUG
        const auto psival2 = GetMachine().LogVal(v_);
        if (std::abs(std::exp(psival2 - psival1 - lvd) - 1.) > 1.0e-8) {
          std::cerr << psival2 - psival1 << " and logvaldiff is " << lvd
                    << std::endl;
          std::cerr << psival2 << " and LogVal with Lt is "
                    << GetMachine().LogVal(v_, lt_) << std::endl;
          std::abort();
        }
#endif
      }
      moves_[0] += 1;
    }
  }

  const Eigen::VectorXd &Visible() const noexcept override { return v_; }

  void SetVisible(const Eigen::VectorXd &v) override { v_ = v; }

  AbstractMachine::VectorType DerLogVisible() override {
    return GetMachine().DerLog(v_, lt_);
  }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < 1; i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }
};

}  // namespace netket

#endif