// boost random/well.hpp header file
//
// Copyright (c) Sergiu Dotenco 2010-2012, 2014
//
// 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)

/**
 * @brief Implementation of the Well Equidistributed Long-period Linear (WELL)
 * pseudo-random number generator.
 */

#ifndef BOOST_RANDOM_WELL_HPP
#define BOOST_RANDOM_WELL_HPP

#include <cstddef>
#include <iomanip>
#include <istream>
#include <limits>
#include <ostream>

#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/bitand.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/generator_seed_seq.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/seed_impl.hpp>
#include <boost/static_assert.hpp>

namespace boost {
namespace random {

//! @cond hide_private

namespace detail {

template<class UIntType, uint_least32_t N>
struct bitshift_left
{
    static UIntType compute(UIntType a)
    {
        return a << N;
    }
};

template<class UIntType, uint_least32_t N>
struct bitshift_right
{
    static UIntType compute(UIntType a)
    {
        return a >> N;
    }
};

template<class UIntType, int_least32_t N>
struct bitshift
{
    static UIntType compute(UIntType a)
    {
        return mpl::if_c< (N < 0),
            bitshift_left<UIntType, static_cast<uint_least32_t>(-N)>,
            bitshift_right<UIntType, static_cast<uint_least32_t>(N)>
        >::type::compute(a);
    }
};

/**
 * @name Transformation matrices @f$M0,\dotsc,M6@f$ from Table I
 * @{
 */

struct M0
{
    template<class T>
    static T transform(T)
    {
        return T(0);
    }
};

struct M1
{
    template<class T>
    static T transform(T x)
    {
        return x;
    }
};

template<int N>
struct M2
{
    template<class T>
    static T transform(T x)
    {
        return bitshift<T, N>::compute(x);
    }
};

template<int N>
struct M3
{
    template<class T>
    static T transform(T x)
    {
        return x ^ bitshift<T, N>::compute(x);
    }
};

template<uint_least32_t a>
struct M4
{
    template<class T>
    static T transform(T x)
    {
        T result = x >> 1;

        if ((x & 1) == 1)
            result ^= a;

        return result;
    }
};

template<int N, uint_least32_t b>
struct M5
{
    template<class T>
    static T transform(T x)
    {
        return x ^ (bitshift<T, N>::compute(x) & b);
    }
};

template
<
    std::size_t w,
    uint_least32_t q,
    uint_least32_t s,
    uint_least32_t t,
    uint_least32_t a
>
struct M6
{
    template<class T>
    static T transform(T x)
    {
        // Set all bits to 1 except the (s+1)th bit. Ensure only w bits are used.
        BOOST_CONSTEXPR_OR_CONST T ds = (~0U >> (std::numeric_limits<T>::digits - w)) & ~(1 << (w - (s + 1)));

        T result = ((x << q) ^ (x >> (w - q))) & ds;

        if (((x >> (w - (t + 1))) & 1) == 1)
            result ^= a;

        return result;
    }
};

//! @}

/**
 * Conditional expression of type (r & (r - 1)) == 0 which allows to check
 * whether a number @f$r@f$ is of type @f$2^n@f$.
 */
typedef mpl::equal_to<
            mpl::bitand_<
                mpl::_,
                mpl::minus<mpl::_, mpl::int_<1>
            >
        >,
        mpl::int_<0>
    > is_power_of_two;

template<class UIntType, UIntType r>
struct power_of_two_modulo
{
    typedef typename mpl::apply<
            is_power_of_two,
            mpl::integral_c<UIntType, r>
        >::type type;

    BOOST_STATIC_ASSERT_MSG(type::value, "value is not power of two");

    static UIntType compute(UIntType value)
    {
        return value & (r - 1);
    }
};

template<class UIntType, UIntType r>
struct generic_modulo
{
    BOOST_STATIC_ASSERT_MSG(!std::numeric_limits<UIntType>::is_signed,
        "UIntType must be unsigned");

    /**
     * @brief Determines @a value modulo @a r.
     *
     * @pre value >= 0 and value < 2 * r
     * @post value >= 0 and value < r
     */
    static UIntType compute(UIntType value)
    {
        BOOST_ASSERT_MSG(value < 2 * r, "value out of range");

        if (value >= r)
            value -= r;

        return value;
    }
};

template<class UIntType, UIntType r>
struct selective_modulo
{
    typedef typename mpl::apply<
            is_power_of_two,
            mpl::integral_c<UIntType, r>
        >::type r_is_power_of_two;

    static UIntType compute(UIntType value)
    {
        // Use the bitwise AND for power 2 modulo arithmetic, or subtraction
        // otherwise. Subtraction is about two times faster than direct modulo
        // computation.
        return mpl::if_<
                    r_is_power_of_two,
                        power_of_two_modulo<UIntType, r>,
                        generic_modulo<UIntType, r>
                >::type::compute(value);
    }
};

template<uint_least32_t b, uint_least32_t c>
struct matsumoto_kurita_tempering
{
    template<class UIntType, std::size_t r, std::size_t N>
    static UIntType apply(UIntType x, UIntType (&)[N], std::size_t)
    {
        x ^= (x << 7) & b;
        x ^= (x << 15) & c;

        return x;
    }
};

template<uint_least32_t mask>
struct harase_tempering
{
    template<class UIntType, std::size_t r, std::size_t N>
    static UIntType apply(UIntType x, UIntType (&s)[N], std::size_t m2)
    {
        return x ^ (s[selective_modulo<std::size_t, r>::compute(m2 + 1)] & mask);
    }
};

struct no_tempering
{
    template<class UIntType, std::size_t r, std::size_t N>
    static UIntType apply(UIntType x, UIntType (&)[N], std::size_t)
    {
        return x;
    }
};

} // namespace detail

//! @endcond

/**
 * @brief well_engine Equidistributed Long-period Linear (WELL) pseudo-random
 * number generator.
 *
 * The implementation is based on the "Improved Long-Period Generators Based on
 * Linear Recurrences selective_modulo 2" paper by Francois Panneton, Pierre
 * L'Ecuyer and Makoto Matsumoto from ACM Transactions on Mathematical Software,
 * 32 (1, March) 2006, pp. 1-16.
 *
 * @tparam UIntType The unsigned integer type.
 * @tparam w Word size.
 * @tparam r State size.
 * @tparam p Mask bits.
 */
template
<
    class UIntType,
    std::size_t w,
    std::size_t r,
    std::size_t p,
    std::size_t m1,
    std::size_t m2,
    std::size_t m3,
    class T0,
    class T1,
    class T2,
    class T3,
    class T4,
    class T5,
    class T6,
    class T7,
    class Tempering // mpl pluggable
>
class well_engine
{
    BOOST_STATIC_ASSERT_MSG(!std::numeric_limits<UIntType>::is_signed,
        "UIntType must be unsigned");
    BOOST_STATIC_ASSERT_MSG(w <=
        static_cast<std::size_t>(std::numeric_limits<UIntType>::digits),
            "word size cannot be represented using UIntType");
    BOOST_STATIC_ASSERT_MSG(r > 0, "state size must be non-zero");
    BOOST_STATIC_ASSERT_MSG(p < w,
        "number of mask bits cannot be greater than word size");
    BOOST_STATIC_ASSERT(m1 > 0 && m1 < r);
    BOOST_STATIC_ASSERT(m2 > 0 && m2 < r);
    BOOST_STATIC_ASSERT(m3 > 0 && m3 < r);

public:
    //! The unsigned integer type.
    typedef UIntType result_type;

    //! Word size.
    BOOST_STATIC_CONSTANT(std::size_t, word_size = w);
    //! State size.
    BOOST_STATIC_CONSTANT(std::size_t, state_size = r);
    //! Number of mask bits.
    BOOST_STATIC_CONSTANT(std::size_t, mask_bits = p);
    //! Default seed value.
    BOOST_STATIC_CONSTANT(UIntType, default_seed = 5489U);
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

    /**
     * Constructs a @c mersenne_twister_engine and calls @c seed().
     */
    well_engine()
    {
        seed();
    }

    /**
     * Constructs a @c mersenne_twister_engine and calls @c seed(value).
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(well_engine, UIntType, value)
    {
        seed(value);
    }

    template<class InputIterator>
    well_engine(InputIterator& first, InputIterator last)
    {
        seed(first, last);
    }

    /**
    * Constructs a well_engine and calls @c seed(gen).
    *
    * @xmlnote
    * The copy constructor will always be preferred over
    * the templated constructor.
    * @endxmlnote
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(well_engine, SeedSeq, seq)
    {
        seed(seq);
    }

    /**
    * Seeds a well_engine using values produced by seq.generate().
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(well_engine, SeedSeq, seq)
    {
        detail::seed_array_int<w>(seq, state_);
        index_ = state_size;

        // fix up the state if it's all zeros.
        if ((state_[0] & (~static_cast<UIntType>(0) << mask_bits)) == 0) {
            for (std::size_t j = 1; j != state_size; ++j) {
                if (state_[j] != 0)
                    return;
            }

            state_[0] = static_cast<UIntType>(1) << (w - 1);
        }
    }

    /**
    * Sets the state x(0) to v mod 2w. Then, iteratively,
    * sets x(i) to
    * (i + f * (x(i-1) xor (x(i-1) rshift w-2))) mod 2w
    * for i = 1 .. n-1. x(n) is the first value to be returned by operator().
    */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(well_engine, UIntType, value)
    {
        // New seeding algorithm from
        // http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/emt19937ar.html
        // In the previous versions, MSBs of the seed affected only MSBs of the
        // state x[].
        const UIntType mask = (max)();

        state_[0] = value & mask;

        std::size_t i = 1;
        UIntType (&s)[state_size] = state_;

        for ( ; i != state_size; ++i) {
            // See Knuth "The Art of Computer Programming"
            // Vol. 2, 3rd ed., page 106
            s[i] = static_cast<UIntType>((1812433253U * (s[i - 1] ^ (s[i - 1] >> (w - 2))) + i) & mask);
        }

        index_ = i;
    }

    /**
     * Calls @c seed(default_seed).
     */
    void seed()
    {
        seed(default_seed);
    }

    template<class InputIterator>
    void seed(InputIterator& first, InputIterator last)
    {
        detail::fill_array_int<w>(first, last, state_);
        index_ = state_size;

        // fix up the state if it's all zeros.
        if ((state_[0] & (~static_cast<UIntType>(0) << mask_bits)) == 0) {
            for (std::size_t j = 1; j < state_size; ++j) {
                if (state_[j] != 0)
                    return;
            }

            state_[0] = static_cast<UIntType>(1) << (w - 1);
        }
    }

    /**
     * @brief Generates a random number.
     */
    result_type operator()()
    {
        BOOST_CONSTEXPR_OR_CONST UIntType upper_mask = ~0U << p;
        BOOST_CONSTEXPR_OR_CONST UIntType lower_mask = ~upper_mask;

        // v[i,j] = state[(r-i+j) mod r]
        const std::size_t i = mod(index_);

        // Equivalent to r-i but allows to avoid negative values in the
        // following two expressions
        const std::size_t j = i + r;
        const std::size_t k = mod(j - 1); // [i,r-1]
        const std::size_t l = mod(j - 2); // [i,r-2]

        const std::size_t im1 = mod(i + m1);
        const std::size_t im2 = mod(i + m2);
        const std::size_t im3 = mod(i + m3);

        UIntType z0;
        UIntType z1;
        UIntType z2;
        UIntType z3;
        UIntType z4;

        z0 = (state_[k] & upper_mask) | (state_[l] & lower_mask);
        z1 = T0::transform(state_[i]) ^
             T1::transform(state_[im1]);
        z2 = T2::transform(state_[im2]) ^
             T3::transform(state_[im3]);
        z3 = z1 ^ z2;
        z4 = T4::transform(z0) ^ T5::transform(z1) ^
             T6::transform(z2) ^ T7::transform(z3);

        state_[i] = z3; // v[i+1,1]
        state_[k] = z4; // v[i+1,0]

        index_ = k;

        return Tempering::template apply<UIntType, r>(z4, state_, im2);
    }

    /**
     * Returns the smallest value that the generator can produce.
     */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
    {
        return 0U;
    }

    /**
     * Returns the largest value that the generator can produce.
     */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    {
        return ~0U >> (std::numeric_limits<UIntType>::digits - w);
    }

    /**
     * Fills a range with random values.
     */
    template<class InputIterator>
    void generate(InputIterator first, InputIterator last)
    {
        detail::generate_from_int(*this, first, last);
    }

    /**
    * Advances the state of the generator by @c z steps.  Equivalent to
    *
    * @code
    * for(unsigned long long i = 0; i < z; ++i) {
    *     gen();
    * }
    * @endcode
    */
    void discard(uintmax_t z)
    {
        while (z-- > 0) {
            (*this)();
        }
    }

    /**
     * @brief Compares the state of two generators for equality.
     */
    friend bool operator==(const well_engine& lhs, const well_engine& rhs)
    {
        for (std::size_t i = 0; i != state_size; ++i)
            if (lhs.compute(i) != rhs.compute(i))
                return false;

        return true;
    }

    /**
     * @brief Compares the state of two generators for inequality.
     */
    friend bool operator!=(const well_engine& lhs, const well_engine& rhs)
    {
        return !(lhs == rhs);
    }

    /**
     * @brief Writes the state to the specified stream.
     */
    template<class E, class T>
    friend std::basic_ostream<E, T>&
        operator<<(std::basic_ostream<E, T>& out, const well_engine& gen)
    {
        const E space = out.widen(' ');

        for (std::size_t i = 0; i != state_size; ++i)
            out << gen.compute(i) << space;

        return out;
    }

    /**
     * @brief Reads the generator state from the specified input stream.
     */
    template<class E, class T>
    friend std::basic_istream<E, T>&
        operator>>(std::basic_istream<E, T>& in, well_engine& gen)
    {
        for (std::size_t i = 0; i != state_size; ++i)
            in >> gen.state_[i] >> std::ws;

        gen.index_ = state_size;

        return in;
    }

private:
    //! @cond show_private

    template<class T>
    static T mod(T value)
    {
        return detail::selective_modulo<T, r>::compute(value);
    }

    UIntType compute(std::size_t index) const
    {
        return state_[mod(index_ + index + r)];
    }

    UIntType state_[r];
    std::size_t index_;

    //! @endcond
};

//! @cond show_private

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
template<class UIntType, std::size_t w, std::size_t r, std::size_t p,
    std::size_t m1, std::size_t m2, std::size_t m3, class T0, class T1,
    class T2, class T3, class T4, class T5, class T6, class T7, class Tempering>
const std::size_t well_engine<UIntType, w, r, p, m1, m2, m3, T0, T1, T2, T3, T4, T5,
      T6, T7, Tempering>::word_size;
template<class UIntType, std::size_t w, std::size_t r, std::size_t p,
    std::size_t m1, std::size_t m2, std::size_t m3, class T0, class T1,
    class T2, class T3, class T4, class T5, class T6, class T7, class Tempering>
const std::size_t well_engine<UIntType, w, r, p, m1, m2, m3, T0, T1, T2, T3, T4, T5,
      T6, T7, Tempering>::state_size;
template<class UIntType, std::size_t w, std::size_t r, std::size_t p,
    std::size_t m1, std::size_t m2, std::size_t m3, class T0, class T1,
    class T2, class T3, class T4, class T5, class T6, class T7, class Tempering>
const std::size_t well_engine<UIntType, w, r, p, m1, m2, m3, T0, T1, T2, T3, T4, T5,
      T6, T7, Tempering>::mask_bits;
template<class UIntType, std::size_t w, std::size_t r, std::size_t p,
    std::size_t m1, std::size_t m2, std::size_t m3, class T0, class T1,
    class T2, class T3, class T4, class T5, class T6, class T7, class Tempering>
const UIntType well_engine<UIntType, w, r, p, m1, m2, m3, T0, T1, T2, T3, T4, T5, T6,
      T7, Tempering>::default_seed;
template<class UIntType, std::size_t w, std::size_t r, std::size_t p,
    std::size_t m1, std::size_t m2, std::size_t m3, class T0, class T1,
    class T2, class T3, class T4, class T5, class T6, class T7, class Tempering>
const bool well_engine<UIntType, w, r, p, m1, m2, m3, T0, T1, T2, T3, T4, T5, T6,
      T7, Tempering>::has_fixed_range;
#endif // BOOST_NO_INCLASS_MEMBER_INITIALIZATION

//! @endcond

//! @cond hide_private

namespace detail {

/**
 * @name Base WELL definitions with pluggable tempering method
 * @{
 */

template
<
    class UIntType,
    std::size_t w,
    std::size_t r,
    std::size_t p,
    std::size_t m1,
    std::size_t m2,
    std::size_t m3,
    class T0,
    class T1,
    class T2,
    class T3,
    class T4,
    class T5,
    class T6,
    class T7
>
struct well_quoted
{
    template<class T>
    struct apply
    {
        typedef well_engine<UIntType, w, r, p, m1, m2, m3, T0, T1, T2, T3, T4, T5, T6,
            T7, T> type;
    };
};

namespace well_params {

const boost::uint_least32_t a1 = 0xda442d24U;
const boost::uint_least32_t a2 = 0xd3e43ffdU;
const boost::uint_least32_t a3 = 0x8bdcb91eU;
const boost::uint_least32_t a4 = 0x86a9d87eU;
const boost::uint_least32_t a5 = 0xa8c296d1U;
const boost::uint_least32_t a6 = 0x5d6b45ccU;
const boost::uint_least32_t a7 = 0xb729fcecU;

} // namespace well_params

typedef well_quoted<uint32_t, 32, 16, 0, 13, 9, 5,
    M3<-16>, M3<-15>, M3<11>, M0, M3<-2>, M3<-18>, M2<-28>,
    M5<-5, well_params::a1> > well512a_base;

typedef well_quoted<uint32_t, 32, 17, 23, 13, 11, 10,
    M3<-13>, M3<-15>, M1, M2<-21>, M3<-13>, M2<1>, M0, M3<11> >
    well521a_base;

typedef well_quoted<uint32_t, 32, 17, 23, 11, 10, 7,
    M3<-21>, M3<6>, M0, M3<-13>, M3<13>, M2<-10>, M2<-5>, M3<13> >
    well521b_base;

typedef well_quoted<uint32_t, 32, 19, 1, 16, 15, 14,
    M3<19>, M3<11>, M3<-14>, M1, M3<18>, M1, M0, M3<-5> > well607a_base;

typedef well_quoted<uint32_t, 32, 19, 1, 16, 18, 13,
    M3<-18>, M3<-14>, M0, M3<18>, M3<-24>, M3<5>, M3<-1>, M0>
    well607b_base;

typedef well_quoted<uint32_t, 32, 25, 0, 14, 18, 17,
    M1, M3<-15>, M3<10>, M3<-11>, M3<16>, M2<20>, M1, M3<-28> >
    well800a_base;

typedef well_quoted<uint32_t, 32, 25, 0, 9, 4, 22,
    M3<-29>, M2<-14>, M1, M2<19>, M1, M3<10>, M4<well_params::a2>, M3<-25> >
    well800b_base;

typedef well_quoted<uint32_t, 32, 32, 0, 3, 24, 10,
    M1, M3<8>, M3<-19>, M3<-14>, M3<-11>, M3<-7>, M3<-13>, M0>
    well1024a_base;

typedef well_quoted<uint32_t, 32, 32, 0, 22, 25, 26,
    M3<-21>, M3<17>, M4<well_params::a3>, M3<15>, M3<-14>, M3<-21>, M1, M0>
    well1024b_base;

typedef well_quoted<uint32_t, 32, 624, 31, 70, 179, 449,
    M3<-25>, M3<27>, M2<9>, M3<1>, M1, M3<-9>, M3<-21>, M3<21> >
    well19937a_base;

typedef well_quoted<uint32_t, 32, 624, 31, 203, 613, 123,
    M3<7>, M1, M3<12>, M3<-10>, M3<-19>, M2<-11>, M3<4>, M3<-10> >
    well19937b_base;

typedef well_quoted<uint32_t, 32, 679, 27, 151, 327, 84,
    M1, M3<-26>, M3<19>, M0, M3<27>, M3<-11>,
    M6<32, 15, 27, 10, well_params::a4>, M3<-16> >
    well21701a_base;

typedef well_quoted<uint32_t, 32, 726, 23, 667, 43, 462,
    M3<28>, M1, M3<18>, M3<3>, M3<21>, M3<-17>, M3<-28>, M3<-1> >
    well23209a_base;

typedef well_quoted<uint32_t, 32, 726, 23, 610, 175, 662,
    M4<well_params::a5>, M1, M6<32, 15, 15, 30, well_params::a6>,
    M3<-24>, M3<-26>, M1, M0, M3<16> > well23209b_base;

typedef well_quoted<uint32_t, 32, 1391, 15, 23, 481, 229,
    M3<-24>, M3<30>, M3<-10>, M2<-26>, M1, M3<20>,
    M6<32, 9, 5, 14, well_params::a7>, M1> well44497a_base;

//! @}

} // namespace detail

//! @endcond

typedef mpl::apply1<detail::well512a_base,
    detail::no_tempering>::type well512a;
typedef mpl::apply1<detail::well521a_base,
    detail::no_tempering>::type well521a;
typedef mpl::apply1<detail::well521b_base,
    detail::no_tempering>::type well521b;
typedef mpl::apply1<detail::well607a_base,
    detail::no_tempering>::type well607a;
typedef mpl::apply1<detail::well607b_base,
    detail::no_tempering>::type well607b;
typedef mpl::apply1<detail::well800a_base,
    detail::no_tempering>::type well800a;
typedef mpl::apply1<detail::well800b_base,
    detail::no_tempering>::type well800b;
typedef mpl::apply1<detail::well1024a_base,
    detail::no_tempering>::type well1024a;
typedef mpl::apply1<detail::well1024b_base,
    detail::no_tempering>::type well1024b;
typedef mpl::apply1<detail::well19937a_base,
    detail::no_tempering>::type well19937a;
typedef mpl::apply1<detail::well19937b_base,
    detail::no_tempering>::type well19937b;
typedef mpl::apply1<detail::well19937a_base,
    detail::matsumoto_kurita_tempering<0xe46e1700U, 0x9b868000U> >::type well19937c;
typedef mpl::apply1<detail::well21701a_base,
    detail::no_tempering>::type well21701a;
typedef mpl::apply1<detail::well23209a_base,
    detail::no_tempering>::type well23209a;
typedef mpl::apply1<detail::well23209b_base,
    detail::no_tempering>::type well23209b;
typedef mpl::apply1<detail::well44497a_base,
    detail::no_tempering>::type well44497a;
typedef mpl::apply1<detail::well44497a_base,
    detail::matsumoto_kurita_tempering<0x93dd1400U, 0xfa118000U> >::type well44497b;

/**
 * @name Maximally equidistributed WELL versions using Harase's tempering method
 * @{
 */

typedef mpl::apply1<detail::well800a_base,
    detail::harase_tempering<0x4880U> >::type maxeqdist_well800a;
typedef mpl::apply1<detail::well800b_base,
    detail::harase_tempering<0x17030806U> >::type maxeqdist_well800b;
typedef mpl::apply1<detail::well19937a_base,
    detail::harase_tempering<0x4118000U> >::type maxeqdist_well19937a;
typedef mpl::apply1<detail::well19937b_base,
    detail::harase_tempering<0x30200010U> >::type maxeqdist_well19937b;
typedef mpl::apply1<detail::well21701a_base,
    detail::harase_tempering<0x1002U> >::type maxeqdist_well21701a;
typedef mpl::apply1<detail::well23209a_base,
    detail::harase_tempering<0x5100000U> >::type maxeqdist_well23209a;
typedef mpl::apply1<detail::well23209b_base,
    detail::harase_tempering<0x34000300U> >::type maxeqdist_well23209b;
typedef mpl::apply1<detail::well44497a_base,
    detail::harase_tempering<0x48000000U> >::type maxeqdist_well44497a;

//! @}

} // namespace random
} // namespace boost

#endif // !defined(BOOST_RANDOM_WELL_HPP)


// Copyright (c) 2021 OUXT Polaris
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <boost/optional.hpp>
#include <cmath>
#include <iostream>
#include <navi_sim/interpreter/data_type.hpp>
#include <navi_sim/interpreter/reach_position_event.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>

namespace navi_sim
{
namespace events
{
ReachPositionEvent::ReachPositionEvent(const std::string & name, const YAML::Node & yaml)
: EventBase(name, yaml)
{
  parse(yaml["target"], goal_);
  radius_ = yaml["radius"].as<double>();
}

void ReachPositionEvent::getDebugString(YAML::Node & yaml) { EventBase::getDebugString(yaml); }

EventState ReachPositionEvent::onUpdate(const BlackBoard & black_board)
{
  const auto ego_pose = black_board.get<boost::optional<geometry_msgs::msg::Pose>>("ego_pose");
  if (!ego_pose) {
    return EventState::ACTIVE;
  }
  double distance = std::sqrt(
    std::pow(ego_pose->position.x - goal_.pose.position.x, 2) +
    std::pow(ego_pose->position.y - goal_.pose.position.y, 2) +
    std::pow(ego_pose->position.z - goal_.pose.position.z, 2));
  if (distance <= radius_) {
    return EventState::FINISHED;
  }
  return EventState::ACTIVE;
}
}  // namespace events
}  // namespace navi_sim


// Geometric Algebra Multivector Estimation Library
// Copyright (c) 2015 Lars Tingelstad <lars.tingeltad@ntnu.no>
//
// 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.
//
// Author: Lars Tingelstad

#include <boost/python.hpp>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <glog/logging.h>

#include "game/vsr/vsr.h"
#include "game/adept_autodiff_cost_function.h"
#include "game/adept_autodiff_local_parameterization.h"
#include "game/motor_parameterization.h"
#include "game/ceres_python_utils.h"

namespace bp = boost::python;

using vsr::cga::Scalar;
using vsr::cga::Vector;
using vsr::cga::Bivector;
using vsr::cga::Point;
using vsr::cga::FlatPoint;
using vsr::cga::DualLine;
using vsr::cga::Motor;
using vsr::cga::Rotor;
using vsr::cga::Translator;
using vsr::cga::Origin;
using vsr::cga::Infinity;
using vsr::cga::TangentVector;
using vsr::cga::DualPlane;
using vsr::cga::PointPair;
using vsr::cga::DualSphere;

using vsr::cga::Vec;
using vsr::cga::Par;
using vsr::cga::Pnt;
using vsr::cga::Dlp;
using vsr::cga::Dll;
using vsr::cga::Mot;
using vsr::cga::Ori;
using vsr::cga::Inf;
using vsr::cga::Con;
using vsr::cga::Tnv;

using vsr::nga::Op;

namespace game {

auto TransformTangentVector(const Mot& mot, const Tnv& tnv) -> Par {
  return mot * tnv * ~mot;
}

class LaserDistanceSensorCalibrator {
 public:
  LaserDistanceSensorCalibrator() {}
  LaserDistanceSensorCalibrator(
      const LaserDistanceSensorCalibrator& lds_calibrator) {}
  LaserDistanceSensorCalibrator(const Mot& motor,
                                const bp::dict& solver_options)
      : motor_(motor) {
    SetSolverOptions(solver_options, options_);
  }

  LaserDistanceSensorCalibrator(const Vec& point, const Vec& direction,
                                const bp::dict& solver_options)
      : point_(point), direction_(direction) {
    SetSolverOptions(solver_options, options_);
  }

  LaserDistanceSensorCalibrator(const Vec& point, const double theta,
                                const double phi,
                                const bp::dict& solver_options)
      : point_(point), theta_(theta), phi_(phi) {
    SetSolverOptions(solver_options, options_);
  }

  struct AnglesDistanceErrorCostFunctor {
    AnglesDistanceErrorCostFunctor(const Mot& forward_kinematics,
                                   const Dlp& measurement_plane,
                                   const double measurement_distance)
        : forward_kinematics_(forward_kinematics),
          measurement_plane_(measurement_plane),
          measurement_distance_(measurement_distance) {}

    template <typename T>
    auto operator()(const T* const point, const T* const theta,
                    const T* const phi, T* residual) const -> bool {
      Motor<T> M_forward_kinematics(forward_kinematics_);
      DualPlane<T> dlp(measurement_plane_);

      Vector<T> l0 = Vector<T>(point).null().spin(M_forward_kinematics);

      Vector<T> direction = Vector<T>(T(1.0), T(0.0), T(0.0));
      direction = direction.spin(Rotor<T>(
          cos(theta[0] / T(2.0)), sin(theta[0] / T(2.0)), T(0.0), T(0.0)));
      // direction = direction.spin(
      // Rotor<T>(cos(phi[0] / T(2.0)), T(0.0), T(0.0), sin(phi[0] / T(2.0))));

      auto l = (Vector<T>(direction).unit() ^ Infinity<T>(T(1.0)))
                   .spin(M_forward_kinematics);

      Vector<T> n(dlp[0], dlp[1], dlp[2]);
      Vector<T> p0(dlp[0] * dlp[3], dlp[1] * dlp[3], dlp[2] * dlp[3]);
      Vector<T> p0l0 = p0 - l0;
      T num = ((p0 - l0) <= n)[0];
      T den = (Vector<T>(l[0], l[1], l[2]) <= n)[0];
      T distance = num / den;
      residual[0] = T(measurement_distance_) - distance;

      return true;
    }

   private:
    const Mot forward_kinematics_;
    const Dlp measurement_plane_;
    const double measurement_distance_;
  };

  struct PointDirectionDistanceErrorCostFunctor {
    PointDirectionDistanceErrorCostFunctor(const Mot& forward_kinematics,
                                           const Dlp& measurement_plane,
                                           const double measurement_distance)
        : forward_kinematics_(forward_kinematics),
          measurement_plane_(measurement_plane),
          measurement_distance_(measurement_distance) {}

    template <typename T>
    auto operator()(const T* const point, const T* const direction,
                    T* residual) const -> bool {
      Motor<T> M_forward_kinematics(forward_kinematics_);
      DualPlane<T> dlp(measurement_plane_);

      Vector<T> l0 = Vector<T>(point).null().spin(M_forward_kinematics);

      auto l = (Vector<T>(direction).unit() ^ Infinity<T>(T(1.0)))
                   .spin(M_forward_kinematics);

      // DualLine<T> dll = (p0.null() ^ l0.unit() ^ Infinity<T>(T(1.0)))
      //.dual()
      //.spin(M_forward_kinematics);

      // FlatPoint<T> flp = (dll ^ dlp).dual();

      // DualSphere<T> s = (p0.null() <= flp);
      // T distance = sqrt((s <= s)[0]);

      // std::cout << distance << std::endl;

      Vector<T> n(dlp[0], dlp[1], dlp[2]);
      Vector<T> p0(dlp[0] * dlp[3], dlp[1] * dlp[3], dlp[2] * dlp[3]);
      Vector<T> p0l0 = p0 - l0;
      T num = ((p0 - l0) <= n)[0];
      T den = (Vector<T>(l[0], l[1], l[2]) <= n)[0];
      T distance = num / den;
      residual[0] = T(measurement_distance_) - distance;

      return true;
    }

   private:
    const Mot forward_kinematics_;
    const Dlp measurement_plane_;
    const double measurement_distance_;
  };

  struct MeasurementDistanceErrorCostFunctor {
    MeasurementDistanceErrorCostFunctor(const Mot& forward_kinematics,
                                        const Dlp& measurement_plane,
                                        const double measurement_distance)
        : forward_kinematics_(forward_kinematics),
          measurement_plane_(measurement_plane),
          measurement_distance_(measurement_distance) {}

    template <typename T>
    auto operator()(const T* const motor, T* residual) const -> bool {
      Motor<T> M_laser_in_ee(motor);
      Motor<T> M_forward_kinematics(forward_kinematics_);
      DualPlane<T> dlp(measurement_plane_);

      Motor<T> M = M_forward_kinematics * M_laser_in_ee;

      PointPair<T> t =
          M * (Origin<T>(T(1.0)) ^ Vector<T>(T(1.0), T(0.0), T(0.0))) * ~M;

      DualSphere<T> s = (dlp <= t) / (Infinity<T>(T(1.0)) <= (dlp <= t));

      T radius = sqrt((s <= s)[0]);

      residual[0] = T(measurement_distance_) - radius;

      return true;
    }

   private:
    const Mot forward_kinematics_;
    const Dlp measurement_plane_;
    const double measurement_distance_;
  };

  struct DirectionNormalizationParameterization {
    template <typename T>
    bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
      std::cout << "Direction Normalization" << std::endl;
      x_plus_delta[0] = x[0] + delta[0];
      x_plus_delta[1] = x[1] + delta[1];
      x_plus_delta[2] = x[2] + delta[2];
      T scale = static_cast<T>(1.0) / sqrt(x_plus_delta[0] * x_plus_delta[0] +
                                           x_plus_delta[1] * x_plus_delta[1] +
                                           x_plus_delta[2] * x_plus_delta[2]);
      x_plus_delta[0] *= scale;
      x_plus_delta[1] *= scale;
      x_plus_delta[2] *= scale;
      return true;
    }
  };

  auto Summary() const -> bp::dict { return game::SummaryToDict(summary_); }

  auto AddTangentVectorResidualBlock(const Mot& forward_kinematics,
                                     const Dlp& measurement_plane,
                                     const double measurement_distance)
      -> bool {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<MeasurementDistanceErrorCostFunctor, 1,
                                        8>(
            new MeasurementDistanceErrorCostFunctor(
                forward_kinematics, measurement_plane, measurement_distance));
    problem_.AddResidualBlock(cost_function, NULL, &motor_[0]);
    return true;
  }

  auto AddPointDirectionResidualBlock(const Mot& forward_kinematics,
                                      const Dlp& measurement_plane,
                                      const double measurement_distance)
      -> bool {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<PointDirectionDistanceErrorCostFunctor,
                                        1, 3, 3>(
            new PointDirectionDistanceErrorCostFunctor(
                forward_kinematics, measurement_plane, measurement_distance));
    problem_.AddResidualBlock(cost_function, NULL, &point_[0], &direction_[0]);
    return true;
  }

  auto AddAngleDistanceResidualBlock(const Mot& forward_kinematics,
                                     const Dlp& measurement_plane,
                                     const double measurement_distance)
      -> bool {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<AnglesDistanceErrorCostFunctor, 1, 3, 1,
                                        1>(new AnglesDistanceErrorCostFunctor(
            forward_kinematics, measurement_plane, measurement_distance));
    problem_.AddResidualBlock(cost_function, NULL, &point_[0], &theta_, &phi_);
    return true;
  }

  auto SetMotorParameterizationTypeFromString(const std::string& type) -> void {
    if (type == "NORMALIZE") {
      std::cout << "game:: Using rotor normalization." << std::endl;
      problem_.SetParameterization(
          &motor_[0],
          new ceres::AutoDiffLocalParameterization<MotorNormalizeRotor, 8, 8>);
    } else if (type == "POLAR_DECOMPOSITION") {
      std::cout << "game:: Using polar decomposition." << std::endl;
      problem_.SetParameterization(
          &motor_[0],
          new ceres::AutoDiffLocalParameterization<MotorPolarDecomposition, 8,
                                                   8>);
    } else if (type == "BIVECTOR_GENERATOR") {
      std::cout << "game:: Using bivector generator (Versor)." << std::endl;
      problem_.SetParameterization(
          &motor_[0],
          new ceres::AutoDiffLocalParameterization<MotorFromBivectorGenerator,
                                                   8, 6>);
    } else if (type == "BIVECTOR_GENERATOR_ADEPT") {
      std::cout << "game:: ADEPT Using bivector generator (Versor)."
                << std::endl;
      problem_.SetParameterization(
          &motor_[0],
          new AdeptAutoDiffLocalParameterization<MotorFromBivectorGenerator, 8,
                                                 6>);
    } else {
      std::cout << "Unknown motor parameterization type" << std::endl;
    }
  }

  auto SetDirectionParameterizationTypeFromString(const std::string& type)
      -> bool {
    if (type == "NORMALIZE") {
      problem_.SetParameterization(
          &direction_[0], new ceres::AutoDiffLocalParameterization<
                              DirectionNormalizationParameterization, 3, 3>);
      return true;
    }
  }

  auto Solve() -> bp::tuple {
    ceres::Solve(options_, &problem_, &summary_);
    return bp::make_tuple(motor_, Summary());
  }

  auto SolvePointDirection() -> bp::tuple {
    ceres::Solve(options_, &problem_, &summary_);
    return bp::make_tuple(point_, direction_, Summary());
  }

  auto SolvePointAngles() -> bp::tuple {
    ceres::Solve(options_, &problem_, &summary_);
    return bp::make_tuple(point_, theta_, phi_, Summary());
  }

 private:
  Mot motor_;
  Vec point_;
  Vec direction_;
  double theta_;
  double phi_;

  ceres::Problem problem_;
  ceres::Solver::Options options_;
  ceres::Solver::Summary summary_;
};

BOOST_PYTHON_MODULE_INIT(liblds_calibration) {
  bp::class_<LaserDistanceSensorCalibrator>(
      "LaserDistanceSensorCalibrator", bp::init<const Mot&, const bp::dict&>())
      .def(bp::init<const Vec&, const Vec&, const bp::dict&>())
      .def(bp::init<const Vec&, const double, const double, const bp::dict&>())
      .def("add_tangent_vector_residual_block",
           &LaserDistanceSensorCalibrator::AddTangentVectorResidualBlock)
      .def("add_point_direction_residual_block",
           &LaserDistanceSensorCalibrator::AddPointDirectionResidualBlock)
      .def("add_point_angles_residual_block",
           &LaserDistanceSensorCalibrator::AddAngleDistanceResidualBlock)
      .def("set_parameterization", &LaserDistanceSensorCalibrator::
                                       SetMotorParameterizationTypeFromString)
      .def("set_direction_parameterization",
           &LaserDistanceSensorCalibrator::
               SetDirectionParameterizationTypeFromString)
      .def("solve", &LaserDistanceSensorCalibrator::Solve)
      .def("solve_point_angles",
           &LaserDistanceSensorCalibrator::SolvePointAngles)
      .def("solve_point_direction",
           &LaserDistanceSensorCalibrator::SolvePointDirection);

  bp::def("transform_tangent_vector", &TransformTangentVector);
}

}  // namespace game


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OCCUPATION_INFO_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OCCUPATION_INFO_HPP

#if ! defined(NDEBUG)
//  #define BOOST_GEOMETRY_DEBUG_BUFFER_OCCUPATION
#endif

#include <algorithm>
#include <boost/range.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/iterators/closing_iterator.hpp>

#include <boost/geometry/algorithms/detail/get_left_turns.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename P>
class relaxed_less
{
    typedef typename geometry::coordinate_type<P>::type coordinate_type;

    coordinate_type epsilon;

public :

    inline relaxed_less()
    {
        // TODO: adapt for ttmath, and maybe build the map in another way
        // (e.g. exact constellations of segment-id's), maybe adaptive.
        epsilon = std::numeric_limits<double>::epsilon() * 100.0;
    }

    inline bool operator()(P const& a, P const& b) const
    {
        coordinate_type const dx = math::abs(geometry::get<0>(a) - geometry::get<0>(b));
        coordinate_type const dy = math::abs(geometry::get<1>(a) - geometry::get<1>(b));

        if (dx < epsilon && dy < epsilon)
        {
            return false;
        }
        if (dx < epsilon)
        {
            return geometry::get<1>(a) < geometry::get<1>(b);
        }

        return geometry::get<0>(a) < geometry::get<0>(b);
    }

    inline bool equals(P const& a, P const& b) const
    {
        typedef typename geometry::coordinate_type<P>::type coordinate_type;

        coordinate_type const dx = math::abs(geometry::get<0>(a) - geometry::get<0>(b));
        coordinate_type const dy = math::abs(geometry::get<1>(a) - geometry::get<1>(b));

        return dx < epsilon && dy < epsilon;
    };
};

template <typename T, typename P1, typename P2>
inline T calculate_angle(P1 const& from_point, P2 const& to_point)
{
    typedef P1 vector_type;
    vector_type v = from_point;
    geometry::subtract_point(v, to_point);
    return atan2(geometry::get<1>(v), geometry::get<0>(v));
}

template <typename Iterator, typename Vector>
inline Iterator advance_circular(Iterator it, Vector const& vector, segment_identifier& seg_id, bool forward = true)
{
	int const increment = forward ? 1 : -1;
	if (it == boost::begin(vector) && increment < 0)
	{
		it = boost::end(vector);
        seg_id.segment_index = boost::size(vector);
	}
	it += increment;
    seg_id.segment_index += increment;
	if (it == boost::end(vector))
	{
        seg_id.segment_index = 0;
		it = boost::begin(vector);
	}
	return it;
}

template <typename Point, typename T>
struct angle_info
{
	typedef T angle_type;
    typedef Point point_type;

    segment_identifier seg_id;
    int turn_index;
    int operation_index;
    Point intersection_point;
    Point direction_point;
    T angle;
    bool incoming;
};

template <typename AngleInfo>
class occupation_info
{
	typedef std::vector<AngleInfo> collection_type;

	struct angle_sort
	{
		inline bool operator()(AngleInfo const& left, AngleInfo const& right) const
		{
			// In this case we can compare even double using equals
			// return geometry::math::equals(left.angle, right.angle)
			return left.angle == right.angle
				? int(left.incoming) < int(right.incoming)
				: left.angle < right.angle
				;
		}
	};

public :
    collection_type angles;
private :
    bool m_occupied;
	bool m_calculated;

	inline bool is_occupied()
	{
		if (boost::size(angles) <= 1)
		{
			return false;
		}

		std::sort(angles.begin(), angles.end(), angle_sort());

		typedef geometry::closing_iterator<collection_type const> closing_iterator;
		closing_iterator vit(angles);
		closing_iterator end(angles, true);

		closing_iterator prev = vit++;
		for( ; vit != end; prev = vit++)
		{
			if (! geometry::math::equals(prev->angle, vit->angle)
				&& ! prev->incoming
				&& vit->incoming)
			{
				return false;
			}
		}
		return true;
	}

public :
    inline occupation_info()
        : m_occupied(false)
		, m_calculated(false)
    {}

	template <typename PointC, typename Point1, typename Point2>
	inline void add(PointC const& map_point, Point1 const& direction_point, Point2 const& intersection_point,
                    int turn_index, int operation_index,
                    segment_identifier const& seg_id, bool incoming)
	{
        //std::cout << "-> adding angle " << geometry::wkt(direction_point) << " .. " << geometry::wkt(intersection_point) << " " << int(incoming) << std::endl;
		if (geometry::equals(direction_point, intersection_point))
		{
			//std::cout << "EQUAL! Skipping" << std::endl;
			return;
		}

        AngleInfo info;
        info.incoming = incoming;
        info.angle = calculate_angle<typename AngleInfo::angle_type>(direction_point, map_point);
        info.seg_id = seg_id;
        info.turn_index = turn_index;
        info.operation_index = operation_index;
        info.intersection_point = intersection_point;
        info.direction_point = direction_point;
        angles.push_back(info);

		m_calculated = false;
	}

	inline bool occupied()
	{
		if (! m_calculated)
		{
			m_occupied = is_occupied();
			m_calculated = true;
		}
		return m_occupied;
	}

    template <typename Turns, typename TurnSegmentIndices>
    inline void get_left_turns(
                    Turns& turns, TurnSegmentIndices const& turn_segment_indices,
                    std::set<int>& keep_indices)
    {
        std::sort(angles.begin(), angles.end(), angle_sort());
        calculate_left_turns<AngleInfo>(angles, turns, turn_segment_indices, keep_indices);
    }
};

template <typename Point, typename Ring, typename Info>
inline void add_incoming_and_outgoing_angles(Point const& map_point, Point const& intersection_point,
				Ring const& ring,
                int turn_index,
                int operation_index,
                segment_identifier seg_id,
                Info& info)
{
    typedef typename boost::range_iterator
        <
            Ring const
        >::type iterator_type;

	int const n = boost::size(ring);
	if (seg_id.segment_index >= n || seg_id.segment_index < 0)
	{
		return;
	}

    segment_identifier real_seg_id = seg_id;
	iterator_type it = boost::begin(ring) + seg_id.segment_index;

    // TODO: if we use turn-info ("to", "middle"), we know if to advance without resorting to equals
    relaxed_less<Point> comparator;

    if (comparator.equals(intersection_point, *it))
    {
		// It should be equal only once. But otherwise we skip it (in "add")
		it = advance_circular(it, ring, seg_id, false);
    }

	info.add(map_point, *it, intersection_point, turn_index, operation_index, real_seg_id, true);

    if (comparator.equals(intersection_point, *it))
    {
		it = advance_circular(it, ring, real_seg_id);
	}
	else
	{
		// Don't upgrade the ID
		it = advance_circular(it, ring, seg_id);
	}
    for (int defensive_check = 0;
		comparator.equals(intersection_point, *it) && defensive_check < n;
		defensive_check++)
    {
		it = advance_circular(it, ring, real_seg_id);
    }

	info.add(map_point, *it, intersection_point, turn_index, operation_index, real_seg_id, false);
}

// Map in two senses of the word: it is a std::map where the key is a point.
// Per point an "occupation_info" record is kept
// Used for the buffer (but will also be used for intersections/unions having complex self-tangencies)
template <typename Point, typename OccupationInfo>
class occupation_map
{
public :
    typedef std::map<Point, OccupationInfo, relaxed_less<Point> > map_type;

    map_type map;
	std::set<int> turn_indices;

    inline OccupationInfo& find_or_insert(Point const& point, Point& mapped_point)
    {
        typename map_type::iterator it = map.find(point);
        if (it == boost::end(map))
        {
            std::pair<typename map_type::iterator, bool> pair
                        = map.insert(std::make_pair(point, OccupationInfo()));
            it = pair.first;
        }
        mapped_point = it->first;
        return it->second;
    }

    inline bool contains(Point const& point) const
    {
        typename map_type::const_iterator it = map.find(point);
        return it != boost::end(map);
    }

    inline bool contains_turn_index(int index) const
    {
        return turn_indices.count(index) > 0;
    }

    inline void insert_turn_index(int index)
    {
        turn_indices.insert(index);
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OCCUPATION_INFO_HPP


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/euler/include/functions/erfinv.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( erfinv_real, NT2_SIMD_REAL_TYPES)
{
  using nt2::erfinv;
  using nt2::tag::erfinv_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::call<erfinv_(vT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(erfinv(nt2::One<vT>()      ),  nt2::Inf<r_t>()   , 10);
  NT2_TEST_ULP_EQUAL(erfinv(nt2::Nan<vT>()      ),  nt2::Nan<r_t>()   , 10);
#endif
  NT2_TEST_ULP_EQUAL(erfinv(nt2::Zero<vT>()     ),  nt2::Mzero<r_t>() , 10);
  NT2_TEST_ULP_EQUAL(erfinv(nt2::splat<vT>(0.52049987781304653768274665389196) ),  nt2::Half<r_t>()  , 10);
  NT2_TEST_ULP_EQUAL(erfinv(nt2::splat<vT>(0.842700792949714869341220635082610) ),  nt2::One<r_t>()   , 10);
  NT2_TEST_ULP_EQUAL(erfinv(nt2::splat<vT>(0.99532226501895273416206925636725) ),  nt2::Two<r_t>()   , 10);
  NT2_TEST_ULP_EQUAL(erfinv(nt2::Zero<vT>()     ),  nt2::Zero<r_t>()  , 10);
}


//
// Created by calebcintary on 3/27/22.
//

#define _USE_MATH_DEFINES

#include <boost/test/unit_test.hpp>
#include <cmath>

#include "pyplot_cpp/Plot3D.h"
#include "pyplot_cpp/plt/Properties.hpp"

BOOST_AUTO_TEST_SUITE(Plot3D_Test)

    BOOST_AUTO_TEST_CASE(Plot3D_SimpleShow_Test) {
        pyplot_cpp::Plot3D plot;
        int a = 0;
        int b = 0;
        std::vector<double> z;
        std::vector<double> r;
        for (double _z = -2; _z < 2; _z += 4.0 / 100.0) {
            z.push_back(_z);
            r.push_back(pow(_z, 2) + 1);
        }
        std::vector<double> x;
        std::vector<double> y;
        int i = 0;
        for (double theta = -4 * M_PI; theta < 4 * M_PI; theta += (8 * M_PI) / 100.0) {
            x.push_back(r[i] * sin(theta));
            y.push_back(r[i++] * cos(theta));
        }

        plot.setData(x, y, z);
        plot.show();
    }

    BOOST_AUTO_TEST_CASE(Plot3D_SimpleSave_Test) {
        pyplot_cpp::Plot3D plot;
        int a = 0;
        int b = 0;
        std::vector<double> z;
        std::vector<double> r;
        for (double _z = -2; _z < 2; _z += 4.0 / 100.0) {
            z.push_back(_z);
            r.push_back(pow(_z, 2) + 1);
        }
        std::vector<double> x;
        std::vector<double> y;
        int i = 0;
        for (double theta = -4 * M_PI; theta < 4 * M_PI; theta += (8 * M_PI) / 100.0) {
            x.push_back(r[i] * sin(theta));
            y.push_back(r[i++] * cos(theta));
        }

        plot.setData(x, y, z);
        plot.save("../../examples/Plot3D_simple.png");
    }

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <string>
#include <random>
#include <set>
#include <sstream>
#include <vector>
#include <algorithm>
#include <util/cityhash_tc.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/SparseCore>

// Constraints
#include <optimization/constraints-inl.hpp>

using namespace turi;
typedef Eigen::Matrix<double,Eigen::Dynamic,1>  DenseVector;
typedef Eigen::SparseVector<double> SparseVector;

struct constraint_interface_test  {

  size_t variables;
  DenseVector solution_orthant;
  DenseVector solution_box;
  double lb_dbl;
  double ub_dbl;
  DenseVector lb;
  DenseVector ub;

  public:

    constraint_interface_test() {

      size_t variables = 10;

      DenseVector init_point(variables);
      DenseVector solution_orthant(variables);
      DenseVector solution_box(variables);
      double lb_dbl = 0;
      double up_dbl = 1;
      DenseVector lb(variables);
      DenseVector ub(variables);
      lb.zeros();
      ub.ones():
      srand(1);
      init_point    << 1 << -1 << 2 << -2 << 3 << -3 << 4 << -4 << 5 << -5;
      solution_orthant << 1 <<  0 << 2 <<  0 << 3 <<  0 << 4 <<  0 << 5 <<  0;
      solution_box  << 1 <<  0 << 1 <<  0 << 1 <<  0 << 1 <<  0 << 1 << 0;

      std::shared_ptr<non_negative_orthant> non_negative;
      non_negative.reset(new non_negative_orthant(variables));

      this->variables = variables;
      this->init_point = init_point;
      this->solution_box = solution_box;
      this->solution_orthant = solution_orthant;
      this->lb = lb;
      this->ub = lb;
      this->lb_dbl = lb_dbl;
      this->ub_dbl = ub_dbl;
    }

    void test_non_negative(){
      std::shared_ptr<non_negative_orthant> non_negative;
      non_negative.reset(new non_negative_orthant(variables));
      TS_ASSERT(arma::approx_equal(solution_orthant, non_negative_orthant.project(init_point),
            1e-10));
      TS_ASSERT_FALSE(non_negative_orthant.is_satisfied(init_point));

      DenseVector test_point(variables);
      test_point.zeros();
      TS_ASSERT_TRUE(non_negative_orthant.is_satisfied(test_point));

    }

    void test_box(){

      // Vector init
      std::shared_ptr<box_constraints> box;
      box.reset(new box_constraints(lb, ub));
      TS_ASSERT(arma::approx_equal(solution_box, box.project(init_point),
            1e-10));
      TS_ASSERT_FALSE(box.is_satisfied(init_point));
      DenseVector test_point(variables);
      test_point.zeros();
      TS_ASSERT_TRUE(box.is_satisfied(test_point));
      box.reset();

      // Double init
      std::shared_ptr<box_constraints> box;
      box.reset(new box_constraints(lb_dbl, ub_dbl, variables));
      TS_ASSERT(arma::approx_equal(solution_box, box.project(init_point),
            1e-10));
      TS_ASSERT_FALSE(box.is_satisfied(init_point));
      DenseVector test_point(variables);
      test_point.zeros();
      TS_ASSERT_TRUE(box.is_satisfied(test_point));

    }

};

BOOST_FIXTURE_TEST_SUITE(_constraint_interface_test, constraint_interface_test)
BOOST_AUTO_TEST_CASE(test_non_negative) {
  constraint_interface_test::test_non_negative();
}
BOOST_AUTO_TEST_CASE(test_box) {
  constraint_interface_test::test_box();
}
BOOST_AUTO_TEST_SUITE_END()


/**
 * Copyright (c) 2015-2019 Przemysław Dobrowolski
 *
 * This file is part of the Configuration Space Library (libcs2), a library
 * for creating configuration spaces of various motion planning problems.
 *
 * 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 "mainwindow.h"
#include "ui_mainwindow.h"
#include "renderview.h"
#include "renderviewarcballcamera.h"
#include "renderviewflycamera.h"
#include "renderviewautocamera.h"
#include "test/testpredg3f.h"
#include <QMessageBox>
#include <QFileDialog>
#include <QFileInfo>
#include <QTextStream>
#include <QFile>
#include <QImage>
#include <boost/scoped_array.hpp>
#include <cassert>
#include <cmath>

const QColor salemColor(9, 127, 75);
const QColor redBerryColor(142, 0, 0);
const QColor darkOrangeColor(255, 140, 0);

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    m_blockPredicateUpdate(false),
    m_currentChanged(false),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    // setup
    if (true)
    {
        QGLFormat fmt;
        fmt.setSamples(4);
        fmt.setSampleBuffers(true);
        QGLFormat::setDefaultFormat(fmt);
    }

    m_rv = new RenderView();

    m_rv->setCaption("cspace");
    m_rv->enableViewModeOption(ViewMode::Culling);

    ui->widgetView->layout()->addWidget(m_rv);

    // update predicate predicate
    updatePredicateInformation();

    // window title
    updateWindowTitle();

    // force no-changes
    m_currentChanged = false;
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::updatePredicateInformation()
{
    if (m_blockPredicateUpdate)
        return;

    // changed
    m_currentChanged = true;
    updateWindowTitle();

    // predicate g
    struct cs2_predg3f_s pred =
        {
            { ui->doubleSpinBoxKX->value(), ui->doubleSpinBoxKY->value(), ui->doubleSpinBoxKZ->value() },
            { ui->doubleSpinBoxLX->value(), ui->doubleSpinBoxLY->value(), ui->doubleSpinBoxLZ->value() },
            { ui->doubleSpinBoxAX->value(), ui->doubleSpinBoxAY->value(), ui->doubleSpinBoxAZ->value() },
            { ui->doubleSpinBoxBX->value(), ui->doubleSpinBoxBY->value(), ui->doubleSpinBoxBZ->value() },
            ui->doubleSpinBoxC->value()
        };

    // param g
    struct cs2_predgparam3f_s param;
    cs2_predg3f_param(&param, &pred);

    double a = param.a;
    double b = param.b;
    double c = param.c;

    double mi = std::min(a - b, b - a);
    double ma = std::max(a - b, b - a);

    // information -> parameters
    ui->labelKval->setText(formatVector(&pred.k));
    ui->labelLval->setText(formatVector(&pred.l));
    ui->labelAval->setText(formatVector(&pred.a));
    ui->labelBval->setText(formatVector(&pred.b));
    ui->labelCval->setText(QString::number(c, 'f', 2));

    // information -> base variables
    struct cs2_vec3f_s p, q, u, v;
    cs2_predg3f_pquv(&p, &q, &u, &v, &pred);

    ui->labelPval->setText(formatVector(&p));
    ui->labelQval->setText(formatVector(&q));
    ui->labelUval->setText(formatVector(&u));
    ui->labelVval->setText(formatVector(&v));
    ui->labelC2val->setText(QString::number(c, 'f', 2));

    // information -> type
    enum cs2_predgtype3f_e type = cs2_predg3f_type(&pred);

    ui->labelproperval->setText(type == cs2_predgtype3f_improper ? "no" : "yes");
    ui->labeltypeval->setText(cs2_predgtype3f_str(type));

    // information -> parametrization variables
    ui->labelaval->setText(QString::number(a, 'f', 4));
    ui->labelbval->setText(QString::number(b, 'f', 4));
    ui->labelcval->setText(QString::number(c, 'f', 4));
    ui->labelminval->setText(QString::number(mi, 'f', 4));
    ui->labelmaxval->setText(QString::number(ma, 'f', 4));

    ui->labelcaseval->setText(cs2_predgparamtype3f_str(param.t));
    ui->labeldimval->setText(QString::number(cs2_predgparamtype3f_dim(param.t)));
    ui->labelcompval->setText(QString::number(cs2_predgparamtype3f_domain_components(param.t)));
    ui->labelismanival->setText(cs2_predgparamtype3f_is_manifold(param.t) ? "yes" : "no");
    ui->labelhasholeval->setText(cs2_predgparamtype3f_has_domain_hole(param.t) ? "yes" : "no");
    ui->labelisconnval->setText(cs2_predgparamtype3f_is_connected(param.t) ? "yes" : "no");

    // information -> eigen decomposition
    ui->labeldetval->setText(QString::number(param.e[0] * param.e[1] * param.e[2] * param.e[3], 'f', 8));

    ui->labell1val->setText(QString::number(param.e[0], 'f', 4));
    ui->labell2val->setText(QString::number(param.e[1], 'f', 4));
    ui->labell3val->setText(QString::number(param.e[2], 'f', 4));
    ui->labell4val->setText(QString::number(param.e[3], 'f', 4));

    ui->labelw1val->setText(formatVector(&param.ev[0]));
    ui->labelw2val->setText(formatVector(&param.ev[1]));
    ui->labelw3val->setText(formatVector(&param.ev[2]));
    ui->labelw4val->setText(formatVector(&param.ev[3]));

    // visual
    m_rv->removeAllObjects();

    switch (cs2_predgparamtype3f_dim(param.t))
    {
        case 1: // 1-dimensional
            {
                LineListPtr lines(new LineList());

                meshing::meshCurveSimple(lines, &param, 0.01);

                m_rv->addLineList(lines, darkOrangeColor);
            }
            break;

        case 2: // 2-dimensional
            {
                TriangleListPtr trianglesFront(new TriangleList());
                TriangleListPtr trianglesBack(new TriangleList());

                if (ui->actionAutoMesh->isChecked())
                    meshing::meshSurfaceAdaptive(trianglesFront, trianglesBack, &param, 0.1, 0.5, 10);
                else
                    meshing::meshSurfaceSimple(trianglesFront, trianglesBack, &param, 0.01);

                m_rv->addTriangleList(trianglesFront, salemColor);
                m_rv->addTriangleList(trianglesBack, redBerryColor);
            }
            break;
    }
}

void MainWindow::setPredicate(double kx, double ky, double kz, double lx, double ly, double lz, double ax, double ay, double az, double bx, double by, double bz, double c)
{
    m_blockPredicateUpdate = true;

    ui->doubleSpinBoxKX->setValue(kx);
    ui->doubleSpinBoxKY->setValue(ky);
    ui->doubleSpinBoxKZ->setValue(kz);

    ui->doubleSpinBoxLX->setValue(lx);
    ui->doubleSpinBoxLY->setValue(ly);
    ui->doubleSpinBoxLZ->setValue(lz);

    ui->doubleSpinBoxAX->setValue(ax);
    ui->doubleSpinBoxAY->setValue(ay);
    ui->doubleSpinBoxAZ->setValue(az);

    ui->doubleSpinBoxBX->setValue(bx);
    ui->doubleSpinBoxBY->setValue(by);
    ui->doubleSpinBoxBZ->setValue(bz);

    ui->doubleSpinBoxC->setValue(c);

    m_blockPredicateUpdate = false;

    updatePredicateInformation();
}

void MainWindow::setPredicate(const cs2_predg3f_s *p)
{
    setPredicate(p->k.x, p->k.y, p->k.z,
                 p->l.x, p->l.y, p->l.z,
                 p->a.x, p->a.y, p->a.z,
                 p->b.x, p->b.y, p->b.z,
                 p->c);
}

QString MainWindow::formatVector(const struct cs2_vec3f_s *v)
{
    return "[" + QString::number(v->x, 'f', 2) + ", " + QString::number(v->y, 'f', 2) + ", " + QString::number(v->z, 'f', 2) + "]";
}

QString MainWindow::formatVector(const struct cs2_vec4f_s *v)
{
    return "[" + QString::number(v->x, 'f', 2) + ", " + QString::number(v->y, 'f', 2) + ", " + QString::number(v->z, 'f', 2) + ", " + QString::number(v->w, 'f', 2) + "]";
}

bool MainWindow::handleUnsavedChanges()
{
    switch (QMessageBox::question(this, tr("Unsaved changes"), tr("Save changes to file?"), QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel))
    {
        case QMessageBox::Yes:
            if (!m_currentFile.isEmpty())
                return saveCurrentFile();
            else
                return saveNewFile();

        case QMessageBox::No:
            return true;

        case QMessageBox::Cancel:
            return false;

    default:
        assert(0);
    }
}

bool MainWindow::saveCurrentFile()
{
    // changes
    m_currentChanged = false;
    updateWindowTitle();

    // save
    QFile out(m_currentFile);

    if (out.open(QFile::WriteOnly))
    {
        //writer
        {
            QTextStream stream(&out);

            stream << ui->doubleSpinBoxKX->value() << " ";
            stream << ui->doubleSpinBoxKY->value() << " ";
            stream << ui->doubleSpinBoxKZ->value();
            stream << "\r\n";
            stream << ui->doubleSpinBoxLX->value() << " ";
            stream << ui->doubleSpinBoxLY->value() << " ";
            stream << ui->doubleSpinBoxLZ->value();
            stream << "\r\n";
            stream << ui->doubleSpinBoxAX->value() << " ";
            stream << ui->doubleSpinBoxAY->value() << " ";
            stream << ui->doubleSpinBoxAZ->value();
            stream << "\r\n";
            stream << ui->doubleSpinBoxBX->value() << " ";
            stream << ui->doubleSpinBoxBY->value() << " ";
            stream << ui->doubleSpinBoxBZ->value();
            stream << "\r\n";
            stream << ui->doubleSpinBoxC->value();
            stream << "\r\n";
        }

        out.close();
    }
    else
    {
        (void)QMessageBox::warning(this, tr("Save file"), tr("Failed to write: ") + m_currentFile, QMessageBox::Ok);
        return false;
    }

    return true;
}

bool MainWindow::saveNewFile()
{
    QString fileName = QFileDialog::getSaveFileName(this, tr("Save as"), QString(), QString("*.vis"));

    if (fileName.isEmpty())
        return false;

    if (!fileName.endsWith(".vis"))
        fileName += ".vis";

    m_currentFile = fileName;
    return saveCurrentFile();
}

void MainWindow::updateWindowTitle()
{
    QString fileName;

    if (m_currentFile.isEmpty())
        fileName = "unnamed.vis";
    else
    {
        QFileInfo fileInfo(m_currentFile);
        fileName = fileInfo.fileName();
    }

    setWindowTitle(fileName + ": vis" + (m_currentChanged ? " *" : ""));
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    if (!m_currentChanged)
        return;

    if (!handleUnsavedChanges())
    {
        event->ignore();
        return;
    }
}

void MainWindow::on_actionArcballCamera_triggered()
{
    m_rv->setRenderViewCamera(new RenderViewArcBallCamera());

    ui->actionArcballCamera->setChecked(true);
    ui->actionFreeCamera->setChecked(false);
    ui->actionAutoCamera->setChecked(false);
}

void MainWindow::on_actionFreeCamera_triggered()
{
    m_rv->setRenderViewCamera(new RenderViewFlyCamera());

    ui->actionArcballCamera->setChecked(false);
    ui->actionFreeCamera->setChecked(true);
    ui->actionAutoCamera->setChecked(false);
}

void MainWindow::on_actionAutoCamera_triggered()
{
    m_rv->setRenderViewCamera(new RenderViewAutoCamera());

    ui->actionArcballCamera->setChecked(false);
    ui->actionFreeCamera->setChecked(false);
    ui->actionAutoCamera->setChecked(true);
}

void MainWindow::on_doubleSpinBoxKX_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxKY_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxKZ_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxLX_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxLY_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxLZ_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxAX_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxAY_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxAZ_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxBX_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxBY_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxBZ_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_doubleSpinBoxC_valueChanged(double)
{
    updatePredicateInformation();
}

void MainWindow::on_labelZeroK_linkActivated(const QString &link)
{
    Q_UNUSED(link)

    ui->doubleSpinBoxKX->setValue(0);
    ui->doubleSpinBoxKY->setValue(0);
    ui->doubleSpinBoxKZ->setValue(0);
}

void MainWindow::on_labelZeroL_linkActivated(const QString &link)
{
    Q_UNUSED(link)

    ui->doubleSpinBoxLX->setValue(0);
    ui->doubleSpinBoxLY->setValue(0);
    ui->doubleSpinBoxLZ->setValue(0);
}

void MainWindow::on_labelZeroA_linkActivated(const QString &link)
{
    Q_UNUSED(link)

    ui->doubleSpinBoxAX->setValue(0);
    ui->doubleSpinBoxAY->setValue(0);
    ui->doubleSpinBoxAZ->setValue(0);
}

void MainWindow::on_labelZeroB_linkActivated(const QString &link)
{
    Q_UNUSED(link)

    ui->doubleSpinBoxBX->setValue(0);
    ui->doubleSpinBoxBY->setValue(0);
    ui->doubleSpinBoxBZ->setValue(0);
}

void MainWindow::on_labelZeroC_linkActivated(const QString &link)
{
    Q_UNUSED(link)

    ui->doubleSpinBoxC->setValue(0);
}

void MainWindow::on_actionWireframe_triggered()
{
    m_rv->setViewModeOption(ViewMode::Wireframe, ui->actionWireframe->isChecked());
}

void MainWindow::on_actionQuit_triggered()
{
    if (!m_currentChanged)
    {
        emit close();
        return;
    }

    if (!handleUnsavedChanges())
        return;

    emit close();
}

void MainWindow::on_actionOpen_triggered()
{
    if (m_currentChanged && !handleUnsavedChanges())
        return;

    QString fileName = QFileDialog::getOpenFileName(this, tr("Open file"), QString(), tr("*.vis"));

    if (fileName.isEmpty())
        return;

    QFile file(fileName);

    if (file.open(QFile::ReadOnly))
    {
        // reader
        {
            QTextStream stream(&file);

            int kx, ky, kz;
            stream >> kx >> ky >> kz;

            int lx, ly, lz;
            stream >> lx >> ly >> lz;

            int ax, ay, az;
            stream >> ax >> ay >> az;

            int bx, by, bz;
            stream >> bx >> by >> bz;

            int c;
            stream >> c;

            if (stream.status() != QTextStream::Ok)
            {
                (void)QMessageBox::warning(this, tr("Open file"), tr("Failed to parse: ") + m_currentFile, QMessageBox::Ok);
                return;
            }

            ui->doubleSpinBoxKX->setValue(kx);
            ui->doubleSpinBoxKY->setValue(ky);
            ui->doubleSpinBoxKZ->setValue(kz);

            ui->doubleSpinBoxLX->setValue(lx);
            ui->doubleSpinBoxLY->setValue(ly);
            ui->doubleSpinBoxLZ->setValue(lz);

            ui->doubleSpinBoxAX->setValue(ax);
            ui->doubleSpinBoxAY->setValue(ay);
            ui->doubleSpinBoxAZ->setValue(az);

            ui->doubleSpinBoxBX->setValue(bx);
            ui->doubleSpinBoxBY->setValue(by);
            ui->doubleSpinBoxBZ->setValue(bz);

            ui->doubleSpinBoxC->setValue(c);
        }

        file.close();
    }
    else
    {
        (void)QMessageBox::warning(this, tr("Open file"), tr("Failed to open: ") + m_currentFile, QMessageBox::Ok);
        return;
    }

    m_currentFile = fileName;
    m_currentChanged = false;
    updateWindowTitle();
}

void MainWindow::on_actionSave_triggered()
{
    if (m_currentChanged)
    {
        if (!m_currentFile.isEmpty())
            (void)saveCurrentFile();
        else
            (void)saveNewFile();
    }
}

void MainWindow::on_actionSaveAs_triggered()
{
    (void)saveNewFile();
}

void MainWindow::on_actionAutoMesh_triggered()
{
    updatePredicateInformation();
}

void MainWindow::on_actionTakeScreenshot_triggered()
{
    QString fileName = QFileDialog::getSaveFileName(this, tr("Save current frame buffer as"), QString(), QString("*.png"));

    if (fileName.isEmpty())
        return;

    if (!fileName.endsWith(".png"))
        fileName += ".png";

    QImage img = m_rv->grabFrameBuffer();

    if (!img.save(fileName, nullptr, 100))
    {
        (void)QMessageBox::warning(this, tr("Save current frame buffer file"), tr("Failed to save frame buffer"), QMessageBox::Ok);
        return;
    }
}

void MainWindow::on_actionModelOnlyView_triggered()
{
    m_rv->setViewModeOption(ViewMode::ModelOnly, ui->actionModelOnlyView->isChecked());
}

void MainWindow::on_actionOutlines_triggered()
{
    m_rv->setViewModeOption(ViewMode::Outlines, ui->actionOutlines->isChecked());
}

void MainWindow::on_actionNormals_triggered()
{
    m_rv->setViewModeOption(ViewMode::Normals, ui->actionNormals->isChecked());
}

void MainWindow::on_actionA_pair_of_points_triggered()
{
    setPredicate(&test_predg3f_a_pair_of_points);
}

void MainWindow::on_actionA_pair_of_separate_ellipsoids_triggered()
{
    setPredicate(&test_predg3f_a_pair_of_separate_ellipsoids);
}

void MainWindow::on_actionA_pair_of_y_touching_ellipsoids_triggered()
{
    setPredicate(&test_predg3f_a_pair_of_y_touching_ellipsoids);
}

void MainWindow::on_actionA_pair_of_yz_crossed_ellipsoids_triggered()
{
    setPredicate(&test_predg3f_a_pair_of_yz_crossed_ellipsoids);
}

void MainWindow::on_actionA_pair_of_z_touching_ellipsoids_triggered()
{
    setPredicate(&test_predg3f_a_pair_of_z_touching_ellipsoids);
}

void MainWindow::on_actionA_y_barrel_triggered()
{
    setPredicate(&test_predg3f_a_y_barrel);
}

void MainWindow::on_actionA_z_barrel_triggered()
{
    setPredicate(&test_predg3f_a_z_barrel);
}

void MainWindow::on_actionA_notched_y_barrel_triggered()
{
    setPredicate(&test_predg3f_a_notched_y_barrel);
}

void MainWindow::on_actionA_notched_z_barrel_triggered()
{
    setPredicate(&test_predg3f_a_notched_z_barrel);
}

void MainWindow::on_actionA_pair_of_separate_yz_caps_triggered()
{
    setPredicate(&test_predg3f_a_pair_of_separate_yz_caps);
}

void MainWindow::on_actionA_xy_zw_torus_triggered()
{
    setPredicate(&test_predg3f_a_xy_zw_torus);
}

void MainWindow::on_actionA_xy_circle_triggered()
{
    setPredicate(&test_predg3f_a_xy_circle);
}

void MainWindow::on_actionA_zw_circle_triggered()
{
    setPredicate(&test_predg3f_a_zw_circle);
}

void MainWindow::on_actionA_xz_yw_torus_triggered()
{
    setPredicate(&test_predg3f_a_xz_yw_torus);
}

void MainWindow::on_actionA_xz_circle_triggered()
{
    setPredicate(&test_predg3f_a_xz_circle);
}

void MainWindow::on_actionA_yw_circle_triggered()
{
    setPredicate(&test_predg3f_a_yw_circle);
}


/**
   RateTicker class implementation.
*/

#include <boost/date_time.hpp>
#include "bites.hpp"

namespace bites {

// Initialize the object with a vector of time periods in seconds.
// For example, use (60, 300, 900) to track rates at 1, 5 and 15 minute
// periods (like when reporting system load.)
RateTicker::RateTicker(const std::vector<int>& periods)
    : m_periods(periods),
      m_history()
{
    /* Empty. */
}

// Tick the ticker.
// Return a vector of values corresponding to periods given in initializer,
// each value representing the rate of ticks (number of ticks per second)
// during that period.
std::vector<float> RateTicker::tick()
{
    auto now = boost::posix_time::microsec_clock::universal_time();
    m_history.add(now);

    // Compute timestamps, one for each period,
    // representing the beginning of that period (i.e. since now.)
    std::vector<boost::posix_time::ptime> tstamps (m_periods.size());
    for(int ii=0; ii<tstamps.size(); ++ii){
        auto period = m_periods[ii];
        auto duration = boost::posix_time::seconds(period);
        tstamps[ii] = now - duration;
    }

    // Trim off any tick values older than the earliest timestamp.
    auto iter = min_element(tstamps.begin(), tstamps.end());
    m_history.removeLT(*iter);

    // Create a list of counts, one for each period and
    // representing the number of ticks in that period.
    std::vector <unsigned int> counts (m_periods.size());
    for(int ii=0; ii<counts.size(); ++ii){
        auto tstamp = tstamps[ii];
        counts[ii] = m_history.getCountGT(tstamp) + 1;
    }

    // Compute a list of rates for the periods.
    std::vector <float> rates (m_periods.size());
    for(int ii=0; ii<counts.size(); ++ii){
        rates[ii] = counts[ii] / m_periods[ii];
    }
    return rates;
}

}  // namespace bites.


// Copyright (C) 2011 by the BEM++ Authors
//
// 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 "../check_arrays_are_close.hpp"
#include "../type_template.hpp"

#include "assembly/assembly_options.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/boundary_operator.hpp"
#include "assembly/context.hpp"
#include "assembly/laplace_3d_single_layer_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"

#include "common/boost_make_shared_fwd.hpp"

#include "grid/grid_factory.hpp"
#include "grid/grid.hpp"

#include "space/piecewise_linear_continuous_scalar_space.hpp"
#include "space/piecewise_constant_scalar_space.hpp"

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>

// Tests

using namespace Bempp;

BOOST_AUTO_TEST_SUITE(Laplace3dSingleLayerBoundaryOperator)

BOOST_AUTO_TEST_CASE_TEMPLATE(symmetric_matches_nonsymmetric_in_aca_mode,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename Fiber::ScalarTraits<RT>::RealType BFT;
    typedef typename Fiber::ScalarTraits<RT>::RealType CT;

    if (boost::is_same<RT, std::complex<float> >::value) {
        // The AHMED support for single-precision complex symmetric matrices
        // is broken
        BOOST_CHECK(true);
        return;
    }

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
                params, "../../meshes/sphere-h-0.4.msh",
                false /* verbose */);

    PiecewiseLinearContinuousScalarSpace<BFT> pwiseLinears(grid);
    PiecewiseConstantScalarSpace<BFT> pwiseConstants(grid);

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.minimumBlockSize = 4;
    assemblyOptions.switchToAcaMode(acaOptions);
    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(4);
    accuracyOptions.doubleSingular.setRelativeQuadratureOrder(2);
    NumericalQuadratureStrategy<BFT, RT> quadStrategy(accuracyOptions);

    Context<BFT, RT> context(make_shared_from_ref(quadStrategy), assemblyOptions);

    BoundaryOperator<BFT, RT> opNonsymmetric =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseLinears),
                "", NO_SYMMETRY);
    BoundaryOperator<BFT, RT> opSymmetric =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseLinears),
                "", SYMMETRIC);

#   ifdef WITH_AHMED
       arma::Mat<RT> matNonsymmetric = opNonsymmetric.weakForm()->asMatrix();
       arma::Mat<RT> matSymmetric = opSymmetric.weakForm()->asMatrix();

       BOOST_CHECK(check_arrays_are_close<RT>(
                       matNonsymmetric, matSymmetric, 2 * acaOptions.eps));
#   else
       BOOST_CHECK(true);
#   endif
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef TIMESERIES_H
#define TIMESERIES_H

#include <cmath>
#include <cstring>
#include <iostream>
#include <limits>
#include <string>

#include <boost/serialization/serialization.hpp>

#define INF std::numeric_limits<data_t>::infinity()
#define EPS 1e-9

// EXPERIMENT
extern int extraTimeSeries;

namespace genex {

typedef double data_t;

int calculateWarpingBandSize(int length, double ratio);

/**
 *  @brief header of a time series
 *
 *  A TimeSeries does not holds the actual data of a time series. It only has a pointer
 *  pointing to a part of data in a TimeSeriesSet. It also contains the starting position
 *  and ending position of the time series so that it can represent a sub-sequence.
 *  To be exact, a TimeSeries is a half-closed interval [start, end) of a time series in
 *  a TimeSeriesSet.
 *
 *  Outside the context of TimeSeriesSet. TimeSeries can be used as a general purpose container
 *  by providing a pointer to an array of data and its length.
 */
class TimeSeries
{
public:

  /**
   *  @brief constructor for TimeSeries
   *
   *  @param data a pointer pointing to the actual data
   *  @param index index of this time series in a TimeSeriesSet
   *  @param start starting position of this time series
   *  @param end ending position of this time series
   */
  TimeSeries(data_t *data, int index, int start, int end);

  /**
   *  @brief constructor for TimeSeries
   *
   *  This constructor is use when only data and length is needed
   *  to specify the time series. Index is set to 0, starting position
   *  is set to 0 and ending position is the length of the time series
   *
   *  @param data a pointer pointing to the actual data
   *  @param length length of the time series
   */
  TimeSeries(data_t *data, int length);

  /**
   *  @brief constructor for TimeSeries
   *
   *  This constructor is use when only length is needed
   *  to specify the time series. Index is set to 0, starting position
   *  is set to 0 and ending position is the length of the time series
   *  Memory is allocated for the data.
   *
   *  @param length length of the time series
   */
  TimeSeries(int length);

  /**
   * @brief Copy constructor
   */
  TimeSeries(const TimeSeries& other);

  /**
   *  @brief Copy assignment and move assignment
   */
  TimeSeries& operator=(const TimeSeries& other);
  TimeSeries& operator=(TimeSeries&& other);

  /**
   *  @brief destructor
   */
  ~TimeSeries();

  /**
   *  @brief get a value of this time series
   *
   *  @param idx index of the value in this time series
   *  @return the value at idx
   *
   *  @throw GenexException if the index is out of bound
   */
  data_t& operator[](int idx) const;

  /**
   *  @brief accumulate data from other time series into this one
   *
   *  The data of the given time series is added to this time series
   *  point-wise. The other time series must have the same length as this
   *  one.
   *  @param other the other time series to be added to this one.
   *  @return the reference to this time series
   *
   *  @throw GenexException if the other time series has different length
   *         from this one
   */
  TimeSeries& operator+=(const TimeSeries& other);

  /**
   * @brief compare two time series
   */
  bool operator==(const TimeSeries& other) const;

  /**
   *  @brief gets the length of this time series
   *
   *  @return length of this time series
   */
  int getLength() const { return this->length; }

  /**
   *  @brief gets the index of the data
   *
   *  @return the index of the data from its timeseries
   */
  int getIndex() const { return this->index; }

  /**
   *  @brief gets the start of the data
   *
   *  @return the start of the data from its timeseries
   */
  int getStart() const { return this->start; }

  /**
   *  @brief gets the end of the data
   *
   *  @return the end of the data from its timeseries
   */
  int getEnd() const { return this->end; }

  /**
   *  @brief returns the average of the whole time series
   */
  data_t average();

  const data_t* getKeoghLower(int warpingBand) const;
  const data_t* getKeoghUpper(int warpingBand) const;

  const data_t* getData() const;
  std::string getIdentifierString() const;

  friend std::ostream& operator<<(std::ostream&, const TimeSeries&);

private:
  // ATTENTION: Always offset by 'start' when indexing 'data' (i.e. data[start + i])
  data_t* data = nullptr;
  bool isOwnerOfData;
  int index;
  int start;
  int end;
  int length;

  mutable bool keoghCacheValid = false;
  mutable data_t* keoghLower = nullptr;
  mutable data_t* keoghUpper = nullptr;
  mutable double cachedWarpingBand;

  std::ostream &printData(std::ostream &out) const;

  /**
   * @brief generates the upper and lower envelope used in Keogh lower bound calculation
   * @param bandSize size of the Sakoe-Chiba warpping band
   */
  void generateKeoghLU(int warpingBand) const;

  /*************************
   *  Start serialization
   *************************/
  friend class boost::serialization::access;

  template <class A>
  void serialize(A &ar, unsigned) {
    for (int i = 0; i < this->length; i++) {
      ar & data[this->start + i];
    }
  }
  /*************************
   *  End serialization
   *************************/
};

/* For printing */
inline std::ostream &operator<<(std::ostream &os, const TimeSeries &ts) {
    return ts.printData(os);
}

/**
 *  @brief a struct pairing a dist with a time series
 *
 */
struct candidate_time_series_t
{
  TimeSeries data;
  data_t dist;

  bool operator<(const candidate_time_series_t& rhs) const
  {
    if (std::abs(dist - rhs.dist) < EPS)
    {
      if (data.getIndex() == rhs.data.getIndex())
      {
        if (data.getStart() == rhs.data.getStart())
        {
          return data.getLength() < rhs.data.getLength();
        }
        return data.getStart() < rhs.data.getStart();
      }
      return data.getIndex() < rhs.data.getIndex();
    }
    return dist < rhs.dist;
  }
  candidate_time_series_t(const TimeSeries& data, data_t dist) : data(data), dist(dist) {};
  candidate_time_series_t() : data(0), dist(0) {}

  bool operator==(const candidate_time_series_t& rhs) const
  {
    return data == rhs.data && std::abs(dist - rhs.dist) < EPS;
  }
};

} // namespace genex

#endif // TIMESERIES_H


#pragma once
#ifndef SOAR_DATATYPES_HPP
#define SOAR_DATATYPES_HPP

/* C/C++ Includes */
#include <stdint.h>
#include <stdlib.h>

/* Eigen Includes */
#include <Eigen/Eigen>

enum LEDInstructions
{
	//Select which led to apply actions to
	LED_BLUE = (1U << 31),
	LED_YELLOW = (1U << 30),
	LED_RED = (1U << 29),
	LED_GREEN = (1U << 28),

	//Available actions for each led
	LED_STATIC_ON = (1U << 1),
	LED_STATIC_OFF = (1U << 2),
	LED_FLASH_SLOW = (1U << 3),
	LED_FLASH_MED = (1U << 4),
	LED_FLASH_FAST = (1U << 5)
};

struct AHRSData_t
{
	AHRSData_t()
	{
		eulerAngles.setZero();
		accel.setZero();
		gyro.setZero();
		mag.setZero();
	}

	void operator()(Eigen::Vector3f euler_deg, Eigen::Vector3f acceleration_ms2,
		Eigen::Vector3f gyroscope_dps, Eigen::Vector3f magnetometer_g)
	{
		eulerAngles = euler_deg;
		accel = acceleration_ms2;
		gyro = gyroscope_dps;
		mag = magnetometer_g;
	}

	Eigen::Vector3f eulerAngles;	/* [PITCH, ROLL, YAW] (deg) */
	Eigen::Vector3f accel;			/* [X, Y, Z] (m/s^2) */
	Eigen::Vector3f gyro;			/* [X, Y, Z] (dps) */
	Eigen::Vector3f mag;			/* [X, Y, Z] (gauss) */

	const float& pitch() { return this->eulerAngles(0); }
	const float& roll() { return this->eulerAngles(1); }
	const float& yaw() { return this->eulerAngles(2); }

	const float& ax() { return this->accel(0); }
	const float& ay() { return this->accel(1); }
	const float& az() { return this->accel(2); }

	const float& gx() { return this->gyro(0); }
	const float& gy() { return this->gyro(1); }
	const float& gz() { return this->gyro(2); }

	const float& mx() { return this->mag(0); }
	const float& my() { return this->mag(1); }
	const float& mz() { return this->mag(2); }
};

#endif