//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Ilias Khairullin <ilias@nil.foundation>
//
// 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_MULTIPRECISION_MODULAR_FUNCTIONS_FIXED_PRECISION_HPP
#define BOOST_MULTIPRECISION_MODULAR_FUNCTIONS_FIXED_PRECISION_HPP

#include <nil/crypto3/multiprecision/modular/modular_policy_fixed.hpp>

#include <boost/mpl/if.hpp>

#include <type_traits>
#include <utility>

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

                template<typename Backend>
                class modular_functions_fixed;

                //
                // the function works correctly only with consistent backend objects,
                // i.e. their limbs should not be manipulated directly
                // as it breaks backend logic of size determination
                // (or real size of such objects should be adjusted then)
                //
                template<typename Backend>
                constexpr typename boost::mpl::if_c<is_trivial_cpp_int<Backend>::value,
                                                    typename trivial_limb_type<max_precision<Backend>::value>::type,
                                                    limb_type>::type
                    get_limb_value(const Backend& b, const std::size_t i) {
                    if (i < b.size()) {
                        return b.limbs()[i];
                    }
                    return 0;
                }

                //
                // function return real limb of nontrivial backend.
                //
                template<typename, typename Backend>
                constexpr typename boost::enable_if_c<!is_trivial_cpp_int<Backend>::value, limb_type>::type
                    custom_get_limb_value(const Backend& b, const std::size_t i) {
                    return b.limbs()[i];
                }

                //
                // function works with trivial backend.
                // return value of logical limb as if trivial backend consists of several logical limbs.
                //
                template<typename internal_limb_type, typename Backend>
                constexpr typename boost::enable_if_c<
                    is_trivial_cpp_int<Backend>::value &&
                        sizeof(typename trivial_limb_type<max_precision<Backend>::value>::type) >=
                            sizeof(internal_limb_type),
                    internal_limb_type>::type
                    custom_get_limb_value(const Backend& b, const std::size_t i) {
                    return static_cast<internal_limb_type>(b.limbs()[0] >> (sizeof(internal_limb_type) * CHAR_BIT * i));
                }

                //
                // function set limb value of nontrivial backend.
                //
                template<typename, typename Backend>
                constexpr typename boost::enable_if_c<!is_trivial_cpp_int<Backend>::value>::type
                    custom_set_limb_value(Backend& b, const std::size_t i, limb_type v) {
                    b.limbs()[i] = v;
                }

                //
                // WARNING: using of this function is correct in current implementation of modular adaptor
                // DO NOT USE THIS FUNCTION IN GENERAL CASE
                //
                // function works with trivial backend.
                // set value of logical limb as if trivial backend consists of several logical limbs.
                // modified logical limb is supposed to have zero value.
                //
                template<typename internal_limb_type, typename Backend>
                constexpr typename boost::enable_if_c<
                    is_trivial_cpp_int<Backend>::value &&
                    sizeof(typename trivial_limb_type<max_precision<Backend>::value>::type) >=
                        sizeof(internal_limb_type)>::type
                    custom_set_limb_value(Backend& b, const std::size_t i, internal_limb_type v) {
                    using local_limb_type = typename trivial_limb_type<max_precision<Backend>::value>::type;

                    //
                    // commented part seems to be correct in general case
                    //
                    // std::size_t upper_bytes_count = sizeof(local_limb_type) - sizeof(internal_limb_type) * (i + 1);
                    // std::size_t lower_bytes_count = sizeof(internal_limb_type) * i;
                    // unsigned char byte_mask = ~0;
                    //
                    // local_limb_type mask = 0;
                    // for (std::size_t j = 0; j < upper_bytes_count; j++)
                    // {
                    //    mask |= byte_mask;
                    //    mask <<= CHAR_BIT;
                    // }
                    // mask <<= (sizeof(internal_limb_type) - 1) * CHAR_BIT;
                    // if (lower_bytes_count)
                    // {
                    //    for (std::size_t j = 0; j < lower_bytes_count - 1; j++)
                    //    {
                    //       mask |= byte_mask;
                    //       mask <<= CHAR_BIT;
                    //    }
                    //    mask |= byte_mask;
                    // }
                    //
                    // b.limbs()[0] &= mask;
                    b.limbs()[0] |= (static_cast<local_limb_type>(v) << (sizeof(internal_limb_type) * CHAR_BIT * i));
                }

                template<typename Backend>
                constexpr typename std::enable_if<!is_trivial_cpp_int<Backend>::value>::type
                    adjust_backend_size(Backend& b, std::size_t mod_size) {
                    assert(mod_size + 1 <= Backend::internal_limb_count);
                    b.resize(b.limbs()[mod_size] != 0 ? mod_size + 1 : mod_size, 1);
                }

                template<typename Backend>
                constexpr typename std::enable_if<is_trivial_cpp_int<Backend>::value>::type
                    adjust_backend_size(Backend& b, std::size_t mod_size) {
                    assert(mod_size == 1);
                    b.resize(mod_size, 1);
                }

                template<typename Backend>
                constexpr bool check_modulus_constraints(const Backend& m) {
                    using ui_type = typename std::tuple_element<0, typename Backend::unsigned_types>::type;
                    using default_ops::eval_lt;

                    return !eval_lt(m, static_cast<ui_type>(0u));
                }

                template<typename Backend>
                constexpr bool check_montgomery_constraints(const Backend& m) {
                    using ui_type = typename std::tuple_element<0, typename Backend::unsigned_types>::type;
                    using default_ops::eval_eq;
                    using default_ops::eval_modulus;

                    Backend tmp;
                    eval_modulus(tmp, m, static_cast<ui_type>(2u));
                    return !eval_eq(tmp, static_cast<ui_type>(0u));
                }

                template<typename Backend>
                constexpr bool check_montgomery_constraints(const modular_functions_fixed<Backend>& mo) {
                    return check_montgomery_constraints(mo.get_mod().backend());
                }

                //
                // a little trick to prevent error in constexpr execution of eval_right_shift
                // due to non-constexpr nature of right_shift_byte
                //
                template<typename Backend>
                constexpr void custom_right_shift(Backend& b, unsigned s) {
                    using default_ops::eval_left_shift;
                    using default_ops::eval_right_shift;

                    if (!s) {
                        return;
                    }

                    limb_type byte_shift_mask = CHAR_BIT - 1;
                    if ((s & byte_shift_mask) == 0) {
                        eval_right_shift(b, s - 1u);
                        eval_right_shift(b, 1u);
                    } else {
                        eval_right_shift(b, s);
                    }
                }

                template<unsigned MinBits, cpp_integer_type SignType, cpp_int_check_type Checked>
                class modular_functions_fixed<modular_fixed_cpp_int_backend<MinBits, SignType, Checked>> {
                protected:
                    typedef modular_fixed_cpp_int_backend<MinBits, SignType, Checked> Backend;

                public:
                    typedef modular_policy<Backend> policy_type;

                protected:
                    typedef typename policy_type::internal_limb_type internal_limb_type;
                    typedef typename policy_type::internal_double_limb_type internal_double_limb_type;

                    typedef typename policy_type::Backend_doubled_1 Backend_doubled_1;
                    typedef typename policy_type::Backend_quadruple_1 Backend_quadruple_1;
                    typedef typename policy_type::Backend_padded_limbs Backend_padded_limbs;
                    typedef typename policy_type::Backend_doubled_limbs Backend_doubled_limbs;
                    typedef typename policy_type::Backend_doubled_padded_limbs Backend_doubled_padded_limbs;

                    typedef typename policy_type::number_type number_type;
                    typedef typename policy_type::dbl_lmb_number_type dbl_lmb_number_type;

                    constexpr static auto limb_bits = policy_type::limb_bits;

                    constexpr void initialize_modulus(const number_type& m) {
                        BOOST_ASSERT(check_modulus_constraints(m.backend()));

                        get_mod() = m;
                    }

                    constexpr void initialize_barrett_params() {
                        using default_ops::eval_bit_set;
                        using default_ops::eval_divide;
                        using default_ops::eval_msb;

                        get_mu() = static_cast<internal_limb_type>(0u);

                        eval_bit_set(get_mu(), 2u * (1u + eval_msb(get_mod().backend())));
                        eval_divide(get_mu(), get_mod().backend());
                    }

                    constexpr void initialize_montgomery_params() {
                        if (check_montgomery_constraints(get_mod().backend())) {
                            find_const_variables();
                            find_modulus_mask();
                        }
                    }

                    constexpr internal_limb_type monty_inverse(internal_limb_type a) {
                        BOOST_ASSERT(check_montgomery_constraints(get_mod().backend()));

                        internal_limb_type b = 1;
                        internal_limb_type r = 0;

                        for (size_t i = 0; i != limb_bits; ++i) {
                            const internal_limb_type bi = b % 2;
                            r >>= 1;
                            r += bi << (limb_bits - 1);

                            b -= a * bi;
                            b >>= 1;
                        }

                        // Now invert in addition space
                        r = (~static_cast<internal_limb_type>(0) - r) + 1;

                        return r;
                    }

                    constexpr void find_const_variables() {
                        using default_ops::eval_bit_set;
                        using default_ops::eval_gt;
                        using default_ops::eval_multiply;

                        BOOST_ASSERT(check_montgomery_constraints(get_mod().backend()) &&
                                     check_modulus_constraints(get_mod().backend()));

                        get_p_dash() = monty_inverse(get_mod().backend().limbs()[0]);

                        Backend_doubled_padded_limbs r;
                        eval_bit_set(r, get_mod().backend().size() * limb_bits);
                        eval_multiply(r, r);
                        barrett_reduce(r);

                        get_r2() = static_cast<Backend>(r);
                    }

                    constexpr void find_modulus_mask() {
                        get_modulus_mask() = static_cast<internal_limb_type>(1u);
                        eval_left_shift(get_modulus_mask(), get_mod().backend().size() * limb_bits);
                        eval_subtract(get_modulus_mask(),
                                      decltype(m_modulus_mask)(static_cast<internal_limb_type>(1u)));
                    }

                    constexpr void initialize(const number_type& m) {
                        initialize_modulus(m);
                        initialize_barrett_params();
                        initialize_montgomery_params();
                    }

                public:
                    constexpr auto& get_mod() {
                        return m_mod;
                    }
                    constexpr auto& get_mu() {
                        return m_barrett_mu;
                    }
                    constexpr auto& get_r2() {
                        return m_montgomery_r2;
                    }
                    constexpr auto& get_p_dash() {
                        return m_montgomery_p_dash;
                    }
                    constexpr auto& get_modulus_mask() {
                        return m_modulus_mask;
                    }

                    constexpr const auto& get_mod() const {
                        return m_mod;
                    }
                    constexpr const auto& get_mu() const {
                        return m_barrett_mu;
                    }
                    constexpr const auto& get_r2() const {
                        return m_montgomery_r2;
                    }
                    constexpr auto get_p_dash() const {
                        return m_montgomery_p_dash;
                    }
                    constexpr const auto& get_modulus_mask() const {
                        return m_modulus_mask;
                    }

                    constexpr modular_functions_fixed() {
                    }

                    constexpr modular_functions_fixed(const number_type& m) {
                        initialize(m);
                    }

                    constexpr modular_functions_fixed(const modular_functions_fixed& o) {
                        get_mod() = o.get_mod();
                        get_mu() = o.get_mu();
                        get_r2() = o.get_r2();
                        get_p_dash() = o.get_p_dash();
                        get_modulus_mask() = o.get_modulus_mask();
                    }

                    template<typename Backend1>
                    constexpr void barrett_reduce(Backend1& result) const {
                        barrett_reduce(result, result);
                    }

                    //
                    // this overloaded barrett_reduce is intended to work with built-in integral types
                    //
                    template<typename Backend1, typename Backend2>
                    constexpr typename std::enable_if<boost::is_integral<Backend2>::value>::type
                        barrett_reduce(Backend1& result, Backend2 input) const {
                        using input_number_type = typename boost::mpl::if_c<
                            bool(sizeof(int) * CHAR_BIT > MinBits),
                            number<modular_fixed_cpp_int_backend<sizeof(int) * CHAR_BIT, SignType, Checked>>,
                            number_type>::type;

                        input_number_type input_b(input);
                        barrett_reduce(result, input_b.backend());
                    }

                    template<typename Backend1, typename Backend2,
                             typename = typename boost::enable_if_c<
                                 /// result should fit in the output parameter
                                 max_precision<Backend1>::value >= max_precision<Backend>::value &&
                                 /// to prevent problems with trivial cpp_int
                                 max_precision<Backend2>::value >= max_precision<Backend>::value>::type>
                    constexpr void barrett_reduce(Backend1& result, Backend2 input) const {
                        using default_ops::eval_add;
                        using default_ops::eval_eq;
                        using default_ops::eval_lt;
                        using default_ops::eval_modulus;
                        using default_ops::eval_msb;
                        using default_ops::eval_multiply;
                        using default_ops::eval_subtract;

                        //
                        // to prevent problems with trivial cpp_int
                        //
                        Backend2 modulus(get_mod().backend());

                        if (eval_lt(input, modulus)) {
                            while (eval_lt(input, 0u)) {
                                eval_add(input, modulus);
                            }
                        } else if (eval_msb(input) < 2u * eval_msb(modulus) + 1u) {
                            Backend_quadruple_1 t1(input);

                            eval_multiply(t1, get_mu());
                            custom_right_shift(t1, 2u * (1u + eval_msb(modulus)));
                            eval_multiply(t1, modulus);
                            eval_subtract(input, t1);

                            if (!eval_lt(input, modulus)) {
                                eval_subtract(input, modulus);
                            }
                        } else {
                            eval_modulus(input, modulus);
                        }
                        result = input;
                    }

                    template<typename Backend1>
                    constexpr void montgomery_reduce(Backend1& result) const {
                        montgomery_reduce(result, result);
                    }

                    template<typename Backend1, typename Backend2,
                             typename = typename boost::enable_if_c<
                                 /// result should fit in the output parameter
                                 max_precision<Backend1>::value >= max_precision<Backend>::value &&
                                 /// input number should be represented by backend of appropriate size
                                 max_precision<Backend_doubled_limbs>::value >= max_precision<Backend2>::value>::type>
                    constexpr void montgomery_reduce(Backend1& result, const Backend2& input) const {
                        using default_ops::eval_add;
                        using default_ops::eval_bitwise_and;
                        using default_ops::eval_left_shift;
                        using default_ops::eval_lt;
                        using default_ops::eval_multiply;
                        using default_ops::eval_subtract;

                        Backend_doubled_padded_limbs accum(input);
                        Backend_doubled_padded_limbs prod;

                        for (auto i = 0; i < get_mod().backend().size(); ++i) {
                            eval_multiply(
                                prod, get_mod().backend(),
                                static_cast<internal_limb_type>(custom_get_limb_value<internal_limb_type>(accum, i) *
                                                                /// to prevent overflow error in constexpr
                                                                static_cast<double_limb_type>(get_p_dash())));
                            eval_left_shift(prod, i * limb_bits);
                            eval_add(accum, prod);
                        }

                        custom_right_shift(accum, get_mod().backend().size() * limb_bits);

                        if (!eval_lt(accum, get_mod().backend())) {
                            eval_subtract(accum, get_mod().backend());
                        }
                        eval_bitwise_and(accum, m_modulus_mask);
                        result = accum;
                    }

                    template<typename Backend1, typename Backend2>
                    constexpr void regular_add(Backend1& result, const Backend2& y) const {
                        regular_add(result, result, y);
                    }

                    template<typename Backend1, typename Backend2, typename Backend3,
                             /// result should fit in the output parameter
                             typename = typename boost::enable_if_c<max_precision<Backend1>::value >=
                                                                    max_precision<Backend>::value>::type>
                    constexpr void regular_add(Backend1& result, const Backend2& x, const Backend3& y) const {
                        using default_ops::eval_add;
                        using default_ops::eval_lt;

                        // TODO: maybe reduce input parameters
                        /// input parameters should be lesser than modulus
                        // BOOST_ASSERT(eval_lt(x, get_mod().backend()) && eval_lt(y, get_mod().backend()));

                        Backend_padded_limbs tmp(x);
                        eval_add(tmp, y);
                        barrett_reduce(result, tmp);
                    }

                    template<typename Backend1, typename Backend2>
                    constexpr void regular_mul(Backend1& result, const Backend2& y) const {
                        regular_mul(result, result, y);
                    }

                    template<typename Backend1, typename Backend2, typename Backend3,
                             /// result should fit in the output parameter
                             typename = typename boost::enable_if_c<max_precision<Backend1>::value >=
                                                                    max_precision<Backend>::value>::type>
                    constexpr void regular_mul(Backend1& result, const Backend2& x, const Backend3& y) const {
                        using default_ops::eval_lt;
                        using default_ops::eval_multiply;

                        // TODO: maybe reduce input parameters
                        /// input parameters should be lesser than modulus
                        // BOOST_ASSERT(eval_lt(x, get_mod().backend()) && eval_lt(y, get_mod().backend()));

                        Backend_doubled_limbs tmp(x);
                        eval_multiply(tmp, y);
                        barrett_reduce(result, tmp);
                    }

                    template<typename Backend1, typename Backend2>
                    constexpr void montgomery_mul(Backend1& result, const Backend2& y) const {
                        montgomery_mul(result, result, y);
                    }

                    //
                    // WARNING: could be errors here due to trivial backend -- more tests needed
                    //
                    template<typename Backend1, typename Backend2, typename Backend3,
                             /// result should fit in the output parameter
                             typename = typename boost::enable_if_c<max_precision<Backend1>::value >=
                                                                    max_precision<Backend>::value>::type>
                    constexpr void montgomery_mul(Backend1& result, const Backend2& x, const Backend3& y) const {
                        using default_ops::eval_bitwise_and;
                        using default_ops::eval_lt;
                        using default_ops::eval_subtract;

                        // TODO: maybe reduce input parameters
                        /// input parameters should be lesser than modulus
                        // BOOST_ASSERT(eval_lt(x, get_mod().backend()) && eval_lt(y, get_mod().backend()));

                        Backend_padded_limbs A(internal_limb_type(0u));

                        for (auto i = 0; i < get_mod().backend().size(); i++) {
                            internal_limb_type u_i =
                                (A.limbs()[0] + get_limb_value(x, i) * get_limb_value(y, 0)) * get_p_dash();

                            // A += x[i] * y + u_i * m followed by a 1 limb-shift to the right
                            internal_limb_type k = 0;
                            internal_limb_type k2 = 0;

                            internal_double_limb_type z =
                                static_cast<internal_double_limb_type>(get_limb_value(y, 0)) *
                                    static_cast<internal_double_limb_type>(get_limb_value(x, i)) +
                                A.limbs()[0] + k;
                            internal_double_limb_type z2 =
                                static_cast<internal_double_limb_type>(get_limb_value(get_mod().backend(), 0)) *
                                    static_cast<internal_double_limb_type>(u_i) +
                                static_cast<internal_limb_type>(z) + k2;
                            k = static_cast<internal_limb_type>(z >> std::numeric_limits<internal_limb_type>::digits);
                            k2 = static_cast<internal_limb_type>(z2 >> std::numeric_limits<internal_limb_type>::digits);

                            for (auto j = 1; j < get_mod().backend().size(); ++j) {
                                internal_double_limb_type t =
                                    static_cast<internal_double_limb_type>(get_limb_value(y, j)) *
                                        static_cast<internal_double_limb_type>(get_limb_value(x, i)) +
                                    A.limbs()[j] + k;
                                internal_double_limb_type t2 =
                                    static_cast<internal_double_limb_type>(get_limb_value(get_mod().backend(), j)) *
                                        static_cast<internal_double_limb_type>(u_i) +
                                    static_cast<internal_limb_type>(t) + k2;
                                A.limbs()[j - 1] = static_cast<internal_limb_type>(t2);
                                k = static_cast<internal_limb_type>(t >>
                                                                    std::numeric_limits<internal_limb_type>::digits);
                                k2 = static_cast<internal_limb_type>(t2 >>
                                                                     std::numeric_limits<internal_limb_type>::digits);
                            }
                            internal_double_limb_type tmp =
                                static_cast<internal_double_limb_type>(
                                    custom_get_limb_value<internal_limb_type>(A, get_mod().backend().size())) +
                                k + k2;
                            custom_set_limb_value<internal_limb_type>(A, get_mod().backend().size() - 1,
                                                                      static_cast<internal_limb_type>(tmp));
                            custom_set_limb_value<internal_limb_type>(
                                A, get_mod().backend().size(),
                                static_cast<internal_limb_type>(tmp >>
                                                                std::numeric_limits<internal_limb_type>::digits));
                        }
                        //
                        // recover correct size of backend content
                        //
                        adjust_backend_size(A, get_mod().backend().size());

                        if (!eval_lt(A, get_mod().backend())) {
                            eval_subtract(A, get_mod().backend());
                        }
                        // TODO: maybe bitwise AND is not necessary
                        // eval_bitwise_and(A, get_modulus_mask());
                        result = A;
                    }

                    template<typename Backend1, typename Backend2>
                    constexpr void regular_exp(Backend1& result, const Backend2& exp) const {
                        regular_exp(result, result, exp);
                    }

                    template<typename Backend1, typename Backend2, typename Backend3,
                             /// result should fit in the output parameter
                             typename = typename boost::enable_if_c<max_precision<Backend1>::value >=
                                                                    max_precision<Backend>::value>::type>
                    constexpr void regular_exp(Backend1& result, Backend2& a, Backend3 exp) const {
                        using default_ops::eval_eq;
                        using default_ops::eval_lt;
                        using default_ops::eval_multiply;

                        // TODO: maybe reduce input parameter
                        /// input parameter should be lesser than modulus
                        // BOOST_ASSERT(eval_lt(a, get_mod().backend()));

                        if (eval_eq(exp, static_cast<internal_limb_type>(0u))) {
                            result = static_cast<internal_limb_type>(1u);
                            return;
                        }
                        if (eval_eq(get_mod().backend(), static_cast<internal_limb_type>(1u))) {
                            result = static_cast<internal_limb_type>(0u);
                            return;
                        }

                        Backend_doubled_limbs base(a), res(static_cast<internal_limb_type>(1u));

                        while (true) {
                            internal_limb_type lsb = exp.limbs()[0] & 1u;
                            custom_right_shift(exp, static_cast<internal_limb_type>(1u));
                            if (lsb) {
                                eval_multiply(res, base);
                                barrett_reduce(res);
                                if (eval_eq(exp, static_cast<internal_limb_type>(0u))) {
                                    break;
                                }
                            }
                            eval_multiply(base, base);
                            barrett_reduce(base);
                        }
                        result = res;
                    }

                    template<typename Backend1, typename Backend2>
                    constexpr void montgomery_exp(Backend1& result, const Backend2& exp) const {
                        montgomery_exp(result, result, exp);
                    }

                    template<typename Backend1, typename Backend2, typename Backend3,
                             /// result should fit in the output parameter
                             typename = typename boost::enable_if_c<max_precision<Backend1>::value >=
                                                                    max_precision<Backend>::value>::type>
                    constexpr void montgomery_exp(Backend1& result, const Backend2& a, Backend3 exp) const {
                        using default_ops::eval_eq;
                        using default_ops::eval_lt;
                        using default_ops::eval_multiply;

                        // TODO: maybe reduce input parameter
                        /// input parameter should be lesser than modulus
                        // BOOST_ASSERT(eval_lt(a, get_mod().backend()));

                        Backend_doubled_limbs tmp(static_cast<internal_limb_type>(1u));
                        eval_multiply(tmp, get_r2());
                        montgomery_reduce(tmp);
                        Backend R_mod_m(tmp);

                        Backend base(a);

                        if (eval_eq(exp, static_cast<internal_limb_type>(0u))) {
                            result = static_cast<internal_limb_type>(1u);
                            //
                            // TODO: restructure code
                            // adjust_modular
                            //
                            eval_multiply(result, get_r2());
                            montgomery_reduce(result);
                            return;
                        }
                        if (eval_eq(get_mod().backend(), static_cast<internal_limb_type>(1u))) {
                            result = static_cast<internal_limb_type>(0u);
                            return;
                        }

                        while (true) {
                            internal_limb_type lsb = exp.limbs()[0] & 1u;
                            custom_right_shift(exp, static_cast<internal_limb_type>(1u));
                            if (lsb) {
                                montgomery_mul(R_mod_m, base);
                                if (eval_eq(exp, static_cast<internal_limb_type>(0u))) {
                                    break;
                                }
                            }
                            montgomery_mul(base, base);
                        }
                        result = R_mod_m;
                    }

                    constexpr void swap(modular_functions_fixed& o) {
                        get_mod().swap(o.get_mod());
                        get_mu().swap(o.get_mu());
                        get_r2().swap(o.get_r2());

                        auto tmp_p_dash = get_p_dash();
                        get_p_dash() = o.get_p_dash();
                        o.get_p_dash() = tmp_p_dash;

                        get_modulus_mask().swap(o.get_modulus_mask());
                    }

                    constexpr modular_functions_fixed& operator=(const modular_functions_fixed& o) {
                        modular_functions_fixed tmp(o);
                        swap(tmp);

                        return *this;
                    }

                    constexpr modular_functions_fixed& operator=(const number_type& m) {
                        initialize(m);

                        return *this;
                    }

                protected:
                    // TODO: replace number_type on backend type
                    number_type m_mod;
                    Backend_doubled_1 m_barrett_mu;
                    Backend m_montgomery_r2;
                    internal_limb_type m_montgomery_p_dash = 0;
                    Backend_padded_limbs m_modulus_mask;
                };

            }    // namespace backends
        }        // namespace multiprecision
    }            // namespace crypto3
}    // namespace nil

#endif    // BOOST_MULTIPRECISION_MODULAR_FUNCTIONS_FIXED_PRECISION_HPP


/*******************************************************************************
 *
 * Construction and management of weak topological orderings (WTOs).
 *
 * The construction of weak topological orderings is based on F. Bourdoncle's
 * paper: "Efficient chaotic iteration strategies with widenings", Formal
 * Methods in Programming and Their Applications, 1993, pages 128-141.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#ifndef IKOS_WTO_HPP
#define IKOS_WTO_HPP

#include <vector>
#include <deque>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/container/slist.hpp>
#include <crab/common/types.hpp>
#include <crab/common/stats.hpp>
#include <crab/domains/intervals.hpp>

namespace ikos {

  template< typename NodeName, typename CFG >
  class wto;

  template< typename NodeName, typename CFG >
  class wto_vertex;

  template< typename NodeName, typename CFG >
  class wto_cycle;

  template< typename NodeName, typename CFG >
  class wto_component_visitor;

  template< typename NodeName, typename CFG >
  class wto_nesting: public writeable {

    friend class wto< NodeName, CFG >;
    friend class wto_vertex< NodeName, CFG >;
    friend class wto_cycle< NodeName, CFG >;

  public:
    typedef wto_nesting< NodeName, CFG > wto_nesting_t;

  private:
    typedef std::vector< NodeName > node_list_t;
    typedef boost::shared_ptr< node_list_t > node_list_ptr;

  private:
    node_list_ptr _nodes;

  public:
    class iterator: public boost::iterator_facade< iterator,
                                                   NodeName&,
                                                   boost::forward_traversal_tag,
                                                   NodeName&> {
      friend class boost::iterator_core_access;

    private:
      typename node_list_t::iterator _it;
      node_list_ptr _l;

    public:
      iterator(node_list_ptr l, bool b): _it(b ? l->begin() : l->end()), _l(l) { }

    private:
      void increment() {
	++(this->_it);
      }

      bool equal(const iterator& other) const {
	return (this->_l == other._l && this->_it == other._it);
      }

      NodeName& dereference() const {
	if (this->_it != this->_l->end()) {
	  return *(this->_it);
	} else {
	  CRAB_ERROR("WTO nesting: trying to dereference an empty iterator");
	}
      }

    }; // class iterator

  private:
    wto_nesting(node_list_ptr l): _nodes(boost::make_shared<node_list_t>(*l)) { }

    int compare(wto_nesting_t& other) {
      iterator this_it = this->begin(), other_it = other.begin();
      while (this_it != this->end()) {
	if (other_it == other.end()) {
	  return 1;
	} else if (*this_it == *other_it) {
	  ++this_it;
	  ++other_it;
	} else {
	  return 2; // Nestings are not comparable
	}
      }
      if (other_it == other.end()) {
	return 0;
      } else {
	return -1;
      }
    }

  public:
    wto_nesting(): _nodes(boost::make_shared<node_list_t>()) { }

    void operator+=(NodeName n) {
      this->_nodes = boost::make_shared<node_list_t>(*(this->_nodes));
      this->_nodes->push_back(n);
    }

    wto_nesting_t operator+(NodeName n) {
      wto_nesting_t res(this->_nodes);
      res._nodes->push_back(n);
      return res;
    }

    iterator begin() {
      return iterator(this->_nodes, true);
    }

    iterator end() {
      return iterator(this->_nodes, false);
    }

    wto_nesting_t operator^(wto_nesting_t other) {
      wto_nesting_t res;
      for (iterator this_it = this->begin(), other_it = other.begin();
           this_it != this->end() && other_it != other.end();
           ++this_it, ++other_it) {
        if (*this_it == *other_it) {
          res._nodes->push_back(*this_it);
        } else {
          break;
        }
      }
      return res;
    }

    bool operator<=(wto_nesting_t other) {
      return this->compare(other) <= 0;
    }

    bool operator==(wto_nesting_t other) {
      return this->compare(other) == 0;
    }

    bool operator>=(wto_nesting_t other) {
      return this->operator<=(other, *this);
    }

    bool operator>(wto_nesting_t other) {
      return this->compare(other) == 1;
    }

    void write(crab::crab_os& o) {
      o << "[";
      for (iterator it = this->begin(); it != this->end(); ) {
	NodeName n = *it;
	o << n;
	++it;
	if (it != this->end()) {
	  o << ", ";
	}
      }
      o << "]";
    }

  }; // class nesting

  template< typename NodeName, typename CFG >
  class wto_component: public writeable {

  public:
    typedef wto_nesting< NodeName, CFG > wto_nesting_t;

  public:
    virtual void accept(wto_component_visitor< NodeName, CFG > *) = 0;

    virtual ~wto_component() { }

  }; // class wto_component

  template< typename NodeName, typename CFG >
  class wto_vertex: public wto_component< NodeName, CFG > {

    friend class wto< NodeName, CFG >;

  private:
    NodeName _node;

  private:
    wto_vertex(NodeName node): _node(node) { }

  public:
    NodeName node() {
      return this->_node;
    }

    void accept(wto_component_visitor< NodeName, CFG > *v) {
      v->visit(*this);
    }

    void write(crab::crab_os& o) {
      o << this->_node;
    }

  }; // class wto_vertex

  template< typename NodeName, typename CFG >
  class wto_cycle: public wto_component< NodeName, CFG > {

    friend class wto< NodeName, CFG >;

  public:
    typedef wto_component< NodeName, CFG > wto_component_t;

  private:
    typedef boost::shared_ptr< wto_component_t > wto_component_ptr;
    typedef boost::container::slist< wto_component_ptr > wto_component_list_t;
    typedef boost::shared_ptr< wto_component_list_t > wto_component_list_ptr;

  private:
    NodeName _head;
    wto_component_list_ptr _wto_components;

  private:
    wto_cycle(NodeName head,
              wto_component_list_ptr wto_components):
        _head(head), _wto_components(wto_components) { }

  public:
    class iterator: public boost::iterator_facade< iterator,
                                                   wto_component_t&,
                                                   boost::forward_traversal_tag,
                                                   wto_component_t&> {

      friend class boost::iterator_core_access;

    private:
      typename wto_component_list_t::iterator _it;
      wto_component_list_ptr _l;

    public:
      iterator(wto_component_list_ptr l, bool b):
          _it(b ? l->begin() : l->end()), _l(l) { }

     private:
      void increment() {
        ++(this->_it);
      }

      bool equal(const iterator& other) const {
        return (this->_l == other._l && this->_it == other._it);
      }

      wto_component_t& dereference() const {
        if (this->_it != this->_l->end()) {
          return **(this->_it);
        } else {
          CRAB_ERROR("WTO cycle: trying to dereference an empty iterator");
	}
      }

    }; // class iterator

  public:

    NodeName head() {
      return this->_head;
    }

    void accept(wto_component_visitor< NodeName, CFG > *v) {
      v->visit(*this);
    }

    iterator begin() {
      return iterator(this->_wto_components, true);
    }

    iterator end() {
      return iterator(this->_wto_components, false);
    }

    void write(crab::crab_os& o) {
      o << "(" << this->_head;
      if (!this->_wto_components->empty()) {
	o << " ";
	for (iterator it = this->begin(); it != this->end(); ) {
	  wto_component_t& c = *it;
	  o << c;
	  ++it;
	  if (it != this->end()) {
	    o << " ";
	  }
	}
      }
      o << ")";
    }

  }; // class wto_cycle

  template< typename NodeName, typename CFG >
  class wto_component_visitor {

  public:
    typedef wto_vertex< NodeName, CFG > wto_vertex_t;
    typedef wto_cycle< NodeName, CFG > wto_cycle_t;

  public:
    virtual void visit(wto_vertex_t&) = 0;
    virtual void visit(wto_cycle_t&) = 0;
    virtual ~wto_component_visitor() {}

  }; // class wto_component_visitor

  template< typename NodeName, typename CFG >
  class wto: public writeable {

  public:
    typedef wto_nesting< NodeName, CFG > wto_nesting_t;
    typedef wto_component< NodeName, CFG > wto_component_t;
    typedef wto_vertex< NodeName, CFG > wto_vertex_t;
    typedef wto_cycle< NodeName, CFG > wto_cycle_t;
    typedef wto< NodeName, CFG > wto_t;

  private:
    typedef boost::shared_ptr< wto_component_t > wto_component_ptr;
    typedef boost::shared_ptr< wto_vertex_t > wto_vertex_ptr;
    typedef boost::shared_ptr< wto_cycle_t > wto_cycle_ptr;
    typedef boost::container::slist< wto_component_ptr > wto_component_list_t;
    typedef boost::shared_ptr< wto_component_list_t > wto_component_list_ptr;
    typedef bound< z_number > dfn_t;
    typedef std::map< NodeName, dfn_t > dfn_table_t;
    typedef boost::shared_ptr< dfn_table_t > dfn_table_ptr;
    typedef std::deque< NodeName > stack_t;
    typedef boost::shared_ptr< stack_t > stack_ptr;
    typedef std::map< NodeName, wto_nesting_t > nesting_table_t;
    typedef boost::shared_ptr< nesting_table_t > nesting_table_ptr;

  private:
    wto_component_list_ptr _wto_components;
    dfn_table_ptr _dfn_table;
    dfn_t _num;
    stack_ptr _stack;
    nesting_table_ptr _nesting_table;

  private:
    class nesting_builder: public wto_component_visitor< NodeName, CFG > {

    public:
      typedef wto_vertex< NodeName, CFG > wto_vertex_t;
      typedef wto_cycle< NodeName, CFG > wto_cycle_t;

    private:
      wto_nesting_t _nesting;
      nesting_table_ptr _nesting_table;

    public:
      nesting_builder(nesting_table_ptr nesting_table):
          _nesting_table(nesting_table) { }

      void visit(wto_cycle_t& cycle) {
        NodeName head = cycle.head();
        wto_nesting_t previous_nesting = this->_nesting;
        this->_nesting_table->insert(std::make_pair(head, this->_nesting));
        this->_nesting += head;
        for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end(); ++it) {
          it->accept(this);
        }
        this->_nesting = previous_nesting;
      }

      void visit(wto_vertex_t& vertex) {
        this->_nesting_table->insert(std::make_pair(vertex.node(), this->_nesting));
      }

    }; // class nesting_builder

  private:
    dfn_t get_dfn(NodeName n) {
      typename dfn_table_t::iterator it = this->_dfn_table->find(n);
      if (it == this->_dfn_table->end()) {
        return 0;
      } else {
        return it->second;
      }
    }

    void set_dfn(NodeName n, dfn_t dfn) {
      std::pair< typename dfn_table_t::iterator, bool > res =
          this->_dfn_table->insert(std::make_pair(n, dfn));
      if (!res.second) {
        (res.first)->second = dfn;
      }
    }

    NodeName pop() {
      if (this->_stack->empty()) {
        CRAB_ERROR("WTO computation: empty stack");
      } else {
        NodeName top = this->_stack->back();
        this->_stack->pop_back();
        return top;
      }
    }

    void push(NodeName n) {
      this->_stack->push_back(n);
    }

    wto_cycle_ptr component(CFG cfg, NodeName vertex) {
      auto partition = boost::make_shared<wto_component_list_t>();
      auto next_nodes = cfg.next_nodes(vertex);
      for (NodeName succ : next_nodes) {
        if (this->get_dfn(succ) == 0) {
          this->visit(cfg, succ, partition);
        }
      }
      return wto_cycle_ptr(new wto_cycle_t(vertex, partition));
    }

    dfn_t visit(CFG cfg, NodeName vertex, wto_component_list_ptr partition) {
      dfn_t head = 0, min = 0;
      bool loop;
      NodeName element;

      this->push(vertex);
      this->_num += 1;
      head = this->_num;
      this->set_dfn(vertex, head);
      loop = false;
      auto next_nodes = cfg.next_nodes(vertex);
      for (NodeName succ: next_nodes) {
        dfn_t succ_dfn = this->get_dfn(succ);
        if (succ_dfn == 0) {
          min = this->visit(cfg, succ, partition);
        } else {
          min = succ_dfn;
        }
        if (min <= head) {
          head = min;
	  loop = true;
        }
      }
      if (head == this->get_dfn(vertex)) {
        this->set_dfn(vertex, dfn_t::plus_infinity());
        element = this->pop();
        if (loop) {
          while (!(element == vertex)) {
            this->set_dfn(element, 0);
            element = this->pop();
          }
          partition->push_front(boost::static_pointer_cast< wto_component_t,
                                wto_cycle_t >(this->component(cfg, vertex)));
        } else {
          partition->push_front(boost::static_pointer_cast< wto_component_t,
                                wto_vertex_t >(wto_vertex_ptr(new wto_vertex_t(vertex))));
	}
      }
      return head;
    }

    void build_nesting() {
      nesting_builder builder(this->_nesting_table);
      for (iterator it = this->begin(); it != this->end(); ++it) {
        it->accept(&builder);
      }
    }

  public:
    class iterator: public boost::iterator_facade< iterator,
                                                   wto_component_t&,
                                                   boost::forward_traversal_tag,
                                                   wto_component_t&> {

      friend class boost::iterator_core_access;

     private:
      typename wto_component_list_t::iterator _it;
      wto_component_list_ptr _l;

     public:
      iterator(wto_component_list_ptr l, bool b):
          _it(b ? l->begin() : l->end()), _l(l) { }

    private:
      void increment() {
        ++(this->_it);
      }

      bool equal(const iterator& other) const {
        return (this->_l == other._l && this->_it == other._it);
      }

      wto_component_t& dereference() const {
        if (this->_it != this->_l->end()) {
          return **(this->_it);
        } else {
          CRAB_ERROR("WTO: trying to dereference an empty iterator");
        }
      }

    }; // class iterator

  public:
    wto(CFG cfg):
        _wto_components(boost::make_shared<wto_component_list_t>()),
        _dfn_table(boost::make_shared<dfn_table_t>()),
        _num(0), _stack(boost::make_shared<stack_t>()),
        _nesting_table(boost::make_shared<nesting_table_t>()) {
      crab::ScopedCrabStats __st__("Fixpo.WTO");

      this->visit(cfg, cfg.entry(), this->_wto_components);
      this->_dfn_table.reset();
      this->_stack.reset();
      this->build_nesting();
    }

    wto(const wto_t& other): _wto_components(other._wto_components),
                             _nesting_table(other._nesting_table) { }

    wto_t& operator=(wto_t other) {
      this->_wto_components = other._wto_components;
      this->_nesting_table = other._nesting_table;
      return *this;
    }

    iterator begin() {
      return iterator(this->_wto_components, true);
    }

    iterator end() {
      return iterator(this->_wto_components, false);
    }

    wto_nesting_t nesting(NodeName n) {
      typename nesting_table_t::iterator it = this->_nesting_table->find(n);
      if (it == this->_nesting_table->end()) {
        CRAB_ERROR("WTO nesting: node ", n," not found");
      } else {
        return it->second;
      }
    }

    void accept(wto_component_visitor< NodeName, CFG > *v) {
      for (iterator it = this->begin(); it != this->end(); ++it) {
        it->accept(v);
      }
    }

    void write(crab::crab_os& o) {
      for (iterator it = this->begin(); it != this->end(); ) {
        wto_component_t& c = *it;
        o << c;
        ++it;
        if (it != this->end()) {
          o << " ";
        }
      }
    }

  }; // class wto

} // namespace ikos

#endif // IKOS_WTO_HPP


/**
 * @file validate.cpp
 * @brief This contains utility function validate things like forward kinematics match inverse kinematics
 *
 * @author Levi Armstrong
 * @date April 15, 2018
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2013, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Geometry>
#include <console_bridge/console.h>
#include <algorithm>
#include <sstream>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_common/utils.h>
#include <tesseract_kinematics/core/validate.h>

namespace tesseract_kinematics
{
bool checkKinematics(const KinematicGroup& manip, double tol)
{
  Eigen::Isometry3d test1;
  Eigen::Isometry3d test2;
  Eigen::VectorXd seed_angles(manip.numJoints());
  Eigen::VectorXd joint_angles2(manip.numJoints());
  std::vector<std::string> tip_links = manip.getAllPossibleTipLinkNames();
  std::vector<std::string> working_frames = manip.getAllValidWorkingFrames();
  const int nj = static_cast<int>(manip.numJoints());

  std::vector<std::vector<double>> passed_data;
  std::vector<std::vector<double>> failed_data;
  int translation_failures{ 0 };
  int angular_failures{ 0 };
  double translation_max{ 0 };
  double angular_max{ 0 };

  std::vector<std::pair<std::string, std::string>> checks;
  checks.reserve(tip_links.size() + working_frames.size());

  for (const auto& tip_link : tip_links)
    checks.emplace_back(working_frames[0], tip_link);

  for (const auto& working_frame : working_frames)
    checks.emplace_back(working_frame, tip_links[0]);

  for (const auto& check : checks)
  {
    seed_angles.setZero();
    joint_angles2.setZero();

    for (int t = 0; t < nj; ++t)
    {
      joint_angles2[t] = M_PI_4;

      auto poses1 = manip.calcFwdKin(joint_angles2);
      test1 = poses1.at(check.first).inverse() * poses1.at(check.second);
      KinGroupIKInput ik_input(test1, check.first, check.second);
      IKSolutions sols = manip.calcInvKin({ ik_input }, seed_angles);
      for (const auto& sol : sols)
      {
        auto poses2 = manip.calcFwdKin(sol);
        test2 = poses2.at(check.first).inverse() * poses2.at(check.second);

        double translation_distance = (test1.translation() - test2.translation()).norm();
        double angular_distance =
            Eigen::Quaterniond(test1.linear()).angularDistance(Eigen::Quaterniond(test2.linear()));
        if (translation_distance > tol || angular_distance > tol)
        {
          if (translation_distance > tol)
            ++translation_failures;

          if (angular_distance > tol)
            ++angular_failures;

          if (angular_distance > angular_max)
            angular_max = angular_distance;

          if (translation_distance > translation_max)
            translation_max = translation_distance;

          std::vector<double> data{ translation_distance, tol, angular_distance, tol };
          for (Eigen::Index i = 0; i < sol.rows(); ++i)
            data.push_back(sol(i));

          failed_data.push_back(data);
        }
        else
        {
          std::vector<double> data{ translation_distance, tol, angular_distance, tol };
          for (Eigen::Index i = 0; i < sol.rows(); ++i)
            data.push_back(sol(i));

          passed_data.push_back(data);
        }
      }

      joint_angles2[t] = 0;
    }
  }

  if (!failed_data.empty())
  {
    CONSOLE_BRIDGE_logError("checkKinematics failed %d out of %d\n           Translation failures %d out of %d (max: "
                            "%f)\n           Angular failures %d out of %d (max: %f)",
                            failed_data.size(),
                            (failed_data.size() + passed_data.size()),
                            translation_failures,
                            failed_data.size(),
                            translation_max,
                            angular_failures,
                            failed_data.size(),
                            angular_max);
    std::stringstream msg;
    msg << std::endl;
    msg << "*****************************" << std::endl;
    msg << "******** Failed Data ********" << std::endl;
    msg << "*****************************" << std::endl;

    std::string header = "Trans. Dist. (m), tol, Angle Dist. (rad), tol";
    for (const auto& jn : manip.getJointNames())
      header += ", " + jn;

    msg << header << std::endl;
    for (const auto& d : failed_data)
    {
      for (const auto& val : d)
        msg << val << ", ";
      msg << std::endl;
    }

    msg << "*****************************" << std::endl;
    msg << "******** Passed Data ********" << std::endl;
    msg << "*****************************" << std::endl;
    if (passed_data.empty())
    {
      msg << "No Data!" << std::endl;
    }
    else
    {
      for (const auto& d : passed_data)
      {
        for (const auto& val : d)
          msg << val << ", ";
        msg << std::endl;
      }
    }

    CONSOLE_BRIDGE_logError("%s", msg.str().c_str());
    return false;
  }

  return true;
}
}  // namespace tesseract_kinematics


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <engine/map/dungeonlayout.hpp>

#include <boost/multi_index_container.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/identity.hpp>

#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/function_property_map.hpp>

#include <boost/geometry/index/rtree.hpp>
#include <math/geometrytraits.hpp>
#include <math/mathtools.hpp>

#include <math/segment.hpp>

#include <math/polygon_def.hpp>

#include <gen/graphutils.hpp>
namespace eXl
{
  using RoomIndexEntry = std::pair<AABB2Di, uint32_t>;
  using RoomIndex = boost::geometry::index::rtree<RoomIndexEntry, boost::geometry::index::linear<16, 4>>;

  typedef DungeonGraph::Graph Graph;
  typedef Graph::vertex_descriptor Vertex;
  typedef Graph::edge_descriptor Edge;

  //struct NodeWithDegree
  //{
  //  Vertex vtx;
  //  uint32_t degree;
  //};
  //
  //struct NodeIdentity
  //{
  //  typedef Vertex result_type;
  //  result_type operator()(NodeWithDegree const& iEntry) const
  //  {
  //    return iEntry.vtx;
  //  }
  //};
  //
  //struct NodeDegree
  //{
  //  typedef uint32_t result_type;
  //  result_type operator()(NodeWithDegree const& iEntry) const
  //  {
  //    return iEntry.degree;
  //  }
  //};
  //
  //typedef boost::multi_index::multi_index_container<NodeWithDegree,
  //  boost::multi_index::indexed_by<
  //  boost::multi_index::hashed_unique<NodeIdentity>
  //  ,boost::multi_index::ordered_non_unique<NodeDegree>
  //  >
  //> NodeDegreeMap;

  struct PhysicalNodesFilter
  {
    PhysicalNodesFilter()
    {}

    PhysicalNodesFilter(DungeonGraph const& iGraph)
      : m_Graph(&iGraph)
    {}

    bool operator()(const DungeonGraph::GraphVtx& e) const
    {
      DungeonGraph::NodeProperties const* props = m_Graph->GetProperties(e);
      return props->IsPhysical() && props->GetSize() > 0;
    }

    bool operator()(const DungeonGraph::GraphEdge& e) const
    {
      return m_Graph->GetProperties(e)->m_PhysicalConnection;
    }

    DungeonGraph const* m_Graph;
  };

  struct MSTFilter
  {
    MSTFilter()
    {}

    MSTFilter(Set<Edge> const& iEdges)
      :m_Edges(&iEdges)
    {}

    template <typename Edge>
    bool operator()(const Edge& e) const
    {
      return m_Edges->count(e) > 0;
    }
    Set<Edge> const* m_Edges;
  };

  struct GraphFilter
  {
    GraphFilter()
    {}

    GraphFilter(Set<Vertex> const& iVtx)
      :m_Vtx(&iVtx)
    {}

    bool operator()(const Vertex& vtx) const
    {
      return m_Vtx->count(vtx) > 0;
    }
    Set<Vertex> const* m_Vtx;
  };

  namespace
  {

    int const s_DoorSize = 4;
    int const s_MaxRoomSize = 5;

    Vector2i SampleRoom(DungeonGraph const& iGraph, Vertex iVtx, Random& iRand)
    {
      DungeonGraph::NodeProperties const* props = iGraph.GetProperties(iVtx);

      Vector2i baseSize = Vector2i(iRand() % 10 + 4, iRand() % 10 + 4);

      //uint32_t const roomSize = Mathi::Min(s_MaxRoomSize, props != nullptr ? props->GetSize() : 1);
      //
      //Vector2i baseSize = Vector2i::ONE * 2 * (roomSize + 1);
      //
      //switch(iRand() % 3)
      //{
      //case 0:
      //  baseSize += Vector2i::ONE * Mathi::Max(roomSize / 4, 1);
      //  break;
      //case 1:
      //  baseSize.m_Data[0] += roomSize / 2;
      //  break;
      //case 2:
      //  baseSize.m_Data[1] += roomSize / 2;
      //  break;
      //}
      return baseSize;
    };

    void AddRoomConfSpace(Vector2i const& iBoxToLayoutSize, AABB2Di const& iBox, Vector<Segmenti>& oSegs)
    {
      for(int dim = 0; dim<2; ++dim)
      {
        int otherDim = 1 - dim;
        Segmenti minSeg;
        minSeg.m_Ext1 = minSeg.m_Ext2 = iBox.m_Data[0];
        minSeg.m_Ext1.m_Data[otherDim] -= iBoxToLayoutSize.m_Data[otherDim];
        minSeg.m_Ext2 = minSeg.m_Ext1;
        minSeg.m_Ext2.m_Data[dim] = iBox.m_Data[1].m_Data[dim];

        minSeg.m_Ext1.m_Data[dim] -= (iBoxToLayoutSize.m_Data[dim] - s_DoorSize);
        minSeg.m_Ext2.m_Data[dim] -= s_DoorSize;

        oSegs.push_back(minSeg);

        Segmenti maxSeg = minSeg;
        maxSeg.m_Ext1.m_Data[otherDim] = maxSeg.m_Ext2.m_Data[otherDim] = iBox.m_Data[1].m_Data[otherDim];

        oSegs.push_back(maxSeg);
      }
    };

    Vector<Segmentf> MergeConfSpace (Vector<Segmentf> const& iSpace1, Vector<Segmentf> const& iSpace2)
    {
      Vector<Segmentf> result;
      for(auto const& seg1 : iSpace1)
      {
        bool seg1IsPoint = seg1.m_Ext1 == seg1.m_Ext2;
        for(auto const& seg2 : iSpace2)
        {
          bool seg2IsPoint = seg2.m_Ext1 == seg2.m_Ext2;
          if(seg1IsPoint)
          {
            if(seg2IsPoint)
            {
              if(seg1 == seg2)
              {
                result.push_back(seg1);
              }
            }
            else
            {
              if(seg2.IsOnSegment(seg1.m_Ext1))
              {
                result.push_back(seg1);
              }
            }
          }
          else
          {
            if(seg2IsPoint)
            {
              if(seg1.IsOnSegment(seg2.m_Ext1))
              {
                result.push_back(seg2);
              }
            }
            else
            {
              Vector2f oPoint;
              uint32_t res = seg1.Intersect(seg2, oPoint);
              if((res & Segmentf::PointOnSegments) == Segmentf::PointOnSegments)
              {
                //Intersection limited to a point.
                Segmentf resultSegment;
                resultSegment.m_Ext1 = resultSegment.m_Ext2 = oPoint;
                result.push_back(resultSegment);
              }
              else if(res & Segmentf::ConfoundSegments)
              {
                Segmentf resultSegment;
                //Segments overlap.
                if(seg1.IsOnSegment(seg2.m_Ext1))
                {
                  resultSegment.m_Ext1 = seg2.m_Ext1;
                }
                else
                {
                  resultSegment.m_Ext1 = seg2.m_Ext2;
                }

                if(seg2.IsOnSegment(seg1.m_Ext1))
                {
                  resultSegment.m_Ext2 = seg1.m_Ext1;
                }
                else
                {
                  resultSegment.m_Ext2 = seg1.m_Ext2;
                }
              }
            }
          }
        }
      }

      return result;
    };
  }

  void ComputeMaxConfSpace(Vector<Segmenti>& ioConfSpace)
  {
    unsigned int const s_NumSegPerRoom = 4;

    //Compute maximum configuration space.
    Vector<std::pair<size_t, Segmenti>> segments;
    boost::polygon::intersect_segments(segments, ioConfSpace.begin(), ioConfSpace.end());

    //Create graph of connected segments and get the biggest connected component.
    Map<Vector2i, uint32_t> pointMap;
    Vector<Vector2i> vertices;
    Vector<Segmenti> edges;
    Vector<Set<uint32_t>> vtxMap;
    Vector<Set<uint32_t>>  edgeMap;
    Map<std::pair<uint32_t, uint32_t>, uint32_t> connections;
    uint32_t numEdges = 0;
    uint32_t numVtx = 0;

    for(auto const& seg : segments)
    {
      auto insertRes = pointMap.insert(std::make_pair(seg.second.m_Ext1, numVtx));
      if(insertRes.second)
      {
        ++numVtx;
        vertices.push_back(seg.second.m_Ext1);
        vtxMap.push_back(Set<uint32_t>());
      }
      auto vtx1 = insertRes.first->second;
      vtxMap[vtx1].insert(seg.first / s_NumSegPerRoom);

      insertRes = pointMap.insert(std::make_pair(seg.second.m_Ext2, numVtx));
      if(insertRes.second)
      {
        ++numVtx;
        vertices.push_back(seg.second.m_Ext1);
        vtxMap.push_back(Set<uint32_t>());
      }
      auto vtx2 = insertRes.first->second;
      vtxMap[vtx2].insert(seg.first / s_NumSegPerRoom);

      auto edgeDesc = std::make_pair(vtx1, vtx2);
      if(edgeDesc.first > edgeDesc.second)
      {
        std::swap(edgeDesc.first, edgeDesc.second);
      }
      auto newEdge = connections.insert(std::make_pair(edgeDesc, numEdges));
      if(newEdge.second)
      {
        ++numEdges;
        edges.push_back(seg.second);
        edgeMap.push_back(Set<uint32_t>());
      }
      edgeMap[newEdge.first->second].insert(seg.first / s_NumSegPerRoom);
    }

    ioConfSpace.clear();

    uint32_t maxScore = 0;
    for(auto const& entry : vtxMap)
    {
      auto curScore = entry.size();
      if(curScore > maxScore)
      {
        maxScore = curScore;
      }
    }
    for(auto const& edge : connections)
    {
      if(edgeMap[edge.second].size() == maxScore)
      {
        ioConfSpace.push_back(edges[edge.second]);
        vtxMap[edge.first.first].clear();
        vtxMap[edge.first.second].clear();
      }
    }

    for(auto const& vtx : pointMap)
    {
      if(vtxMap[vtx.second].size() == maxScore)
      {
        ioConfSpace.push_back({vtx.first, vtx.first});
      }
    }
  }

  Vector<Segmenti> CullConfigurationSpace(Vector2i const& iRoomSize, Vector<Segmenti> const& iConfSpace, RoomIndex const& iRoomIndex)
  {
    Vector<Segmenti> finalConfSpace;
    Vector<RoomIndexEntry> results;
    for(auto const& seg : iConfSpace)
    {
      results.clear();
      if(seg.m_Ext1 == seg.m_Ext2)
      {
        AABB2Di boxToTest(seg.m_Ext1, iRoomSize);
        iRoomIndex.query(boost::geometry::index::overlaps(boxToTest), std::back_inserter(results));
        for(auto& result : results)
        {
          if(boxToTest.Intersect(result.first))
            continue;
        }
        finalConfSpace.push_back(seg);
      }
      else
      {
        int dim = seg.m_Ext1.m_Data[0] != seg.m_Ext2.m_Data[0] ? 0 : 1;
        int otherDim = 1 - dim;

        int minSeg = seg.m_Ext1.m_Data[dim] < seg.m_Ext2.m_Data[dim] ? 0 : 1;
        int maxSeg = 1 - minSeg;

        Vector2i const* segPts = reinterpret_cast<Vector2i const*>(&seg);
        AABB2Di boxToTest;
        boxToTest.m_Data[0] = segPts[minSeg];
        boxToTest.m_Data[1] = segPts[maxSeg] + iRoomSize;

        Segmenti finalSeg;
        finalSeg.m_Ext1 = segPts[minSeg];
        finalSeg.m_Ext2 = segPts[maxSeg];

        Vector<AABB2Di> interBoxes;

        iRoomIndex.query(boost::geometry::index::overlaps(boxToTest), std::back_inserter(results));
        for(auto& result : results)
        {
          if(boxToTest.Intersect(result.first))
          {
            AABB2Di intersection;
            intersection.SetCommonBox(result.first, boxToTest);

            interBoxes.push_back(intersection);
          }
        }

        std::sort(interBoxes.begin(), interBoxes.end(), [dim](AABB2Di const& iBox1, AABB2Di const& iBox2)
        {
          return iBox1.m_Data[0].m_Data[dim] < iBox2.m_Data[0].m_Data[dim];
        });

        for(auto& box : interBoxes)
        {
          if(box.m_Data[0].m_Data[dim] <= finalSeg.m_Ext1.m_Data[dim])
          {
            finalSeg.m_Ext1.m_Data[dim] = Mathi::Max(box.m_Data[1].m_Data[dim], finalSeg.m_Ext1.m_Data[dim]);
          }
          else
          {
            if(box.m_Data[0].m_Data[dim] > finalSeg.m_Ext1.m_Data[dim] + iRoomSize.m_Data[dim])
            {
              Segmenti seg;
              seg.m_Ext1 = seg.m_Ext2 = finalSeg.m_Ext1;
              seg.m_Ext2.m_Data[dim] = box.m_Data[0].m_Data[dim] - iRoomSize.m_Data[dim];

              finalConfSpace.push_back(seg);
            }

            finalSeg.m_Ext1.m_Data[dim] = box.m_Data[1].m_Data[dim];
          }
        }

        if(finalSeg.m_Ext2.m_Data[dim] - finalSeg.m_Ext1.m_Data[dim] > 0)
        {
          finalConfSpace.push_back(finalSeg);
        }
      }
    }

    return finalConfSpace;
  }

  struct RoomViolation
  {
    Map<uint32_t, float> m_Overlaps;
    Map<uint32_t, float> m_MissingDoor;
  };

  //Insert the room and compute its energy regarding collision against other rooms.
  template <typename FilteredGraph>
  void InsertRoomAndUpdateViolations(Vertex iVtx, uint32_t const iRoomIdx, AABB2Di const& iRoom,
    RoomIndex& iRoomIndex, Map<uint32_t, RoomViolation>& oMap,
    FilteredGraph const& iGraph, Map<Vertex, uint32_t> const& iVtxToRoom, Layout const& iCurLayout)
  {
    Vector<RoomIndexEntry> results;
    Set<uint32_t> touchingRooms;

    iRoomIndex.query(
      boost::geometry::index::intersects(iRoom),
      std::back_inserter(results));

    float totArea = 0;
    for(auto const& entry : results)
    {
      AABB2Di intersectionBox;
      intersectionBox.SetCommonBox(iRoom, entry.first);

      auto size = intersectionBox.GetSize();

      if(size.X() > 0 || size.Y() > 0)
      {
        if(size.X() * size.Y() == 0)
        {
          if(size.X() + size.Y() >= s_DoorSize)
          {
            touchingRooms.insert(entry.second);
          }
        }
        else
        {
          float curArea = size.X() * size.Y();
          if(curArea > 0)
          {
            totArea += curArea;
            if(size.X() >= s_DoorSize || size.Y() >= s_DoorSize)
            {
              touchingRooms.insert(entry.second);
            }

            auto insertRes = oMap.insert(std::make_pair(entry.second, RoomViolation()));
            insertRes.first->second.m_Overlaps[iRoomIdx] = curArea;

            insertRes = oMap.insert(std::make_pair(iRoomIdx, RoomViolation()));
            insertRes.first->second.m_Overlaps[entry.second] = curArea;
          }
        }
      }
    }

    iRoomIndex.insert(std::make_pair(iRoom, iRoomIdx));

    Vector2f center1 = MathTools::ToFVec(iRoom.m_Data[0]) + MathTools::ToFVec(iRoom.GetSize()) * 0.5;

    for(auto edge : OutEdgesIter(iGraph, iVtx))
    {
      Vertex target = GetTarget(iVtx, edge);
      uint32_t neighIdx = iVtxToRoom.find(target)->second;

      auto const& neightRoom = iCurLayout[neighIdx].m_Box;

      if(touchingRooms.count(neighIdx) == 0)
      {
        Vector2f center2 = MathTools::ToFVec(neightRoom.m_Data[0]) + MathTools::ToFVec(neightRoom.GetSize()) * 0.5;
        float sqDist = (center2 - center1).SquaredLength();

        auto insertRes = oMap.insert(std::make_pair(neighIdx, RoomViolation()));
        insertRes.first->second.m_MissingDoor[iRoomIdx] = sqDist;

        insertRes = oMap.insert(std::make_pair(iRoomIdx, RoomViolation()));
        insertRes.first->second.m_MissingDoor[neighIdx] = sqDist;
      }
    }
  }

  void RemoveRoomAndUpdateViolations(uint32_t const iRoomIdx, AABB2Di const& iRoom, RoomIndex& iRoomIndex, Map<uint32_t, RoomViolation>& oMap)
  {
    iRoomIndex.remove(RoomIndexEntry(iRoom, iRoomIdx));

    auto violations = oMap.find(iRoomIdx);
    if(violations != oMap.end())
    {
      for(auto overlap : violations->second.m_Overlaps)
      {
        auto otherViolation = oMap.find(overlap.first);
        if(otherViolation != oMap.end())
        {
          otherViolation->second.m_Overlaps.erase(iRoomIdx);
        }
      }
      for(auto missingDoor : violations->second.m_MissingDoor)
      {
        auto otherViolation = oMap.find(missingDoor.first);
        if(otherViolation != oMap.end())
        {
          otherViolation->second.m_MissingDoor.erase(iRoomIdx);
        }
      }
      oMap.erase(violations);
    }
  }

  AABB2Di SampleConfigurationSpace(Vector2i const& iRoomSize, Random& iRand, Vector<Segmenti> const& iConfSpace, RoomIndex const& iRoomIndex)
  {
    Vector<Segmenti> culledConfSpace = CullConfigurationSpace(iRoomSize, iConfSpace, iRoomIndex);

    Vector<Segmenti> const& confSpace = (culledConfSpace.empty() ? iConfSpace : culledConfSpace);

    Multimap<float, AABB2Di> sampledPos;
    while(sampledPos.size() < 20)
    {
      // Should try to get a uniform sampling.
      auto const& seg = confSpace[iRand() % confSpace.size()];
      Vector2i roomOrigin;
      if(seg.m_Ext1 != seg.m_Ext2)
      {
        Vector2i segDir = seg.m_Ext2 - seg.m_Ext1;
        uint32_t segLen = Mathi::Abs(Mathi::Max(segDir.X(), segDir.Y()));
        segDir /= segLen;
        segLen = iRand() % segLen;

        roomOrigin = seg.m_Ext1 + segDir * segLen;
      }
      else
      {
        roomOrigin = seg.m_Ext1;
      }

      AABB2Di newBox;
      newBox.m_Data[0] = roomOrigin;
      newBox.m_Data[1] = roomOrigin + iRoomSize;

      Vector<RoomIndexEntry> results;

      iRoomIndex.query(
        boost::geometry::index::intersects(newBox),
        std::back_inserter(results));

      float totArea = 0;
      for(auto const& entry : results)
      {
        AABB2Di intersectionBox;
        intersectionBox.SetCommonBox(newBox, entry.first);

        auto size = intersectionBox.GetSize();

        totArea += size.X() * size.Y();
      }

      sampledPos.insert(std::make_pair(totArea, newBox));

      if(totArea == 0)
        break;
    }

    return sampledPos.begin()->second;
  }

  using PhysicalGraph = boost::filtered_graph<Graph, PhysicalNodesFilter, PhysicalNodesFilter>;

  Vector<Vector<Vertex>> GetChainList(/*DungeonGraph const& iGraph, */PhysicalGraph const& graph)
  {
    auto const& idxMap = boost::get(boost::vertex_index, graph);

    //Test if the graph can be layout.
    if (!boost::boyer_myrvold_planarity_test(graph))
    {
      return Vector<Vector<Vertex>>();
    }
    Vector<Vector<Vertex>> cycles;
    Vector<Vector<Vertex>> chains;

    Set<Edge> origEdges;
    for (auto edges = boost::edges(graph); edges.first != edges.second; ++edges.first)
    {
      origEdges.insert(*edges.first);
    }

    auto dummyWhFunc = [](Edge) {return 1.0; };

    Set<Edge> mstEdges;
    boost::kruskal_minimum_spanning_tree(graph, std::inserter(mstEdges, mstEdges.begin()),
      boost::weight_map(boost::make_function_property_map<Edge>(dummyWhFunc)).vertex_index_map(idxMap));

    //--> Could replace it with a planar embedding and process faces.
    Set<Edge> cycleEdges;
    std::set_difference(origEdges.begin(), origEdges.end(), mstEdges.begin(), mstEdges.end(), std::inserter(cycleEdges, cycleEdges.begin()));

    //NodeDegreeMap nodeDegreeMap;
    Map<Vertex, uint32_t> nodeDegreeMap;
    for (auto vtx : VerticesIter(graph))
    {
      nodeDegreeMap.insert({ vtx, uint32_t(boost::degree(vtx, graph)) });
    }

    MSTFilter filter(mstEdges);

    boost::filtered_graph<PhysicalGraph, MSTFilter, boost::keep_all> filteredGr(graph, filter, boost::keep_all());

    for (auto edge : cycleEdges)
    {
      Vector<Vertex> newCycle;
      Vertex ext1 = edge.m_source;
      Vertex ext2 = edge.m_target;

      Vector<Vertex> p(boost::num_vertices(filteredGr), ext1);
      Vector<float> d(boost::num_vertices(filteredGr));

      boost::dijkstra_shortest_paths(filteredGr, ext1,
        boost::make_iterator_property_map(p.begin(), idxMap),
        boost::make_iterator_property_map(d.begin(), idxMap),
        boost::make_function_property_map<Edge>(dummyWhFunc),
        idxMap,
        std::less<float>(), boost::closed_plus<float>(), Mathf::MAX_REAL, 0.0, boost::dijkstra_visitor<boost::null_visitor>());

      float dist1 = d[idxMap[ext1]];
      float dist2 = d[idxMap[ext2]];

      Vertex farthestVtx = dist1 > dist2 ? ext1 : ext2;
      Vertex nearestVtx = dist1 > dist2 ? ext2 : ext1;

      do
      {
        newCycle.push_back(farthestVtx);
        farthestVtx = p[idxMap[farthestVtx]];
      } while (farthestVtx != nearestVtx);

      newCycle.push_back(farthestVtx);

      cycles.emplace_back(std::move(newCycle));
    }

    std::sort(cycles.begin(), cycles.end(),
      [](Vector<Vertex> const& iCycle1, Vector<Vertex> const& iCycle2)
    {
      return iCycle1.size() < iCycle2.size();
    });

    for (int i = 0; i < (int)cycles.size(); ++i)
    {
      bool cycleNested = false;
      auto& curCycle = cycles[i];
      auto cycleCopy = curCycle;
      for (auto& vtx : curCycle)
      {
        for (auto outEdge : OutEdgesIter(graph, vtx))
        {
          auto entry = nodeDegreeMap.find(outEdge.m_target);
          if (entry != nodeDegreeMap.end())
          {
            entry->second--;
          }
          //auto entry = nodeDegreeMap.get<0>().find(outEdge.m_target);
          //if (entry != nodeDegreeMap.get<0>().end())
          //{
          //  NodeWithDegree node = *entry;
          //  node.degree--;
          //  nodeDegreeMap.replace(entry, node);
          //}
        }
        if (nodeDegreeMap.count(vtx) == 0)
        {
          cycleNested = true;
          vtx = Graph::null_vertex();
        }
        else
        {
          nodeDegreeMap.erase(vtx);
        }
      }

      //Consecutive sequences of non-null vertices are chains to add.
      if (cycleNested)
      {
        Vector<Vertex> newChain;
        for (int i = -1; i < (int)curCycle.size(); ++i)
        {
          int loopedIdx = i >= 0 ? i : curCycle.size() - 1;
          Vertex vtx = curCycle[loopedIdx];

          if (vtx != Graph::null_vertex())
          {
            newChain.push_back(vtx);
          }
          else
          {
            if (!newChain.empty())
            {
              // Add vertex for connection.
              newChain.push_back(cycleCopy[loopedIdx]);
              chains.emplace_back(std::move(newChain));
            }
          }
        }

        if (!newChain.empty())
        {
          chains.emplace_back(std::move(newChain));
        }

        cycles[i] = std::move(cycles.back());
        cycles.pop_back();
        --i;
      }
    }

    Vertex pathSearchStartVtx = Graph::null_vertex();
    for (auto vtx : VerticesIter(filteredGr))
    {
      auto edges = boost::out_edges(vtx, filteredGr);
      if (std::distance(edges.first, edges.second) == 1)
      {
        pathSearchStartVtx = vtx;
        break;
      }
    }

    if (pathSearchStartVtx != Graph::null_vertex())
    {
      Vector<Vertex> p(boost::num_vertices(filteredGr), pathSearchStartVtx);
      Vector<float> d(boost::num_vertices(filteredGr));

      boost::dijkstra_shortest_paths(filteredGr, pathSearchStartVtx,
        boost::make_iterator_property_map(p.begin(), idxMap),
        boost::make_iterator_property_map(d.begin(), idxMap),
        boost::make_function_property_map<Edge>(dummyWhFunc), idxMap,
        std::less<float>(), boost::closed_plus<float>(), Mathf::MAX_REAL, 0.0, boost::dijkstra_visitor<boost::null_visitor>());

      while (!nodeDegreeMap.empty())
      {
        //auto chainExtremityEntry = nodeDegreeMap.get<1>().begin();
        auto chainExtremityEntry = std::min_element(nodeDegreeMap.begin(), nodeDegreeMap.end(),
          [](std::pair<Vertex, uint32_t> const& iElem1, std::pair<Vertex, uint32_t> const& iElem2)
        {
          return iElem1.second < iElem2.second;
        });

        //if (chainExtremityEntry->vtx == pathSearchStartVtx)
        if (chainExtremityEntry->first == pathSearchStartVtx)
        {
          nodeDegreeMap.erase(chainExtremityEntry);
          continue;
        }

        Vector<Vertex> newChain;
        Vertex chainExtremity = chainExtremityEntry->first;
        if (chainExtremityEntry->second == 0)
        {
          nodeDegreeMap.erase(chainExtremityEntry);
          //Was connected to cycles
          auto edges = boost::out_edges(chainExtremity, filteredGr);
          if (std::distance(edges.first, edges.second) >= 1)
          {
            newChain.push_back(edges.first->m_target);
          }
        }
        else
        {
          //auto iter = nodeDegreeMap.get<0>().find(chainExtremity);
          //NodeWithDegree entry = *iter;

          eXl_ASSERT(chainExtremityEntry->second == 1);

          while (true)
          {
            newChain.push_back(chainExtremity);
            //degree--;
            //if (degree == 0)
            {
              nodeDegreeMap.erase(chainExtremity);
              chainExtremityEntry = nodeDegreeMap.end();
            }

            {
              auto predecessor = p[idxMap[chainExtremity]];
              if (predecessor == chainExtremity)
              {
                break;
              }

              chainExtremity = predecessor;
            }

            chainExtremityEntry = nodeDegreeMap.find(chainExtremity);
            if (chainExtremityEntry == nodeDegreeMap.end())
            {
              // Was removed when processing cycles, stop here.
              break;
            }
            // decrease degree of next node.
            //entry = *iter;
            uint32_t& degree = chainExtremityEntry->second;
            degree--;
            //nodeDegreeMap.replace(iter, entry);
            if (degree == 0)
            {
              nodeDegreeMap.erase(chainExtremity);
            }
            else
            {
              //nodeDegreeMap.replace(iter, entry);
              if (degree != 1)
              {
                //Next vertex has more neighbours.
                break;
              }
            }
          }
        }
        newChain.push_back(chainExtremity);

        chains.emplace_back(std::move(newChain));
      }
    }

    if (cycles.empty() && chains.empty())
    {
      return Vector<Vector<Vertex>>();
    }

    //First process smaller cycle.
    //Next order all chain through a BFS exploration.

    Vector<Vector<Vertex>> orderedChains;
    orderedChains.reserve(cycles.size() + chains.size());
    if (!cycles.empty())
    {
      orderedChains.emplace_back(std::move(cycles.front()));
      cycles.front() = std::move(cycles.back());
      cycles.pop_back();

      for (auto& cycle : cycles)
      {
        chains.emplace_back(std::move(cycle));
      }
      cycles.clear();
    }
    else
    {
      orderedChains.push_back(std::move(chains.back()));
      chains.pop_back();
    }

    Map<Vertex, Set<uint32_t>> vtxChains;

    for (uint32_t i = 0; i < chains.size(); ++i)
    {
      auto const& chain = chains[i];
      for (auto vtx : chain)
      {
        auto& set = vtxChains.insert(std::make_pair(vtx, Set<uint32_t>())).first->second;
        set.insert(i);
      }
    }

    uint32_t curChainBegin = 0;
    uint32_t curChainEnd = 1;
    uint32_t nextChainEnd = 1;

    uint32_t remainingChains = chains.size();
    while (remainingChains > 0)
    {
      for (; curChainBegin < curChainEnd; ++curChainBegin)
      {
        auto& curChain = orderedChains[curChainBegin];
        for (auto vtx : curChain)
        {
          for (auto edge : OutEdgesIter(graph, vtx))
          {
            auto entry = vtxChains.find(edge.m_target);
            if (entry != vtxChains.end())
            {
              for (auto connectedChain : entry->second)
              {
                if (!chains[connectedChain].empty())
                {
                  orderedChains.emplace_back(chains[connectedChain]);
                  chains[connectedChain].clear();
                  ++nextChainEnd;
                  --remainingChains;
                }
              }
            }
          }
        }
      }
      eXl_ASSERT_REPAIR_RET(curChainEnd != nextChainEnd, Vector<Vector<Vertex>>());
      curChainEnd = nextChainEnd;
    }

    return orderedChains;
  }

  LayoutCollection LayoutGraph(DungeonGraph const& iGraph, Random& iRand)
  {
    auto const& fullGraph = iGraph.GetGraph();

    PhysicalNodesFilter phFilter;
    PhysicalGraph graph(fullGraph, phFilter, phFilter);

    Vector<Vector<Vertex>> orderedChains = GetChainList(graph);

    if (orderedChains.empty())
    {
      return LayoutCollection();
    }

    uint32_t curChainToAdd = 0;

    Vector<LayoutCollection> layoutStack;

    layoutStack.push_back(LayoutCollection());
    layoutStack.back().push_back(Layout());

    while(!layoutStack.empty() && layoutStack.size() < (orderedChains.size() + 1))
    {
      uint32_t chainToLayoutIdx = layoutStack.size() - 1;

      LayoutCollection const& baseLayoutCol = layoutStack.back();

      LayoutCollection nextCollection;

      uint32_t const s_NumAttempts = 50;
      uint32_t const s_CollectionSize = 15;

      for(uint32_t attempt = 0; attempt < s_NumAttempts && nextCollection.size() < s_CollectionSize; ++attempt)
      {
        Layout const& baseLayout = baseLayoutCol[attempt % baseLayoutCol.size()];
        Layout curLayout = baseLayout;
        Set<Vertex> vtxToConsider;
        // Copy chain to layout because vtx might not be sorted in connection order.
        Vector<Vertex> chainToLayout = orderedChains[chainToLayoutIdx];

        Map<Vertex, uint32_t> vtxToRoom;

        Vector<RoomIndexEntry> roomEntries;
        for(unsigned int i = 0; i<curLayout.size(); ++i)
        {
          roomEntries.push_back(std::make_pair(curLayout[i].m_Box, i));
          vtxToConsider.insert(curLayout[i].m_Node);
          vtxToRoom.insert(std::make_pair(curLayout[i].m_Node, i));
        }

        RoomIndex roomIndex(roomEntries.begin(), roomEntries.end());
        // Initial layout is intersection free.

        //Only consider current layout + chain vtx.
        boost::filtered_graph<decltype(graph), boost::keep_all, GraphFilter> filteredGr(graph, boost::keep_all(), GraphFilter(vtxToConsider));

        float curEnergy = 0.0;
        Map<uint32_t, RoomViolation> violatingRoom;

        uint32_t expectedSize = curLayout.size() + chainToLayout.size();

        for(int i = 0; i<chainToLayout.size(); ++i)
        {
          auto chainVtx = chainToLayout[i];

          if(vtxToConsider.count(chainVtx))
          {
            // Chain extermities are repeated
            chainToLayout[i] = chainToLayout.back();
            chainToLayout.pop_back();
            --expectedSize;
            i = -1;
            continue;
          }

          vtxToConsider.insert(chainVtx);

          Vector<Segmenti> confSpace;
          Vector<Segmenti> maxConfSpace;
          vtxToConsider.insert(chainVtx);

          Vector2i roomSize = SampleRoom(iGraph, chainVtx, iRand);

          if(curLayout.empty())
          {
            Room newRoom;
            newRoom.m_Node = chainVtx;
            newRoom.m_Box = AABB2Di(Vector2i::ZERO, roomSize);
            vtxToRoom[chainVtx] = curLayout.size();
            roomIndex.insert(RoomIndexEntry(newRoom.m_Box, curLayout.size()));
            curLayout.push_back(newRoom);

            chainToLayout[i] = chainToLayout.back();
            chainToLayout.pop_back();
            i = -1;

            continue;
          }
          else
          {
            uint32_t numNeigh = 0;
            for(auto edge : OutEdgesIter(filteredGr, chainVtx))
            {
              auto target = GetTarget(chainVtx, edge);

              auto const& neightRoom = curLayout[vtxToRoom[target]].m_Box;

              AddRoomConfSpace(roomSize, neightRoom, confSpace);
              ++numNeigh;
            }

            if(numNeigh == 0)
            {
              vtxToConsider.erase(chainVtx);
              continue;
            }
            else
            {
              // Found a way to connect the graph.
              chainToLayout[i] = chainToLayout.back();
              chainToLayout.pop_back();
              i = -1;
            }

            if(numNeigh > 1)
            {
              ComputeMaxConfSpace(confSpace);
            }
          }

          if(!confSpace.empty())
          {
            Room newRoom;
            newRoom.m_Node = chainVtx;
            newRoom.m_Box = SampleConfigurationSpace(roomSize, iRand, confSpace, roomIndex);

            uint32_t newRoomIdx = curLayout.size();

            vtxToRoom[chainVtx] = newRoomIdx;
            curLayout.push_back(newRoom);
            vtxToConsider.insert(chainVtx);

            InsertRoomAndUpdateViolations(chainVtx, newRoomIdx, newRoom.m_Box, roomIndex, violatingRoom, filteredGr, vtxToRoom, curLayout);
          }
          else
          {
            // This layout cannot be extended.
            break;
          }

          if(!violatingRoom.empty())
          {
            RoomIndex wkIndex = roomIndex;
            Layout wkLayout = curLayout;

            // Block area is between (3*3) and (6*6), so let's just take 15 as an average for now.
            float const s_AreaFactor = 15 * 100;
            uint32_t s_MaxTrials = 500;

            auto ComputeTotalEnergy = [&violatingRoom, s_AreaFactor] ()
            {
              float sumCol = 0;
              float sumDist = 0;
              for(auto const& room : violatingRoom)
              {
                for(auto const& overlap : room.second.m_Overlaps)
                {
                  sumCol += overlap.second;
                }
                for(auto const& missingDoor : room.second.m_MissingDoor)
                {
                  sumDist += missingDoor.second;
                }
              }

              return exp(sumCol / s_AreaFactor) * exp(sumDist / s_AreaFactor) - 1;
            };

            float prevEnergy = ComputeTotalEnergy();
            float const initTemp = 0.6;
            float const finalTemp = 0.2;
            float const tempRatio = pow(0.2 / 0.6, 1.0 / float(s_MaxTrials));
            float const kB = 1.38064852e10-23;
            float temp = initTemp;

            Vector<uint32_t> candidates;
            candidates.reserve(curLayout.size());

            for(uint32_t trial = 0; trial < s_MaxTrials && !violatingRoom.empty(); ++trial)
            {
              candidates.clear();
              for(auto room : violatingRoom)
              {
                candidates.push_back(room.first);
              }

              uint32_t roomIdx = candidates[iRand() % candidates.size()];
              Room& curRoom = wkLayout[roomIdx];
              AABB2Di prevRoom = curRoom.m_Box;
              RemoveRoomAndUpdateViolations(roomIdx, curRoom.m_Box, wkIndex, violatingRoom);

              Vector2i roomSize = curRoom.m_Box.GetSize();

              bool changeRoom = (iRand() % 10) > 7;
              if(changeRoom)
              {
                roomSize = SampleRoom(iGraph, curRoom.m_Node, iRand);
              }

              uint32_t numNeigh = 0;
              for(auto edge : OutEdgesIter(filteredGr, curRoom.m_Node))
              {
                auto target = GetTarget(curRoom.m_Node, edge);

                auto const& neightRoom = curLayout[vtxToRoom[target]].m_Box;

                AddRoomConfSpace(roomSize, neightRoom, confSpace);
                ++numNeigh;
              }

              if(numNeigh > 1)
              {
                ComputeMaxConfSpace(confSpace);
              }

              if(confSpace.empty())
              {
                InsertRoomAndUpdateViolations(curRoom.m_Node, roomIdx, curRoom.m_Box, wkIndex, violatingRoom, filteredGr, vtxToRoom, wkLayout);
                continue;
              }
              else
              {
                curRoom.m_Box = SampleConfigurationSpace(roomSize, iRand, confSpace, wkIndex);
                InsertRoomAndUpdateViolations(curRoom.m_Node, roomIdx, curRoom.m_Box, wkIndex, violatingRoom, filteredGr, vtxToRoom, wkLayout);
              }

              float curEnergy = ComputeTotalEnergy();
              float deltaEnergy = curEnergy - prevEnergy;

              if(deltaEnergy <= Mathf::ZERO_TOLERANCE
              || exp(deltaEnergy / (temp * kB)) > iRand())
              {
                //Accept move.
                roomIndex = wkIndex;
                curLayout = wkLayout;
                prevEnergy = curEnergy;
              }
              else
              {
                RemoveRoomAndUpdateViolations(roomIdx, curRoom.m_Box, wkIndex, violatingRoom);
                curRoom.m_Box = prevRoom;
                InsertRoomAndUpdateViolations(curRoom.m_Node, roomIdx, curRoom.m_Box, wkIndex, violatingRoom, filteredGr, vtxToRoom, wkLayout);
              }
              temp *= tempRatio;
            }
          }
        }

        if(violatingRoom.empty() && curLayout.size() == expectedSize)
        {
          nextCollection.push_back(curLayout);
        }
        else
        {
          //printf("Blah");
        }
      }

      if(nextCollection.empty())
      {
        if(!layoutStack.empty() /*&& layoutStack.back().empty()*/)
        {
          // Could not build on top on previous conf, backtrack.
          layoutStack.pop_back();
        }
      }
      else
      {
        layoutStack.emplace_back(std::move(nextCollection));
      }
    }

    if(!layoutStack.empty())
    {
      return layoutStack.back();
    }
    else
    {
      return LayoutCollection();
    }
  }
}