///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MATH_FLOAT_BACKEND_HPP
#define BOOST_MATH_FLOAT_BACKEND_HPP

#include <iostream>
#include <iomanip>
#include <sstream>
#include <boost/cstdint.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <nil/crypto3/multiprecision/number.hpp>
#include <boost/integer/common_factor_rt.hpp>
#include <boost/type_traits/common_type.hpp>
#include <boost/container_hash/hash.hpp>

namespace nil {
    namespace crypto3 {
        namespace multiprecision {
            namespace backends {

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable : 4389 4244 4018 4244 4127)
#endif

                template<class Arithmetic>
                struct arithmetic_backend {
                    typedef boost::mpl::list<short, int, long, long long> signed_types;
                    typedef boost::mpl::list<unsigned short, unsigned, unsigned long, unsigned long long>
                        unsigned_types;
                    typedef boost::mpl::list<float, double, long double> float_types;
                    typedef int exponent_type;

                    BOOST_MP_CXX14_CONSTEXPR arithmetic_backend() : m_value(0) {
                    }
                    BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const arithmetic_backend& o) : m_value(o.m_value) {
                    }
                    template<class A>
                    BOOST_MP_CXX14_CONSTEXPR
                        arithmetic_backend(const A& o,
                                           const typename boost::enable_if<boost::is_arithmetic<A>>::type* = 0) :
                        m_value(o) {
                    }
                    template<class A>
                    BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const arithmetic_backend<A>& o) : m_value(o.data()) {
                    }
                    BOOST_MP_CXX14_CONSTEXPR arithmetic_backend& operator=(const arithmetic_backend& o) {
                        m_value = o.m_value;
                        return *this;
                    }
                    template<class A>
                    BOOST_MP_CXX14_CONSTEXPR
                        typename boost::enable_if<boost::is_arithmetic<A>, arithmetic_backend&>::type
                        operator=(A i) {
                        m_value = i;
                        return *this;
                    }
                    template<class A>
                    BOOST_MP_CXX14_CONSTEXPR arithmetic_backend& operator=(const arithmetic_backend<A>& i) {
                        m_value = i.data();
                        return *this;
                    }
                    arithmetic_backend& operator=(const char* s) {
#ifndef BOOST_NO_EXCEPTIONS
                        try {
#endif
                            m_value = boost::lexical_cast<Arithmetic>(s);
#ifndef BOOST_NO_EXCEPTIONS
                        } catch (const boost::bad_lexical_cast&) {
                            throw std::runtime_error(std::string("Unable to interpret the string provided: \"") + s +
                                                     std::string("\" as a compatible number type."));
                        }
#endif
                        return *this;
                    }
                    BOOST_MP_CXX14_CONSTEXPR void swap(arithmetic_backend& o) {
                        std::swap(m_value, o.m_value);
                    }
                    std::string str(std::streamsize digits, std::ios_base::fmtflags f) const {
                        std::stringstream ss;
                        ss.flags(f);
                        ss << std::setprecision(digits ? digits : std::numeric_limits<Arithmetic>::digits10 + 4)
                           << m_value;
                        return ss.str();
                    }
                    BOOST_MP_CXX14_CONSTEXPR void do_negate(const boost::mpl::true_&) {
                        m_value = 1 + ~m_value;
                    }
                    BOOST_MP_CXX14_CONSTEXPR void do_negate(const boost::mpl::false_&) {
                        m_value = -m_value;
                    }
                    BOOST_MP_CXX14_CONSTEXPR void negate() {
                        do_negate(boost::mpl::bool_<boost::is_unsigned<Arithmetic>::value>());
                    }
                    BOOST_MP_CXX14_CONSTEXPR int compare(const arithmetic_backend& o) const {
                        return m_value > o.m_value ? 1 : (m_value < o.m_value ? -1 : 0);
                    }
                    template<class A>
                    BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A>, int>::type
                        compare(A i) const {
                        return m_value > static_cast<Arithmetic>(i) ? 1 :
                                                                      (m_value < static_cast<Arithmetic>(i) ? -1 : 0);
                    }
                    BOOST_MP_CXX14_CONSTEXPR Arithmetic& data() {
                        return m_value;
                    }
                    BOOST_MP_CXX14_CONSTEXPR const Arithmetic& data() const {
                        return m_value;
                    }

                private:
                    Arithmetic m_value;
                };

                template<class R, class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_integral<R>::value>::type
                    eval_convert_to(R* result, const arithmetic_backend<Arithmetic>& backend) {
                    typedef typename boost::common_type<R, Arithmetic>::type c_type;
                    BOOST_CONSTEXPR const c_type max = static_cast<c_type>((std::numeric_limits<R>::max)());
                    BOOST_CONSTEXPR const c_type min = static_cast<c_type>((std::numeric_limits<R>::min)());
                    c_type ct = static_cast<c_type>(backend.data());
                    if ((backend.data() < 0) && !std::numeric_limits<R>::is_signed)
                        BOOST_THROW_EXCEPTION(std::range_error("Attempt to convert negative number to unsigned type."));
                    if (ct > max)
                        *result = boost::is_signed<R>::value ? (std::numeric_limits<R>::max)() : backend.data();
                    else if (std::numeric_limits<Arithmetic>::is_signed && (ct < min))
                        *result = (std::numeric_limits<R>::min)();
                    else
                        *result = backend.data();
                }

                template<class R, class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::disable_if_c<boost::is_integral<R>::value>::type
                    eval_convert_to(R* result, const arithmetic_backend<Arithmetic>& backend) {
                    *result = backend.data();
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR bool eval_eq(const arithmetic_backend<Arithmetic>& a,
                                                             const arithmetic_backend<Arithmetic>& b) {
                    return a.data() == b.data();
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>, bool>::type
                    eval_eq(const arithmetic_backend<Arithmetic>& a, const A2& b) {
                    return a.data() == static_cast<Arithmetic>(b);
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR bool eval_lt(const arithmetic_backend<Arithmetic>& a,
                                                             const arithmetic_backend<Arithmetic>& b) {
                    return a.data() < b.data();
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>, bool>::type
                    eval_lt(const arithmetic_backend<Arithmetic>& a, const A2& b) {
                    return a.data() < static_cast<Arithmetic>(b);
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR bool eval_gt(const arithmetic_backend<Arithmetic>& a,
                                                             const arithmetic_backend<Arithmetic>& b) {
                    return a.data() > b.data();
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>, bool>::type
                    eval_gt(const arithmetic_backend<Arithmetic>& a, const A2& b) {
                    return a.data() > static_cast<Arithmetic>(b);
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_add(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& o) {
                    result.data() += o.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_subtract(arithmetic_backend<Arithmetic>& result,
                                                                   const arithmetic_backend<Arithmetic>& o) {
                    result.data() -= o.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_multiply(arithmetic_backend<Arithmetic>& result,
                                                                   const arithmetic_backend<Arithmetic>& o) {
                    result.data() *= o.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::enable_if_c<std::numeric_limits<Arithmetic>::has_infinity>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o) {
                    result.data() /= o.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::disable_if_c<std::numeric_limits<Arithmetic>::has_infinity>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o) {
                    if (!o.data())
                        BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
                    result.data() /= o.data();
                }

                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>>::type
                    eval_add(arithmetic_backend<Arithmetic>& result, const A2& o) {
                    result.data() += o;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>>::type
                    eval_subtract(arithmetic_backend<Arithmetic>& result, const A2& o) {
                    result.data() -= o;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>>::type
                    eval_multiply(arithmetic_backend<Arithmetic>& result, const A2& o) {
                    result.data() *= o;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::enable_if_c<(boost::is_arithmetic<A2>::value &&
                                                 !std::numeric_limits<Arithmetic>::has_infinity)>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const A2& o) {
                    if (!o)
                        BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
                    result.data() /= o;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::enable_if_c<(boost::is_arithmetic<A2>::value &&
                                                 std::numeric_limits<Arithmetic>::has_infinity)>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const A2& o) {
                    result.data() /= o;
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_add(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& a,
                                                              const arithmetic_backend<Arithmetic>& b) {
                    result.data() = a.data() + b.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_subtract(arithmetic_backend<Arithmetic>& result,
                                                                   const arithmetic_backend<Arithmetic>& a,
                                                                   const arithmetic_backend<Arithmetic>& b) {
                    result.data() = a.data() - b.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_multiply(arithmetic_backend<Arithmetic>& result,
                                                                   const arithmetic_backend<Arithmetic>& a,
                                                                   const arithmetic_backend<Arithmetic>& b) {
                    result.data() = a.data() * b.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::enable_if_c<std::numeric_limits<Arithmetic>::has_infinity>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a,
                                const arithmetic_backend<Arithmetic>& b) {
                    result.data() = a.data() / b.data();
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::disable_if_c<std::numeric_limits<Arithmetic>::has_infinity>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a,
                                const arithmetic_backend<Arithmetic>& b) {
                    if (!b.data())
                        BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
                    result.data() = a.data() / b.data();
                }

                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>>::type
                    eval_add(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a,
                             const A2& b) {
                    result.data() = a.data() + b;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>>::type
                    eval_subtract(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a,
                                  const A2& b) {
                    result.data() = a.data() - b;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_arithmetic<A2>>::type
                    eval_multiply(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a,
                                  const A2& b) {
                    result.data() = a.data() * b;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::enable_if_c<(boost::is_arithmetic<A2>::value &&
                                                 !std::numeric_limits<Arithmetic>::has_infinity)>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a,
                                const A2& b) {
                    if (!b)
                        BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
                    result.data() = a.data() / b;
                }
                template<class Arithmetic, class A2>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::enable_if_c<(boost::is_arithmetic<A2>::value &&
                                                 std::numeric_limits<Arithmetic>::has_infinity)>::type
                    eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a,
                                const A2& b) {
                    result.data() = a.data() / b;
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR bool eval_is_zero(const arithmetic_backend<Arithmetic>& val) {
                    return val.data() == 0;
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::enable_if_c<(!std::numeric_limits<Arithmetic>::is_specialized ||
                                                 std::numeric_limits<Arithmetic>::is_signed),
                                                int>::type
                    eval_get_sign(const arithmetic_backend<Arithmetic>& val) {
                    return val.data() == 0 ? 0 : val.data() < 0 ? -1 : 1;
                }
                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR
                    typename boost::disable_if_c<(std::numeric_limits<Arithmetic>::is_specialized ||
                                                  std::numeric_limits<Arithmetic>::is_signed),
                                                 int>::type
                    eval_get_sign(const arithmetic_backend<Arithmetic>& val) {
                    return val.data() == 0 ? 0 : 1;
                }

                template<class T>
                inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if<boost::is_unsigned<T>, T>::type abs(T v) {
                    return v;
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_abs(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& o) {
                    using nil::crypto3::multiprecision::backends::abs;
                    using std::abs;
                    result.data() = abs(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_fabs(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    result.data() = std::abs(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_floor(arithmetic_backend<Arithmetic>& result,
                                                                const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = floor(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_ceil(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = ceil(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_sqrt(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = sqrt(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR int eval_fpclassify(const arithmetic_backend<Arithmetic>& o) {
                    return (boost::math::fpclassify)(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_trunc(arithmetic_backend<Arithmetic>& result,
                                                                const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = trunc(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_round(arithmetic_backend<Arithmetic>& result,
                                                                const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = round(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_frexp(arithmetic_backend<Arithmetic>& result,
                                                                const arithmetic_backend<Arithmetic>& a, int* v) {
                    BOOST_MATH_STD_USING
                    result.data() = frexp(a.data(), v);
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_ldexp(arithmetic_backend<Arithmetic>& result,
                                                                const arithmetic_backend<Arithmetic>& a, int v) {
                    BOOST_MATH_STD_USING
                    result.data() = ldexp(a.data(), v);
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_exp(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = exp(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_log(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = log(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_log10(arithmetic_backend<Arithmetic>& result,
                                                                const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = log10(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_sin(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = sin(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_cos(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = cos(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_tan(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = tan(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_acos(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = acos(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_asin(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = asin(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_atan(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = atan(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_sinh(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = sinh(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_cosh(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = cosh(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_tanh(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& o) {
                    BOOST_MATH_STD_USING
                    result.data() = tanh(o.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_fmod(arithmetic_backend<Arithmetic>& result,
                                                               const arithmetic_backend<Arithmetic>& a,
                                                               const arithmetic_backend<Arithmetic>& b) {
                    BOOST_MATH_STD_USING
                    result.data() = fmod(a.data(), b.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_pow(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& a,
                                                              const arithmetic_backend<Arithmetic>& b) {
                    BOOST_MATH_STD_USING
                    result.data() = pow(a.data(), b.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_atan2(arithmetic_backend<Arithmetic>& result,
                                                                const arithmetic_backend<Arithmetic>& a,
                                                                const arithmetic_backend<Arithmetic>& b) {
                    BOOST_MATH_STD_USING
                    result.data() = atan2(a.data(), b.data());
                }

                template<class Arithmetic, class I>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_left_shift(arithmetic_backend<Arithmetic>& result, I val) {
                    result.data() <<= val;
                }

                template<class Arithmetic, class I>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_right_shift(arithmetic_backend<Arithmetic>& result, I val) {
                    result.data() >>= val;
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_modulus(arithmetic_backend<Arithmetic>& result,
                                                                  const arithmetic_backend<Arithmetic>& a) {
                    result.data() %= a.data();
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_and(arithmetic_backend<Arithmetic>& result,
                                                                      const arithmetic_backend<Arithmetic>& a) {
                    result.data() &= a.data();
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_or(arithmetic_backend<Arithmetic>& result,
                                                                     const arithmetic_backend<Arithmetic>& a) {
                    result.data() |= a.data();
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_xor(arithmetic_backend<Arithmetic>& result,
                                                                      const arithmetic_backend<Arithmetic>& a) {
                    result.data() ^= a.data();
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_complement(arithmetic_backend<Arithmetic>& result,
                                                                     const arithmetic_backend<Arithmetic>& a) {
                    result.data() = ~a.data();
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_gcd(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& a,
                                                              const arithmetic_backend<Arithmetic>& b) {
                    result.data() = boost::integer::gcd(a.data(), b.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR void eval_lcm(arithmetic_backend<Arithmetic>& result,
                                                              const arithmetic_backend<Arithmetic>& a,
                                                              const arithmetic_backend<Arithmetic>& b) {
                    result.data() = boost::integer::lcm(a.data(), b.data());
                }

                template<class Arithmetic>
                inline BOOST_MP_CXX14_CONSTEXPR std::size_t hash_value(const arithmetic_backend<Arithmetic>& a) {
                    boost::hash<Arithmetic> hasher;
                    return hasher(a.data());
                }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

            }    // namespace backends

            using nil::crypto3::multiprecision::backends::arithmetic_backend;

            template<class Arithmetic>
            struct number_category<arithmetic_backend<Arithmetic>>
                : public boost::mpl::int_<boost::is_integral<Arithmetic>::value ? number_kind_integer :
                                                                                number_kind_floating_point> { };

            namespace detail {

                template<class Backend>
                struct double_precision_type;

                template<class Arithmetic, nil::crypto3::multiprecision::expression_template_option ET>
                struct double_precision_type<number<arithmetic_backend<Arithmetic>, ET>> {
                    typedef number<arithmetic_backend<typename double_precision_type<Arithmetic>::type>, ET> type;
                };
                template<>
                struct double_precision_type<arithmetic_backend<boost::int32_t>> {
                    typedef arithmetic_backend<boost::int64_t> type;
                };

            }    // namespace detail

        }    // namespace multiprecision
    }        // namespace crypto3
}    // namespace nil
#if !(defined(__SGI_STL_PORT) || defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS))
     //
// We shouldn't need these to get code to compile, however for the sake of
// "level playing field" performance comparisons they avoid the very slow
// lexical_cast's that would otherwise take place.  Definition has to be guarded
// by the inverse of pp-logic in real_concept.hpp which defines these as a workaround
// for STLPort plus some other old/broken standartd libraries.
//
namespace boost {
    namespace math {
        namespace tools {

            template<>
            inline unsigned int real_cast<unsigned int, concepts::real_concept>(concepts::real_concept r) {
                return static_cast<unsigned int>(r.value());
            }

            template<>
            inline int real_cast<int, concepts::real_concept>(concepts::real_concept r) {
                return static_cast<int>(r.value());
            }

            template<>
            inline long real_cast<long, concepts::real_concept>(concepts::real_concept r) {
                return static_cast<long>(r.value());
            }

            // Converts from T to narrower floating-point types, float, double & long double.

            template<>
            inline float real_cast<float, concepts::real_concept>(concepts::real_concept r) {
                return static_cast<float>(r.value());
            }
            template<>
            inline double real_cast<double, concepts::real_concept>(concepts::real_concept r) {
                return static_cast<double>(r.value());
            }
            template<>
            inline long double real_cast<long double, concepts::real_concept>(concepts::real_concept r) {
                return r.value();
            }

        }    // namespace tools
    }        // namespace math
}    // namespace boost
#endif

namespace std {

    template<class Arithmetic, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates>
    class numeric_limits<nil::crypto3::multiprecision::number<
        nil::crypto3::multiprecision::arithmetic_backend<Arithmetic>, ExpressionTemplates>>
        : public std::numeric_limits<Arithmetic> {
        typedef std::numeric_limits<Arithmetic> base_type;
        typedef nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::arithmetic_backend<Arithmetic>,
                                                     ExpressionTemplates>
            number_type;

    public:
        BOOST_STATIC_CONSTEXPR number_type(min)() BOOST_NOEXCEPT {
            return (base_type::min)();
        }
        BOOST_STATIC_CONSTEXPR number_type(max)() BOOST_NOEXCEPT {
            return (base_type::max)();
        }
        BOOST_STATIC_CONSTEXPR number_type lowest() BOOST_NOEXCEPT {
            return -(max)();
        }
        BOOST_STATIC_CONSTEXPR number_type epsilon() BOOST_NOEXCEPT {
            return base_type::epsilon();
        }
        BOOST_STATIC_CONSTEXPR number_type round_error() BOOST_NOEXCEPT {
            return epsilon() / 2;
        }
        BOOST_STATIC_CONSTEXPR number_type infinity() BOOST_NOEXCEPT {
            return base_type::infinity();
        }
        BOOST_STATIC_CONSTEXPR number_type quiet_NaN() BOOST_NOEXCEPT {
            return base_type::quiet_NaN();
        }
        BOOST_STATIC_CONSTEXPR number_type signaling_NaN() BOOST_NOEXCEPT {
            return base_type::signaling_NaN();
        }
        BOOST_STATIC_CONSTEXPR number_type denorm_min() BOOST_NOEXCEPT {
            return base_type::denorm_min();
        }
    };

    template<>
    class numeric_limits<boost::math::concepts::real_concept> : public std::numeric_limits<long double> {
        typedef std::numeric_limits<long double> base_type;
        typedef boost::math::concepts::real_concept number_type;

    public:
        static const number_type(min)() BOOST_NOEXCEPT {
            return (base_type::min)();
        }
        static const number_type(max)() BOOST_NOEXCEPT {
            return (base_type::max)();
        }
        static const number_type lowest() BOOST_NOEXCEPT {
            return -(max)();
        }
        static const number_type epsilon() BOOST_NOEXCEPT {
            return base_type::epsilon();
        }
        static const number_type round_error() BOOST_NOEXCEPT {
            return epsilon() / 2;
        }
        static const number_type infinity() BOOST_NOEXCEPT {
            return base_type::infinity();
        }
        static const number_type quiet_NaN() BOOST_NOEXCEPT {
            return base_type::quiet_NaN();
        }
        static const number_type signaling_NaN() BOOST_NOEXCEPT {
            return base_type::signaling_NaN();
        }
        static const number_type denorm_min() BOOST_NOEXCEPT {
            return base_type::denorm_min();
        }
    };

}    // namespace std

#include <nil/crypto3/multiprecision/detail/integer_ops.hpp>

#endif


#include <iostream>
#include <algorithm>

#include "render/Colour.h"
#include "utilities/Utilities.h"
#include "error/Exception.h"

#include <boost/format.hpp>

using namespace epsilon;

Colour::Colour() : r(1.0f), g(1.0f), b(1.0f), a(1.0f)
{
}

Colour::Colour(float grey)
{
	grey = NormaliseColourValue(grey);

	r = grey;
	g = grey;
	b = grey;
	a = grey;
}

Colour::Colour(const Vector3& other)
{
	r = NormaliseColourValue(other.x);
	g = NormaliseColourValue(other.y);
	b = NormaliseColourValue(other.z);
	a = 1.0f;
}

Colour::Colour(const Vector4& other)
{
	r = NormaliseColourValue(other.x);
	g = NormaliseColourValue(other.y);
	b = NormaliseColourValue(other.z);
	a = NormaliseColourValue(other.w);
}

Colour::Colour(float ir, float ig, float ib, float ia)
{
	r = NormaliseColourValue(ir);
	g = NormaliseColourValue(ig);
	b = NormaliseColourValue(ib);
	a = NormaliseColourValue(ia);
}

// Assignment Operators
Colour& Colour::operator=(const Colour& other)
{
	r = other.r;
	g = other.g;
	b = other.b;
	a = other.a;
	return *this;
}

Colour& Colour::operator=(const Vector4& other)
{
	r = other.x;
	g = other.y;
	b = other.z;
	a = other.w;
	return *this;
}

Colour Colour::FromVector4(const Vector4& other)
{
	return Colour(other.x, other.y, other.z, other.w);
}

Vector4 Colour::ToVector4()
{
	return Vector4(r, g, b, a);
}

float &Colour::operator[](int i)
{
    if ( i == 0 )
	{
		return r;
	}
	else if ( i == 1 )
	{
		return g;
	}
	else if ( i == 2 )
	{
		return b;
	}
	else if ( i == 3 )
	{
		return a;
	}
    else
    {
        throw BoundaryException("Colour", i, 4);
    }
}

std::string Colour::ToString()
{
    return boost::str(format("r: %3.2f g: %3.2f b: %3.2f a: %1.1f") % r % g % b % a);
}

Colour Colour::FromHex(std::string hexStr)
{
	// If invalid hex string, just return white.
	if (hexStr.length() < 3)
		return Colour();

	// If short hand hex is used e.g. "FFF"
	if ( hexStr.length() == 3 )
	{
		// Expand it
		size_t i = 0;
		while (i < hexStr.length())
		{
			hexStr.insert(i, 2, hexStr.at(i));
			i+=3;
		}
	}

	// If the hex value isn't prefixed with an '0x'
	if ( hexStr.at(1) != 'x' )
	{
		// Add one
		hexStr = std::string("0x") + hexStr;
	}

	int hexValue = lexical_cast<int>(hexStr);

	return Colour(static_cast<float>(NormaliseColourValue(hexValue & 255)),
				  static_cast<float>(NormaliseColourValue((hexValue >> 8) & 255)),
				  static_cast<float>(NormaliseColourValue((hexValue >> 16) & 255)),
				  1.0f);
}

void Colour::To8bit()
{
	 r *= 255.0f;
	 g *= 255.0f;
	 b *= 255.0f;
	 a *= 255.0f;
}

std::list<float> Colour::ToList()
{
	float l[] = {r,g,b,a};
	return std::list<float>(l, l + sizeof(l) / sizeof(float) );
}

void Colour::Invert()
{
	r = 1.0f - r;
	g = 1.0f - g;
	b = 1.0f - b;
	a = 1.0f;
}

Colour Colour::Inverted()
{
	return Colour(1.0f - r,
				  1.0f - g,
				  1.0f - b,
				  1.0f);
}

void Colour::Tint(const Colour &other, float prop)
{
	r = (r * (1.0f - prop) + (other.r * prop) );
	g = (g * (1.0f - prop) + (other.g * prop) );
	b = (b * (1.0f - prop) + (other.b * prop) );
	a = (a * (1.0f - prop) + (other.a * prop) );
}

Colour Colour::Tinted(const Colour &other, float prop)
{
	return Colour( (r * (1.0f - prop) + (other.r * prop) ),
				   (g * (1.0f - prop) + (other.g * prop) ),
				   (b * (1.0f - prop) + (other.b * prop) ),
				   (a * (1.0f - prop) + (other.a * prop) ) );
}

float Colour::NormaliseColourValue(float value)
{
	value = std::max( std::min(value, 255.0f), 0.0f );

	if ( value > 1.0f )
	{
		value /= 255.0f;
	}
	return value;
}

const Colour Colour::RED(   1.f, 0.f,  0.f, 1.f);
const Colour Colour::GREEN( 0.f, 1.f,  0.f, 1.f);
const Colour Colour::BLUE(  0.f, 0.f,  1.f, 1.f);
const Colour Colour::YELLOW(1.f, 1.f,  0.f, 1.f);
const Colour Colour::ORANGE(1.f, 0.5f, 0.f, 1.f);
const Colour Colour::CYAN(  0.f, 1.f,  1.f, 1.f);
const Colour Colour::PURPLE(1.f, 0.f,  1.f, 1.f);
const Colour Colour::BLACK( 0.f, 0.f,  0.f, 1.f);
const Colour Colour::GREY(  0.5f,0.5f, 0.5f, 1.f);


#pragma once

#include <vector>

#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_iterator.hpp>
#include <boost/graph/filtered_graph.hpp>

#include <boost/multi_index_container.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/random_access_index.hpp>
#include <boost/multi_index/composite_key.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/tag.hpp>

#include <boost/optional.hpp>

#include "node.hpp"

struct edge_info_t {
    int weight;
    bool is_tmp;
    int interm_inode;
};

typedef boost::adjacency_list<
        boost::listS,
        boost::vecS,
        boost::bidirectionalS,
        boost::no_property,
        edge_info_t
> graph_t;
typedef boost::property_map<graph_t, int edge_info_t::*>::type edge_info_map_t;
typedef boost::property_map<graph_t, int edge_info_t::*>::const_type const_edge_info_map_t;
typedef graph_t::vertex_descriptor vertex_descriptor;
typedef graph_t::edge_descriptor edge_descriptor;

typedef boost::property_map<graph_t, boost::vertex_index_t>::type IndexMap;
typedef boost::graph_traits<graph_t>::adjacency_iterator adjacency_iterator;
typedef boost::graph_traits<graph_t>::out_edge_iterator out_edge_iterator;
typedef boost::graph_traits<graph_t>::in_edge_iterator in_edge_iterator;

class FilterEdges {
public:
    void add_edge(const edge_descriptor &e);
    void rm_edge(const edge_descriptor &e);
    bool exists(const edge_descriptor &e) const;
    template <typename EdgeDesc>
    bool operator()(const EdgeDesc &e) const;

private:
    std::set<edge_descriptor> edges_;
};

struct found_goal {}; // exception for termination

template <class Graph, class CostType>
class astar_heuristic : public boost::astar_heuristic<Graph, CostType> {
public:
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex;

    astar_heuristic(int col_num, Node goal_node);
    CostType operator()(vertex u);

private:
    int col_num_;
    Node goal_node_;
};

// visitor that terminates when we find the goal
template <class Vertex>
class astar_goal_visitor : public boost::default_astar_visitor {
public:
    astar_goal_visitor(Vertex goal);
    template<class Graph> void examine_vertex(Vertex u, Graph& /* g */);
private:
    Vertex m_goal;
};

#ifdef USE_BOARD_GRAPH_CACHE
struct by_node {};
typedef boost::multi_index_container<
    Node,
    boost::multi_index::indexed_by<
        boost::multi_index::random_access<>,
        boost::multi_index::ordered_unique<
            boost::multi_index::tag<by_node>,
            boost::multi_index::identity<Node>>
    >
> path_t;

struct cached_path_data_t {
    Node start_node;
    Node end_node;
    path_t path;
    size_t len;
    bool is_exists;
};

struct by_node_len {};
struct by_node_node {};
typedef boost::multi_index_container<
    cached_path_data_t,
    boost::multi_index::indexed_by<
        boost::multi_index::ordered_non_unique<
            boost::multi_index::tag<by_node_len>,
            boost::multi_index::composite_key<
                cached_path_data_t,
                boost::multi_index::member<
                    cached_path_data_t, Node, &cached_path_data_t::start_node>,
                boost::multi_index::member<
                    cached_path_data_t, size_t, &cached_path_data_t::len>
            >
        >,
        boost::multi_index::ordered_non_unique<
            boost::multi_index::tag<by_node_node>,
            boost::multi_index::composite_key<
                cached_path_data_t,
                boost::multi_index::member<
                    cached_path_data_t, Node, &cached_path_data_t::start_node>,
                boost::multi_index::member<
                    cached_path_data_t, Node, &cached_path_data_t::end_node>
            >
        >
    >
> path_data_list_t;
#endif

struct goal_nodes_t {
    Node node;
    const std::set<Node> *goal_nodes;
};

class BoardGraph {
public:
    BoardGraph(int row_num, int col_num);

    bool remove_edges(const std::vector<std::pair<Node, Node>> &node_pair_list,
            const std::vector<goal_nodes_t> &goal_nodes_list, bool check_only);

    void block_node(const Node &node);
    void unblock_node(const Node &node);
    std::list<Node> adjacent_nodes(const Node &node) const;

    size_t shortest_path(const Node &start_node,
            const std::set<Node> &goal_nodes, std::list<Node> *path) const;
    boost::optional<std::list<Node>> find_path(const Node &start_node,
            const Node &end_node) const;
    bool is_adjacent(const Node &from_node, const Node &to_node) const;

private:
    bool check_paths_to_goal_nodes(
            const std::vector<goal_nodes_t> &goal_nodes_list,
            const std::vector<std::pair<int, int>> &removed_edges) const;
    bool is_adjacent(int from_inode, int to_inode, bool check_tmp_edges) const;
    void block_inode(int inode);
    void unblock_inode(int inode);
    bool block_edge(int from_inode, int to_inode, bool is_tmp);
    void block_edge(edge_descriptor e, bool is_tmp);
    bool unblock_edge(int from_inode, int to_inode, bool is_tmp, int interm_inode = -1);
    bool is_inode_valid(int inode) const;
    std::vector<edge_descriptor> find_tmp_edges(int inode) const;

#ifdef USE_BOARD_GRAPH_CACHE
private:
    void add_path_to_cache(const Node &start_node, const Node &end_node,
            const std::list<Node> &path, bool is_exists) const;
    size_t cached_shortest_path(const Node &start_node,
            const std::set<Node> &goal_nodes, std::list<Node> *path) const;
    boost::optional<std::list<Node>> cached_path(const Node &start_node,
            const Node &end_node) const;
    void update_cached_path(const Node &node) const;
    void remove_cached_path(const Node &node) const;
#endif

private:
    graph_t g_;
    int row_num_;
    int col_num_;
    FilterEdges fe_;
    std::map<int, std::set<std::pair<int, int>>> tmp_edges_;
#ifdef USE_BOARD_GRAPH_CACHE
    mutable path_data_list_t path_data_list_;
#endif
};


#include <bluetoe/delta_time.hpp>

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

namespace bll = bluetoe::link_layer;

BOOST_FIXTURE_TEST_CASE( is_default_constructable, bluetoe::link_layer::delta_time )
{
    BOOST_CHECK_EQUAL( usec(), 0u );
    BOOST_CHECK_EQUAL( bluetoe::link_layer::delta_time::now().usec(), 0u );
}

BOOST_AUTO_TEST_CASE( constructable_from_number_of_micro_seconds )
{
    BOOST_CHECK_EQUAL( bluetoe::link_layer::delta_time( 4711 ).usec(), 4711u );
    BOOST_CHECK_EQUAL( bluetoe::link_layer::delta_time::usec( 4711 ).usec(), 4711u );
}

BOOST_AUTO_TEST_CASE( constructable_from_number_of_milli_seconds )
{
    BOOST_CHECK_EQUAL( bluetoe::link_layer::delta_time::msec( 4711 ).usec(), 4711000u );
}

BOOST_AUTO_TEST_CASE( constructable_from_number_of_seconds )
{
    BOOST_CHECK_EQUAL( bluetoe::link_layer::delta_time::seconds( 11 ).usec(), 11000000u );
}

BOOST_AUTO_TEST_CASE( provides_equal_comparison )
{
    BOOST_CHECK( bll::delta_time::usec( 5000 ) == bll::delta_time::msec( 5 ) );
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) == bll::delta_time::usec( 5001 ) ) );
}

BOOST_AUTO_TEST_CASE( provides_inequal_comparison )
{
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) != bll::delta_time::msec( 5 ) ) );
    BOOST_CHECK( bll::delta_time::usec( 5000 ) != bll::delta_time::usec( 5001 ) );
}

BOOST_AUTO_TEST_CASE( provides_less_than_comparison )
{
    BOOST_CHECK( bll::delta_time::usec( 5000 ) < bll::delta_time::usec( 5001 ) );
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) < bll::delta_time::usec( 5000 ) ) );
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) < bll::delta_time::usec( 4999 ) ) );
}

BOOST_AUTO_TEST_CASE( provides_greater_than_comparison )
{
    BOOST_CHECK( bll::delta_time::usec( 5000 ) > bll::delta_time::usec( 4999 ) );
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) > bll::delta_time::usec( 5000 ) ) );
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) > bll::delta_time::usec( 5001 ) ) );
}

BOOST_AUTO_TEST_CASE( provides_less_than_or_equal_comparison )
{
    BOOST_CHECK( bll::delta_time::usec( 5000 ) <= bll::delta_time::usec( 5001 ) );
    BOOST_CHECK( bll::delta_time::usec( 5000 ) <= bll::delta_time::usec( 5000 ) );
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) <= bll::delta_time::usec( 4999 ) ) );
}

BOOST_AUTO_TEST_CASE( provides_greater_than_or_equal_comparison )
{
    BOOST_CHECK( bll::delta_time::usec( 5000 ) >= bll::delta_time::usec( 4999 ) );
    BOOST_CHECK( bll::delta_time::usec( 5000 ) >= bll::delta_time::usec( 5000 ) );
    BOOST_CHECK( !( bll::delta_time::usec( 5000 ) >= bll::delta_time::usec( 5001 ) ) );
}

BOOST_AUTO_TEST_CASE( provides_a_zero_check )
{
    BOOST_CHECK( bll::delta_time::now().zero() );
    BOOST_CHECK( !bll::delta_time::usec( 5000 ).zero() );
}

BOOST_AUTO_TEST_CASE( multiplication_with_zero )
{
    bll::delta_time z( 0 );
    bll::delta_time o( 1 );
    bll::delta_time h( 100 );

    z *= 0;
    o *= 0;
    h *= 0;

    BOOST_CHECK_EQUAL( z, bll::delta_time::now() );
    BOOST_CHECK_EQUAL( o, bll::delta_time::now() );
    BOOST_CHECK_EQUAL( h, bll::delta_time::now() );
}

BOOST_AUTO_TEST_CASE( multiplication_with_one )
{
    bll::delta_time z( 0 );
    bll::delta_time o( 1 );
    bll::delta_time h( 100 );

    z *= 1;
    o *= 1;
    h *= 1;

    BOOST_CHECK_EQUAL( z, bll::delta_time( 0 ) );
    BOOST_CHECK_EQUAL( o, bll::delta_time( 1 ) );
    BOOST_CHECK_EQUAL( h, bll::delta_time( 100 ) );
}

BOOST_AUTO_TEST_CASE( multiplication_with_greater_than_1 )
{
    bll::delta_time z( 0 );
    bll::delta_time o( 1 );
    bll::delta_time h( 100 );

    z *= 5;
    o *= 5;
    h *= 5;

    BOOST_CHECK_EQUAL( z, bll::delta_time( 0 ) );
    BOOST_CHECK_EQUAL( o, bll::delta_time( 5 ) );
    BOOST_CHECK_EQUAL( h, bll::delta_time( 500 ) );
}

BOOST_AUTO_TEST_CASE( binary_multiplication_operators )
{
    BOOST_CHECK_EQUAL( bll::delta_time( 5 ) * 5, bll::delta_time( 25 ) );
    BOOST_CHECK_EQUAL( 5 * bll::delta_time( 5 ), bll::delta_time( 25 ) );
}

BOOST_AUTO_TEST_CASE( division_by_delta_time )
{
    bll::delta_time h( 100 );
    bll::delta_time z( 20 );
    bll::delta_time s( 1000 );

    BOOST_CHECK_EQUAL( h / z, 5u );
    BOOST_CHECK_EQUAL( h / s, 0u );
    BOOST_CHECK_EQUAL( h / h, 1u );
}

BOOST_AUTO_TEST_SUITE( Abort_Operation_Procedure )

static constexpr long required_accuary = 1000000;

static void check_close( const bll::delta_time& result, std::uint32_t expected )
{
    const std::uint32_t tolerance = std::max< std::uint32_t >( 1, expected / required_accuary );

    BOOST_CHECK_GE(
        static_cast< std::int64_t >( result.usec() ),
        std::int64_t( expected ) - tolerance );

    BOOST_CHECK_LE(
        static_cast< std::int64_t >( result.usec() ),
        std::int64_t( expected ) + tolerance );
}

BOOST_AUTO_TEST_CASE( can_calculate_part_per_million )
{
    check_close( bll::delta_time::usec( 5000 ).ppm( 1000 ), 5 );
}

BOOST_AUTO_TEST_CASE( ppm_with_calculations_that_should_overflow_32bit_math )
{
    check_close(
        bll::delta_time::usec( 100 * 1000 * 1000 ).ppm( 1000 ),
        100 * 1000 );
}

BOOST_AUTO_TEST_CASE( ppm_with_some_extrem_values )
{
    check_close( bll::delta_time::usec( 1000 * 1000 * 1000 ).ppm( 0 ), 0 );
    check_close( bll::delta_time::usec( 1000 * 1000 * 1000 ).ppm( 1 ), 1000 );
}

BOOST_AUTO_TEST_CASE( ppm_rounded_down )
{
    check_close( bll::delta_time::usec( 1000 * 1000 - 2 ).ppm( 44 ), 43 );
    check_close( bll::delta_time::usec( 44 ).ppm( 1000 * 1000 - 2 ), 43 );
}

BOOST_AUTO_TEST_SUITE_END()


/*
  Training from MC trajectories
 */
#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <boost/program_options.hpp>

#include "msa.hpp"
#include "model.hpp"
#include "MCseq.hpp"
#include "OptParam.hpp"
#include "AnaTraj.hpp"
using namespace std;

namespace po = boost::program_options;

int main(int argc, char *argv[])
{
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h","produce help message")
    ("param,p", po::value<string>(), "input LGM parameter file")
    ("logbase,l", po::value<string>(), "basename for log files")
    ("ftraj", po::value<string>(), "trajectory file to be parsed")
    ("fmmt", po::value<string>(), "momentum file to be parsed")
    ("alphaH", po::value<double>(), "alphaH")
    ("alphaJ", po::value<double>(), "alphaJ")
    ("alphaK", po::value<double>(), "alphaK")
    ("betaH", po::value<double>(), "betaH")
    ("betaJ", po::value<double>(), "betaJ")
    ("betaK", po::value<double>(), "betaK")
    ;

  po::positional_options_description p;
  po::variables_map vm;
  po::store(po::command_line_parser(argc,argv).
	    options(desc).positional(p).run(), vm);
  po::notify(vm);

  if(vm.count("help")) {
    cerr << desc << endl;
    return 1;
  }
  string logbase = "ana";
  if(vm.count("logbase")) {
    logbase = vm["logbase"].as<string>();
  }
  cerr << "# LOGBASE= " << logbase << endl;

  Model model;
  if(vm.count("param")) {
    model.read_parameters(vm["param"].as<string>());
  } else {
    cerr << "Specify the parameter file!" << endl;
    exit(1);
  }

  AnaTraj atraj(model);
  string ftraj;
  if(vm.count("ftraj")) {
    ftraj = vm["ftraj"].as<string>();
    cerr << "# Trajectory file: " <<  ftraj << endl;
    atraj.set_trajectory_file(ftraj);
  } else {
    cerr << "Specify a trajectory file!" << endl;
    exit(1);
  }

  OptParam optpar(model);
  string fmmt;
  if(vm.count("fmmt")) {
    fmmt = vm["fmmt"].as<string>();
    cerr << "reading momentum from " << fmmt << endl;
    optpar.read_momentum(model,fmmt);
  }
  if(vm.count("alphaH")) {
    optpar.alpha_H = vm["alphaH"].as<double>();
  }
  if(vm.count("alphaJ")) {
    optpar.alpha_J = vm["alphaJ"].as<double>();
  }
  if(vm.count("alphaK")) {
    optpar.alpha_K = vm["alphaK"].as<double>();
  }
  cerr << "# alpha_{H,J,K}= "
       << optpar.alpha_H << "\t"
       << optpar.alpha_J << "\t"
       << optpar.alpha_K << endl;

  if(vm.count("betaH")) {
    optpar.beta_H = vm["betaH"].as<double>();
  }
  if(vm.count("betaJ")) {
    optpar.beta_J = vm["betaJ"].as<double>();
  }
  if(vm.count("betaK")) {
    optpar.beta_K = vm["betaK"].as<double>();
  }
  cerr << "# beta_{H,J,K}= "
       << optpar.beta_H << "\t"
       << optpar.beta_J << "\t"
       << optpar.beta_K << endl;

  optpar.optimize_traj(model, atraj);
  atraj.unset_trajectory_file();
  model.write_parameters(logbase + ".param");
  optpar.write_momentum(model, logbase + ".mmt");

  return 0;
}


#include <cpp-base64/base64.h>
#include <zlib.h>

#include <Eigen/Core>
#include <array>
#include <chrono>
#include <filesystem>
#include <iostream>
#include <limits>
#include <nlohmann/json.hpp>
#include <ratio>
#include <string>
#include <vector>
#include <websocketpp/config/asio_no_tls.hpp>
#include <websocketpp/server.hpp>

extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/imgutils.h>
#include <libswscale/swscale.h>
}

#include "gaze_view_points.h"
#include "opencl_manager.h"
#include "parameters.h"
#include "projections.h"
#include "sat_decoder.h"
#include "sat_encoder.h"
#include "save_frame.h"
#include "video_decoder.h"
#include "video_encoder.h"

namespace fs = std::filesystem;

int TestFunction(const std::vector<std::string> &args);
int SingleFrameExtraction(const std::vector<std::string> &args);
int ExtractSampledFrameRect(const std::vector<std::string> &args);
int InterpolateSampledFrameRect(const std::vector<std::string> &args);
int EncodeVideo(const std::vector<std::string> &args);
int EncodeLogCartesianVideo(const std::vector<std::string> &args);
int EncodeLogCartesianVideoBitrate(const std::vector<std::string> &args);
int DecodeLogCartesianVideo(const std::vector<std::string> &args);
int FoveateLogCartesianVideo(const std::vector<std::string> &args);

struct AVFrameDeleter {
  void operator()(AVFrame *p) { av_frame_free(&p); }
};

/**
 * @brief Code for testing sampling from the summed area table
 *
 * @return int
 */
int main(int argc, char *argv[]) {
  std::vector<std::string> args(argv, argv + argc);
  if (args[1] == "single_frame") {
    return SingleFrameExtraction(args);
  } else if (args[1] == "interpolate_sampled") {
    return InterpolateSampledFrameRect(args);
  } else if (args[1] == "encode_bitrate") {
    return EncodeLogCartesianVideoBitrate(args);
  } else if (args[1] == "decode") {
    return DecodeLogCartesianVideo(args);
  } else if (args[1] == "foveate_no_encoding") {
    return FoveateLogCartesianVideo(args);
  }
  return EXIT_SUCCESS;
}

int TestFunction(const std::vector<std::string> args) {
  // int64_t kilo = std::kilo::num / std::kilo::den;
  // int64_t mega = std::mega::num / std::mega::den;
  // for (int64_t bitrate = 100 * kilo; bitrate <= 10 * mega; bitrate *= 2) {
  //   std::cerr << "Foveating" << std::endl;
  //   EncodeLogCartesianVideoBitrate(args, bitrate);
  //   std::cerr << "Unfoveating" << std::endl;
  //   UnwarpLogCartesianVideoBitrate(args, bitrate);
  // }
  // return EXIT_SUCCESS;
  using std::unique_ptr;
  using std::filesystem::path;

  path source_video = "360_em_dataset/1080p_videos/03_drone_d5d4gnuAJLo.mp4";
  path output_file = "output_videos/temp/tt.png";
  uint64_t frame_to_extract = 21 * 30;

  float center_x = 0.0;
  float center_y = 1.0;

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);

  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;

  int ret = -1;

  // Get frame from decoder
  unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  for (uint64_t i = 0; i < frame_to_extract; i++) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
  }
  if (ret != 0) {
    std::cerr << "Failed to get rgb frame" << std::endl;
  }

  int width = source_codec_ctx->width;
  int height = source_codec_ctx->height;
  int reduced_width = 16 * std::ceil(width / 1.8 / 16);
  int reduced_height = 16 * std::ceil(height / 1.8 / 16);
  sat_decoder.InitializeGrid(reduced_width, reduced_height,
                             source_codec_ctx->width, source_codec_ctx->height);

  unique_ptr<AVFrame, AVFrameDeleter> rect_frame(av_frame_alloc());
  rect_frame->width = reduced_width;
  rect_frame->height = reduced_height;
  rect_frame->format = AV_PIX_FMT_RGB0;
  av_frame_get_buffer(rect_frame.get(), 0);

  // Copy the frame to the OpenCL GPU
  int source_frame_size = rgb_frame->linesize[0] * rgb_frame->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_ONLY,
                             source_frame_size);
  // Copy RGB frame to GPU
  ret = cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                 rgb_frame->data[0] + source_frame_size, cl_source_frame);

  int sat_buffer_size = width * height * 3 * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           sat_buffer_size);

  // Encode SAT
  sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                             rgb_frame->width, rgb_frame->height,
                             rgb_frame->linesize[0]);

  int cl_output_buffer_size = rect_frame->height * rect_frame->linesize[0];
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);
  sat_decoder.SampleFrameRectGPU(cl_output_buffer(), rect_frame->width,
                                 rect_frame->height, rect_frame->linesize[0],
                                 cl_sat_buffer(), source_codec_ctx, center_x,
                                 center_y);

  sat_decoder.InterpolateFrameRectGPU(
      cl_source_frame(), rgb_frame->width, rgb_frame->height,
      rgb_frame->linesize[0], cl_output_buffer(), rect_frame->width,
      rect_frame->height, rect_frame->linesize[0], center_x, center_y);
  ret = cl::copy(
      cl_manager.command_queue, cl_source_frame, rgb_frame->data[0],
      rgb_frame->data[0] + (rgb_frame->height * rgb_frame->linesize[0]));

  // // Copy off of GPU
  // ret =
  //     cl::copy(cl_manager.command_queue, cl_output_buffer,
  //     rect_frame->data[0],
  //              rect_frame->data[0] + cl_output_buffer_size);
  // if (ret != CL_SUCCESS) {
  //   std::cout << "Failed to copy frame off of GPU" << std::endl;
  //   return EXIT_FAILURE;
  // }

  // sat_decoder.InterpolateFrameRectCPU(rgb_frame.get(), rect_frame.get(),
  //                                     center_x, center_y);
  SaveFramePNG(rgb_frame.get(), output_file);
  return EXIT_SUCCESS;
}

int SingleFrameExtraction(const std::vector<std::string> &args) {
  std::string source_video =
      "360_em_dataset/1080p_videos/03_drone_d5d4gnuAJLo.mp4";
  std::string output_file = "single_frame_satlogrectilinear";
  uint64_t frame_to_extract = 100;

  float center_x = 0.65f;
  float center_y = 0.75f;

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);

  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;

  int ret = -1;

  std::unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  for (uint64_t i = 0; i < frame_to_extract; i++) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
  }
  if (ret != 0) {
    std::cerr << "Failed to get rgb frame" << std::endl;
    exit(EXIT_FAILURE);
  }

  int width = source_codec_ctx->width;
  int height = source_codec_ctx->height;

  std::unique_ptr<AVFrame, AVFrameDeleter> rect_frame(av_frame_alloc());
  rect_frame->width = REDUCED_BUFFER_WIDTH;
  rect_frame->height = REDUCED_BUFFER_HEIGHT;
  rect_frame->format = AV_PIX_FMT_RGB0;
  av_frame_get_buffer(rect_frame.get(), 0);

  // Copy the frame to the OpenCL GPU
  int source_frame_size = rgb_frame->linesize[0] * rgb_frame->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_ONLY,
                             source_frame_size);
  ret = cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                 rgb_frame->data[0] + source_frame_size, cl_source_frame);

  int sat_buffer_size = width * height * 3 * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           sat_buffer_size);

  sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                             rgb_frame->width, rgb_frame->height,
                             rgb_frame->linesize[0]);

  int cl_output_buffer_size = rect_frame->height * rect_frame->linesize[0];
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);

  sat_decoder.SampleFrameRectGPU(cl_output_buffer(), rect_frame->width,
                                 rect_frame->height, rect_frame->linesize[0],
                                 cl_sat_buffer(), source_codec_ctx, center_x,
                                 center_y);

  // Copy off of GPU
  ret =
      cl::copy(cl_manager.command_queue, cl_output_buffer, rect_frame->data[0],
               rect_frame->data[0] + cl_output_buffer_size);

  SaveFramePNG(rect_frame.get(), output_file);
  return 0;
}

int ExtractSampledFrameRect(const std::vector<std::string> &args) {
  using namespace std::chrono;

  std::string source_video = "bulgaria_1080_30.mp4";
  source_video = "360_em_dataset/1080p_videos/03_drone_d5d4gnuAJLo.mp4";
  std::string output_file = "large_rect_buffer_expanded";
  uint64_t frame_to_extract = 100;

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);

  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;

  int ret = -1;
  std::array<char, 256> err_buf;

  const float center_x = 0.5;
  const float center_y = 0.5;

  // Get frame from decoder
  std::unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  for (uint64_t i = 0; i < frame_to_extract; i++) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
  }
  if (ret != 0) {
    std::cout << "Failed to get rgb frame" << std::endl;
    exit(EXIT_FAILURE);
  }

  int width = source_codec_ctx->width;
  int height = source_codec_ctx->height;

  std::unique_ptr<AVFrame, AVFrameDeleter> rect_frame(av_frame_alloc());
  rect_frame->width = REDUCED_BUFFER_WIDTH;
  rect_frame->height = REDUCED_BUFFER_HEIGHT;
  rect_frame->format = AV_PIX_FMT_RGB0;
  av_frame_get_buffer(rect_frame.get(), 1);

  // Copy the frame to the OpenCL GPU
  int source_frame_size = rgb_frame->linesize[0] * rgb_frame->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_ONLY,
                             source_frame_size);
  // Copy RGB frame to GPU
  ret = cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                 rgb_frame->data[0] + source_frame_size, cl_source_frame);

  std::memset(rgb_frame->data[0], 0,
              rgb_frame->height * rgb_frame->linesize[0]);

  int sat_buffer_size = width * height * 3 * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           sat_buffer_size);

  // Encode SAT
  sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                             rgb_frame->width, rgb_frame->height,
                             rgb_frame->linesize[0]);

  int cl_output_buffer_size = rect_frame->height * rect_frame->linesize[0];
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);

  sat_decoder.SampleFrameRectGPU(cl_output_buffer(), rect_frame->width,
                                 rect_frame->height, rect_frame->linesize[0],
                                 cl_sat_buffer(), source_codec_ctx, center_x,
                                 center_y);

  // Copy off of GPU
  ret =
      cl::copy(cl_manager.command_queue, cl_output_buffer, rect_frame->data[0],
               rect_frame->data[0] + cl_output_buffer_size);
  if (ret != CL_SUCCESS) {
    std::cerr << "Failed to copy rect frame off gpu" << std::endl;
    exit(EXIT_FAILURE);
  }

  sat_decoder.ExpandSampledFrameRectCPU(rgb_frame.get(), rect_frame.get(),
                                        center_x, center_y);
  SaveFramePNG(rgb_frame.get(), output_file);
  return 0;
}

int InterpolateSampledFrameRect(const std::vector<std::string> &args) {
  using std::unique_ptr;
  using std::filesystem::path;

  path source_video = "360_em_dataset/1080p_videos/03_drone_d5d4gnuAJLo.mp4";
  path output_file = "interpolated_satlogrectilinear";
  uint64_t frame_to_extract = 100;

  float center_x = 0.65f;
  float center_y = 0.75f;

  if (args.size() >= 4) {
    source_video = args[2];
    output_file = args[3];
  }

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);

  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;

  int ret = -1;

  // Get frame from decoder
  unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  for (uint64_t i = 0; i < frame_to_extract; i++) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
  }
  if (ret != 0) {
    std::cerr << "Failed to get rgb frame" << std::endl;
  }

  int width = source_codec_ctx->width;
  int height = source_codec_ctx->height;
  int reduced_width = 16 * std::ceil(width / 1.8 / 16);
  int reduced_height = 16 * std::ceil(height / 1.8 / 16);
  sat_decoder.InitializeGrid(reduced_width, reduced_height,
                             source_codec_ctx->width, source_codec_ctx->height);

  unique_ptr<AVFrame, AVFrameDeleter> rect_frame(av_frame_alloc());
  rect_frame->width = reduced_width;
  rect_frame->height = reduced_height;
  rect_frame->format = AV_PIX_FMT_RGB0;
  av_frame_get_buffer(rect_frame.get(), 0);

  // Copy the frame to the OpenCL GPU
  int source_frame_size = rgb_frame->linesize[0] * rgb_frame->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_ONLY,
                             source_frame_size);
  // Copy RGB frame to GPU
  ret = cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                 rgb_frame->data[0] + source_frame_size, cl_source_frame);

  int sat_buffer_size = width * height * 3 * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           sat_buffer_size);

  // Encode SAT
  sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                             rgb_frame->width, rgb_frame->height,
                             rgb_frame->linesize[0]);

  int cl_output_buffer_size = rect_frame->height * rect_frame->linesize[0];
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);
  sat_decoder.SampleFrameRectGPU(cl_output_buffer(), rect_frame->width,
                                 rect_frame->height, rect_frame->linesize[0],
                                 cl_sat_buffer(), source_codec_ctx, center_x,
                                 center_y);

  // Copy off of GPU
  ret =
      cl::copy(cl_manager.command_queue, cl_output_buffer, rect_frame->data[0],
               rect_frame->data[0] + cl_output_buffer_size);
  if (ret != CL_SUCCESS) {
    std::cout << "Failed to copy frame off of GPU" << std::endl;
    return EXIT_FAILURE;
  }

  sat_decoder.InterpolateFrameRectCPU(rgb_frame.get(), rect_frame.get(),
                                      center_x, center_y);
  SaveFramePNG(rgb_frame.get(), output_file);
  return EXIT_SUCCESS;
}

int EncodeVideo(const std::vector<std::string> &args) {
  using std::filesystem::path;
  path source_video = path("360_em_dataset") / "1080p_videos" /
                      "04_turtle_rescue_ncSdc12VzUg.mp4";
  path output_video = path("output_videos") / "turtle_sat_foveated_view.mp4";
  path gaze_file =
      path("360_em_dataset") / "reformatted_data" / "003" / "04.txt";
  auto length = std::chrono::hours(5);
  bool static_gaze_position = false;
  bool use_view_position = false;

  source_video = "input_videos/static_park.mp4";
  output_video = "output_videos/staticpark_sat_foveated_view.mp4";
  gaze_file = path("360_em_dataset") / "reformatted_data" / "003" / "01.txt";

  source_video = "360_em_dataset/1080p_videos/03_drone_d5d4gnuAJLo.mp4";
  output_video = "output_videos/drone_sat_view_foveated.mp4";
  gaze_file = path("360_em_dataset") / "reformatted_data" / "003" / "03.txt";
  use_view_position = true;

  source_video = "360_em_dataset/1080p_videos/13_drone_low_4m7NouQFaxc.mp4";
  output_video = "output_videos/13_drone_low_sat_view_foveated.mp4";
  gaze_file = path("360_em_dataset") / "reformatted_data" / "003" / "13.txt";
  use_view_position = true;

  if (args.size() == 4) {
    source_video = args[1];
    gaze_file = args[2];
    output_video = args[3];
  }

  // static_gaze_position = true;

  float center_x = 0.5;
  float center_y = 0.5;

  std::cout << "Foveating " << std::chrono::seconds(length).count()
            << " seconds of video: " << source_video.filename().string()
            << std::endl;
  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);
  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;
  VideoEncoder video_encoder(source_codec_ctx, NULL, output_video);
  sat_decoder.InitializeGrid(REDUCED_BUFFER_WIDTH, REDUCED_BUFFER_HEIGHT,
                             source_codec_ctx->width, source_codec_ctx->height);
  GazeViewPoints gv_points(gaze_file);

  int ret = -1;

  bool continue_loop = true;
  double elapsed_time;
  int frame = 0;

  std::unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  std::unique_ptr<AVFrame, AVFrameDeleter> output_frame(av_frame_alloc());
  output_frame->format = AV_PIX_FMT_RGB0;
  output_frame->width = REDUCED_BUFFER_WIDTH;
  output_frame->height = REDUCED_BUFFER_HEIGHT;
  av_frame_get_buffer(output_frame.get(), 1);

  int cl_source_frame_size = cl_source_frame_size =
      4 * source_codec_ctx->width * source_codec_ctx->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_WRITE,
                             cl_source_frame_size);
  int cl_sat_buffer_size =
      3 * source_codec_ctx->width * source_codec_ctx->height * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           cl_sat_buffer_size);
  int cl_output_buffer_size = output_frame->linesize[0] * output_frame->height;
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);

  // Get frame from decoder
  while (continue_loop && frame < 30 * std::chrono::seconds(length).count()) {
    // if (frame > 3) exit(EXIT_SUCCESS);
    // for (int i = 0; i < 675; i++) {
    //   ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
    //   frame++;
    // }
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
    if (ret == 0) {
      if (frame % 30 == 0) {
        std::cout << "Processing frame " << frame << std::endl;
      }

      // Copy RGB frame to GPU
      ret =
          cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                   rgb_frame->data[0] + cl_source_frame_size, cl_source_frame);

      sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                                 rgb_frame->width, rgb_frame->height,
                                 rgb_frame->linesize[0]);

      memset(rgb_frame->data[0], 0, rgb_frame->height * rgb_frame->linesize[0]);

      if (!static_gaze_position) {
        center_x = gv_points.points[frame].gaze_point[0];
        center_y = gv_points.points[frame].gaze_point[1];
      }
      if (use_view_position) {
        center_x = gv_points.points[frame].view_point[0];
        center_y = gv_points.points[frame].view_point[1];
      }

      sat_decoder.SampleFrameRectGPU(
          cl_output_buffer(), output_frame->width, output_frame->height,
          output_frame->linesize[0], cl_sat_buffer(),
          video_decoder.source_codec_ctx, center_x, center_y);
      // ret = cl::copy(
      //     cl_manager.command_queue, cl_output_buffer, output_frame->data[0],
      //     output_frame->data[0] + (output_frame->height *
      //     output_frame->linesize[0]));

      // SaveFramePNG(output_frame.get(), "test_output/buffer.png");

      sat_decoder.InterpolateFrameRectGPU(
          cl_source_frame(), rgb_frame->width, rgb_frame->height,
          rgb_frame->linesize[0], cl_output_buffer(), output_frame->width,
          output_frame->height, output_frame->linesize[0], center_x, center_y);
      ret = cl::copy(
          cl_manager.command_queue, cl_source_frame, rgb_frame->data[0],
          rgb_frame->data[0] + (rgb_frame->height * rgb_frame->linesize[0]));
      ret = video_encoder.EncodeFrameToFile(rgb_frame.get());
      // SaveFramePNG(rgb_frame.get(), "test_output/" + std::to_string(frame));
      frame++;
    } else {
      continue_loop = false;
    }
  }

  video_encoder.EncodeFrameToFile(NULL);
  video_encoder.WriteTrailerAndCloseFile();
  return EXIT_SUCCESS;
}

int EncodeLogCartesianVideo(const std::vector<std::string> &args) {
  std::string source_video =
      "360_em_dataset/1080p_videos/03_drone_d5d4gnuAJLo.mp4";
  std::string output_video = "drone_sat_foveated_logcartesian.mp4";
  std::string gaze_file = "360_em_dataset/reformatted_data/003/03.txt";
  auto length = std::chrono::hours(5);

  if (args.size() >= 4) {
    source_video = args[1];
    gaze_file = args[2];
    output_video = args[3];
    std::cout << "Using gaze" << gaze_file << std::endl;
  }

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);
  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;
  AVCodecContext output_codec_ctx = *source_codec_ctx;
  output_codec_ctx.width = REDUCED_BUFFER_WIDTH;
  output_codec_ctx.height = REDUCED_BUFFER_HEIGHT;
  VideoEncoder video_encoder(&output_codec_ctx, NULL, output_video);
  sat_decoder.InitializeGrid(REDUCED_BUFFER_WIDTH, REDUCED_BUFFER_HEIGHT,
                             source_codec_ctx->width, source_codec_ctx->height);

  GazeViewPoints gv_points(gaze_file);

  int ret = -1;
  bool continue_loop = true;
  int frame = 0;

  std::unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  std::unique_ptr<AVFrame, AVFrameDeleter> output_frame(av_frame_alloc());
  output_frame->format = AV_PIX_FMT_RGB0;
  output_frame->width = REDUCED_BUFFER_WIDTH;
  output_frame->height = REDUCED_BUFFER_HEIGHT;
  av_frame_get_buffer(output_frame.get(), 0);

  float center_x = 0.5f;
  float center_y = 0.5f;

  int cl_source_frame_size = cl_source_frame_size =
      4 * source_codec_ctx->width * source_codec_ctx->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_WRITE,
                             cl_source_frame_size);
  int cl_sat_buffer_size =
      3 * source_codec_ctx->width * source_codec_ctx->height * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           cl_sat_buffer_size);
  int cl_output_buffer_size = output_frame->linesize[0] * output_frame->height;
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);

  // Get frame from decoder
  while (continue_loop && frame < 30 * std::chrono::seconds(length).count()) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
    if (ret == 0) {
      if (frame % 30 == 0) {
        std::cout << "Processing frame " << frame << std::endl;
      }

      // Copy RGB frame to GPU
      ret =
          cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                   rgb_frame->data[0] + cl_source_frame_size, cl_source_frame);

      sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                                 rgb_frame->width, rgb_frame->height,
                                 rgb_frame->linesize[0]);

      memset(rgb_frame->data[0], 0, rgb_frame->height * rgb_frame->linesize[0]);

      center_x = gv_points.points[frame].gaze_point[0];
      center_y = gv_points.points[frame].gaze_point[1];

      sat_decoder.SampleFrameRectGPU(
          cl_output_buffer(), output_frame->width, output_frame->height,
          output_frame->linesize[0], cl_sat_buffer(),
          video_decoder.source_codec_ctx, center_x, center_y);
      ret = cl::copy(cl_manager.command_queue, cl_output_buffer,
                     output_frame->data[0],
                     output_frame->data[0] +
                         (output_frame->height * output_frame->linesize[0]));
      output_frame->pts = rgb_frame->pts;
      output_frame->pkt_dts = rgb_frame->pkt_dts;
      ret = video_encoder.EncodeFrameToFile(output_frame.get());

      frame++;
    } else {
      continue_loop = false;
    }
  }

  video_encoder.EncodeFrameToFile(NULL);
  video_encoder.WriteTrailerAndCloseFile();
  return EXIT_SUCCESS;
}

int EncodeLogCartesianVideoBitrate(const std::vector<std::string> &args) {
  using namespace std::filesystem;
  std::string source_video;
  // std::string output_video = "drone_sat_foveated_logcartesian.mp4";
  path output_video;
  std::string gaze_file;
  int bitrate = -1;
  auto length = std::chrono::hours(5);

  if (args.size() >= 6) {
    source_video = args[2];
    gaze_file = args[3];
    output_video = args[4];
    bitrate = std::stoi(args[5]);
  } else {
    exit(EXIT_FAILURE);
  }

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);
  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;
  AVCodecContext output_codec_ctx = *source_codec_ctx;
  output_codec_ctx.width = REDUCED_BUFFER_WIDTH;
  output_codec_ctx.height = REDUCED_BUFFER_HEIGHT;
  VideoEncoder video_encoder(&output_codec_ctx, NULL, output_video, bitrate);
  sat_decoder.InitializeGrid(REDUCED_BUFFER_WIDTH, REDUCED_BUFFER_HEIGHT,
                             source_codec_ctx->width, source_codec_ctx->height);

  GazeViewPoints gv_points(gaze_file);

  int ret = -1;
  bool continue_loop = true;
  int frame = 0;

  std::unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  std::unique_ptr<AVFrame, AVFrameDeleter> output_frame(av_frame_alloc());
  output_frame->format = AV_PIX_FMT_RGB0;
  output_frame->width = REDUCED_BUFFER_WIDTH;
  output_frame->height = REDUCED_BUFFER_HEIGHT;
  av_frame_get_buffer(output_frame.get(), 0);

  float center_x = 0.5f;
  float center_y = 0.5f;

  int cl_source_frame_size = cl_source_frame_size =
      4 * source_codec_ctx->width * source_codec_ctx->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_WRITE,
                             cl_source_frame_size);
  int cl_sat_buffer_size =
      3 * source_codec_ctx->width * source_codec_ctx->height * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           cl_sat_buffer_size);
  int cl_output_buffer_size = output_frame->linesize[0] * output_frame->height;
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);

  // Get frame from decoder
  while (continue_loop && frame < 30 * std::chrono::seconds(length).count()) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
    if (ret == 0) {
      if (frame % 30 == 0) {
        std::cout << "Processing frame " << frame << std::endl;
      }

      // Copy RGB frame to GPU
      ret =
          cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                   rgb_frame->data[0] + cl_source_frame_size, cl_source_frame);

      sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                                 rgb_frame->width, rgb_frame->height,
                                 rgb_frame->linesize[0]);

      memset(rgb_frame->data[0], 0, rgb_frame->height * rgb_frame->linesize[0]);

      center_x = gv_points.points[frame].gaze_point[0];
      center_y = gv_points.points[frame].gaze_point[1];

      sat_decoder.SampleFrameRectGPU(
          cl_output_buffer(), output_frame->width, output_frame->height,
          output_frame->linesize[0], cl_sat_buffer(),
          video_decoder.source_codec_ctx, center_x, center_y);
      ret = cl::copy(cl_manager.command_queue, cl_output_buffer,
                     output_frame->data[0],
                     output_frame->data[0] +
                         (output_frame->height * output_frame->linesize[0]));
      output_frame->pts = rgb_frame->pts;
      output_frame->pkt_dts = rgb_frame->pkt_dts;
      ret = video_encoder.EncodeFrameToFile(output_frame.get());

      frame++;
    } else {
      continue_loop = false;
    }
  }

  video_encoder.EncodeFrameToFile(NULL);
  video_encoder.WriteTrailerAndCloseFile();
  return EXIT_SUCCESS;
}

int DecodeLogCartesianVideo(const std::vector<std::string> &args) {
  fs::path source_video;
  fs::path output_video;
  fs::path gaze_file;
  auto length = std::chrono::hours(5);

  if (args.size() >= 5) {
    source_video = args[2];
    gaze_file = args[3];
    output_video = args[4];
  } else {
    return EXIT_FAILURE;
  }

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);
  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;
  AVCodecContext output_codec_ctx = *source_codec_ctx;
  output_codec_ctx.width = 1920;
  output_codec_ctx.height = 1080;
  VideoEncoder video_encoder(&output_codec_ctx, NULL, output_video);
  sat_decoder.InitializeGrid(REDUCED_BUFFER_WIDTH, REDUCED_BUFFER_HEIGHT,
                             source_codec_ctx->width, source_codec_ctx->height);

  GazeViewPoints gv_points(gaze_file);

  int ret = -1;
  bool continue_loop = true;
  int frame = 0;

  std::unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  std::unique_ptr<AVFrame, AVFrameDeleter> output_frame(av_frame_alloc());
  output_frame->format = AV_PIX_FMT_RGB0;
  output_frame->width = 1920;
  output_frame->height = 1080;
  av_frame_get_buffer(output_frame.get(), 0);

  float center_x = 0.5f;
  float center_y = 0.5f;

  int cl_source_frame_size = cl_source_frame_size =
      4 * source_codec_ctx->width * source_codec_ctx->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_WRITE,
                             cl_source_frame_size);
  int cl_output_buffer_size = output_frame->linesize[0] * output_frame->height;
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);

  // Get frame from decoder
  while (continue_loop && frame < 30 * std::chrono::seconds(length).count()) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
    if (ret == 0) {
      if (frame % 30 == 0) {
        std::cout << "Processing frame " << frame << std::endl;
      }

      // Copy RGB frame to GPU
      ret =
          cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                   rgb_frame->data[0] + cl_source_frame_size, cl_source_frame);

      center_x = gv_points.points[frame].gaze_point[0];
      center_y = gv_points.points[frame].gaze_point[1];

      sat_decoder.InterpolateFrameRectGPU(
          cl_output_buffer(), output_frame->width, output_frame->height,
          output_frame->linesize[0], cl_source_frame(), rgb_frame->width,
          rgb_frame->height, rgb_frame->linesize[0], center_x, center_y);

      ret = cl::copy(cl_manager.command_queue, cl_output_buffer,
                     output_frame->data[0],
                     output_frame->data[0] + cl_output_buffer_size);

      output_frame->pts = rgb_frame->pts;
      output_frame->pkt_dts = rgb_frame->pkt_dts;

      video_encoder.EncodeFrameToFile(output_frame.get());
      frame++;
    } else {
      continue_loop = false;
    }
  }

  video_encoder.EncodeFrameToFile(NULL);
  video_encoder.WriteTrailerAndCloseFile();
  return EXIT_SUCCESS;
}

int FoveateLogCartesianVideo(const std::vector<std::string> &args) {
  std::string source_video =
      "360_em_dataset/1080p_videos/03_drone_d5d4gnuAJLo.mp4";
  std::string gaze_file = "360_em_dataset/reformatted_data/003/03.txt";
  std::string output_video = "foveated_test.mp4";
  auto length = std::chrono::hours(5);

  if (args.size() >= 5) {
    source_video = args[2];
    gaze_file = args[3];
    output_video = args[4];
    std::cout << "Using gaze" << gaze_file << std::endl;
  } else {
    return EXIT_FAILURE;
  }

  OpenCLManager cl_manager;
  cl_manager.InitializeContext();
  VideoDecoder video_decoder;
  video_decoder.OpenVideo(source_video);
  SATEncoder sat_encoder(&cl_manager);
  SATDecoder sat_decoder(&cl_manager);
  AVCodecContext *source_codec_ctx = video_decoder.source_codec_ctx;
  AVCodecContext output_codec_ctx = *source_codec_ctx;
  VideoEncoder video_encoder(&output_codec_ctx, NULL, output_video);
  sat_decoder.InitializeGrid(REDUCED_BUFFER_WIDTH, REDUCED_BUFFER_HEIGHT,
                             source_codec_ctx->width, source_codec_ctx->height);

  GazeViewPoints gv_points(gaze_file);

  int ret = -1;
  bool continue_loop = true;
  int frame = 0;

  std::unique_ptr<AVFrame, AVFrameDeleter> rgb_frame(av_frame_alloc());
  std::unique_ptr<AVFrame, AVFrameDeleter> output_frame(av_frame_alloc());
  output_frame->format = AV_PIX_FMT_RGB0;
  output_frame->width = REDUCED_BUFFER_WIDTH;
  output_frame->height = REDUCED_BUFFER_HEIGHT;
  av_frame_get_buffer(output_frame.get(), 0);

  float center_x = 0.5f;
  float center_y = 0.5f;

  int cl_source_frame_size = cl_source_frame_size =
      4 * source_codec_ctx->width * source_codec_ctx->height;
  cl::Buffer cl_source_frame(cl_manager.context, CL_MEM_READ_WRITE,
                             cl_source_frame_size);
  int cl_sat_buffer_size =
      3 * source_codec_ctx->width * source_codec_ctx->height * sizeof(uint32_t);
  cl::Buffer cl_sat_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                           cl_sat_buffer_size);
  int cl_output_buffer_size = output_frame->linesize[0] * output_frame->height;
  cl::Buffer cl_output_buffer(cl_manager.context, CL_MEM_READ_WRITE,
                              cl_output_buffer_size);

  // Get frame from decoder
  while (continue_loop && frame < 30 * std::chrono::seconds(length).count()) {
    ret = video_decoder.GetFrame(rgb_frame.get(), AV_PIX_FMT_RGB0);
    if (ret == 0) {
      if (frame % 30 == 0) {
        std::cout << "Processing frame " << frame << std::endl;
      }

      // Copy RGB frame to GPU
      ret =
          cl::copy(cl_manager.command_queue, rgb_frame->data[0],
                   rgb_frame->data[0] + cl_source_frame_size, cl_source_frame);

      sat_encoder.EncodeFrameGPU(cl_sat_buffer(), cl_source_frame(),
                                 rgb_frame->width, rgb_frame->height,
                                 rgb_frame->linesize[0]);

      memset(rgb_frame->data[0], 0, rgb_frame->height * rgb_frame->linesize[0]);

      center_x = gv_points.points[frame].gaze_point[0];
      center_y = gv_points.points[frame].gaze_point[1];

      sat_decoder.SampleFrameRectGPU(
          cl_output_buffer(), output_frame->width, output_frame->height,
          output_frame->linesize[0], cl_sat_buffer(),
          video_decoder.source_codec_ctx, center_x, center_y);

      sat_decoder.InterpolateFrameRectGPU(
          cl_source_frame(), rgb_frame->width, rgb_frame->height,
          rgb_frame->linesize[0], cl_output_buffer(), output_frame->width,
          output_frame->height, output_frame->linesize[0], center_x, center_y);

      ret = cl::copy(cl_manager.command_queue, cl_source_frame,
                     rgb_frame->data[0],
                     rgb_frame->data[0] + cl_source_frame_size);

      ret = video_encoder.EncodeFrameToFile(rgb_frame.get());

      frame++;
    } else {
      continue_loop = false;
    }
  }

  video_encoder.EncodeFrameToFile(NULL);
  video_encoder.WriteTrailerAndCloseFile();
  return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////
// factor.hpp                                                                //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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 BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_FACTOR_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_FACTOR_HPP_ER_2010

#include <boost/statistics/detail/non_parametric/contingency_table/factor/check_against_domain.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/domain_error_logger.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/levels.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/filter.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/keyword.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/vec_levels.hpp>

#endif


/**
 * $Id$
 *
 * Software License Agreement (GNU General Public License)
 *
 *  Copyright (C) 2015:
 *
 *    Johann Prankl, prankl@acin.tuwien.ac.at
 *    Aitor Aldoma, aldoma@acin.tuwien.ac.at
 *
 *      Automation and Control Institute
 *      Vienna University of Technology
 *      Gusshausstraße 25-29
 *      1170 Vienn, Austria
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * @author Johann Prankl
 *
 */

#ifndef KP_GLOBAL_CLOUD_FILTERING_HH
#define KP_GLOBAL_CLOUD_FILTERING_HH

#include <iostream>
#include <fstream>
#include <float.h>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <boost/thread/mutex.hpp>
#include <boost/thread.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/octree/octree_pointcloud_voxelcentroid.h>
#include <pcl/octree/octree.h>
#include <pcl/octree/octree_impl.h>
#include <pcl/octree/octree_pointcloud.h>
#include <v4r/camera_tracking_and_mapping/OctreeVoxelCentroidContainerXYZRGBNormal.hpp>
#include <boost/smart_ptr.hpp>
#include <v4r/keypoints/impl/triple.hpp>
#include <v4r/common/impl/DataMatrix2D.hpp>
#include <v4r/camera_tracking_and_mapping/TSFData.h>
#include <v4r/camera_tracking_and_mapping/TSFFrame.hh>
#include <v4r/core/macros.h>

namespace v4r
{

/**
 * TSFGlobalCloudFiltering
 */
class V4R_EXPORTS TSFGlobalCloudFiltering
{
public:

  /**
   * Parameter
   */
  class Parameter
  {
  public:
    float thr_angle;
    float max_weight;
    float sigma_pts;
    float sigma_depth;
    float z_cut_off_integration;
    float voxel_size;
    float max_dist_integration;
    Parameter()
      : thr_angle(80), max_weight(20), sigma_pts(10), sigma_depth(0.008), z_cut_off_integration(0.02), voxel_size(0.001), max_dist_integration(7.) {}
  };

private:
  Parameter param;

  float cos_thr_angle;
  float neg_inv_sqr_sigma_pts;

  cv::Mat_<double> intrinsic;

  std::vector<cv::Vec4i> npat;
  std::vector<float> exp_error_lookup;

  std::vector< cv::Mat_<float> > reliability;

  pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZRGBNormal,pcl::octree::OctreeVoxelCentroidContainerXYZRGBNormal<pcl::PointXYZRGBNormal> >::Ptr octree;
  typedef pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZRGBNormal,pcl::octree::OctreeVoxelCentroidContainerXYZRGBNormal<pcl::PointXYZRGBNormal> >::AlignedPointTVector AlignedPointXYZRGBNormalVector;
  boost::shared_ptr< AlignedPointXYZRGBNormalVector > oc_cloud;

//  void integrateData(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const Eigen::Matrix4f &pose, const Eigen::Matrix4f &filt_pose, v4r::DataMatrix2D<TSFData::Surfel> &filt_cloud);
  void computeReliability(const std::vector<TSFFrame::Ptr> &frames);
  void maxReliabilityIndexing(const std::vector<TSFFrame::Ptr> &frames);
  void getMaxPoints(const std::vector<TSFFrame::Ptr> &frames, pcl::PointCloud<pcl::PointXYZRGBNormal> &cloud);
  void computeNormals(v4r::DataMatrix2D<v4r::Surfel> &sf_cloud);

  inline float sqr(const float &d) {return d*d;}

public:
  cv::Mat dbg;

  TSFGlobalCloudFiltering(const Parameter &p=Parameter());
  ~TSFGlobalCloudFiltering();

  void start();
  void stop();

  void computeRadius(v4r::DataMatrix2D<v4r::Surfel> &sf_cloud);

  void filter(const std::vector<TSFFrame::Ptr> &frames, pcl::PointCloud<pcl::PointXYZRGBNormal> &cloud, const Eigen::Matrix4f &base_transform=Eigen::Matrix4f::Identity());

  void setCameraParameter(const cv::Mat &_intrinsic);
  void setParameter(const Parameter &p);

  typedef boost::shared_ptr< ::v4r::TSFGlobalCloudFiltering> Ptr;
  typedef boost::shared_ptr< ::v4r::TSFGlobalCloudFiltering const> ConstPtr;
};

/*************************** INLINE METHODES **************************/

} //--END--

#endif


/**
 * @example ublas/test/randgen_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/ublas/randgen.h>
#include <usml/ublas/vector_math.h>

using namespace usml::ublas;

BOOST_AUTO_TEST_SUITE(randgen_test)

/**
 * @ingroup ublas_test
 * @{
 */

/**
 * Generate vectors of random numbers. Re-use the same seed each time.
 *
 * Assume that testers will visually inspect the results.
 */
BOOST_AUTO_TEST_CASE( rand_vector_test ) {

    randgen::seed(100);

    cout << "=== randgen_test: randVectorTest ===" << endl;
    cout << "uniform:  " << randgen::uniform(7) << endl;
    cout << "gaussian: " << randgen::gaussian(7) << endl;
    cout << "noise:    " << randgen::noise(3) << endl;
}

/**
 * Generate matrices of random numbers. Re-use the same seed each time.
 *
 * Assume that testers will visually inspect the results.
 */
BOOST_AUTO_TEST_CASE( rand_matrix_test ) {

    randgen::seed(100);

    cout << "=== randgen_test: randMatrixTest ===" << endl;
    cout << "uniform:  " << randgen::uniform(2,3) << endl;
    cout << "gaussian: " << randgen::gaussian(2,3) << endl;
    cout << "noise:    " << randgen::noise(2,3) << endl;
}

/// @}

BOOST_AUTO_TEST_SUITE_END()