/* boost random/xoroshiro.hpp header file
 *
 * Copyright degski 2017-2018
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 */

#ifndef BOOST_RANDOM_XOROSHIRO_HPP
#define BOOST_RANDOM_XOROSHIRO_HPP

#include <cstdint>
#include <istream>
#include <ostream>
#include <stdexcept>
#include <vector>
#include <iterator>
#include <algorithm>

#include <boost/config.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/seed_impl.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)

    namespace detail {

        std::uint64_t xoroshiro_integer_hash ( std::uint64_t x );
    }

    /**
    * This is a fixed-increment version of Java 8's SplittableRandom generator
    * See http://dx.doi.org/10.1145/2714064.2660195 and
    * http://docs.oracle.com/javase/8/docs/api/java/util/SplittableRandom.html
    *
    * It is a very fast generator passing BigCrush, and it can be useful if
    * for some reason one absolutely want 64 bits of state;
    *
    * c-code by Sebastiano Vigna: http://xoroshiro.di.unimi.it/splitmix64.c
    */
    class splitmix64 {
        friend class xoroshiro128plus;
        friend class xoshiro256starstar;
        friend class xoroshiro128plusshixo;
        friend class xoroshiro128plusshixostar;
        friend class xoroshiro128plusshixostarshixo;
        friend class xorshift128plus;
        friend class xorshift1024star;
    public:
        typedef std::uint64_t result_type;

        // Required for old Boost.Random concept.
        static const bool has_fixed_range = true;
        static const std::uint64_t default_seed = std::uint64_t { 0x9E3779B97F4A7C15 };

        /**
        * Constructs a @c splitmix64, using the default seed.
        */
        splitmix64 ( )
        {
            seed ( );
        }

        /**
        * Constructs a @c splitmix64, seeding it with @c value.
        */
        BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR ( splitmix64,
            std::uint64_t, value )
        {
            seed ( value );
        }

        /**
        * Constructs a @c splitmix64, seeding it with values
        * produced by a call to @c seq.generate().
        */
        BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR ( splitmix64,
            SeedSeq, seq )
        {
            seed ( seq );
        }

        /**
        * Constructs a @c splitmix64 and seeds it with values taken
        * from the iterator range [first, last) and adjusts first to
        * point to the element after the last one used. If there are
        * not enough elements, throws @c std::invalid_argument.
        *
        * first and last must be input iterators.
        */
        template<class It>
        splitmix64 ( It& first, It last )
        {
            seed ( first, last );
        }

        // compiler-generated copy constructor and assignment operator are fine.

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

        /**
        * Seeds a @c splitmix64 using the supplied value. 'Hashes' @c value
        * using the bijection described by:
        *
        * std::uint64_t integer_hash(std::uint64_t x) {
        *
        *     x = ((x >> 32) ^ x) * 0xDE17C195AA959A81;
        *	   x = ((x >> 32) ^ x) * 0xDE17C195AA959A81;
        *     x = ((x >> 32) ^ x);
        *
        *	   return x;
        * }
        */
        BOOST_RANDOM_DETAIL_ARITHMETIC_SEED ( splitmix64, std::uint64_t, value )
        {
            _s [ 0 ] = detail::xoroshiro_integer_hash ( value );
        }

        /**
        * Seeds a @c splitmix64 using values from a SeedSeq.
        */
        BOOST_RANDOM_DETAIL_SEED_SEQ_SEED ( splitmix64, SeedSeq, seq )
        {
            detail::seed_array_int<64, 1, SeedSeq, std::uint64_t> ( seq, _s );
        }

        /**
        * seeds a @c splitmix64 with values taken from the iterator
        * range [first, last) and adjusts @c first to point to the
        * element after the last one used.  If there are not enough
        * elements, throws @c std::invalid_argument.
        *
        * @c first and @c last must be input iterators.
        */
        template<class It>
        void seed ( It& first, It last )
        {
            detail::fill_array_int<64, 1, It, std::uint64_t> ( first, last, _s );
        }

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

        /**
        * Returns the largest value that the @c splitmix64
        * can produce.
        */
        static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ( )
        {
            return UINT64_MAX;
        }

        /** Returns the next value of the @c splitmix64. */
        std::uint64_t operator()( )
        {
            return hash ( next ( ) );
        }

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

        /** Advances the state of the generator by @c z. */
        void discard ( boost::uintmax_t z )
        {
            // This seems to be the fastest way (release),
            // as opposed to anything more fancy.
            while ( z-- ) {
                next ( );
            }
        }

        friend bool operator==( const splitmix64& x,
            const splitmix64& y )
        {
            return x._s [ 0 ] == y._s [ 0 ];
        }

        friend bool operator!=( const splitmix64& x,
            const splitmix64& y )
        {
            return !( x == y );
        }

        /** Writes a @c splitmix64 to a @c std::ostream. */
        template<class CharT, class Traits>
        friend std::basic_ostream<CharT, Traits>&
            operator<<( std::basic_ostream<CharT, Traits>& os,
                const splitmix64& sm64 )
        {
            os << sm64._s [ 0 ];
            return os;
        }

        /** Reads a @c splitmix64 from a @c std::istream. */
        template<class CharT, class Traits>
        friend std::basic_istream<CharT, Traits>&
            operator >> ( std::basic_istream<CharT, Traits>& is,
                splitmix64& sm64 )
        {
            is >> sm64._s [ 0 ];
            return is;
        }

    private:

        /// \cond show_private

        std::uint64_t next ( )
        {
            return ( _s [ 0 ] += std::uint64_t { 0x9E3779B97F4A7C15 } );
        }

        static std::uint64_t hash ( std::uint64_t z )
        {
            z = ( z ^ ( z >> 30 ) ) * std::uint64_t { 0xBF58476D1CE4E5B9 };
            z = ( z ^ ( z >> 27 ) ) * std::uint64_t { 0x94D049BB133111EB };
            return z ^ ( z >> 31 );
        }

        /// \endcond

        std::uint64_t _s [ 1 ];
    };

namespace detail {

    // const std::uint64_t v = 0x1AEC805299990163, y = 0xCDFB859A3DD0884B;

    std::uint64_t xoroshiro_integer_hash(std::uint64_t x)
    {
        x = ((x >> 32) ^ x) * std::uint64_t { 0x1AEC805299990163 };
        x = ((x >> 32) ^ x);
        return x;
    }

    template<class SeedSeq, std::size_t n>
    void seed_array_non_zero_int(SeedSeq &seq, std::uint64_t (&x)[n])
    {
        std::uint_least32_t storage[2 * n];
        seq.generate(std::begin(storage), std::end(storage));

        std::size_t j = 0;
        for (; j < n; ++j) {
            x[j] = (static_cast<std::uint64_t>(storage[2 * j + 1]) << 32)
                + static_cast<std::uint64_t>(storage [2 * j]);
            if (x[j]) {
                // non-zero seed detected, carry on, without checking...
                ++j;
                for (; j < n; ++j) {
                    x[j] = (static_cast<std::uint64_t>(storage[2 * j + 1]) << 32)
                        + static_cast<std::uint64_t>(storage[2 * j]);
                }
                return;
            }
        }

        // Fix zeros, generating some kind of seed from the
        // SeedSeq, subsequently use this seed for the seeding
        // of boost::random::splitmix64.
        std::uint64_t seed = 0;

        std::vector<int> v;
        v.reserve(seq.size());
        seq.param(std::back_inserter(v));
        int shift = 32;

        for (auto i : v) {
            seed ^= detail::xoroshiro_integer_hash((static_cast<std::uint64_t>(i) << (shift ^= int { 32 })) ^ seed);
        }

        boost::random::splitmix64 gen(seed);
        std::generate(std::begin(x), std::end(x), gen);
    }

    template<class It, std::size_t n>
    void fill_array_non_zero_int(It& first, It last, std::uint64_t (&x)[n])
    {
        if (std::distance(first, last) < (2 * n)) {
            throw(std::invalid_argument("Not enough elements in call to seed."));
        }

        std::size_t j = 0;

        for (; j < n; ++j) {
            x[j] = static_cast<std::uint64_t>(*first);
            ++first;
            x[j] |= static_cast<std::uint64_t>(*first) << 32;
            ++first;
            if (x[j]) {
                ++j;
                for (; j < n; ++j) {
                     x[j] = static_cast<std::uint64_t>(*first);
                     ++first;
                     x[j] |= static_cast<std::uint64_t>(*first) << 32;
                     ++first;
                }
                return;
            }
        }

        // Fix zeros.
        boost::random::splitmix64 gen;
        std::generate(std::begin(x), std::end(x), gen);
    }

} // namespace detail

/**
 * xoroshiro128+
 *
 * xoroshiro128+ (XOR/rotate/shift/rotate) is the successor to xorshift128+.
 *
 * Instead of perpetuating  Marsaglia's tradition  of xorshift as a  basic
 * operation, xoroshiro128+ uses a carefully handcrafted shift/rotate-based
 * linear transformation designed by Sebastiano Vigna in collaboration with
 * David Blackman.
 *
 * It is the fastest full-period generator passing BigCrush without systematic
 * failures, but due to the relatively short period it is acceptable only for
 * applications with a mild amount of parallelism; otherwise, use a
 * xorshift1024* generator.
 *
 * Beside passing BigCrush, this generator passes the PractRand test suite
 * up to (and included)  16TB, with  the exception of  binary rank  tests,
 * which fail due to the lowest bit being an LFSR; all other bits pass all
 * tests. Use a sign test to extract a random Boolean value.
 *
 * The state must be seeded so that it is not everywhere zero.
 *
 * Web-site: http://xoroshiro.di.unimi.it/
 */
class xoroshiro128plus
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
     * Constructs a @c xoroshiro128plus, using the default seed.
     */
    xoroshiro128plus()
    { seed(); }

    /**
     * Constructs a @c xoroshiro128plus, seeding it with @c value.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(xoroshiro128plus,
                                               std::uint64_t, value)
    { seed(value); }

    /**
     * Constructs a @c xoroshiro128plus, seeding it with values
     * produced by a call to @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(xoroshiro128plus,
                                             SeedSeq, seq)
    { seed(seq); }

    /**
     * Constructs a @c xoroshiro128plus and seeds it with values
     * taken from the iterator range [first, last) and adjusts
     * first to point to the element after the last one used.
     * If there are not enough elements, throws @c std::invalid_argument.
     *
     * first and last must be input iterators.
     */
    template<class It>
    xoroshiro128plus(It& first, It last)
    { seed(first, last); }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
     * Calls seed(default_seed)
     */
    void seed()
    { seed(default_seed); }

    /**
     * seeds a @c xoroshiro128plus with splitmix64, as per Sebastiano
     * Vigna's recommendation.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(xoroshiro128plus, std::uint64_t, value)
    {
        std::uint64_t s = value + std::uint64_t { 0x9E3779B97F4A7C15 };
        _s[0] = detail::xoroshiro_integer_hash (s);
        _s[1] = detail::xoroshiro_integer_hash ((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
    }

    /**
     * Seeds a @c xoroshiro128plus using values from a SeedSeq. If a
     * valid seed cannot be generated throws @c std::runtime_error.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(xoroshiro128plus, SeedSeq, seq)
    {
        detail::seed_array_non_zero_int(seq, _s);
        warmup();
    }

    /**
     * Seeds a @c xoroshiro128plus with values taken from the
     * iterator range [first, last) and adjusts @c first to
     * point to the element after the last one used. If there are
     * not enough elements or all the whole input range is zero,
     * throws @c std::invalid_argument.
     *
     * @c first and @c last must be input iterators.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_non_zero_int(first, last, _s);
        warmup();
    }

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

    /**
     * Returns the largest value that the @c xoroshiro128plus
     * can produce.
     */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return UINT64_MAX; }

    /** Returns the next value of the @c xoroshiro128plus. */
    std::uint64_t operator()()
    {
        std::uint64_t r = _s[0] + _s[1];
        next();
        return r;
    }

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

    /** Advances the state of the generator by @c z. */
    void discard(std::uintmax_t z)
    {
        while (z--) {
            next();
        }
    }

    /**
     * This is a jump function for the generator. It is equivalent
     * to calling @c discard(2^64) @c z times; it can be used to
     * generate 2^64 non-overlapping subsequences for parallel
     * computations.
     */
    void jump(std::uintmax_t z = 1)
    {
        while(z--) {
            std::uint64_t s0 = 0, s1 = 0;
            for (std::size_t b = 0; b < 64; ++b ) {
                if (std::uint64_t { 0xBEAC0467EBA5FACB } & std::uint64_t { 1 } << b) {
                    s0 ^= _s[0], s1 ^= _s[1];
                }
                next();
            }
            for ( std::size_t b = 0; b < 64; ++b ) {
                if (std::uint64_t { 0xD86B048B86AA9922 } & std::uint64_t { 1 } << b) {
                    s0 ^= _s[0], s1 ^= _s[1];
                }
                next();
            }
            _s[0] = s0, _s[1] = s1;
        }
    }

    friend bool operator==(const xoroshiro128plus& x,
                           const xoroshiro128plus& y)
    { return x._s[0] == y._s[0] && x._s[1] == y._s[1]; }

    friend bool operator!=(const xoroshiro128plus& x,
                           const xoroshiro128plus& y)
    { return !(x == y); }

    /** Writes a @c xoroshiro128plus to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const xoroshiro128plus& xoro)
    {
        os << xoro._s[0] << ' ' << xoro._s[1];
        return os;
    }

    /** Reads a @c xoroshiro128plus from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is,
               xoroshiro128plus& xoro)
    {
        is >> xoro._s[0] >> std::ws >> xoro._s[1];
        return is;
    }

private:

    /// \cond show_private

    // Rotate left, use of intrinsic shows no speed-up. */
    static std::uint64_t rotl(const std::uint64_t x, const int k)
    { return (x << k) | (x >> (64 - k)); }

    /** Advance the state by 1 step. */
    void next()
    {
        _s[1] ^= _s [0];
        _s[0] = rotl(_s[0], 55);
        _s[0] ^= _s[1];
        _s[0] ^= _s[1] << 14;
        _s[1] = rotl(_s[1], 36);
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup()
    {
        discard(8);
    }

    /// \endcond

    std::uint64_t _s[2];
};

/**
 * From the source implmentation, expressing the opinions of the original
 * authors:
 *
 * Written in 2018 by David Blackman and Sebastiano Vigna (vigna@acm.org)
 *
 * To the extent possible under law, the author has dedicated all copyright
 * and related and neighboring rights to this software to the public domain
 * worldwide. This software is distributed without any warranty.
 *
 * See <http:*creativecommons.org/publicdomain/zero/1.0/>.
 *
 * This is xoshiro256starstar 1.0, our all-purpose, rock-solid generator. It
 * has excellent (sub-ns) speed, a state (256 bits) that is large enough for
 * any parallel application, and it passes all tests we are aware of.
 *
 * For generating just floating-point numbers, xoshiro256plus is even faster.
 *
 * The state must be seeded so that it is not everywhere zero. If you have
 * a 64-bit seed, we suggest to seed a splitmix64 generator and use its
 * output to fill s.
 */

class xoshiro256starstar
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
     * Constructs a @c xoshiro256starstar, using the default seed.
     */
    xoshiro256starstar()
    { seed(); }

    /**
     * Constructs a @c xoshiro256starstar, seeding it with @c value.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(xoshiro256starstar,
                                               std::uint64_t, value)
    { seed(value); }

    /**
     * Constructs a @c xoshiro256starstar, seeding it with values
     * produced by a call to @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(xoshiro256starstar,
                                             SeedSeq, seq)
    { seed(seq); }

    /**
     * Constructs a @c xoshiro256starstar and seeds it with values
     * taken from the iterator range [first, last) and adjusts
     * first to point to the element after the last one used.
     * If there are not enough elements, throws @c std::invalid_argument.
     *
     * first and last must be input iterators.
     */
    template<class It>
    xoshiro256starstar(It& first, It last)
    { seed(first, last); }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
     * Calls seed(default_seed)
     */
    void seed()
    { seed(default_seed); }

    /**
     * seeds a @c xoshiro256starstar with splitmix64, as per Sebastiano
     * Vigna's recommendation.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(xoshiro256starstar, std::uint64_t, value)
    {
        std::uint64_t s = value + std::uint64_t ( 0x9E3779B97F4A7C15 );
        _s[0] = detail::xoroshiro_integer_hash(s);
        _s[1] = detail::xoroshiro_integer_hash((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
        _s[2] = detail::xoroshiro_integer_hash((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
        _s[3] = detail::xoroshiro_integer_hash((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
    }

    /**
     * Seeds a @c xoshiro256starstar using values from a SeedSeq. If a
     * valid seed cannot be generated throws @c std::runtime_error.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(xoshiro256starstar, SeedSeq, seq)
    {
        detail::seed_array_non_zero_int(seq, _s);
        warmup();
    }

    /**
     * Seeds a @c xoshiro256starstar with values taken from the
     * iterator range [first, last) and adjusts @c first to
     * point to the element after the last one used. If there are
     * not enough elements or all the whole input range is zero,
     * throws @c std::invalid_argument.
     *
     * @c first and @c last must be input iterators.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_non_zero_int(first, last, _s);
        warmup();
    }

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

    /**
     * Returns the largest value that the @c xoshiro256starstar
     * can produce.
     */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return UINT64_MAX; }

    /** Returns the next value of the @c xoshiro256starstar. */
    std::uint64_t operator()()
    {
        const std::uint64_t r = rotl ( _s[1] * 5, 7 ) * 9;
        next();
        return r;
    }

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

    /** Advances the state of the generator by @c z. */
    void discard(std::uintmax_t z)
    {
        while (z--) {
            next();
        }
    }

    /**
     * This is the jump function for @c xoshiro256starstar. It is equivalent
     * to 2^128 calls to next() @c z times; it can be used to generate
     * 2^128 non-overlapping subsequences for parallel computations.
     */
    void jump(std::uintmax_t z = 1)
    {
        static const std::uint64_t JUMP[4] = { 0x180EC6D33CFD0ABA, 0xD5A61266F0C9392C, 0xA9582618E03FC9AA, 0x39ABDC4529B1661C };
        while(z--) {
            std::uint64_t s0 = 0, s1 = 0, s2 = 0, s3 = 0;
            for ( int i = 0; i < sizeof JUMP / sizeof *JUMP; i++ )
                for ( int b = 0; b < 64; ++b ) {
                    if ( JUMP[i] & std::uint64_t(1) << b ) {
                        s0 ^= _s[0];
                        s1 ^= _s[1];
                        s2 ^= _s[2];
                        s3 ^= _s[3];
                    }
                    next();
                }

            _s[0] = s0;
            _s[1] = s1;
            _s[2] = s2;
            _s[3] = s3;
        }
    }

    /**
     * This is the long-jump function for @c xoshiro256starstar. It is
     * equivalent to 2^192 calls to next(); it can be used to generate
     * 2^64 starting points, from each of which jump() will generate
     * 2^64 non-overlapping subsequences for parallel distributed
     * computations.
     */
    void long_jump()
    {
        static const std::uint64_t LONG_JUMP[4] = { 0x76E15D3EFEFDCBBF, 0xC5004E441C522FB3, 0x77710069854EE241, 0x39109BB02ACBE635 };
        std::uint64_t s0 = 0, s1 = 0, s2 = 0, s3 = 0;
        for ( std::size_t i = 0; i < sizeof LONG_JUMP / sizeof *LONG_JUMP; ++i )
            for ( std::size_t b = 0; b < std::size_t ( 64 ); ++b ) {
                if ( LONG_JUMP[i] & std::uint64_t ( 1 ) << b ) {
                    s0 ^= _s[0];
                    s1 ^= _s[1];
                    s2 ^= _s[2];
                    s3 ^= _s[3];
                }
                next();
            }

        _s[0] = s0;
        _s[1] = s1;
        _s[2] = s2;
        _s[3] = s3;
    }

    friend bool operator==(const xoshiro256starstar& x,
                           const xoshiro256starstar& y)
    { return x._s[0] == y._s[0] && x._s[1] == y._s[1] && x._s [2] == y._s [2] && x._s [3] == y._s [3]; }

    friend bool operator!=(const xoshiro256starstar& x,
                           const xoshiro256starstar& y)
    { return !(x == y); }

    /** Writes a @c xoshiro256starstar to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const xoshiro256starstar& xoro)
    {
        os << xoro._s[0] << ' ' << xoro._s[1] << ' ' << xoro._s[2] << ' ' << xoro._s[3];
        return os;
    }

    /** Reads a @c xoshiro256starstar from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is,
               xoshiro256starstar& xoro)
    {
        is >> xoro._s[0] >> std::ws >> xoro._s[1] >> std::ws >> xoro._s[2] >> std::ws >> xoro._s[3];
        return is;
    }

private:

    /// \cond show_private

    // Rotate left, use of intrinsic shows no speed-up. */
    static std::uint64_t rotl(const std::uint64_t x, const int k)
    { return (x << k) | (x >> (64 - k)); }

    /** Advance the state by 1 step. */
    void next()
    {
        const std::uint64_t t = _s[1] << 17;

        _s[2] ^= _s[0];
        _s[3] ^= _s[1];
        _s[1] ^= _s[2];
        _s[0] ^= _s[3];

        _s[2] ^= t;

        _s[3] = rotl ( _s[3], 45 );
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup()
    {
        discard(8);
    }

    /// \endcond

    std::uint64_t _s[4];
};

/**
 * From the source implmentation, expressing the opinions of the original
 * authors:
 *
 *  Written in 2018 by David Blackman and Sebastiano Vigna (vigna@acm.org)
 *
 * To the extent possible under law, the author has dedicated all copyright
 * and related and neighboring rights to this software to the public domain
 * worldwide. This software is distributed without any warranty.
 *
 * See <http://creativecommons.org/publicdomain/zero/1.0/>.
 *
 *
 * This is xoshiro256plus 1.0, our best and fastest generator for floating-point
 * numbers. We suggest to use its upper bits for floating-point
 * generation, as it is slightly faster than xoshiro256starstar. It passes all
 * tests we are aware of except for the lowest three bits, which might
 * fail linearity tests (and just those), so if low linear complexity is
 * not considered an issue (as it is usually the case) it can be used to
 * generate 64-bit outputs, too.
 *
 * We suggest to use a sign test to extract a random Boolean value, and
 * right shifts to extract subsets of bits.
 *
 * The state must be seeded so that it is not everywhere zero. If you have
 * a 64-bit seed, we suggest to seed a splitmix64 generator and use its
 * output to fill s.
 */

class xoshiro256plus
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
     * Constructs a @c xoshiro256plus, using the default seed.
     */
    xoshiro256plus()
    { seed(); }

    /**
     * Constructs a @c xoshiro256plus, seeding it with @c value.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(xoshiro256plus,
                                               std::uint64_t, value)
    { seed(value); }

    /**
     * Constructs a @c xoshiro256plus, seeding it with values
     * produced by a call to @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(xoshiro256plus,
                                             SeedSeq, seq)
    { seed(seq); }

    /**
     * Constructs a @c xoshiro256plus and seeds it with values
     * taken from the iterator range [first, last) and adjusts
     * first to point to the element after the last one used.
     * If there are not enough elements, throws @c std::invalid_argument.
     *
     * first and last must be input iterators.
     */
    template<class It>
    xoshiro256plus(It& first, It last)
    { seed(first, last); }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
     * Calls seed(default_seed)
     */
    void seed()
    { seed(default_seed); }

    /**
     * seeds a @c xoshiro256plus with splitmix64, as per Sebastiano
     * Vigna's recommendation.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(xoshiro256plus, std::uint64_t, value)
    {
        std::uint64_t s = value + std::uint64_t ( 0x9E3779B97F4A7C15 );
        _s[0] = detail::xoroshiro_integer_hash(s);
        _s[1] = detail::xoroshiro_integer_hash((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
        _s[2] = detail::xoroshiro_integer_hash((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
        _s[3] = detail::xoroshiro_integer_hash((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
    }

    /**
     * Seeds a @c xoshiro256plus using values from a SeedSeq. If a
     * valid seed cannot be generated throws @c std::runtime_error.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(xoshiro256plus, SeedSeq, seq)
    {
        detail::seed_array_non_zero_int(seq, _s);
        warmup();
    }

    /**
     * Seeds a @c xoshiro256plus with values taken from the
     * iterator range [first, last) and adjusts @c first to
     * point to the element after the last one used. If there are
     * not enough elements or all the whole input range is zero,
     * throws @c std::invalid_argument.
     *
     * @c first and @c last must be input iterators.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_non_zero_int(first, last, _s);
        warmup();
    }

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

    /**
     * Returns the largest value that the @c xoshiro256plus
     * can produce.
     */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return UINT64_MAX; }

    /** Returns the next value of the @c xoshiro256plus. */
    std::uint64_t operator()()
    {
        const std::uint64_t r = _s[0] + _s[3];
        next();
        return r;
    }

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

    /** Advances the state of the generator by @c z. */
    void discard(std::uintmax_t z)
    {
        while (z--) {
            next();
        }
    }

    /**
     * This is the jump function for @c xoshiro256plus. It is equivalent
     * to 2^128 calls to next() @c z times; it can be used to generate
     * 2^128 non-overlapping subsequences for parallel computations.
     */
    void jump(std::uintmax_t z = 1)
    {
        static const std::uint64_t JUMP[4] = { 0x180EC6D33CFD0ABA, 0xD5A61266F0C9392C, 0xA9582618E03FC9AA, 0x39ABDC4529B1661C };
        while(z--) {
            std::uint64_t s0 = 0, s1 = 0, s2 = 0, s3 = 0;
            for(std::size_t i = 0; i < sizeof JUMP / sizeof *JUMP; ++i)
                for(std::size_t b = 0; b < std::size_t(64); ++b) {
                    if (JUMP[i] & std::uint64_t(1) << b) {
                        s0 ^= _s[0];
                        s1 ^= _s[1];
                        s2 ^= _s[2];
                        s3 ^= _s[3];
                    }
                    next();
                }

            _s[0] = s0;
            _s[1] = s1;
            _s[2] = s2;
            _s[3] = s3;
        }
    }

    /**
     * This is the long-jump function for @c xoshiro256plus. It is
     * equivalent to 2^192 calls to next(); it can be used to generate
     * 2^64 starting points, from each of which jump() will generate
     * 2^64 non-overlapping subsequences for parallel distributed
     * computations.
     */
    void long_jump()
    {
        static const std::uint64_t LONG_JUMP[] = { 0x76E15D3EFEFDCBBF, 0xC5004E441C522FB3, 0x77710069854EE241, 0x39109BB02ACBE635 };

        std::uint64_t s0 = 0, s1 = 0, s2 = 0, s3 = 0;
        for(std::size_t i = 0; i < sizeof LONG_JUMP / sizeof *LONG_JUMP; ++i)
            for(std::size_t b = 0; b < std::size_t(64); ++b) {
                if (LONG_JUMP[i] & std::uint64_t(1) << b) {
                    s0 ^= _s[0];
                    s1 ^= _s[1];
                    s2 ^= _s[2];
                    s3 ^= _s[3];
                }
                next();
            }

        _s[0] = s0;
        _s[1] = s1;
        _s[2] = s2;
        _s[3] = s3;
    }

    friend bool operator==(const xoshiro256plus& x,
                           const xoshiro256plus& y)
    { return x._s[0] == y._s[0] && x._s[1] == y._s[1] && x._s [2] == y._s [2] && x._s [3] == y._s [3]; }

    friend bool operator!=(const xoshiro256plus& x,
                           const xoshiro256plus& y)
    { return !(x == y); }

    /** Writes a @c xoshiro256plus to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const xoshiro256plus& xoro)
    {
        os << xoro._s[0] << ' ' << xoro._s[1] << ' ' << xoro._s[2] << ' ' << xoro._s[3];
        return os;
    }

    /** Reads a @c xoshiro256plus from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is,
               xoshiro256plus& xoro)
    {
        is >> xoro._s[0] >> std::ws >> xoro._s[1] >> std::ws >> xoro._s[2] >> std::ws >> xoro._s[3];
        return is;
    }

private:

    /// \cond show_private

    // Rotate left, use of intrinsic shows no speed-up. */
    static std::uint64_t rotl(const std::uint64_t x, const int k)
    { return (x << k) | (x >> (64 - k)); }

    /** Advance the state by 1 step. */
    void next()
    {
        const uint64_t t = _s[1] << 17;

        _s[2] ^= _s[0];
        _s[3] ^= _s[1];
        _s[1] ^= _s[2];
        _s[0] ^= _s[3];

        _s[2] ^= t;

        _s[3] = rotl(_s[3], 45);
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup()
    {
        discard(8);
    }

    /// \endcond

    std::uint64_t _s[4];
};

class xoroshiro128plusshixo
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
    * Constructs a @c xoroshiro128plusshixo, using the default seed.
    */
    xoroshiro128plusshixo ( )
    {
        seed ( );
    }

    /**
    * Constructs a @c xoroshiro128plusshixo, seeding it with @c value.
    */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR ( xoroshiro128plusshixo,
        std::uint64_t, value )
    {
        seed ( value );
    }

    /**
    * Constructs a @c xoroshiro128plusshixo, seeding it with values
    * produced by a call to @c seq.generate().
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR ( xoroshiro128plusshixo,
        SeedSeq, seq )
    {
        seed ( seq );
    }

    /**
    * Constructs a @c xoroshiro128plusshixo and seeds it with values
    * taken from the iterator range [first, last) and adjusts
    * first to point to the element after the last one used.
    * If there are not enough elements, throws @c std::invalid_argument.
    *
    * first and last must be input iterators.
    */
    template<class It>
    xoroshiro128plusshixo ( It& first, It last )
    {
        seed ( first, last );
    }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
    * Calls seed(default_seed)
    */
    void seed ( )
    {
        seed ( default_seed );
    }

    /**
    * seeds a @c xoroshiro128plusshixo with splitmix64, as per Sebastiano
    * Vigna's recommendation.
    */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED ( xoroshiro128plusshixo, std::uint64_t, value )
    {
        std::uint64_t s = value + std::uint64_t { 0x9E3779B97F4A7C15 };
        _s [ 0 ] = detail::xoroshiro_integer_hash ( s );
        _s [ 1 ] = detail::xoroshiro_integer_hash ( ( s += std::uint64_t { 0x9E3779B97F4A7C15 } ) );
    }

    /**
    * Seeds a @c xoroshiro128plusshixo using values from a SeedSeq. If a
    * valid seed cannot be generated throws @c std::runtime_error.
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED ( xoroshiro128plusshixo, SeedSeq, seq )
    {
        detail::seed_array_non_zero_int ( seq, _s );
        warmup ( );
    }

    /**
    * Seeds a @c xoroshiro128plusshixo with values taken from the
    * iterator range [first, last) and adjusts @c first to
    * point to the element after the last one used. If there are
    * not enough elements or all the whole input range is zero,
    * throws @c std::invalid_argument.
    *
    * @c first and @c last must be input iterators.
    */
    template<class It>
    void seed ( It& first, It last )
    {
        detail::fill_array_non_zero_int ( first, last, _s );
        warmup ( );
    }

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

    /**
    * Returns the largest value that the @c xoroshiro128plusshixo
    * can produce.
    */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ( )
    {
        return UINT64_MAX;
    }

    /** Returns the next value of the @c xoroshiro128plusshixo. */
    std::uint64_t operator()( )
    {
        std::uint64_t r = _s[0] + _s [1];
        next ( );
        return ( r >> 32 ) ^ r;
    }

    /** Fills a range with random values. */
    template<class Iter>
    void generate ( Iter first, Iter last ) {

        detail::generate_from_int ( *this, first, last );

        /*

        while ( first != last ) {

            std::uint64_t tmp = _s [ 0 ] + _s [ 1 ];
            tmp ^= tmp >> 32;

            next ( );

            *first = tmp;
            ++first;
        }

        */
    }

    /** Advances the state of the generator by @c z. */
    void discard ( std::uintmax_t z )
    {
        while ( z-- ) {
            next ( );
        }
    }

    /**
    * This is a jump function for the generator. It is equivalent
    * to calling @c discard(2^64) @c z times; it can be used to
    * generate 2^64 non-overlapping subsequences for parallel
    * computations.
    */
    void jump ( std::uintmax_t z = 1 )
    {
        while ( z-- ) {
            std::uint64_t s0 = 0, s1 = 0;
            for ( std::size_t b = 0; b < 64; ++b ) {
                if ( std::uint64_t { 0xBEAC0467EBA5FACB } &std::uint64_t { 1 } << b ) {
                    s0 ^= _s [ 0 ], s1 ^= _s [ 1 ];
                }
                next ( );
            }
            for ( std::size_t b = 0; b < 64; ++b ) {
                if ( std::uint64_t { 0xD86B048B86AA9922 } &std::uint64_t { 1 } << b ) {
                    s0 ^= _s [ 0 ], s1 ^= _s [ 1 ];
                }
                next ( );
            }
            _s [ 0 ] = s0, _s [ 1 ] = s1;
        }
    }

    friend bool operator==( const xoroshiro128plusshixo& x,
        const xoroshiro128plusshixo& y )
    {
        return x._s [ 0 ] == y._s [ 0 ] && x._s [ 1 ] == y._s [ 1 ];
    }

    friend bool operator!=( const xoroshiro128plusshixo& x,
        const xoroshiro128plusshixo& y )
    {
        return !( x == y );
    }

    /** Writes a @c xoroshiro128plusshixo to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT, Traits>&
        operator<<( std::basic_ostream<CharT, Traits>& os,
            const xoroshiro128plusshixo& xoro )
    {
        os << xoro._s [ 0 ] << ' ' << xoro._s [ 1 ];
        return os;
    }

    /** Reads a @c xoroshiro128plusshixo from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT, Traits>&
        operator>>( std::basic_istream<CharT, Traits>& is,
            xoroshiro128plusshixo& xoro )
    {
        is >> xoro._s [ 0 ] >> std::ws >> xoro._s [ 1 ];
        return is;
    }

private:

    /// \cond show_private

    // Rotate left, use of intrinsic shows no speed-up. */
    static std::uint64_t rotl ( const std::uint64_t x, const int k )
    {
        return ( x << k ) | ( x >> ( 64 - k ) );
    }

    /** Advance the state by 1 step. */
    void next ( )
    {
        _s [ 1 ] ^= _s [ 0 ];
        _s [ 0 ] = rotl ( _s [ 0 ], 55 );
        _s [ 0 ] ^= _s [ 1 ];
        _s [ 0 ] ^= _s [ 1 ] << 14;
        _s [ 1 ] = rotl ( _s [ 1 ], 36 );
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup ( )
    {
        discard ( 8 );
    }

    /// \endcond

    std::uint64_t _s [ 2 ];
};

class xoroshiro128plusshixostar
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
    * Constructs a @c xoroshiro128plusshixostar, using the default seed.
    */
    xoroshiro128plusshixostar ( )
    {
        seed ( );
    }

    /**
    * Constructs a @c xoroshiro128plusshixostar, seeding it with @c value.
    */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR ( xoroshiro128plusshixostar,
        std::uint64_t, value )
    {
        seed ( value );
    }

    /**
    * Constructs a @c xoroshiro128plusshixostar, seeding it with values
    * produced by a call to @c seq.generate().
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR ( xoroshiro128plusshixostar,
        SeedSeq, seq )
    {
        seed ( seq );
    }

    /**
    * Constructs a @c xoroshiro128plusshixostar and seeds it with values
    * taken from the iterator range [first, last) and adjusts
    * first to point to the element after the last one used.
    * If there are not enough elements, throws @c std::invalid_argument.
    *
    * first and last must be input iterators.
    */
    template<class It>
    xoroshiro128plusshixostar ( It& first, It last )
    {
        seed ( first, last );
    }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
    * Calls seed(default_seed)
    */
    void seed ( )
    {
        seed ( default_seed );
    }

    /**
    * seeds a @c xoroshiro128plusshixostar with splitmix64, as per Sebastiano
    * Vigna's recommendation.
    */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED ( xoroshiro128plusshixostar, std::uint64_t, value )
    {
        std::uint64_t s = value + std::uint64_t { 0x9E3779B97F4A7C15 };
        _s [ 0 ] = detail::xoroshiro_integer_hash ( s );
        _s [ 1 ] = detail::xoroshiro_integer_hash ( ( s += std::uint64_t { 0x9E3779B97F4A7C15 } ) );
    }

    /**
    * Seeds a @c xoroshiro128plusshixostar using values from a SeedSeq. If a
    * valid seed cannot be generated throws @c std::runtime_error.
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED ( xoroshiro128plusshixostar, SeedSeq, seq )
    {
        detail::seed_array_non_zero_int ( seq, _s );
        warmup ( );
    }

    /**
    * Seeds a @c xoroshiro128plusshixostar with values taken from the
    * iterator range [first, last) and adjusts @c first to
    * point to the element after the last one used. If there are
    * not enough elements or all the whole input range is zero,
    * throws @c std::invalid_argument.
    *
    * @c first and @c last must be input iterators.
    */
    template<class It>
    void seed ( It& first, It last )
    {
        detail::fill_array_non_zero_int ( first, last, _s );
        warmup ( );
    }

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

    /**
    * Returns the largest value that the @c xoroshiro128plusshixostar
    * can produce.
    */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ( )
    {
        return UINT64_MAX;
    }

    /** Returns the next value of the @c xoroshiro128plusshixostar. */
    std::uint64_t operator()( )
    {
        std::uint64_t r = _s [ 0 ] + _s [ 1 ];
        next ( );
        return ((r >> 32) ^ r) * std::uint64_t { 0x1AEC805299990163 };
    }

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

    /** Advances the state of the generator by @c z. */
    void discard ( std::uintmax_t z )
    {
        while ( z-- ) {
            next ( );
        }
    }

    /**
    * This is a jump function for the generator. It is equivalent
    * to calling @c discard(2^64) @c z times; it can be used to
    * generate 2^64 non-overlapping subsequences for parallel
    * computations.
    */
    void jump ( std::uintmax_t z = 1 )
    {
        while ( z-- ) {
            std::uint64_t s0 = 0, s1 = 0;
            for ( std::size_t b = 0; b < 64; ++b ) {
                if ( std::uint64_t { 0xBEAC0467EBA5FACB } &std::uint64_t { 1 } << b ) {
                    s0 ^= _s [ 0 ], s1 ^= _s [ 1 ];
                }
                next ( );
            }
            for ( std::size_t b = 0; b < 64; ++b ) {
                if ( std::uint64_t { 0xD86B048B86AA9922 } &std::uint64_t { 1 } << b ) {
                    s0 ^= _s [ 0 ], s1 ^= _s [ 1 ];
                }
                next ( );
            }
            _s [ 0 ] = s0, _s [ 1 ] = s1;
        }
    }

    friend bool operator==( const xoroshiro128plusshixostar& x,
        const xoroshiro128plusshixostar& y )
    {
        return x._s [ 0 ] == y._s [ 0 ] && x._s [ 1 ] == y._s [ 1 ];
    }

    friend bool operator!=( const xoroshiro128plusshixostar& x,
        const xoroshiro128plusshixostar& y )
    {
        return !( x == y );
    }

    /** Writes a @c xoroshiro128plusshixostar to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT, Traits>&
        operator<<( std::basic_ostream<CharT, Traits>& os,
            const xoroshiro128plusshixostar& xoro )
    {
        os << xoro._s [ 0 ] << ' ' << xoro._s [ 1 ];
        return os;
    }

    /** Reads a @c xoroshiro128plusshixostar from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT, Traits>&
        operator>>( std::basic_istream<CharT, Traits>& is,
            xoroshiro128plusshixostar& xoro )
    {
        is >> xoro._s [ 0 ] >> std::ws >> xoro._s [ 1 ];
        return is;
    }

private:

    /// \cond show_private

    // Rotate left, use of intrinsic shows no speed-up. */
    static std::uint64_t rotl ( const std::uint64_t x, const int k )
    {
        return ( x << k ) | ( x >> ( 64 - k ) );
    }

    /** Advance the state by 1 step. */
    void next ( )
    {
        _s [ 1 ] ^= _s [ 0 ];
        _s [ 0 ] = rotl ( _s [ 0 ], 55 );
        _s [ 0 ] ^= _s [ 1 ];
        _s [ 0 ] ^= _s [ 1 ] << 14;
        _s [ 1 ] = rotl ( _s [ 1 ], 36 );
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup ( )
    {
        discard ( 8 );
    }

    /// \endcond

    std::uint64_t _s [2];
};

class xoroshiro128plusshixostarshixo
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
    * Constructs a @c xoroshiro128plusshixostarshixo, using the default seed.
    */
    xoroshiro128plusshixostarshixo ( )
    {
        seed ( );
    }

    /**
    * Constructs a @c xoroshiro128plusshixostarshixo, seeding it with @c value.
    */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR ( xoroshiro128plusshixostarshixo,
        std::uint64_t, value )
    {
        seed ( value );
    }

    /**
    * Constructs a @c xoroshiro128plusshixostarshixo, seeding it with values
    * produced by a call to @c seq.generate().
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR ( xoroshiro128plusshixostarshixo,
        SeedSeq, seq )
    {
        seed ( seq );
    }

    /**
    * Constructs a @c xoroshiro128plusshixostarshixo and seeds it with values
    * taken from the iterator range [first, last) and adjusts
    * first to point to the element after the last one used.
    * If there are not enough elements, throws @c std::invalid_argument.
    *
    * first and last must be input iterators.
    */
    template<class It>
    xoroshiro128plusshixostarshixo ( It& first, It last )
    {
        seed ( first, last );
    }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
    * Calls seed(default_seed)
    */
    void seed ( )
    {
        seed ( default_seed );
    }

    /**
    * seeds a @c xoroshiro128plusshixostarshixo with splitmix64, as per Sebastiano
    * Vigna's recommendation.
    */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED ( xoroshiro128plusshixostarshixo, std::uint64_t, value )
    {
        std::uint64_t s = value + std::uint64_t { 0x9E3779B97F4A7C15 };
        _s [ 0 ] = detail::xoroshiro_integer_hash ( s );
        _s [ 1 ] = detail::xoroshiro_integer_hash ( ( s += std::uint64_t { 0x9E3779B97F4A7C15 } ) );
    }

    /**
    * Seeds a @c xoroshiro128plusshixostarshixo using values from a SeedSeq. If a
    * valid seed cannot be generated throws @c std::runtime_error.
    */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED ( xoroshiro128plusshixostarshixo, SeedSeq, seq )
    {
        detail::seed_array_non_zero_int ( seq, _s );
        warmup ( );
    }

    /**
    * Seeds a @c xoroshiro128plusshixostarshixo with values taken from the
    * iterator range [first, last) and adjusts @c first to
    * point to the element after the last one used. If there are
    * not enough elements or all the whole input range is zero,
    * throws @c std::invalid_argument.
    *
    * @c first and @c last must be input iterators.
    */
    template<class It>
    void seed ( It& first, It last )
    {
        detail::fill_array_non_zero_int ( first, last, _s );
        warmup ( );
    }

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

    /**
    * Returns the largest value that the @c xoroshiro128plusshixostarshixo
    * can produce.
    */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ( )
    {
        return UINT64_MAX;
    }

    /** Returns the next value of the @c xoroshiro128plusshixostarshixo. */
    std::uint64_t operator()( )
    {
        std::uint64_t r = _s [ 0 ] + _s [ 1 ];
        next ( );
        r = ( ( r >> 32 ) ^ r ) * std::uint64_t { 0x1AEC805299990163 };
        return ( r >> 32 ) ^ r;
    }

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

    /** Advances the state of the generator by @c z. */
    void discard ( std::uintmax_t z )
    {
        while ( z-- ) {
            next ( );
        }
    }

    /**
    * This is a jump function for the generator. It is equivalent
    * to calling @c discard(2^64) @c z times; it can be used to
    * generate 2^64 non-overlapping subsequences for parallel
    * computations.
    */
    void jump ( std::uintmax_t z = 1 )
    {
        while ( z-- ) {
            std::uint64_t s0 = 0, s1 = 0;
            for ( std::size_t b = 0; b < 64; ++b ) {
                if ( std::uint64_t { 0xBEAC0467EBA5FACB } &std::uint64_t { 1 } << b ) {
                    s0 ^= _s [ 0 ], s1 ^= _s [ 1 ];
                }
                next ( );
            }
            for ( std::size_t b = 0; b < 64; ++b ) {
                if ( std::uint64_t { 0xD86B048B86AA9922 } &std::uint64_t { 1 } << b ) {
                    s0 ^= _s [ 0 ], s1 ^= _s [ 1 ];
                }
                next ( );
            }
            _s [ 0 ] = s0, _s [ 1 ] = s1;
        }
    }

    friend bool operator==( const xoroshiro128plusshixostarshixo& x,
        const xoroshiro128plusshixostarshixo& y )
    {
        return x._s [ 0 ] == y._s [ 0 ] && x._s [ 1 ] == y._s [ 1 ];
    }

    friend bool operator!=( const xoroshiro128plusshixostarshixo& x,
        const xoroshiro128plusshixostarshixo& y )
    {
        return !( x == y );
    }

    /** Writes a @c xoroshiro128plusshixostarshixo to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT, Traits>&
        operator<<( std::basic_ostream<CharT, Traits>& os,
            const xoroshiro128plusshixostarshixo& xoro )
    {
        os << xoro._s [ 0 ] << ' ' << xoro._s [ 1 ];
        return os;
    }

    /** Reads a @c xoroshiro128plusshixostarshixo from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT, Traits>&
        operator>>( std::basic_istream<CharT, Traits>& is,
            xoroshiro128plusshixostarshixo& xoro )
    {
        is >> xoro._s [ 0 ] >> std::ws >> xoro._s [ 1 ];
        return is;
    }

private:

    /// \cond show_private

    // Rotate left, use of intrinsic shows no speed-up. */
    static std::uint64_t rotl ( const std::uint64_t x, const int k )
    {
        return ( x << k ) | ( x >> ( 64 - k ) );
    }

    /** Advance the state by 1 step. */
    void next ( )
    {
        _s [ 1 ] ^= _s [ 0 ];
        _s [ 0 ] = rotl ( _s [ 0 ], 55 );
        _s [ 0 ] ^= _s [ 1 ];
        _s [ 0 ] ^= _s [ 1 ] << 14;
        _s [ 1 ] = rotl ( _s [ 1 ], 36 );
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup ( )
    {
        discard ( 8 );
    }

    /// \endcond

    std::uint64_t _s [ 2 ];
};

/**
 * xorshift128+
 *
 * This generator has been replaced by xoroshiro128+, which is
 * significantly faster and has better statistical properties.
 *
 * Due to the relatively short period it is acceptable only for
 * applications with a mild amount of parallelism; otherwise, use a
 * xorshift1024* generator.
 *
 * The lowest bit of this generator is an LSFR, and thus it is
 * slightly less random than the other bits. Use a sign test to
 * extract a random Boolean value.
 *
 * The state must be seeded so that it is not everywhere zero.
 *
 * Web-site: http://xoroshiro.di.unimi.it/
 */
class xorshift128plus
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
     * Constructs a @c xorshift128plus, using the default seed.
     */
    xorshift128plus()
    { seed(); }

    /**
     * Constructs a @c xorshift128plus, seeding it with @c value.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(xorshift128plus,
                                               std::uint64_t, value)
    { seed(value); }

    /**
     * Constructs a @c xorshift128plus, seeding it with values
     * produced by a call to @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(xorshift128plus,
                                             SeedSeq, seq)
    { seed(seq); }

    /**
     * Constructs a @c xorshift128plus and seeds it with values
     * taken from the iterator range [first, last) and adjusts
     * first to point to the element after the last one used.
     * If there are not enough elements, throws @c std::invalid_argument.
     *
     * first and last must be input iterators.
     */
    template<class It>
    xorshift128plus(It& first, It last)
    { seed(first, last); }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
     * Calls seed(default_seed)
     */
    void seed()
    { seed(default_seed); }

    /**
     * seeds a @c xorshift128plus with splitmix64, as per Sebastiano
     * Vigna's recommendation.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(xorshift128plus, std::uint64_t, value)
    {
        std::uint64_t s = value + std::uint64_t { 0x9E3779B97F4A7C15 };
        _s[0] = detail::xoroshiro_integer_hash(s);
        _s[1] = detail::xoroshiro_integer_hash((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
    }

    /**
     * Seeds a @c xorshift128plus using values from a SeedSeq. If a
     * valid seed cannot be generated throws @c std::runtime_error.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(xorshift128plus, SeedSeq, seq)
    {
        detail::seed_array_non_zero_int(seq, _s);
        warmup();
    }

    /**
     * Seeds a @c xorshift128plus with values taken from the
     * iterator range [first, last) and adjusts @c first to
     * point to the element after the last one used. If there are
     * not enough elements or all the whole input range is zero,
     * throws @c std::invalid_argument.
     *
     * @c first and @c last must be input iterators.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_non_zero_int(first, last, _s);
        warmup();
    }

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

    /**
     * Returns the largest value that the @c xorshift128plus
     * can produce.
     */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return UINT64_MAX; }

    /** Returns the next value of the @c xorshift128plus. */
    std::uint64_t operator()()
    {
        const std::uint64_t r = _s[0] + _s[1];
        next();
        return r;
    }

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

    /** Advances the state of the generator by @c z. */
    void discard(std::uintmax_t z)
    {
        while (z--) {
            next();
        }
    }

    /**
     * This is a jump function for the generator. It is equivalent
     * to calling @c discard(2^64) @c z times; it can be used to
     * generate 2^64 non-overlapping subsequences for parallel
     * computations.
     */
    void jump(std::uintmax_t z = 1)
    {
        static const std::uint64_t jmp [2] {
            0x8a5cd789635d2dff, 0x121fd2155c472f96
        };

        while(z--) {
            std::uint64_t s0 = 0, s1 = 0;
            for (std::size_t i = 0; i < 2; ++i) {
                for (std::size_t b = 0; b < 64; ++b) {
                    if (jmp[i] & std::uint64_t { 1 } << b) {
                        s0 ^= _s[0], s1 ^= _s[1];
                    }
                    next();
                }
            }
            _s[0] = s0, _s[1] = s1;
        }
    }

    friend bool operator==(const xorshift128plus& x,
                           const xorshift128plus& y)
    { return x._s[0] == y._s[0] && x._s[1] == y._s[1]; }

    friend bool operator!=(const xorshift128plus& x,
                           const xorshift128plus& y)
    { return !(x == y); }

    /** Writes a @c xorshift128plus to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const xorshift128plus& xosh)
    {
        os << xosh._s[0] << ' ' << xosh._s[1];
        return os;
    }

    /** Reads a @c xorshift128plus from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is,
               xorshift128plus& xosh)
    {
        is >> xosh._s[0] >> std::ws >> xosh._s[1];
        return is;
    }

private:

    /// \cond show_private

    /** Advance the state by 1 step. */
    void next()
    {
        std::uint64_t s1 = _s[0];
        _s[0] = _s[1];
        s1 ^= s1 << 23;
        _s[1] = s1 ^ _s[0] ^ (s1 >> 18) ^ (_s[0] >> 5);
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup()
    {
        discard(8);
    }

    /// \endcond

    std::uint64_t _s[2];
};

/**
 * xorshift1024*.
 *
 * This is a fast, top-quality generator. If 1024 bits of state are too
 * much, a xoroshiro128+ generator can be used.
 *
 * The three lowest bits of this generator are LSFRs, and thus they are
 * slightly less random than the other bits. Use a sign test to extract
 * a random Boolean value.
 *
 * The state must be seeded so that it is not everywhere zero.
 *
 * Web-site: http://xoroshiro.di.unimi.it/
 *
 * There was an issue with an earlier version of this code, the corrected
 * version is implemented below.
 *
 * https://stackoverflow.com/questions/34574701/xorshift1024-jump-not-commutative
 */
class xorshift1024star
{
public:
    typedef std::uint64_t result_type;

    // Required for old Boost.Random concept.
    static const bool has_fixed_range = true;
    static const std::uint64_t default_seed = 1;

    /**
     * Constructs a @c xorshift1024star, using the default seed.
     */
    xorshift1024star()
    { seed(); }

    /**
     * Constructs a @c xorshift1024star, seeding it with @c value.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(xorshift1024star,
                                               std::uint64_t, value)
    { seed(value); }

    /**
     * Constructs a @c xorshift1024star, seeding it with values
     * produced by a call to @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(xorshift1024star,
                                             SeedSeq, seq)
    { seed(seq); }

    /**
     * Constructs a @c xorshift1024star and seeds it with values
     * taken from the iterator range [first, last) and adjusts
     * first to point to the element after the last one used.
     * If there are not enough elements, throws @c std::invalid_argument.
     *
     * first and last must be input iterators.
     */
    template<class It>
    xorshift1024star(It& first, It last)
    { seed(first, last); }

    // compiler-generated copy constructor and assignment operator are fine.

    /**
     * Calls seed(default_seed)
     */
    void seed()
    { seed(default_seed); }

    /**
     * seeds a @c xorshift1024star with splitmix64, as per Sebastiano
     * Vigna's recommendation.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(xorshift1024star, std::uint64_t, value)
    {
        std::uint64_t s = value + std::uint64_t { 0x9E3779B97F4A7C15 };
        _s[0] = detail::xoroshiro_integer_hash(s);
        for (std::size_t i = 1; i < 16; ++i) {
            _s[i] = detail::xoroshiro_integer_hash ((s += std::uint64_t { 0x9E3779B97F4A7C15 }));
        }
        _p = 0;
    }

    /**
     * Seeds a @c xorshift1024star using values from a SeedSeq. If a
     * valid seed cannot be generated throws @c std::runtime_error.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(xorshift1024star, SeedSeq, seq)
    {
        detail::seed_array_non_zero_int(seq, _s);
        _p = 0;
        warmup();
    }

    /**
     * Seeds a @c xorshift1024star with values taken from the
     * iterator range [first, last) and adjusts @c first to
     * point to the element after the last one used. If there are
     * not enough elements or all the whole input range is zero,
     * throws @c std::invalid_argument.
     *
     * @c first and @c last must be input iterators.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_non_zero_int(first, last, _s);
        _p = 0;
        warmup();
    }

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

    /**
     * Returns the largest value that the @c xorshift1024star
     * can produce.
     */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return UINT64_MAX; }

    /** Returns the next value of the @c xorshift1024star. */
    std::uint64_t operator()()
    {
        next();
        return _s[_p] * std::uint64_t { 0x106689D45497FDB5 };
    }

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

    /** Advances the state of the generator by @c z. */
    void discard(std::uintmax_t z)
    {
        while (z--) {
            next();
        }
    }

    /**
     * This is a jump function for the generator. It is equivalent
     * to calling @c discard(2^512) @c z times; it can be used to
     * generate 2^512 non-overlapping subsequences for parallel
     * computations.
     */
    void jump(std::uintmax_t z = 1)
    {
        static const std::uint64_t jmp[16] {
            0x84242F96ECA9C41D, 0xA3C65B8776F96855,
            0x5B34A39F070B5837, 0x4489AFFCE4F31A1E,
            0x2FFEEB0A48316F40, 0xDC2D9891FE68C022,
            0x3659132BB12FEA70, 0xAAC17D8EFA43CAB8,
            0xC4CB815590989B13, 0x5EE975283D71C93B,
            0x691548C86C1BD540, 0x7910C41D10A1E6A5,
            0x0B5FC64563B3E2A8, 0x047F7684E9FC949D,
            0xB99181F2D8F685CA, 0x284600E3F30E38C3
        };

        while(z--) {
            std::uint64_t t[16];
            for (std::size_t i = 0; i < 16; ++i) {
                for (std::size_t b = 0; b < 64; ++b) {
                    if (jmp[i] & std::uint64_t { 1 } << b) {
                        for (std::size_t j = 0; j < 16; ++j) {
                            t [j] ^= _s[(j + _p) & 15];
                        }
                    }
                    next();
                }
            }
            for (std::size_t j = 0; j < 16; ++j) {
                _s [(j + _p) & 15] = t[j];
            }
        }
    }

    friend bool operator==(const xorshift1024star& x,
        const xorshift1024star& y)
    {
        std::size_t i = x._p, j = y._p;
        for (; i < 16; ++i, j = (j + 1) & 15) {
            if (x._s[i] != y._s[j]) {
                return false;
            }
        }
        i = 0;
        for ( ; i < x._p; ++i, j = (j + 1) & 15) {
            if (x._s[i] != y._s[j]) {
                return false;
            }
        }
        return true;
    }

    friend bool operator!=(const xorshift1024star& x,
                           const xorshift1024star& y)
    { return !(x == y); }

    /** Writes a @c xorshift1024star to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const xorshift1024star& xosh)
    {
        std::size_t i = xosh._p;
        for (; i < 16; ++i) {
            os << xosh._s[i] << ' ';
        }
        i = 0;
        for (; i < xosh._p; ++i) {
            os << xosh._s[i] << ' ';
        }
        return os;
    }

    /** Reads a @c xorshift1024star from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is,
               xorshift1024star& xosh)
    {
        for (std::size_t i = 0; i < 16; ++i) {
            is >> xosh._s[i] >> std::ws;
        }
        xosh._p = 0;
        return is;
    }

private:

    /// \cond show_private

    /** Advance the state by 1 step. */
    void next()
    {
        const std::uint64_t s0 = _s[_p];
        std::uint64_t s1 = _s[(_p = (_p + 1) & 15)];
        s1 ^= s1 << 31;
        _s[_p] = s1 ^ s0 ^ (s1 >> 11) ^ (s0 >> 30);
    }

    // As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
    void warmup()
    {
        discard(64);
    }

    /// \endcond

    std::uint64_t _s[16];
    std::size_t _p;
};

#endif /* !BOOST_NO_INT64_T && !BOOST_NO_INTEGRAL_INT64_T */

} // namespace random
} // namespace boost

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

#endif // BOOST_RANDOM_XOROSHIRO_HPP


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

#include <vector>
#include <set>

#include "graph/PBQPGraph.hpp"
#include "graph/Vector.hpp"
#include "graph/PBQPNode.hpp"
#include "graph/PBQPEdge.hpp"
#include "reduction/PBQPReduction.hpp"
#include "reduction/ConnectedComponentSeparator.hpp"
#include "graph/PBQPSolution.hpp"

namespace pbqppapa {

BOOST_AUTO_TEST_CASE(singleNodeTest) {
	PBQPGraph<signed int> graph = PBQPGraph<signed int>();
	signed int vekData [] = {2 ,2};
	Vector<signed int> vek = Vector<signed int>(2, vekData);
	graph.addNode(vek);
	ConnectedComponentSeparator<signed int> sep = ConnectedComponentSeparator<signed int>(&graph);
	std::vector<PBQPGraph<signed int>*> components = sep.reduce();
	PBQPSolution<signed int> sol (0);
	sep.solve(sol);
	BOOST_CHECK_EQUAL(components.size(), 1);
	PBQPGraph<signed int>* retrievedGraph = components[0];
	BOOST_CHECK_EQUAL(0, retrievedGraph->getEdgeCount());
	BOOST_CHECK_EQUAL(1, retrievedGraph->getNodeCount());
	BOOST_CHECK_EQUAL((*(graph.getNodeBegin()))->getIndex(),
			(*(retrievedGraph->getNodeBegin()))->getIndex());
	if (retrievedGraph != &graph) {
		//not neccessary for our implementation, but just to make sure
		delete retrievedGraph;
	}
}

BOOST_AUTO_TEST_CASE(emptyGraphTest) {
	PBQPGraph<signed int> graph = PBQPGraph<signed int>();
	ConnectedComponentSeparator<signed int> sep (&graph);
	std::vector<PBQPGraph<signed int>*> components = sep.reduce();
	PBQPSolution<signed int> sol (0);
	sep.solve(sol);
	BOOST_CHECK_EQUAL(components.size(), 1);
	PBQPGraph<signed int>* retrievedGraph = components[0];
	BOOST_CHECK_EQUAL(0, retrievedGraph->getEdgeCount());
	BOOST_CHECK_EQUAL(0, retrievedGraph->getNodeCount());
}

BOOST_AUTO_TEST_CASE(basicNodeTest) {
	PBQPGraph<signed int> graph = PBQPGraph<signed int>();
	int size = 50;
	for (int i = 0; i < size; i++) {
		int arr [] = {2, 2};
		Vector<signed int> vek = Vector<signed int>(2, arr);
		graph.addNode(vek);
	}
	ConnectedComponentSeparator<signed int> sep = ConnectedComponentSeparator<signed int>(&graph);
	std::vector<PBQPGraph<signed int>*> components = sep.reduce();
	PBQPSolution<signed int> sol = PBQPSolution<int>(0);
	sep.solve(sol);
	BOOST_CHECK_EQUAL(components.size(), size);
	std::set<signed int> nodeIndices = std::set<signed int>();
	for (int i = 0; i < size; i++) {
		PBQPGraph<signed int>* retrievedGraph = components[i];
		BOOST_CHECK_EQUAL(0, retrievedGraph->getEdgeCount());
		BOOST_CHECK_EQUAL(1, retrievedGraph->getNodeCount());
		unsigned int index = (*(retrievedGraph->getNodeBegin()))->getIndex();
		BOOST_CHECK_EQUAL(0, nodeIndices.count(index));
		nodeIndices.insert(index);
		if (retrievedGraph != &graph) {
			delete retrievedGraph;
		}
	}
}

BOOST_AUTO_TEST_CASE(advancedNodeTest) {
	PBQPGraph<signed int>* graph = new PBQPGraph<int>();
	int subgraphs = 10;
	int localSize = 10;
	int edgeCount = 0;
	for (int i = 0; i < subgraphs; i++) {
		edgeCount = 0;
		std::vector<PBQPNode<int>*> otherNodes = std::vector<PBQPNode<int>*>();
		for (int k = 0; k < localSize; k++) {
			int arr [] = {2, 2};
			Vector<signed int> vek = Vector<signed int>(2, arr);
			PBQPNode<signed int>* node = graph->addNode(vek);
			otherNodes.push_back(node);
			for (PBQPNode<signed int>* otherNode : otherNodes) {
				int arr2 [] = {3, 2, 5, 8};
				Matrix<signed int> mat = Matrix<int>(2, 2, arr2);
				graph->addEdge(node, otherNode, mat);
				edgeCount++;
			}
		}
	}
	ConnectedComponentSeparator<int> sep = ConnectedComponentSeparator<int>(graph);
	std::vector<PBQPGraph<int>*> components = sep.reduce();
	PBQPSolution<int> sol = PBQPSolution<int>(0);
	sep.solve(sol);
	BOOST_CHECK_EQUAL(components.size(), subgraphs);
	std::set<int> nodeIndices = std::set<int>();
	for (int i = 0; i < subgraphs; i++) {
		PBQPGraph<int>* retrievedGraph = components[i];
		BOOST_CHECK_EQUAL(edgeCount - localSize, retrievedGraph->getEdgeCount());
		BOOST_CHECK_EQUAL(localSize, retrievedGraph->getNodeCount());
		//ensure each node is only in one subgraph
		for (auto iter = retrievedGraph->getNodeBegin();
				iter != retrievedGraph->getNodeEnd(); ++iter) {
			unsigned int index = (*iter)->getIndex();
			BOOST_CHECK_EQUAL(0, nodeIndices.count(index));
			nodeIndices.insert(index);
		}
	}
	for (PBQPGraph<int>* subGraph : components) {
		delete subGraph;
	}
}

}


//            Copyright Daniel Trebbien 2010.
// Distributed under the Boost Software License, Version 1.0.
//   (See accompanying file LICENSE_1_0.txt or the copy at
//         http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP
#define BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP 1

#include <boost/assert.hpp>
#include <set>
#include <vector>
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/maximum_adjacency_search.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/utility/result_of.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost {

  namespace detail {
    template < typename ParityMap, typename WeightMap, typename IndexMap >
    class mas_min_cut_visitor : public boost::default_mas_visitor {
      typedef one_bit_color_map <IndexMap> InternalParityMap;
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
    public:
      template < typename Graph >
      mas_min_cut_visitor(const Graph& g,
                          ParityMap parity,
                          weight_type& cutweight,
                          const WeightMap& weight_map,
                          IndexMap index_map)
        : m_bestParity(parity),
          m_parity(make_one_bit_color_map(num_vertices(g), index_map)),
          m_bestWeight(cutweight),
          m_cutweight(0),
          m_visited(0),
          m_weightMap(weight_map)
      {
        // set here since the init list sets the reference
        m_bestWeight = (std::numeric_limits<weight_type>::max)();
      }

      template < typename Vertex, typename Graph >
      void initialize_vertex(Vertex u, const Graph & g)
      {
        typedef typename boost::property_traits<ParityMap>::value_type parity_type;
        typedef typename boost::property_traits<InternalParityMap>::value_type internal_parity_type;

        put(m_parity, u, internal_parity_type(0));
        put(m_bestParity, u, parity_type(0));
      }

      template < typename Edge, typename Graph >
      void examine_edge(Edge e, const Graph & g)
      {
        weight_type w = get(m_weightMap, e);

        // if the target of e is already marked then decrease cutweight
        // otherwise, increase it
        if (get(m_parity, boost::target(e, g))) {
          m_cutweight -= w;
        } else {
          m_cutweight += w;
        }
      }

      template < typename Vertex, typename Graph >
      void finish_vertex(Vertex u, const Graph & g)
      {
        typedef typename boost::property_traits<ParityMap>::value_type parity_type;
        typedef typename boost::property_traits<InternalParityMap>::value_type internal_parity_type;

        ++m_visited;
        put(m_parity, u, internal_parity_type(1));

        if (m_cutweight < m_bestWeight && m_visited < num_vertices(g)) {
          m_bestWeight = m_cutweight;
          BGL_FORALL_VERTICES_T(i, g, Graph) {
            put(m_bestParity,i, get(m_parity,i));
          }
        }
      }

      inline void clear() {
        m_bestWeight = (std::numeric_limits<weight_type>::max)();
        m_visited = 0;
        m_cutweight = 0;
      }

    private:
      ParityMap m_bestParity;
      InternalParityMap m_parity;
      weight_type& m_bestWeight;
      weight_type m_cutweight;
      unsigned m_visited;
      const WeightMap& m_weightMap;
    };

    /**
     * \brief Computes a min-cut of the input graph
     *
     * Computes a min-cut of the input graph using the Stoer-Wagner algorithm.
     *
     * \pre \p g is a connected, undirected graph
     * \pre pq.empty()
     * \param[in] g the input graph
     * \param[in] weights a readable property map from each edge to its weight (a non-negative value)
     * \param[out] parities a writable property map from each vertex to a bool type object for
     *     distinguishing the two vertex sets of the min-cut
     * \param[out] assignments a read/write property map from each vertex to a \c vertex_descriptor object. This
     *     map serves as work space, and no particular meaning should be derived from property values
     *     after completion of the algorithm.
     * \param[out] pq a keyed, updatable max-priority queue
     * \returns the cut weight of the min-cut
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.114.6687&rep=rep1&type=pdf
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.31.614&rep=rep1&type=pdf
     *
     * \author Daniel Trebbien
     * \date 2010-09-11
     */
    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue, class IndexMap>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq, IndexMap index_map) {
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::graph_traits<UndirectedGraph>::vertices_size_type vertices_size_type;
      typedef typename boost::graph_traits<UndirectedGraph>::edge_descriptor edge_descriptor;
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
      typedef typename boost::property_traits<ParityMap>::value_type parity_type;

      typename graph_traits<UndirectedGraph>::vertex_iterator u_iter, u_end;

      weight_type bestW = (std::numeric_limits<weight_type>::max)();
      weight_type bestThisTime = (std::numeric_limits<weight_type>::max)();
      vertex_descriptor bestStart = boost::graph_traits<UndirectedGraph>::null_vertex();

      detail::mas_min_cut_visitor<ParityMap, WeightMap, IndexMap>
        vis(g, parities, bestThisTime, weights, index_map);

      // for each node in the graph,
      for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
        // run the MAS and find the min cut
        vis.clear();
        boost::maximum_adjacency_search(g,
            boost::weight_map(weights).
            visitor(vis).
            root_vertex(*u_iter).
            vertex_assignment_map(assignments).
            max_priority_queue(pq));
        if (bestThisTime < bestW) {
          bestW = bestThisTime;
          bestStart = *u_iter;
        }
      }

      // Run one more time, starting from the best start location, to
      // ensure the visitor has the best values.
      vis.clear();
      boost::maximum_adjacency_search(g,
        boost::vertex_assignment_map(assignments).
        weight_map(weights).
        visitor(vis).
        root_vertex(bestStart).
        max_priority_queue(pq));

      return bestW;
    }
  } // end `namespace detail` within `namespace boost`

    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue, class IndexMap>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq, IndexMap index_map) {
      BOOST_CONCEPT_ASSERT((boost::IncidenceGraphConcept<UndirectedGraph>));
      BOOST_CONCEPT_ASSERT((boost::VertexListGraphConcept<UndirectedGraph>));
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::graph_traits<UndirectedGraph>::vertices_size_type vertices_size_type;
      typedef typename boost::graph_traits<UndirectedGraph>::edge_descriptor edge_descriptor;
      BOOST_CONCEPT_ASSERT((boost::Convertible<typename boost::graph_traits<UndirectedGraph>::directed_category, boost::undirected_tag>));
      BOOST_CONCEPT_ASSERT((boost::ReadablePropertyMapConcept<WeightMap, edge_descriptor>));
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
      BOOST_CONCEPT_ASSERT((boost::WritablePropertyMapConcept<ParityMap, vertex_descriptor>));
      typedef typename boost::property_traits<ParityMap>::value_type parity_type;
      BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<VertexAssignmentMap, vertex_descriptor>));
      BOOST_CONCEPT_ASSERT((boost::Convertible<vertex_descriptor, typename boost::property_traits<VertexAssignmentMap>::value_type>));
      BOOST_CONCEPT_ASSERT((boost::KeyedUpdatableQueueConcept<KeyedUpdatablePriorityQueue>));

      vertices_size_type n = num_vertices(g);
      if (n < 2)
        throw boost::bad_graph("the input graph must have at least two vertices.");
      else if (!pq.empty())
        throw std::invalid_argument("the max-priority queue must be empty initially.");

      return detail::stoer_wagner_min_cut(g, weights,
                                          parities, assignments, pq, index_map);
    }

namespace graph {
  namespace detail {
    template <class UndirectedGraph, class WeightMap>
    struct stoer_wagner_min_cut_impl {
      typedef typename boost::property_traits<WeightMap>::value_type result_type;
      template <typename ArgPack>
      result_type operator() (const UndirectedGraph& g, WeightMap weights, const ArgPack& arg_pack) const {
        using namespace boost::graph::keywords;
        typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
        typedef typename boost::property_traits<WeightMap>::value_type weight_type;

        typedef boost::detail::make_priority_queue_from_arg_pack_gen<boost::graph::keywords::tag::max_priority_queue, weight_type, vertex_descriptor, std::greater<weight_type> > gen_type;

        gen_type gen(choose_param(get_param(arg_pack, boost::distance_zero_t()), weight_type(0)));

        typename boost::result_of<gen_type(const UndirectedGraph&, const ArgPack&)>::type pq = gen(g, arg_pack);

        return boost::stoer_wagner_min_cut(g,
          weights,
          arg_pack [_parity_map | boost::dummy_property_map()],
          boost::detail::make_property_map_from_arg_pack_gen<tag::vertex_assignment_map, vertex_descriptor>(vertex_descriptor())(g, arg_pack),
          pq,
          boost::detail::override_const_property(arg_pack, _vertex_index_map, g, vertex_index)
        );
      }
    };
  }
  BOOST_GRAPH_MAKE_FORWARDING_FUNCTION(stoer_wagner_min_cut,2,4)
}

  // Named parameter interface
  BOOST_GRAPH_MAKE_OLD_STYLE_PARAMETER_FUNCTION(stoer_wagner_min_cut, 2)
namespace graph {
    // version without IndexMap kept for backwards compatibility
    // (but requires vertex_index_t to be defined in the graph)
    // Place after the macro to avoid compilation errors
    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq) {

      return stoer_wagner_min_cut(g, weights,
                                  parities, assignments, pq,
                                  get(vertex_index, g));
    }
} // end `namespace graph`
} // end `namespace boost`

#include <boost/graph/iteration_macros_undef.hpp>

#endif // !BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP


/**
 * August 8th, 2018
 * source: https://zhuanlan.zhihu.com/p/38745950
 * Constant velocity prediction Kalman filter for shield global position
 */

#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <queue>
#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;

string attitude_topic, publisher_topic, debug_topic, real_visual_topic, transform_topic;
string debug_angle_topic;
ros::Publisher filter_pub, debug_pub, transform_pub, debug_angle_pub;
MatrixXd gimbal_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
MatrixXd init_R_gimbal= MatrixXd::Identity(3, 3);
Vector3d gimbal_T_camera = MatrixXd::Zero(3, 1);

VectorXd x(6);      // state
MatrixXd P = MatrixXd::Identity(6, 6); // covariance
MatrixXd Q = MatrixXd::Identity(6, 6); // prediction noise covariance
MatrixXd R = MatrixXd::Identity(3, 3); // observation noise covariance
MatrixXd A = MatrixXd::Identity(6, 6); // state transfer function

bool visual_initialized = false, visual_valid = false;

ros::Time t_prev;
double R_pos, Q_pos, Q_vel, P_weight;
const double CV_UPDATE_TIME_MAX = 0.2; // maxium allowed update time
const double DELAY_MAX = 0.05;
const int ROS_FREQ = 100;
const int MAX_VISUAL_QUEUE_SIZE = 5;
const int MAX_IMU_QUEUE_SIZE = 1000;
double OUTLIER_THRESHOLD = 10000.0;

Vector3d init_T_shield_prev = MatrixXd::Zero(3, 1);
Vector3d imu_T_shield_prev = MatrixXd::Zero(3, 1);
bool pos_is_outlier = false;
double chi_square = 0;
double outlier_l2_norm_ratio = 1.5;
double yaw_delay = 0.0;
double pitch_delay = 0.0;
Vector3d OUTPUT_BOUND = MatrixXd::Zero(3, 1);

// synchronization
queue<geometry_msgs::QuaternionStamped::ConstPtr> imu_queue;
queue<Vector3d> visual_queue;
int imu_back_time = 10;

double angle_diff = 0;
bool angle_diff_not_inited = true;

static void pub_result(const ros::Time &stamp, double delay_dt)
{
//    geometry_msgs::TwistStamped odom;
    rm_cv::ArmorRecord odom;
    odom.header.stamp = stamp;
    double predict_x = x(0) + (delay_dt + yaw_delay)   * x(3);
    double predict_y = x(1) + (delay_dt + pitch_delay) * x(4);
    double predict_z = x(2) + delay_dt * x(5);
    odom.armorPose.linear.x  = (predict_x < OUTPUT_BOUND[0]) ? predict_x * 1000 : OUTPUT_BOUND[0] * 1000;
    odom.armorPose.linear.y  = (predict_y < OUTPUT_BOUND[1]) ? predict_y * 1000 : OUTPUT_BOUND[1] * 1000;
    odom.armorPose.linear.z  = (predict_z < OUTPUT_BOUND[2]) ? predict_z * 1000 : OUTPUT_BOUND[2] * 1000;
    odom.armorPose.angular.x = x(3) * 1000;
    odom.armorPose.angular.y = x(4) * 1000;
    odom.armorPose.angular.z = x(5) * 1000;
    filter_pub.publish(odom);
}

static void pub_preprocessed(const std_msgs::Header &header,
                      const Ref<const Vector3d> p,
                      const Ref<const Vector3d> v,
                      ros::Publisher &publisher)
{
    geometry_msgs::TwistStamped debug;
    debug.header = header;
    debug.twist.linear.x  = p[0];
    debug.twist.linear.y  = p[1];
    debug.twist.linear.z  = p[2];
    debug.twist.angular.x = v[0];
    debug.twist.angular.y = v[1];
    debug.twist.angular.z = v[2];
    publisher.publish(debug);
}

static void pub_angle_debug(const std_msgs::Header &header,
                            const double yaw,
                            const Ref<const Vector3d> p,
                            const double imuz)
{
    geometry_msgs::TwistStamped debug_angle;
    debug_angle.header = header;
    debug_angle.twist.angular.x = yaw;
    debug_angle.twist.angular.y = -atan2(p[1], p[0]);
    debug_angle.twist.angular.z = imuz;
    debug_angle_pub.publish(debug_angle);
}

static double imu_to_yaw_angle(const geometry_msgs::QuaternionStamped::ConstPtr &imu)
{
    // Quaterniond q = Quaterniond( imu->quaternion.w, imu->quaternion.x, imu->quaternion.y, imu->quaternion.z );
    double qw = imu->quaternion.w;
    double qx = imu->quaternion.x;
    double qy = imu->quaternion.y;
    double qz = imu->quaternion.z;

    // convert to ZYX Euler angle yaw angle
    // double euler_angle_0 = atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy) );
    // double euler_angle_1 = asin( 2.0 * (qw * qy - qz * qx) );
    return atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz) );
}

// Chi-square test for outlier rejection
static bool translation_is_outlier(const Vector3d &pos)
{
    VectorXd r = MatrixXd::Zero(3, 1);
    VectorXd z = MatrixXd::Zero(3, 1);
    MatrixXd S = MatrixXd::Zero(3, 3);
    z << pos[0], pos[1], pos[2];

    double pos_norm   = z.segment<3>(0).norm();
    double state_norm = x.segment<3>(0).norm();
    // ROS_INFO("pos_norm %f, state_norm %f", pos_norm, state_norm);
    if (pos_norm > state_norm * outlier_l2_norm_ratio) {
        return true;
    } else if (pos_norm < state_norm / outlier_l2_norm_ratio) {
        return true;
    }

    // r = z - H * x, residual
    // S = H P H' + R, residual covariance
    MatrixXd H = MatrixXd::Identity(3, 6); // observation matrix

//    MatrixXd K_next = (H * P * H.transpose() + R).ldlt().solve(P * H.transpose());
//    MatrixXd x_next = x + K_next * (z - H * x);
//    MatrixXd P_next = P - K_next * H * P;

    r = z - H * x;
    S = H * P * H.transpose() + R;
    chi_square = r.transpose() * S * r;
    // cout << "chi_square " << endl << chi_square << endl;

    return (chi_square > OUTLIER_THRESHOLD);
}

static void preprocess_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist,
                              Vector3d &pos, double dt_update)
{
    Vector3d camera_T_shield, gimbal_T_shield, init_T_shield;
    Vector3d camera_vel_shield, gimbal_vel_shield, init_vel_shield;
    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;
    gimbal_T_shield = gimbal_R_camera * camera_T_shield + gimbal_T_camera;
    gimbal_T_shield *= 0.001; // Convert millimeter to meter

    Matrix3d world_R_gimbal;// = MatrixXd::Identity(3, 3);
    world_R_gimbal << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    double yaw = 0;
    double imuz= 0;

/*    int pop_time = 0;
    if (!imu_queue.empty()) {
        pop_time = imu_queue.size() - imu_back_time;

        if (pop_time > 0) {
            for (int i = 0; i < pop_time; ++i) {
                imu_queue.pop();
            }
            imuz = imu_queue.front()->quaternion.z;
            yaw = imu_to_yaw_angle(imu_queue.front());
            world_R_gimbal << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1;

            if (angle_diff_not_inited) {
                angle_diff =  yaw + atan2(gimbal_T_shield[1], gimbal_T_shield[0]);
                angle_diff_not_inited = false;
                ROS_INFO("angle_diff is, %f", angle_diff);
            }
        }
    }*/
    init_T_shield = world_R_gimbal * gimbal_T_shield;

    // calculate and check the velocity
    init_vel_shield = (init_T_shield - init_T_shield_prev) / dt_update;
    init_T_shield_prev = init_T_shield;

    pos_is_outlier = translation_is_outlier(init_T_shield);

    if (pos_is_outlier) {
        pos = x.segment<3>(0);
        x.segment<3>(3) *= 0.5;
        ROS_INFO("outlier rejected");
    }
    else {
        pos = init_T_shield;
    }

    // store the state
    pub_preprocessed(header, gimbal_T_shield, gimbal_vel_shield, debug_pub);

    pub_preprocessed(header, init_T_shield_prev, init_vel_shield, transform_pub);

    pub_angle_debug(header, yaw - angle_diff, gimbal_T_shield, imuz);
}

/**
 * Core Kalman Filter math, propagate and update
 */
static void propagate(const double &dt) {
    A.topRightCorner(3, 3) = dt * MatrixXd::Identity(3, 3);
    x = A * x;
    P = A * P * A.transpose() + Q;
}

static void update(const Vector3d &pos)
{
    MatrixXd H, K, z;

    H = MatrixXd::Identity(3, 6); // observation matrix
    K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
    z = MatrixXd::Zero(3, 1);
    z << pos[0], pos[1], pos[2];

    x = x + K * (z - H * x);
    P = P - K * H * P;
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist)
{
    ros::Time t_update = header.stamp;
    ROS_INFO("visual init at %f", t_update.toSec());

    Vector3d camera_T_shield, gimbal_T_shield, init_T_shield;
    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;

    gimbal_T_shield = gimbal_R_camera * camera_T_shield + gimbal_T_camera;
    gimbal_T_shield *= 0.001; // Convert millimeter to meter

    Matrix3d world_R_gimbal;// = MatrixXd::Identity(3, 3);
    world_R_gimbal << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    double yaw = 0;

/*    int pop_time = 0;
    if (!imu_queue.empty()) {
        pop_time = imu_queue.size() - imu_back_time;

        if (pop_time > 0) {
            for (int i = 0; i < pop_time; ++i) {
                imu_queue.pop();
            }
            yaw = imu_to_yaw_angle(imu_queue.front());
            // double euler_angle_2 = imu_to_yaw_angle(imu_queue.back());
            world_R_gimbal << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1;
            double DEBUG_imu_delay_time = header.stamp.toSec() - imu_queue.front()->header.stamp.toSec();
            cout << "imu to visual time difference " << DEBUG_imu_delay_time << endl;
        }
        cout << "final poped times " << pop_time << endl;
    }*/

    cout << "world_R_gimbal " << endl << world_R_gimbal << endl;

    init_T_shield = world_R_gimbal * gimbal_T_shield;

    // TODO: using RANSAC instead of averaging filter
    visual_queue.push(init_T_shield);
    if (visual_queue.size() >= MAX_VISUAL_QUEUE_SIZE) {
        Vector3d T_sum;
        T_sum.setZero();

        for (int i = 0; i < MAX_VISUAL_QUEUE_SIZE; ++i) {
            T_sum += visual_queue.front();
            visual_queue.pop();
        }

        x.segment<3>(0) = T_sum / MAX_VISUAL_QUEUE_SIZE; // init velocity with zero
        x.segment<3>(3).setZero(); // init velocity with zero

        P.setZero();
        P.topLeftCorner(3, 3)     = P_weight * MatrixXd::Identity(3, 3);
        P.bottomRightCorner(3, 3) = 2 * P_weight * MatrixXd::Identity(3, 3);

        cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;
        cout << "P " << endl << P << endl;

        init_T_shield_prev = init_T_shield;
        visual_initialized = true;
    }
    else {
        cout << "DEBUG: current x reading: " << init_T_shield.transpose() << endl;
    }
}

/**
 * handle and save attitude of the gimbal
 * @param imu
 */
void attitude_cb(const geometry_msgs::QuaternionStamped::ConstPtr &imu)
{
    // synchronize the timestamp
    imu_queue.push(imu);
    if (imu_queue.size() > MAX_IMU_QUEUE_SIZE) imu_queue.pop();
}

/**
 * handle, save, and process visual messages
 * @param armor
 */
void real_visual_cb(const rm_cv::ArmorRecord::ConstPtr &armor)
{
    visual_valid = !(armor->armorPose.linear.x == 0 &&
                     armor->armorPose.linear.y == 0 &&
                     armor->armorPose.linear.z == 0 );

    if (visual_valid) {
        ros::Time t_update = armor->header.stamp;
        double dt_update = (t_update - t_prev).toSec();

        if (!visual_initialized) {
            initialize_visual(armor->header, armor->armorPose);
        }
        else {
            if (dt_update < CV_UPDATE_TIME_MAX) {
                Vector3d pos;

                preprocess_visual(armor->header, armor->armorPose, pos, dt_update);

                update(pos);

                propagate(dt_update);

                double running_time = ros::Time::now().toSec();
                double delay_dt = running_time - armor->header.stamp.toSec();
                delay_dt = (delay_dt < DELAY_MAX) ? delay_dt : DELAY_MAX;

                pub_result(armor->header.stamp, delay_dt);
            }
            else {
                visual_queue.push(x.segment<3>(0));

                visual_initialized = false;

                initialize_visual(armor->header, armor->armorPose);
            }
        }
        t_prev = t_update;
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "prediction_kalman_filter");
    ros::NodeHandle n("~");

    n.param("attitude_topic", attitude_topic, string("/can_receive_node/attitude"));
    n.param("real_visual_topic", real_visual_topic, string("/detected_armor"));
    n.param("publisher_topic", publisher_topic, string("/prediction_kf_global/predict"));
    n.param("debug_topic", debug_topic, string("/prediction_kf_global/preprocessed"));
    n.param("transform_topic", transform_topic, string("/prediction_kf_global/transformed"));
    n.param("debug_angle_topic", debug_angle_topic, string("/prediction_kf/debug_angle"));
    n.param("chi_square_threshold", OUTLIER_THRESHOLD, 10000.0);
    n.param("outlier_l2_norm_ratio", outlier_l2_norm_ratio, 1.5);
    n.param("imu_back_time", imu_back_time, 10);
    n.param("R_pos", R_pos, 16.0);
    n.param("Q_pos", Q_pos, 0.2);
    n.param("Q_vel", Q_vel, 1.0);
    n.param("P_matrix_weight", P_weight, 1.0);
    n.param("yaw_delay", yaw_delay, 0.0);
    n.param("pitch_delay", pitch_delay, 0.0);

    // For chassis reading only
    gimbal_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;
    gimbal_T_camera <<  30, 0, -120; // in millimeter

    OUTPUT_BOUND << 10.0, 10.0, 20.0;

    x.setZero();
    R = R_pos * MatrixXd::Identity(3, 3);
    Q.topLeftCorner(3, 3)     = Q_pos * MatrixXd::Identity(3, 3);
    Q.bottomRightCorner(3, 3) = Q_vel * MatrixXd::Identity(3, 3);
    cout << "R " << endl << R << endl;
    cout << "Q " << endl << Q << endl;

    ros::Subscriber s1 = n.subscribe(attitude_topic, 100, attitude_cb);
    ros::Subscriber s2 = n.subscribe(real_visual_topic, 40, real_visual_cb);

    filter_pub = n.advertise<rm_cv::ArmorRecord>(publisher_topic, 40);
    debug_pub  = n.advertise<geometry_msgs::TwistStamped>(debug_topic, 40);
    transform_pub = n.advertise<geometry_msgs::TwistStamped>(transform_topic, 40);
    debug_angle_pub = n.advertise<geometry_msgs::TwistStamped>(debug_angle_topic, 100);
    ros::Rate r(ROS_FREQ);

    while (ros::ok()) {
        if (visual_initialized) {

        }

        r.sleep();
        ros::spinOnce();
    }
}