#ifndef ORDERED_PATCH_MAP_H
#define ORDERED_PATCH_MAP_H

#include <algorithm>
#include <bitset>
#include <cassert>
#include <cstdint>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <boost/container/allocator.hpp>
#include <boost/interprocess/allocators/allocator.hpp>
#include <typeinfo>
#include <exception>
#include <memory>

namespace whash{
  bool constexpr VERBOSE_PATCHMAP = false;
  using std::allocator_traits;
  using std::array;
  using std::cerr;
  using std::conditional;
  using std::cout;
  using std::enable_if;
  using std::endl;
  using std::false_type;
  using std::get;
  using std::index_sequence;
  using std::index_sequence_for;
  using std::initializer_list;
  using std::integral_constant;
  using std::is_const;
  using std::is_fundamental;
  using std::is_same;
  using std::is_trivially_copyable;
  using std::numeric_limits;
  using std::pair;
  using std::setw;
  using std::true_type;
  using std::tuple;
  using std::swap;

  template<class T>
  double frac(const T& n){
    return n*pow(0.5,numeric_limits<T>::digits);
  }

  template<typename T>
  struct dummy_comp{ // dummy comparator for when we don't need a comparator
    constexpr bool operator()(const T&,const T&) const {return false;}
  };

  template <typename T>
  constexpr size_t digits(const T& n=0){
    return numeric_limits<T>::digits;
  }

  template<typename T>
  typename std::enable_if<std::is_unsigned<T>::value,tuple<T,T>>::type
  constexpr long_mul(const T& a, const T& b);

  // calculate a * b = r0r1
  template<typename T>
  typename std::enable_if<std::is_unsigned<T>::value,tuple<T,T>>::type
  constexpr long_mul(const T& a, const T& b){
    const T N  = digits<T>()/2;
    const T t0 = (a>>N)*(b>>N);
    const T t1 = ((a<<N)>>N)*(b>>N);
    const T t2 = (a>>N)*((b<<N)>>N);
    const T t3 = ((a<<N)>>N)*((b<<N)>>N);
    const T t4 = t3+(t1<<N);
    const T r1 = t4+(t2<<N);
    const T r0 = (r1<t4)+(t4<t3)+(t1>>N)+(t2>>N)+t0;
    return {r0,r1};
  }

#ifdef __SIZEOF_INT128__
  template<>
  tuple<uint64_t,uint64_t>
  constexpr long_mul(const uint64_t& a, const uint64_t& b){
    unsigned __int128 r = ((unsigned __int128)(a))*((unsigned __int128)(b));
    return {r>>64,r};
  }
#endif

  template<>
  tuple<uint8_t,uint8_t> constexpr long_mul(const uint8_t& a,const uint8_t& b){
    const int_fast16_t r = int_fast16_t(a)*int_fast16_t(b);
    return {uint8_t(r>>8),uint8_t(r)};
  }

  template<>
  tuple<uint16_t,uint16_t> constexpr long_mul(
      const uint16_t& a,
      const uint16_t& b){
    const int_fast32_t r = int_fast32_t(a)*int_fast32_t(b);
    return {uint16_t(r>>16),uint16_t(r)};
  }

  template<>
  tuple<uint32_t,uint32_t> constexpr long_mul(
      const uint32_t& a,
      const uint32_t& b){
    const int_fast64_t r = int_fast64_t(a)*int_fast64_t(b);
    return {uint32_t(r>>32),uint32_t(r)};
  }

  template <typename T>
  constexpr size_t popcount(const T n){
    size_t c=0;
    while(n) (n&=(n-1),++c);
    return c;
  }

  constexpr size_t popcount(const uint32_t n){
    return __builtin_popcountl(n);
  }

  constexpr size_t popcount(const uint64_t n){
    return __builtin_popcountll(n);
  }

  template <typename T>
  typename std::enable_if<std::is_unsigned<T>::value,T>::type
  constexpr distribute(const T& a); // mix the hash value good, clmul_circ
                                    // with odious integer is suitable

  uint8_t  constexpr inline distribute(const uint8_t& a){
    return (a+111)*97;
  }

  uint16_t constexpr inline distribute(const uint16_t& a){
    return (a+36690)*43581;
  }

  uint32_t const distribute(uint32_t a){
    const uint32_t  b = 0x55555555ul;
    const uint32_t c0 = 3107070805ul;
    const uint32_t c1 = 3061963241ul;
    a = (a^(a>>16))*b;
    a = a^(a>>16);
    return a;
  }

  uint64_t constexpr distribute(uint64_t a){
    const uint64_t  b =   0x5555555555555555ull;
    const uint64_t c0 = 16123805160827025777ull;
    const uint64_t c1 = 13834579444137454003ull;
    const uint64_t c2 = 14210505232527258663ull;
    a^=(a>>32); a*=b; // a^=(a>>32);
    return a;
  }

  template<typename,typename=void>
  struct is_injective : false_type {};

  template<typename T>
  struct is_injective<T,typename enable_if<T::is_injective::value>::type>
  : true_type {};

  template<typename,typename=void>
  struct has_std_hash : false_type {};

  template<typename T>
  struct has_std_hash<T,decltype(std::hash<T>()(std::declval<T>()),void())>
  : true_type {};

  template<typename T>
  typename
  enable_if<has_std_hash<T>::value&&(!is_fundamental<T>::value),size_t>::type
  constexpr hash(const T& v){
    return std::hash<T>()(v);
  }

  size_t constexpr hash() {
    return 0;
  }

  size_t constexpr hash(const size_t& seed,const size_t& n) {
    return seed^distribute(n);
  }

  template<class K>
  typename enable_if<(sizeof(K)>sizeof(size_t)
                   &&is_fundamental<K>::value),size_t>::type
  constexpr hash(const K& k){
    size_t h(k);
    const size_t n = sizeof(K)/sizeof(size_t);
    for (size_t i=sizeof(size_t);i<sizeof(K);i+=sizeof(size_t))
      h = hash(h,size_t(k>>(i*CHAR_BIT)));
    return h;
  }

  uint8_t constexpr hash(const uint8_t& v){
    return v;
  }

  uint8_t constexpr hash(const int8_t& v){
    return v;
  }

  uint16_t constexpr hash(const uint16_t& v){
    return v;
  }

  uint16_t constexpr hash(const int16_t& v){
    return v;
  }

  uint32_t constexpr hash(const uint32_t& v){
    return v;
  }

  uint32_t constexpr hash(const int32_t& v){
    return v;
  }

  uint64_t constexpr hash(const uint64_t& v){
    return v;
  }

  uint64_t constexpr hash(const int64_t& v){
    return v;
  }

  template <typename T,typename... Rest>
  size_t constexpr hash(const T& v,Rest... rest);

  uint16_t constexpr hash(const uint8_t& v0,const uint8_t& v1){
    return (uint16_t(v0)<<8)^uint16_t(v1);
  }

  uint32_t constexpr hash(const uint16_t& v0,const uint16_t& v1){
    return (uint32_t(v0)<<16)^(uint32_t(v1));
  }

  uint64_t constexpr hash(const uint32_t& v0,const uint32_t& v1){
    return (uint64_t(v0)<<32)^(uint64_t(v1));
  }

  template<typename T,size_t... I>
  size_t constexpr hash_tuple_impl(const T& t, index_sequence<I...>){
    return hash(std::get<I>(t)...);
  }

  template<typename... Ts>
  size_t constexpr hash(const tuple<Ts...>& t){
    return hash_tuple_impl(t,index_sequence_for<Ts...>{});
  }

  template<typename T,size_t n>
  size_t constexpr hash(const array<T,n> a){
    size_t h(0);
    for (size_t i=0;i!=n;++i) {
      if constexpr(sizeof(T)<=sizeof(size_t))
        if (i%(sizeof(size_t)/sizeof(T))==0) h = distribute(h);
      h = hash(h,a[i]);
      if constexpr(sizeof(T)>sizeof(size_t)) h = distribute(h);
    }
    return h;
  }

  template <typename T, typename... Rest>
  size_t constexpr hash(const T& v, Rest... rest) {
    return hash(hash(v),hash(rest...));
  }

  template<class K,class enable = void>
  struct hash_functor{
    typedef typename false_type::type is_injective;
    size_t operator()(const K& k) const {
      return hash(k);
    }
  };

  template<class K>
  struct hash_functor<
    K,
    typename enable_if<is_fundamental<K>::value,void>::type
  >{
    // if size_t has at least as many digits as the hashed type the hash can
    // be injective and I will make it so
    typedef typename integral_constant<bool,sizeof(K)<=sizeof(size_t)>::type
      is_injective;
    auto constexpr operator()(const K& k) const {
      return hash(k);
    }
  };

  template<typename T,size_t n>
  struct hash_functor<
    array<T,n>,void>
  {
    // if size_t has at least as many digits as the hashed type the hash can
    // be injective and I will make it so
    typedef typename integral_constant<bool,n*sizeof(T)<=sizeof(size_t)>::type
      is_injective;
    auto constexpr operator()(const array<T,n>& k) const {
      return hash(k);
    }
  };

  template<typename T0,typename T1,typename T2>
  constexpr T0 clip(const T0& n,const T1& l,const T2& h){
    return n<l?l:n>h?h:n;
  }

  template <typename T>
  typename std::enable_if<std::is_unsigned<T>::value,T>::type
  constexpr clz(const T x,const T lower=0,const T upper=digits<T>()){
    return (upper-lower==T(1))?digits<T>()-upper:
      (x&(T(0)-T(1)<<((upper+lower)/2))?
           clz(x,(upper+lower)/2,upper):
           clz(x,lower,(upper+lower)/2));
  }

  template <typename T>
  typename std::enable_if<std::is_unsigned<T>::value,T>::type
  constexpr ctz(const T x,const T lower=0,const T upper=digits<T>()){
    return
      (upper-lower==T(1))?lower:(x&(T(0)-T(1)<<((upper+lower)/2))?
          ctz(x,(upper+lower)/2,upper):
          ctz(x,lower,(upper+lower)/2));
    // TODO
  }

  template <typename T>
  typename std::enable_if<std::is_unsigned<T>::value,T>::type
  constexpr log2(const T x,const T lower=0,const T upper=digits<T>()){
    return (upper-lower==T(1))?lower:(x&(T(0)-T(1)<<((upper+lower)/2))?
           log2(x,(upper+lower)/2,upper):
           log2(x,lower,(upper+lower)/2));
  }

#if __GNUC__ > 3 || __clang__
  uint32_t constexpr clz(const uint32_t x){
    return x==0?32:__builtin_clz(x);
  }

  uint32_t constexpr ctz(const uint32_t x){
    return x==0?32:__builtin_ctz(x);
  }

  uint64_t constexpr clz(const uint64_t x){
    return x==0?64:__builtin_clzll(x);
  }

  uint64_t constexpr ctz(const uint64_t x){
    return x==0?64:__builtin_ctzll(x);
  }

  uint32_t constexpr log2(const uint32_t x){
    return x==0?0:31-__builtin_clz(x);
  }

  uint64_t constexpr log2(const uint64_t x){
    return x==0?0:63-__builtin_clzll(x);
  }
#endif

  template <typename T,typename S>
  typename std::enable_if<std::is_unsigned<T>::value,T>::type
  constexpr shl(const T n, const S i){
    if ((i<digits<T>())&&(i>=0)) return n<<i;
    return 0;
  }

  template <typename T,typename S>
  typename std::enable_if<std::is_unsigned<T>::value,T>::type
  constexpr shr(const T n, const S i){
    if ((i<digits<T>())&&(i>=0)) return n>>i;
    return 0;
  }

  template<class key_type    = int,  // int is the default, why not
           class mapped_type = int,  // int is the default, why not
           class hash        = hash_functor<key_type>,
           class equal       = std::equal_to<key_type>,
           class comp        = typename conditional<
             is_injective<hash>::value,
             dummy_comp<key_type>,
             std::less<key_type>>::type,
           bool dynamic      = true
          >
  class patchmap{
    public:
      typedef typename conditional<is_same<mapped_type,void>::value,
                                  std::true_type,
                                  mapped_type>::type
                                  _mapped_type;
      typedef pair<key_type,_mapped_type>  value_type;
      typedef value_type* value_pointer;
      typedef value_type& reference;
      typedef const value_type& const_reference;
      typedef size_t size_type;
      typedef uint64_t flag_type;
      typedef typename std::result_of<hash(key_type)>::type hash_type;
      static const size_type stride = 64;
      struct bucket {
        value_type values[stride];
        flag_type flag;
      };
    private:
      size_type num_data;
      size_type datasize;
      bucket* data;
      comp  comparator;
      equal equator;
      hash  hasher;
      using uphold_iterator_validity = false_type;
      // size_type const inline masksize() const {
      //  return (datasize+digits<size_type>()-1)/digits<size_type>();
      //}
      template<typename T>
      const key_type& key_of(T&& value) const {
        if constexpr (is_same<void,mapped_type>::value) return value;
        else return value.first;
      }
      size_type inline map(
          const hash_type& h,
          const hash_type& n
          ) const {
        return get<0>(long_mul(h,n));
      }
      size_type inline map(const hash_type& h) const {
        return map(h,datasize);
      }
      size_type inline map_diff(
          const hash_type& h0,
          const hash_type& h1,
          const hash_type& n
          ) const {
        const auto lm = long_mul(hash_type(h0-h1),n);
        return get<0>(lm);
      }
      size_type inline map_diff(
          const hash_type& h0,
          const hash_type& h1
          ) const {
        return map_diff(h0,h1,datasize);
      }
      size_type inline map_diff_round(
          const hash_type& h0,
          const hash_type& h1,
          const hash_type& n
          ) const {
        const auto lm = long_mul(hash_type(h0-h1),n);
        return get<0>(lm)+(get<1>(lm)>((~hash_type(0))>>1));
      }
      size_type inline map_diff_round(
          const hash_type& h0,
          const hash_type& h1
          ) const {
        return map_diff_round(h0,h1,datasize);
      }
      hash_type inline order(const key_type& k) const {
        return distribute(hasher(k));
      }
      inline const value_type& get_const_value_at(const size_type& i) const {
        return data[i/stride].values[i%stride];
      }
      inline value_type& get_value_at(const size_type& i) {
        return data[i/stride].values[i%stride];
      }
      inline const key_type& get_const_key_at(const size_type& i) const {
        return data[i/stride].values[i%stride].first;
      }
      inline key_type& get_key_at(const size_type& i) {
        return data[i/stride].values[i%stride].first;
      }
      inline const mapped_type& get_const_mapped_at(const size_type& i) const {
        return data[i/stride].values[i%stride].second;
      }
      inline mapped_type& get_mapped_at(const size_type& i) {
        return data[i/stride].values[i%stride].second;
      }
      bool inline is_less(
          const key_type& a,
          const key_type& b,
          const hash_type& oa,
          const hash_type& ob
          ) const {
        if constexpr (is_injective<hash>::value){
          assert(equator(a,b)==(oa==ob));
          if (oa<ob) return true;
          else       return false;
        } else {
          if (oa<ob) return true;
          if (oa>ob) return false;
          return comparator(a,b);
        }
      }
      bool inline is_less(
          const key_type& a,
          const key_type& b,
          const hash_type& oa
          ) const {
        return is_less(a,b,oa,order(b));
      }
      bool inline is_less(const key_type& a,const key_type& b) const {
        return is_less(a,b,order(a),order(b));
      }
      bool inline is_more(
          const key_type& a,
          const key_type& b,
          const hash_type& oa,
          const hash_type& ob
          ) const {
        if constexpr (is_injective<hash>::value){
          assert(equator(a,b)==(oa==ob));
          if (oa>ob) return true;
          else       return false;
        } else {
          if (oa>ob) return true;
          if (oa<ob) return false;
          return !((comparator(a,b))||(equator(a,b)));
        }
      }
      bool inline is_more(
          const key_type& a,
          const key_type& b,
          const hash_type& oa
          ) const {
        return is_more(a,b,oa,order(b));
      }
      bool inline is_more(
          const key_type& a,
          const key_type& b
          ) const {
        return is_more(a,b,order(a),order(b));
      }
      bool inline is_set(const size_type& n) const {
        const size_type i = n/stride;
        const size_type j = n%stride;
        return data[i].flag&(flag_type(1)<<j);
      }
      void inline set(const size_type& n) {
        const size_type i = n/stride;
        const size_type j = n%stride;
        data[i].flag|=flag_type(1)<<j;
      }
      void inline unset(const size_type& n) {
        const size_type i = n/stride;
        const size_type j = n%stride;
        data[i].flag&=(~flag_type(0))^(flag_type(1)<<j);
      }
      void inline swap_set(const size_type& i,const size_type& j){
        if (is_set(i)==is_set(j)) return;
        if (is_set(i)){
          set(j);
          unset(i);
        }else{
          set(i);
          unset(j);
        }
      }
      bool inline index_key_is_less(const size_type& i,const key_type& k) const{
        if (is_set(i)) return is_less(get_const_key_at(i),k);
        return i<map(order(k));
      }
      bool inline key_index_is_less(const key_type& k,const size_type& i) const{
        if (is_set(i)) return is_less(k,get_const_key_at(i));
        return map(order(k))<i;
      }
      bool inline index_index_is_less(const size_type& i,const size_type& j)
        const {
        assert(i<datasize);
        assert(j<datasize);
        if (is_set(i)&&is_set(j))
          return is_less(get_const_key_at(i),get_const_key_at(j));
        if (is_set(i)) return map(order(get_const_key_at(i)))<j;
        if (is_set(j)) return i<map(order(get_const_key_at(j)));
        return i<j;
      }
      bool inline index_index_is_more(const size_type& i,const size_type& j)
        const {
        return index_index_is_less(j,i);
      }
      size_type inline find_first() const {
        size_type i=0;
        while (i!=datasize) if (is_set(i)) return i;
        return ~size_type(0);
      }
      // search for free bucket in decreasing order
      size_type inline search_free_dec(size_type i) const {
        while(true) {
          if (!is_set(i)) return i;
          if (i--==0) return ~size_type(0);
        }
        return ~size_type(0);
      }
      // search for free bucket in increasing order
      size_type inline search_free_inc(size_type i) const {
        while(true) {
          if (!is_set(i)) return i;
          if (i++==0) return ~size_type(0);
        }
        return ~size_type(0);
      }
      // search for free bucket bidirectional
      size_type inline search_free_bidir(const size_type& i) const {
        if (!is_set(i)) return i;
        for (size_t j=1;;++j) {
          if (i+j==datasize) {
            for (;i-j!=~size_type(0);++j) if (!is_set(i-j)) return i-j;
            return ~size_type(0);
          }
          if (i-j==~size_type(0)) {
            for (;i+j!=datasize     ;++j) if (!is_set(i+j)) return i+j;
            return ~size_type(0);
          }
          if (!is_set(i+j)) return i+j;
          if (!is_set(i-j)) return i-j;
        }
      }
      size_type const inline reserve_node(
          const  key_type&   k,
          const hash_type&  ok,
          const size_type& mok
          ){
        if (!is_set(mok)) {
          set(mok);
          ++num_data;
          return mok;
        }
        const size_type j = search_free_bidir(mok);
        assert(j<datasize);
        assert(!is_set(j));
        set(j);
        ++num_data;
        size_type i = j;
        while(true){
          if (i==0) break;
          if (!is_set(i-1)) break;
          if (is_less(get_key_at(i-1),k,order(get_key_at(i-1)),ok)) break;
          swap(get_value_at(i),get_value_at(i-1));
          --i;
        }
        if (i!=j) return i;
        while(true){
          if (i+1>=datasize) break;
          if (!is_set(i+1)) break;
          if (is_less(k,get_key_at(i+1),ok,order(get_key_at(i+1)))) break;
          swap(get_value_at(i),get_value_at(i+1));
          ++i;
        }
        return i;
      }
      size_type inline reserve_node(
          const key_type&   k,
          const hash_type& ok){
        const hash_type mok = map(ok);
        return reserve_node(k,ok,mok);
      }
      size_type inline reserve_node(const key_type& k){
        const hash_type  ok = order( k);
        const size_type mok =   map(ok);
        assert(mok<datasize);
        return reserve_node(k,ok,mok);
      }

      size_type inline interpol(
          const hash_type& ok,
          const hash_type& olo,
          const hash_type& ohi,
          const size_type& lo,
          const size_type& hi
          ) const {
        auto lm             = long_mul(size_type(ok)-size_type(olo),hi-lo);
        const size_type n   = clz(get<0>(lm));
        const size_type m   = digits<size_type>()-n;
        const hash_type den = (size_type(ohi)-size_type(olo))>>m;
        const hash_type nom = (get<0>(lm)<<n)+(get<1>(lm)>>m);
        return lo+nom/den;
      }

      size_type inline find_node_interpol(
        const  key_type&   k,
        const hash_type&  ok,
        const size_type& mok,
              size_type   lo,
              hash_type  olo,
              bool is_set_lo,
              size_type   hi,
              size_type  ohi,
              bool is_set_hi
          ) const {
        assert(lo<=hi||datasize==0);
        size_type mi;
        //size_t i = 0;
        while(true) {
          //if (i++>16) cout << lo << " " << hi << endl;
          //cerr << lo << " " << hi << " " << datasize << endl;
          //cerr << frac(olo) << " " << frac(ohi) << endl;
          //cerr << frac(order(get_const_key_at(lo))) << " "
          //     << frac(order(get_const_key_at(hi))) << endl;
          if (hi-lo<8) {
            if (hi-lo<4) {
              if (hi-lo<2) {
                if (hi-lo<1) {
                  if (is_set(lo))
                    if (equator(k,get_const_key_at(lo))) return lo;
                  return ~size_type(0);
                } else {
                  if (is_set_lo&&is_set_hi)
                    return ~size_type(0);
                  if (is_set(lo)) if (equator(k,get_const_key_at(lo)))
                    return lo;
                  if (is_set(hi)) if (equator(k,get_const_key_at(hi)))
                    return hi;
                  return ~size_type(0);
                }
              } else {
                mi = lo + ((hi-lo)>>1);
              }
            } else {
              if (is_set_hi && is_set_lo) {
                mi = lo + ((hi-lo)>>1);
              } else if (is_set_lo) {
                mi = lo + ((hi-lo+2)>>2);
              } else if (is_set_hi) {
                mi = hi - ((hi-lo+2)>>2);
              } else {
                return ~size_type(0);
              }
            }
          } else {
            if (is_set_hi && is_set_lo) {
              mi = interpol(ok,olo,ohi,lo,hi);
            } else if (is_set_lo) {
              const size_type st = map_diff(ok,olo);
              mi = lo+st<hi?lo+st:hi;
            } else if (is_set_hi) {
              const size_type st = map_diff(ohi,ok);
              mi = lo+st<hi?hi-st:lo;
            } else {
              return ~size_type(0);
            }
            mi = clip(mi,lo+1,hi-1);
          }
          if (!is_set(mi)) {
            if (mi<mok) {
              lo = mi;
              is_set_lo=false;
              continue;
            }
            if (mi>mok) {
              hi = mi;
              is_set_hi=false;
              continue;
            }
            return ~size_type(0);
          }
          if (equator(k,get_const_key_at(mi))) return mi;
          const hash_type omi = order(get_const_key_at(mi));
          if (ok<omi) {
            hi = mi;
            ohi = omi;
            is_set_hi = true;
            continue;
          }
          if (ok>omi) {
            lo = mi;
            olo = omi;
            is_set_lo = true;
            continue;
          }
          if constexpr (is_injective<hash>::value) {
            return ~size_type(0);
          } else {
            if (comparator(k,get_const_key_at(mi))) {
              hi = mi;
              ohi = omi;
              is_set_hi = true;
              continue;
            }
            if (comparator(get_const_key_at(mi),k)) {
              lo = mi;
              olo = omi;
              is_set_lo = true;
              continue;
            }
          }
          return ~size_type(0);
        }
        return ~size_t(0);
      }

      size_type inline find_node(
          const key_type &   k,
          const hash_type&  ok,
          const size_type& mok)
        const {
        assert((mok<datasize)||(datasize==0));
        if (datasize==0) return ~size_type(0);
        //cout << "find_node " << frac(ok) << " " << datasize << endl;
        if (!is_set(mok)) return ~size_type(0);
        if (equator(get_const_key_at(mok),k)) return mok;
        const hash_type omi = order(get_const_key_at(mok));
        if (omi<ok) {
          return find_node_interpol(k,ok,mok,
              mok       ,omi          ,true ,
              datasize-1,~size_type(0),false);
        } else {
          return find_node_interpol(k,ok,mok,
              0         ,0            ,false,
              mok       ,          omi,true );
        }
      }

      size_type const inline find_node(
          const  key_type&  k,
          const size_type& ok
          ) const { return find_node(k,ok,map(ok)); }

      size_type const inline find_node(const key_type& k)
      const { return find_node(k,order(k)); }

      size_type const inline find_node_bruteforce(const key_type& k) const {
        for (size_type i=0;i!=datasize;++i)
          if (is_set(i)) if (equator(get_const_key_at(i),k)) return i;
        return ~size_type(0);
      }

      template<typename map_type>
      typename conditional<is_const<map_type>::value,
        const mapped_type&,
              mapped_type&>::type
      static inline const_noconst_at(map_type& hashmap,const key_type& k) {
        size_type i = hashmap.find_node(k);
        if (i<hashmap.datasize){
          assert(hashmap.is_set(i));
          return hashmap.data[i/8].values[i%8].second;
        } else throw std::out_of_range(
            std::string(typeid(hashmap).name())
            +".const_noconst_at("+typeid(k).name()+" k)"
            +"key not found, array index "
            +std::to_string(i)+" out of bounds"
           );
      }
      void const resize_out_of_place(const size_type& n){
        //cout << "resize out of place" << endl;
        size_type old_datasize = n;
        bucket* old_data = new bucket[(old_datasize+stride-1)/stride];
        for (size_type i=0;i!=(old_datasize+stride-1)/stride;++i)
          old_data[i].flag=0;
        num_data = 0;
        swap(old_data,data);
        swap(old_datasize,datasize);
        for (size_type n=0;n<old_datasize;++n) {
          const size_type i = n/stride;
          const size_type j = n%stride;
          if (old_data[i].flag&(flag_type(1)<<j)) {
            const key_type key = old_data[i].values[j].first;
            const size_type l = reserve_node(key);
            get_value_at(l) = old_data[i].values[j];
            //cout << "copied " << frac(order(get_const_key_at(l))) << endl;
            set(l);
          }
        }
        delete[] old_data;
      }
    public:
      // constructor
      patchmap(const size_type& datasize = 0)
        :datasize(datasize)
      {
        num_data = 0;
        if (datasize) data = new bucket[(datasize+stride-1)/stride];
        else          data = nullptr;
        for (size_type i=0;i!=(datasize+stride-1)/stride;++i) data[i].flag=0;
      }
      ~patchmap(){                                  // destructor
        delete[] data;
      }
      patchmap(patchmap&& other) noexcept  // move constructor
      {
        data = nullptr;
        datasize = 0;
        swap(data,other.data);
        swap(datasize,other.datasize);
      }
      template<
        class key_type_other,
        class mapped_type_other,
        class hash_other,
        class equal_other,
        class comp_other,
        class alloc_other
              >
      inline patchmap& operator=                   // copy assignment
        (const patchmap<
           key_type_other,
           mapped_type_other,
           hash_other,
           equal_other,
           comp_other
           //alloc_other
         >& other)
      {
        typedef patchmap<
           key_type_other,
           mapped_type_other,
           hash_other,
           equal_other,
           comp_other
           //alloc_other
         > other_type;
        num_data = other.num_data;
        datasize = other.datasize;
        if (datasize) data = new bucket[(datasize+stride-1)/stride];
        else data = nullptr;
        for (size_type i=0;i!=(datasize+stride-1)/stride;++i) data[i].flag=0;
        if constexpr (
            is_same<hash , hash_other>::value
          &&is_same<equal,equal_other>::value
          &&is_same<comp , comp_other>::value
          ){
          if constexpr (
              is_trivially_copyable<value_type>::value
            &&is_same<value_type,typename other_type::value_type>::value)
            memcpy(reinterpret_cast<void*>(data),
                   reinterpret_cast<void*>(other.data),
                   datasize*sizeof(value_type));
          else for (size_type i=0;i!=datasize;++i)
            get_value_at(i)=other.get_value_at(i);;
        } else {
          for (auto it=other.begin();it!=other.end();++it) insert(*it);
        }
      }
      patchmap(const patchmap& other){
        num_data = other.num_data;
        datasize = other.datasize;
        if (datasize) data = new bucket[(datasize+stride-1)/stride];
        else data = nullptr;
        if constexpr (is_trivially_copyable<value_type>::value) {
            memcpy(reinterpret_cast<void*>(data),
                   reinterpret_cast<void*>(other.data),
                   (datasize+7)/8*sizeof(bucket));
        } else {
          for (size_type i=0;i!=(datasize+stride-1)/stride;++i) data[i].flag=0;
          for (size_type i=0;i!=datasize;++i)
            if (is_set(i)) get_value_at(i)=other.get_value_at(i);
        }
      }
      inline patchmap& operator=                   // copy assignment
        (const patchmap& other)
      {
        return *this = patchmap(other);
      }
      inline patchmap& operator=                   // move assignment
        (patchmap&& other)
        noexcept{
        swap(data,other.data);
        swap(datasize,other.datasize);
        return *this;
      }
      size_type erase(
          const  key_type&   k,
          const hash_type&  ok,
          const size_type& mok){
        size_type i = find_node(k,ok,mok);
        if (i>=datasize) return 0;
        //cout << "erasing " << frac(ok) << endl;
        //cout << "found at " << i << endl;
        const size_type j = i;
        while(true){
          if (i+1==datasize) break;
          if (!is_set(i+1)) break;
          if (map(order(get_const_key_at(i+1)))>i) break;
          swap(get_value_at(i),get_value_at(i+1));
          ++i;
        }
        if (i==j){
          while(true){
            if (i==0) break;
            if (!is_set(i-1)) break;
            if (map(order(get_const_key_at(i-1)))<i) break;
            swap(get_value_at(i),get_value_at(i-1));
            --i;
          }
        }
        unset(i);
        get_value_at(i)=value_type();
        --num_data;
        assert(num_data<datasize);
        return 1;
      }
      size_type erase(const  key_type& k,const size_type& ok){
        const hash_type hint = map(ok);
        return erase(k,ok,hint);
      }
      size_type erase(const key_type & k){
        const size_type ok = order(k);
        return erase(k,ok);
      }
      void inline clear(){
        for (size_type i=0;i!=datasize/stride;++i) {
          for (size_type j=0;j!=stride;++j) data[i].values[j] = value_type();
          data[i].flag=0;
        }
        num_data=0;
      }
      void const resize(const size_type& n){
        resize_out_of_place(n); return;
      }
      size_type inline size() const { return num_data; }
      size_type const test_size() const {
        size_type test = 0;
        for (size_type i=0;i!=datasize;++i) test += is_set(i);
        return test;
      }
      void inline ensure_size() {
        if constexpr (!dynamic) return;
#if defined PATCHMAP_EXPANSIVE
        //if (num_data*9<datasize*7) return;
        if (num_data*5<datasize*4) return;
        if (datasize) resize(stride*(((12*datasize+6)/7+stride-1)/stride));
        //if (datasize) resize((7*datasize+2)/4);
        else resize(256);
        return;
#endif
        if (num_data*8 < datasize*7 ) return;
        size_type nextsize;
        if (datasize < 257) {
          if (datasize == 0) nextsize = 8;
          else nextsize = 2*datasize;
        } else {
          //nextsize = 50*datasize/31;
          //nextsize = 48*datasize/31;
          nextsize = 47*datasize/31;
          //nextsize = 45*datasize/31;
          nextsize = (nextsize+stride-1)/stride;
          nextsize*= stride;
        }
        resize(nextsize);
      }
      mapped_type& operator[](const key_type& k){
        const size_type i = find_node(k);
        if (i<datasize) return get_mapped_at(i);
        ensure_size();
        const size_type j = reserve_node(k);
        get_value_at(j) = {k,_mapped_type()};
        return get_mapped_at(j);
      }
      const mapped_type& operator[](const key_type& k) const {
        const size_type i = find_node(k);
        if (i<datasize) return get_const_mapped_at(i);
        else throw std::out_of_range(
            std::string(typeid(*this).name())
            +".const_noconst_at("+typeid(k).name()+" k)"
            +"key not found, array index "
            +std::to_string(i)+" out of bounds"
           );
      }
      mapped_type& at(const key_type& k){
        return const_noconst_at(*this,k);
      }
      const mapped_type& at(const key_type& k) const {
        return const_noconst_at(*this,k);
      }
      size_type const inline count(const key_type& k) const {
        return (find_node(k)<datasize);
      }
      template<class key_type_other,
               class mapped_type_other,
               class hash_other,
               class equal_other,
               class comp_other
               //class alloc_other
              >
      bool operator==(
          const patchmap<
            key_type_other,
            mapped_type_other,
            hash_other,
            equal_other,
            comp_other
            //alloc_other
            >& other)
      const {
        if (datasize!=other.datasize) return false;
        if constexpr (
            is_same<hash , hash_other>::value
          &&is_same<equal,equal_other>::value
          &&is_same<comp , comp_other>::value
          ){
          auto it0 = begin();
          auto it1 = other.begin();
          while (true){
            if (it0==end()) return true;
            if ((*it0)!=(*it1)) return false;
            ++it0;++it1;
          }
        } else {
          for (auto it=other.begin();it!=other.end();++it){
            if (nount(it->first)) if (at(it->first)==it->second) continue;
            return false;
          }
          return true;
        }
      }
      template<class key_type_other,
               class mapped_type_other,
               class hash_other,
               class equal_other,
               class comp_other
               //class alloc_other
              >
      bool operator!=(
          const patchmap<
            key_type_other,
            mapped_type_other,
            hash_other,
            equal_other,
            comp_other
            //alloc_other
            >& o)
      const{ return !((*this)==o); }
      equal key_eq() const{ // get key equivalence predicate
        return equal{};
      }
      comp key_comp() const{ // get key order predicate
        return comp{};
      }
      /*
      alloc get_allocator() const{
        return allocator;
      }
      */
      hash hash_function() const{ // get hash function
        return hash{};
      }
      template<bool is_const>
      class const_noconst_iterator {
        friend class patchmap;
        public:
          size_type hint;
          key_type key;
          typename conditional<is_const,const patchmap&,
                                              patchmap&>::type map;
        private:
          void inline update_hint() {
            if constexpr (!uphold_iterator_validity::value) return;
            if (hint<map.datasize)
              if (equator(map.get_const_key_at(hint),key)) return;
            hint = map.find_node(key,hint);
            if (hint>=map.datasize) hint = ~size_type(0);
          }
          void inline unsafe_increment() { // assuming hint is valid
            while (++hint<map.datasize) if (map.is_set(hint)) return;
          }
          void inline unsafe_decrement(){ // assuming hint is valid
            while (--hint<map.datasize) if (map.is_set(hint)) return;
          }
        public:
          //typedef typename alloc::difference_type difference_type;
          typedef ptrdiff_t difference_type;
          typedef pair<key_type,_mapped_type> value_type;
          typedef typename
            conditional<is_const,const value_type&,value_type&>::type
            reference;
          typedef typename
            conditional<is_const,const value_type*,value_type*>::type
            pointer;
          typedef std::bidirectional_iterator_tag iterator_category;
          const_noconst_iterator(){
            //cout << "constructor 0" << endl;
          }
          const_noconst_iterator(
            const size_t& hint,
            typename conditional<is_const,
                                 const patchmap*,
                                       patchmap*
                                >::type map)
            :hint(hint),key(key_type{}),map(map){
            //cout << "constructor 1 " << hint << endl;
          }
          const_noconst_iterator(
            const size_t& hint,
            const key_type& key,
            typename conditional<is_const,
                                 const patchmap*,
                                       patchmap*
                                >::type map)
            :hint(hint),key(key),map(map) {
              //cout << "constructor 2 " << hint << endl;
          }
          ~const_noconst_iterator(){
          //cout << "destructor of const_noconst_iterator " << is_const << endl;
          //cout << hint << endl;
          }
          // copy constructor
          template<bool is_const_other>
          const_noconst_iterator(const const_noconst_iterator<is_const_other>& o)
          :hint(o.hint),key(o.key),map(o.map){
            //cout << "copy constructor" << endl;
          }
          // move constructor
          template<bool is_const_other>
          const_noconst_iterator(
              const_noconst_iterator<is_const_other>&& o) noexcept{
            //cout << "move constructor" << endl;
            swap(hint,o.hint);
            swap(key,o.key);
            swap(map,o.map);
          }
          // copy assignment
          template<bool is_const_other>
          const_noconst_iterator<is_const>& operator=(
              const const_noconst_iterator<is_const_other>& other){
            //cout << "copy assignment" << endl;
            return  (*this=const_noconst_iterator<is_const>(other));
          }
          template<bool is_const_other>
          bool operator==(
              const const_noconst_iterator<is_const_other>& o) const {
            if ((hint>=map.datasize)&&(o.hint>=o.map.datasize)) return true;
            if ((hint>=map.datasize)||(o.hint>=o.map.datasize)) return false;
            // comparisons are only valid for iterators on same container
            // if ((&map)!=(&o.map)) return false;
            if (key!=o.key) return false;
            return true;
          }
          template<bool is_const_other>
          bool operator!=(
              const const_noconst_iterator<is_const_other>& o) const{
            return !((*this)==o);
          }
          template<bool is_const_other>
          bool operator< (
              const const_noconst_iterator<is_const_other>& o) const{
            if ((o.hint<o.map.datasize)){
              if (hint<map.datasize){
                return comp(key,o.key);
              }else{
                return false;
              }
            } else {
              return false;
            }
          }
          template<bool is_const_other>
          bool operator> (
              const const_noconst_iterator<is_const_other>& o) const{
            if ((o.hint<o.map.datasize)){
              if (hint<map.datasize){
                return (!comp(key,o.key))&&(!equal(key,o.key));
              }else{
                return true;
              }
            } else {
              return false;
            }
          }
          template<bool is_const_other>
          bool operator<=(
              const const_noconst_iterator<is_const_other>& o) const{
            if ((o.hint<o.map.datasize)){
              if (hint<map.datasize){
                return comp(key,o.key)||equal(key,o.key);
              }else{
                return false;
              }
            } else {
              return true;
            }
          }
          template<bool is_const_other>
          bool operator>=(
              const const_noconst_iterator<is_const_other>& o) const{
            if ((o.hint<o.map.datasize)){
              if (hint<map.datasize){
               return !comp(key,o.key);
              }else{
                return true;
              }
            } else {
              return true;
            }
          }
          const_noconst_iterator<is_const>& operator++(){   // prefix
            update_hint();
            unsafe_increment();
            return *this;
          }
          const_noconst_iterator<is_const> operator++(int){ // postfix
            update_hint();
            iterator pre(*this);
            unsafe_increment();
            return pre;
          }
          const_noconst_iterator<is_const>& operator--(){   // prefix
            update_hint();
            unsafe_decrement();
            return *this;
          }
          const_noconst_iterator<is_const> operator--(int){ // postfix
            update_hint();
            iterator pre(*this);
            unsafe_decrement();
            return pre;
          }
          reference operator*() {
            update_hint();
            return map.data[hint/8].values[hint%8];
          }
          pointer operator->() {
            update_hint();
            return &(map.data[hint/8].values[hint%8]);
          }
          reference operator*() const {
            if (hint<map->datasize) if (equator(map.get_const_key_at(hint),key))
              return map.data[hint/stride].values[hint%stride];
            const size_type i = map.find_node(key);
            return map.data[i/stride].value[i%stride];
          }
          pointer operator->() const {
            return &(operator*());
          }
    };
    typedef const_noconst_iterator<false> iterator;
    typedef const_noconst_iterator<true>  const_iterator;
    iterator begin() {
      const size_type i = find_first();
      return iterator(i,get_key_at(i),this);
    }
    const_iterator begin() const {
      const size_type i = find_first();
      return const_iterator(i,get_const_key_at(i),this);
    }
    const_iterator cbegin() const {
      const size_type i = find_first();
      return const_iterator(i,get_const_key_at(i),this);
    }
    iterator end() {
      const size_type i = find_first();
      return iterator(~size_type(0),this);
    }
    const_iterator end() const {
      return const_iterator(~size_type(0),this);
    }
    const_iterator cend() const {
      return const_iterator(~size_type(0),this);
    }
    // void swap(unpatchmap&); // TODO
    size_type max_size()         const {
      return std::numeric_limits<size_type>::max();
    }
    bool empty()                 const {return (num_data==0);}
    size_type bucket_count()     const {return datasize;}
    size_type max_bucket_count() const {
      return std::numeric_limits<size_type>::max();
    }
    void rehash(const size_type& n) { if (n>=size()) resize(n); }
    void reserve(const size_type& n){ if (3*n>=2*(size()+1)) resize(n*3/2); }
    pair<iterator,bool> insert ( const value_type& val ){
      const size_type i = find_node(key_of(val));
      if (i<datasize) return {iterator(i,key_of(val),this),false};
      ensure_size();
      const size_type j = reserve_node(key_of(val));
      get_value_at(j) = val;
      return {{j,key_of(val),this},true};
    }
    template <class P>
    pair<iterator,bool> insert ( P&& val ){
      const size_type i = find_node(key_of(val));
      if (i<datasize) return {iterator(i,key_of(val),this),false};
      ensure_size();
      const size_type j = reserve_node(key_of(val));
      if constexpr (is_same<void,mapped_type>::value)
        get_value_at(j) = {val,true_type{}};
      else
        get_value_at(j) = val;
      return {{j,key_of(val),this},true};
    }
    iterator insert ( const_iterator hint, const value_type& val ){
      return insert(val); // hint is useless
    }
    template <class P>
    iterator insert ( const_iterator hint, P&& val ) {
      return insert(val); // hint is useless
    }
    template <class InputIterator>
    void insert ( InputIterator first, InputIterator last ){
      for (auto it(first);it!=last;++it){
        insert(*it);
      }
    }
    void insert ( initializer_list<value_type> il ){
      insert(il.begin(),il.end());
    }
    template <class... Args>
    pair<iterator, bool> emplace ( Args&&... args ){
      insert(value_type(args...));
    }
    template <class... Args>
    iterator emplace_hint(const_iterator position,Args&&... args){
      insert(position,value_type(args...));
    }
    pair<iterator,iterator> equal_range(const key_type& k){
      const size_type i = find_node(k);
      if (i>=datasize) return {end(),end()};
      iterator lo(i,get_const_key_at(i),this);
      iterator hi(lo);
      ++hi;
      return {lo,hi};
    }
    pair<const_iterator,const_iterator>
    equal_range ( const key_type& k ) const {
      const size_type i = find_node(k);
      if (i>=datasize) return {cend(),cend()};
      iterator lo(i,get_const_key_at(i),this);
      iterator hi(lo);
      ++hi;
      return {lo,hi};
    }
    float load_factor() const noexcept{
      return float(num_data)/float(datasize);
    }
    float const max_load_factor() const noexcept {
      return 1.0;
    }
    template<bool is_const>
    iterator erase(const_noconst_iterator<is_const> position){
      iterator it(position);
      ++it;
      erase(position.key);//,position.hint);
      return it;
    }
    template<bool is_const>
    iterator erase(
        const_noconst_iterator<is_const> first,
        const_noconst_iterator<is_const> last){
      for (auto it=first;it!=last;it=erase(it));
    }
    [[deprecated(
        "disabled for performance reasons"
    )]] void max_load_factor(float z) {
      // m = number of elements ; n = number of buckets
      // n*load_factor >= m
      // n*mul_n >= m*mul_m
      size_type mul_n =  ceil(z*16);
      size_type mul_m = floor(z*16);
    }
  };

  /* TODO
  template<class K,
           class T,
           class hash=hash_functor<K>,
           class equal = std::equal_to<K>,
           class comp = std::less<K>,
           class A = std::allocator<std::pair<K,T>>
          >
  void swap(patchmap<K,K,hash,equal,comp,A>&,
            patchmap<K,K,hash,equal,comp,A>&);
  */

  template<class key_type    = int,  // int is the default, why not
           class mapped_type = int,  // int is the default, why not
           class hash        = hash_functor<key_type>,
           class equal       = std::equal_to<key_type>,
           class comp        = typename conditional<is_injective<hash>::value,
                                                    dummy_comp<key_type>,
                                                    std::less<key_type>>::type
           /*class alloc       = typename boost::container::allocator<
             typename conditional<
               std::is_same<mapped_type,void>::value,
               std::pair<key_type,true_type>,
               std::pair<key_type,mapped_type>
             >::type,2>*/
          >
  using static_patchmap =
    patchmap<key_type,mapped_type,hash,equal,comp,false>;

  template<class key_type,           // unordered_map has no default key_type
           class mapped_type,        // unordered_map has no default mapped_type
           class hash        = hash_functor<key_type>,
           class equal       = std::equal_to<key_type>,
           //class alloc       = typename // mapped_type must not be void
           //  boost::container::allocator<std::pair<key_type,mapped_type>,2>,
           class comp        = typename conditional<is_injective<hash>::value,
             dummy_comp<key_type>,typename std::less<key_type>::type>::type
          >
  using unordered_map =
    patchmap<key_type,mapped_type,hash,equal,comp>;

  template<class key_type,           // unordered_set has no default key_type
           class hash        = hash_functor<key_type>,
           class equal       = std::equal_to<key_type>,
           //class alloc       = typename
           //  boost::container::allocator<pair<key_type,true_type>,2>,
           class comp        = typename conditional<is_injective<hash>::value,
             dummy_comp<key_type>,typename std::less<key_type>::type>::type
          >
  using unordered_set =
    patchmap<key_type,void,hash,equal,comp>;

}
#endif // ORDERED_PATCH_MAP_H


/**
 * @file test-jump32.cpp
 *
 * @brief test jump function.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include <time.h>
#include <errno.h>
#include "mtgp32-calc-poly.hpp"
#include "mtgp-calc-jump.hpp"
#include "mtgp32-fast-jump.h"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/ZZ.h>

using namespace NTL;
using namespace std;

static void test(mtgp32_fast_t * mtgp, GF2X& poly);
static int check(mtgp32_fast_t *a, mtgp32_fast_t *b);
static void print_state(mtgp32_fast_t * a, mtgp32_fast_t * b);
static void print_sequence(mtgp32_fast_t * a, mtgp32_fast_t * b);
static void speed(mtgp32_fast_t * mtgp, GF2X& characteristic);

/**
 * test main
 * @param[in] argc number of arguments.
 * @param[in] argv an array of arguments.
 * @return 0 if normal, others abnormal.
 */
int main(int argc, char * argv[]) {
    if (argc <= 3) {
	printf("%s -s|-c mexp no.\n", argv[0]);
	return -1;
    }
    errno = 0;
    GF2X characteristic;
    mtgp32_params_fast_t *params;
    mtgp32_fast_t mtgp;
    uint32_t seed = 0;
    //    int rc;
    int mexp = strtol(argv[2], NULL, 10);
    int no = strtol(argv[3], NULL, 10);
    if (no < 0 || no > 127) {
	cout << "error in no" << endl;
	return 1;
    }
    switch (mexp) {
    case 11213:
        params = mtgp32_params_fast_11213;
        break;
    case 23209:
        params = mtgp32_params_fast_23209;
        break;
    case 44497:
        params = mtgp32_params_fast_44497;
        break;
    default:
        printf("%s: mexp no.\n", argv[0]);
        printf("mexp shuould be 11213, 23209 or 44497\n");
        return 2;
    }
    params += no;
    mtgp32_init(&mtgp, params, seed);
    calc_characteristic(characteristic, &mtgp);
    if (argv[1][1] == 's') {
	speed(&mtgp, characteristic);
    } else {
	test(&mtgp, characteristic);
    }
    mtgp32_free(&mtgp);
    return 0;
}

/**
 * check speed
 * @param[in] mtgp generator
 * @param[in] characteristic characteristic polynomial
 */
static void speed(mtgp32_fast_t * mtgp, GF2X& characteristic)
{
    long step = 10000;
    int exp = 4;
    ZZ test_count;
    string jump_string;
    clock_t start;
    double elapsed1;
    double elapsed2;

    test_count = step;
    for (int i = 0; i < 10; i++) {
	start = clock();
	calc_jump(jump_string, test_count, characteristic);
	elapsed1 = clock() - start;
	elapsed1 = elapsed1 * 1000 / CLOCKS_PER_SEC;
	cout << "mexp "
	     << setw(5)
	     << mtgp->params.mexp
	     << " jump 10^"
	     << setfill('0') << setw(2)
	     << exp
	     << " steps calc_jump:"
	     << setfill(' ') << setiosflags(ios::fixed)
	     << setw(6) << setprecision(3)
	     << elapsed1
	     << "ms"
	     << endl;
	start = clock();

	for (int j = 0; j < 10; j++) {
	    mtgp32_fast_jump(mtgp, jump_string.c_str());
	}
	elapsed2 = clock() - start;
	elapsed2 = elapsed2 * 1000 / 10 / CLOCKS_PER_SEC;
	cout << "mexp "
	     << setw(5)
	     << mtgp->params.mexp
	     << " jump 10^"
	     << setfill('0') << setw(2)
	     << exp
	     << " steps MTGP_jump:"
	     << setfill(' ') << setiosflags(ios::fixed)
	     << setw(6) << setprecision(3)
	     << elapsed2
	     << "ms"
	     << endl;
	test_count *= 100;
	exp += 2;
    }
}

/**
 * equality check
 * @param[in] a mtgp generator
 * @param[in] b mtgp generator
 * @return 0 if equal
 */
static int check(mtgp32_fast_t *a, mtgp32_fast_t *b)
{
    int check = 0;
    for (int i = 0; i < 100; i++) {
	uint32_t x = mtgp32_genrand_uint32(a);
	uint32_t y = mtgp32_genrand_uint32(b);
	if (x != y) {
	    print_state(a, b);
	    print_sequence(a, b);
	    check = 1;
	    break;
	}
    }
    if (check == 0) {
      cout << "OK!" << endl;
    } else {
      cout << "NG!" << endl;
    }
    return check;
}

/**
 * print internal state of two mtgp generators for checking by human eyes.
 * @param[in] a mtgp generator
 * @param[in] b mtgp generator
 */
static void print_state(mtgp32_fast_t *a, mtgp32_fast_t * b)
{
  int large_size = a->status->large_size;
  cout << "idx = " << dec << a->status->idx
       << "   " << dec << b->status->idx
       << endl;
    for (int i = 0; (i < 10) && (i < large_size); i++) {
      cout << setfill('0') << setw(8) << hex
	   << a->status->array[(i + a->status->idx) % large_size];
      cout << " ";
      cout << setfill('0') << setw(8) << hex
	   << b->status->array[(i + b->status->idx) % large_size];
      cout << endl;
    }
}

/**
 * print output sequences of two mtgp generators for checking by human eyes.
 * @param[in] a mtgp generator
 * @param[in] b mtgp generator
 */
static void print_sequence(mtgp32_fast_t *a, mtgp32_fast_t * b)
{
    for (int i = 0; i < 25; i++) {
	uint32_t c, d;
	c = mtgp32_genrand_uint32(a);
	d = mtgp32_genrand_uint32(b);
	cout << setfill('0') << setw(8) << hex << c;
	cout << " " << setfill('0') << setw(8) << hex << d;
	cout << endl;
    }
}

/**
 * sanity check
 * @param[in] mtgp generator
 * @param[in] characteristic characteristic polynomial
 */
static void test(mtgp32_fast_t * mtgp, GF2X& characteristic)
{
    mtgp32_fast_t new_mtgp_z;
    mtgp32_fast_t * new_mtgp = &new_mtgp_z;
//    uint32_t seed[] = {1, 998102, 1234, 0, 5};
//    uint32_t seed[20] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    long steps[] = {1, 2, mtgp->status->size + 1,
		    mtgp->status->size * 32 - 1,
		    mtgp->status->size * 32 + 1,
		    3003,
		    200004,
		    10000005};
    int steps_size = sizeof(steps) / sizeof(long);
    ZZ test_count;
    string jump_string;
    mtgp32_params_fast_t params = mtgp->params;
    mtgp32_init(new_mtgp, &params, 0);
    mtgp32_genrand_uint32(mtgp);
    mtgp32_genrand_uint32(mtgp);
    mtgp32_genrand_uint32(mtgp);
    /* plus jump */
    for (int index = 0; index < steps_size; index++) {
//	mtgp_init_gen_rand(mtgp, seed[index]);
	test_count = steps[index];
	cout << "mexp " << dec << mtgp->params.mexp << " jump "
	     << test_count << " steps" << endl;
//	*new_mtgp = *mtgp;
	mtgp32_copy(new_mtgp, mtgp);
	for (long i = 0; i < steps[index]; i++) {
	    mtgp32_genrand_uint32(mtgp);
	}
	calc_jump(jump_string, test_count, characteristic);
#if defined(DEBUG)
	cout << "jump string:" << jump_string << endl;
	cout << "before jump:" << endl;
	print_state(new_mtgp, mtgp);
#endif
	mtgp32_fast_jump(new_mtgp, jump_string.c_str());
#if defined(DEBUG)
	cout << "after jump:" << endl;
	print_state(new_mtgp, mtgp);
#endif
	if (check(new_mtgp, mtgp)) {
	    return;
	}
    }
}


#ifndef PA_MATH_INTEGRATORS_HPP
#define PA_MATH_INTEGRATORS_HPP

#include <variant>

#include <boost/numeric/odeint.hpp>

#include <pa/math/types.hpp>

namespace pa
{
using value_type                              = scalar ;
using state_type                              = vector4;
using algebra_type                            = boost::numeric::odeint::vector_space_algebra;

using euler_integrator                        = boost::numeric::odeint::euler                  <   state_type, value_type, state_type, value_type, algebra_type>;
using modified_midpoint_integrator            = boost::numeric::odeint::modified_midpoint      <   state_type, value_type, state_type, value_type, algebra_type>;
using runge_kutta_4_integrator                = boost::numeric::odeint::runge_kutta4           <   state_type, value_type, state_type, value_type, algebra_type>;
using runge_kutta_cash_karp_54_integrator     = boost::numeric::odeint::runge_kutta_cash_karp54<   state_type, value_type, state_type, value_type, algebra_type>;
using runge_kutta_dormand_prince_5_integrator = boost::numeric::odeint::runge_kutta_dopri5     <   state_type, value_type, state_type, value_type, algebra_type>;
using runge_kutta_fehlberg_78_integrator      = boost::numeric::odeint::runge_kutta_fehlberg78 <   state_type, value_type, state_type, value_type, algebra_type>;
using adams_bashforth_2_integrator            = boost::numeric::odeint::adams_bashforth        <2, state_type, value_type, state_type, value_type, algebra_type>;
using adams_bashforth_moulton_2_integrator    = boost::numeric::odeint::adams_bashforth_moulton<2, state_type, value_type, state_type, value_type, algebra_type>;

using variant_integrator                      = std::variant<
  euler_integrator                            ,
  modified_midpoint_integrator                ,
  runge_kutta_4_integrator                    ,
  runge_kutta_cash_karp_54_integrator         ,
  runge_kutta_dormand_prince_5_integrator     ,
  runge_kutta_fehlberg_78_integrator          ,
  adams_bashforth_2_integrator                ,
  adams_bashforth_moulton_2_integrator        >;
}

#endif


#include "osgviewer/osgviewer.hpp"
#include "sco/expr_ops.hpp"
#include "sco/modeling_utils.hpp"
#include "trajopt/collision_checker.hpp"
#include "trajopt/collision_terms.hpp"
#include "trajopt/common.hpp"
#include "trajopt/plot_callback.hpp"
#include "trajopt/problem_description.hpp"
#include "trajopt/rave_utils.hpp"
#include "trajopt/trajectory_costs.hpp"
#include "utils/clock.hpp"
#include "utils/config.hpp"
#include "utils/eigen_conversions.hpp"
#include "utils/stl_to_string.hpp"
#include "sco/expr_op_overloads.hpp"
#include <boost/assign.hpp>
#include <boost/foreach.hpp>
#include <ctime>
#include <openrave-core.h>
#include <openrave/openrave.h>
#include <boost/timer.hpp>

using namespace trajopt;
using namespace std;
using namespace OpenRAVE;
using namespace util;
using namespace boost::assign;
using namespace Eigen;

/**
Let \f$ \bar \theta = (\theta_t + \theta_{t+1})/2 \f$. Then
\f{align*}{
  x_{t+1} - x_t = L(sin(\bar \theta), cos(\bar \theta))
}
*/
struct CarError : public VectorOfVector {
  VectorXd operator()(const VectorXd& a) const {
    double x0 = a(0), y0 = a(1), t0 = a(2), x1 = a(3), y1 = a(4), t1 = a(5), L = a(6);
    double ang = (t0+t1)/2;
    return Vector2d(x1 - x0 - L*cos(ang), y1 - y0 - L*sin(ang));
  }
};

int main(int argc, char** argv)
{

  bool plotting=false, verbose=false;
  string envfile;
  int decimation;

  {
    Config config;
    config.add(new Parameter<bool>("plotting", &plotting, "plotting"));
    config.add(new Parameter<bool>("verbose", &verbose, "verbose"));
    config.add(new Parameter<string>("envfile", &envfile, "jagged_narrow.xml"));
    config.add(new Parameter<int>("decimation", &decimation, "plot every n"));
    CommandParser parser(config);
    parser.read(argc, argv);
  }

  RaveInitialize(false, verbose ? Level_Debug : Level_Info);
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  OSGViewerPtr viewer = OSGViewer::GetOrCreate(env);
  assert(viewer);

  env->Load(string(DATA_DIR) + "/" + envfile);
  RobotBasePtr robot = GetRobot(*env);
  RobotAndDOFPtr rad(new RobotAndDOF(robot, vector<int>(), 11, OR::Vector(0,0,1)));

//  viewer->SetAllTransparency(.3);

  int n_dof = 3;
  int n_steps;
  Vector3d start,goal;
  if (envfile == "jagged_narrow.xml") {
    start = Vector3d(-2,-2,0);
    goal = Vector3d(4,2,0);
    n_steps = 19;
  }
  else if (envfile == "cargapprob.env.xml") {
    start = Vector3d(-2.5,0,0);
    goal = Vector3d(6.5,0,-M_PI);
    n_steps = 55;
    vector<KinBodyPtr> bodies; env->GetBodies(bodies);
//    for (int i=0; i < bodies.size(); ++i) if (bodies[i]->GetName() == "obstacle") viewer->SetTransparency(bodies[i], .5);
        for (int i=0; i < bodies.size(); ++i) if (bodies[i]->GetName() == "ground") CollisionChecker::GetOrCreate(*env)->ExcludeCollisionPair(*bodies[i]->GetLinks()[0], *robot->GetLinks()[0]);
    vector<GraphHandlePtr> handles;
    rad->SetDOFValues(toDblVec(start));
    handles.push_back(viewer->PlotKinBody(robot));
    rad->SetDOFValues(toDblVec(goal));
    handles.push_back(viewer->PlotKinBody(robot));
    Vector3d away(100,100,100);
    rad->SetDOFValues(toDblVec(away));
    viewer->Idle();
  }
  else {
    throw runtime_error("asdf");
  }

  OptProbPtr prob(new OptProb());
  VarArray trajvars;
  AddVarArray(*prob, n_steps, n_dof, "j", trajvars);

  // penalize dx^2 + dy^2 + dtheta^2
  VectorXd vel_coeffs = VectorXd::Ones(3);
  prob->addCost(CostPtr(new JointVelCost(trajvars, vel_coeffs)));

  double maxdtheta=.2;
  for (int i = 0; i < n_steps-1; ++i) {
    AffExpr vel = trajvars(i+1,2) - trajvars(i,2);
    prob->addLinearConstraint(vel - maxdtheta, INEQ);
    prob->addLinearConstraint(-vel - maxdtheta, INEQ);
  }

  // lower bound on length per step
  double length_lb = (goal - start).norm() / (n_steps-1);
  // length per step variable
  Var lengthvar = prob->createVariables(singleton<string>("speed"), singleton<double>(length_lb),singleton<double>(INFINITY))[0];

  // Car dynamics constraints
  for (int i=0; i < n_steps-1; ++i) {
    VarVector vars0 = trajvars.row(i), vars1 = trajvars.row(i+1);
    VectorOfVectorPtr f(new CarError());
    VectorXd coeffs = 1*VectorXd::Ones(2);
    VarVector vars = concat(vars0, vars1);  vars.push_back(lengthvar);
    prob->addConstraint(ConstraintPtr(new ConstraintFromFunc(f, vars, coeffs, EQ, (boost::format("car%i")%i).str())));
    if (i > 0) {
      prob->addCost(CostPtr(new CollisionCost(.05, 10, rad, vars0, vars1)));
//      prob->addCost(CostPtr(new CollisionCost(.1, 10, rad, vars0)));
    }
  }

  // start and end constraints
  for (int j=0; j < n_dof; ++j) {
    prob->addLinearConstraint(exprSub(AffExpr(trajvars(0,j)), start[j]), EQ);
  }
  for (int j=0; j < n_dof; ++j) {
    prob->addLinearConstraint(exprSub(AffExpr(trajvars(n_steps-1,j)), goal[j]), EQ);
  }

  // optimization
  BasicTrustRegionSQP opt(prob);

  CollisionChecker::GetOrCreate(*env)->SetContactDistance(.15);

  // straight line initialization
  MatrixXd initTraj(n_steps, n_dof);
  for (int idof = 0; idof < n_dof; ++idof) {
    initTraj.col(idof) = VectorXd::LinSpaced(n_steps, start[idof], goal[idof]);
  }
  DblVec initVec = trajToDblVec(initTraj);
  // length variable
  initVec.push_back(length_lb);
  opt.initialize(initVec);

  TrajPlotter plotter(env, rad, trajvars);
  plotter.Add(prob->getCosts());
  if (plotting) opt.addCallback(boost::bind(&TrajPlotter::OptimizerCallback, boost::ref(plotter), _1, _2));
  plotter.SetDecimation(decimation);
  plotter.AddLink(rad->GetAffectedLinks()[0]);

  boost::timer timer;
  opt.optimize();
  cout << timer.elapsed() << " elapsed" << endl;

  RaveDestroy();

}


#include "DVMBase.hpp"
#include <armadillo>
#include <cassert>
#include <cmath>
#include <iomanip>
#include <iostream>

DVMBase::DVMBase(XmlHandler &xml, const std::string &timestamp)
    : m_vortex(xml, timestamp), m_vortsheet(xml, timestamp),
      m_probe(xml, timestamp)
{
	m_step = 0;

	// Some helpers so we can do less typing
	auto getVal = [&xml](const char *l, const char *p) {
		return xml.getValueAttribute(l, p);
	};

	auto getStr = [&xml](const char *l, const char *p) {
		return xml.getStringAttribute(l, p);
	};

	m_nu = getVal("constants", "nu");

	m_dt = getVal("time", "dt");
	m_steps = getVal("time", "steps");

	m_Ux = getVal("flow", "ux");
	m_Uz = getVal("flow", "uz");
	m_Ur = std::sqrt(m_Ux * m_Ux + m_Uz * m_Uz);

	auto seed = xml.getIntAttribute("constants", "seed");
	m_rand.seed(seed);

	auto scheme = getStr("time", "scheme");
	if (scheme.compare("euler") == 0) {
		m_scheme = Scheme::Euler;
	} else if (scheme.compare("RK3") == 0) {
		m_scheme = Scheme::RK3;
	} // Invalid cases dealt with by the xml handler

	auto surfacecross = getStr("algorithms", "surface_crossing");
	if (surfacecross.compare("DELETE") == 0) {
		m_surfcross = SurfaceCross::DELETE;
	} else if (surfacecross.compare("ABSORB") == 0) {
		m_surfcross = SurfaceCross::ABSORB;
	} else if (surfacecross.compare("REFLECT") == 0) {
		m_surfcross = SurfaceCross::REFLECT;
	} // Invalid cases dealt with by the xml handler
}

void DVMBase::solve()
{
	// Timeloop
	for (unsigned j = 1; j <= get_steps(); j++) {

		increment_step();
		compute_step();

		// Output
		write_outputs();

		// Screen output
		std::cout << "Simulation time          = " << get_time() << "\tStep "
		          << j << "/" << get_steps() << std::endl;
		std::cout << "Number of vortex blobs   = " << m_vortex.size()
		          << std::endl;
	}
}

void DVMBase::compute_step()
{
	//************** Advection substep*****************//
	convect();

	// remesh();
	// Fror remeshing both triangulated meshes (with RBF) and structured meshes
	// with morgenthal M6 interpolators
	// will be used.

	//************** Diffusion substep ****************//
	// The diffussion problem in an infinite domain
	m_vortex.diffusion_random_walk(m_rand, m_nu, m_dt);

	// A diffussion problem with only a flux of vorticity in the boundaries
	// dgamma/dn=a
	VortexBlobs NewVortices = m_vortsheet.release_nascent_vortices_rw(m_rand);
	m_vortex.append_vortices(NewVortices);

	// If a large time step is used some vortices may cross the boundary due to
	// random walk!
	// Care is taken of these vortices by 1) Deleting them, 2) Absorbing them,
	// 3) Reflecting them back to the flow
	m_vortsheet.reflect(m_vortex);

	//********************* Computing Loads/ Moving body *********************//
	// We compute the loads at the end of the time step
	m_vortsheet.compute_loads(m_Ur);

	//********************* Merge/Delete Vortices ****************************//
}

double DVMBase::get_time()
{
	return m_time;
}

unsigned DVMBase::get_steps()
{
	return m_steps;
}

void DVMBase::increment_step()
{
	m_step++;
	m_time = m_step * m_dt;
}

void DVMBase::write_outputs()
{
	m_vortex.write_step(m_time, m_step);

	m_vortsheet.write_step(m_time);

	m_probe.write_step(m_time);
}

void DVMBase::convect()
{
	switch (m_scheme) {
	case Scheme::Euler:
		// Find the free-space velocity
		m_vortex.biotsavart();
		// Find the boundary-imposed velocities-these two should be combined
		// together into one
		m_vortsheet.solvevortexsheet(m_vortex);
		m_vortsheet.vortexsheetbc(m_vortex);

		// Added them together
		m_vortex.m_x += (m_vortex.m_u + m_vortex.m_uvs + m_Ux) * m_dt;
		m_vortex.m_z += (m_vortex.m_w + m_vortex.m_wvs + m_Uz) * m_dt;
		break;
	case Scheme::RK3:
		// coefficients for Low Storage-Runge Kutta 3rd
		const double a[3] = {0, -17. / 32, -32. / 27};
		const double b[3] = {1. / 4, 8. / 9, 3. / 4};

		// execute 3 stages of Low Storage Runge-Kutta 3rd
		Vector q1 = arma::zeros(m_vortex.size());
		Vector q2 = arma::zeros(m_vortex.size());

		for (int i = 0; i < 3; ++i) {

			// Compute the right-hand side of the system
			// Find the free-space velocity
			m_vortex.biotsavart();
			// Find the boundary-imposed velocities-these two may be combined
			// together into one
			m_vortsheet.solvevortexsheet(m_vortex);
			m_vortsheet.vortexsheetbc(m_vortex);

			q1 = a[i] * q1 + (m_vortex.m_u + m_vortex.m_uvs + m_Ux) * m_dt;
			q2 = a[i] * q2 + (m_vortex.m_w + m_vortex.m_wvs + m_Uz) * m_dt;

			m_vortex.m_x += b[i] * q1;
			m_vortex.m_z += b[i] * q2;
		}
		break;
	}
}


#include "super4pcs/algorithms/super4pcs.h"
#include "super4pcs/io/io.h"
#include "super4pcs/utils/geometry.h"

#include <Eigen/Dense>

int main(int argc, char **argv) {
  using namespace GlobalRegistration;
  using namespace std;

  vector<Point3D> set1, set2;
  vector<Eigen::Matrix2f> tex_coords1, tex_coords2;
  vector<typename Point3D::VectorType> normals1, normals2;
  vector<tripple> tris1, tris2;
  vector<std::string> mtls1, mtls2;

  IOManager iomanager;

  // dummy call, to test symbols accessibility
  iomanager.ReadObject("", set1, tex_coords1, normals1, tris1, mtls1);

  // check availability of the Utils functions
  if (tris1.size() == 0)
    Utils::CleanInvalidNormals(set1, normals1);

  // Our matcher.
  Match4PCSOptions options;

  // Set parameters.
  Match4PCSBase::MatrixType mat;
  double overlap (1);
  options.configureOverlap(overlap);

  typename Point3D::Scalar score = 0;

  constexpr Utils::LogLevel loglvl = Utils::Verbose;
  using TrVisitorType = Match4PCSBase::DummyTransformVisitor;
  using SamplerType   = Match4PCSBase::DefaultSampler;
  Utils::Logger logger(loglvl);

  MatchSuper4PCS matcher(options, logger);
  score = matcher.ComputeTransformation<SamplerType,TrVisitorType>(set1, &set2, mat);

  logger.Log<Utils::Verbose>( "Score: ", score );

  iomanager.WriteMatrix("output.map", mat.cast<double>(), IOManager::POLYWORKS);

  return 0;
}


/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _POLYNOMIAL_TRAJ_H
#define _POLYNOMIAL_TRAJ_H

#include <Eigen/Eigen>
#include <vector>

using std::vector;

class PolynomialTraj {
private:
  vector<double> times;        // time of each segment
  vector<vector<double>> cxs;  // coefficient of x of each segment, from high order to low
  vector<vector<double>> cys;  // coefficient of y of each segment
  vector<vector<double>> czs;  // coefficient of z of each segment

  double time_sum;
  int num_seg;

  /* evaluation */
  vector<Eigen::Vector3d> traj_vec3d;
  double length;

public:
  PolynomialTraj(/* args */) {
  }
  ~PolynomialTraj() {
  }

  void reset() {
    times.clear(), cxs.clear(), cys.clear(), czs.clear();
    time_sum = 0.0, num_seg = 0;
  }

  void addSegment(vector<double> cx, vector<double> cy, vector<double> cz, double t) {
    cxs.push_back(cx), cys.push_back(cy), czs.push_back(cz), times.push_back(t);
  }

  void init() {
    num_seg = times.size();
    time_sum = 0.0;
    for (int i = 0; i < times.size(); ++i) {
      time_sum += times[i];
    }
  }

  Eigen::Vector3d evaluate(double t) {
    /* detetrmine segment num */
    int idx = 0;
    while (times[idx] < t) {
      t -= times[idx];
      ++idx;
    }

    /* evaluation */
    int order = cxs[idx].size();
    Eigen::VectorXd cx(order), cy(order), cz(order), tv(order);
    for (int i = 0; i < order; ++i) {
      cx(i) = cxs[idx][i], cy(i) = cys[idx][i], cz(i) = czs[idx][i];
      tv(order - 1 - i) = std::pow(t, double(i));
    }

    Eigen::Vector3d pt;
    pt(0) = tv.dot(cx), pt(1) = tv.dot(cy), pt(2) = tv.dot(cz);
    return pt;
  }

  Eigen::Vector3d evaluateVel(double t) {
    /* detetrmine segment num */
    int idx = 0;
    while (times[idx] < t) {
      t -= times[idx];
      ++idx;
    }

    /* evaluation */
    int order = cxs[idx].size();
    Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);

    /* coef of vel */
    for (int i = 0; i < order - 1; ++i) {
      vx(i) = double(i + 1) * cxs[idx][order - 2 - i];
      vy(i) = double(i + 1) * cys[idx][order - 2 - i];
      vz(i) = double(i + 1) * czs[idx][order - 2 - i];
    }
    double ts = t;
    Eigen::VectorXd tv(order - 1);
    for (int i = 0; i < order - 1; ++i)
      tv(i) = pow(ts, i);

    Eigen::Vector3d vel;
    vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
    return vel;
  }

  Eigen::Vector3d evaluateAcc(double t) {
    /* detetrmine segment num */
    int idx = 0;
    while (times[idx] < t) {
      t -= times[idx];
      ++idx;
    }

    /* evaluation */
    int order = cxs[idx].size();
    Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);

    /* coef of vel */
    for (int i = 0; i < order - 2; ++i) {
      ax(i) = double((i + 2) * (i + 1)) * cxs[idx][order - 3 - i];
      ay(i) = double((i + 2) * (i + 1)) * cys[idx][order - 3 - i];
      az(i) = double((i + 2) * (i + 1)) * czs[idx][order - 3 - i];
    }
    double ts = t;
    Eigen::VectorXd tv(order - 2);
    for (int i = 0; i < order - 2; ++i)
      tv(i) = pow(ts, i);

    Eigen::Vector3d acc;
    acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
    return acc;
  }

  /* for evaluating traj, should be called in sequence!!! */
  double getTimeSum() {
    return this->time_sum;
  }

  vector<Eigen::Vector3d> getTraj() {
    double eval_t = 0.0;
    traj_vec3d.clear();
    while (eval_t < time_sum) {
      Eigen::Vector3d pt = evaluate(eval_t);
      traj_vec3d.push_back(pt);
      eval_t += 0.01;
    }
    return traj_vec3d;
  }

  double getLength() {
    length = 0.0;

    Eigen::Vector3d p_l = traj_vec3d[0], p_n;
    for (int i = 1; i < traj_vec3d.size(); ++i) {
      p_n = traj_vec3d[i];
      length += (p_n - p_l).norm();
      p_l = p_n;
    }
    return length;
  }

  double getMeanVel() {
    double mean_vel = length / time_sum;
  }

  double getAccCost() {
    double cost = 0.0;
    int order = cxs[0].size();

    for (int s = 0; s < times.size(); ++s) {
      Eigen::Vector3d um;
      um(0) = 2 * cxs[s][order - 3], um(1) = 2 * cys[s][order - 3], um(2) = 2 * czs[s][order - 3];
      cost += um.squaredNorm() * times[s];
    }

    return cost;
  }

  double getJerk() {
    double jerk = 0.0;

    /* evaluate jerk */
    for (int s = 0; s < times.size(); ++s) {
      Eigen::VectorXd cxv(cxs[s].size()), cyv(cys[s].size()), czv(czs[s].size());
      /* convert coefficient */
      int order = cxs[s].size();
      for (int j = 0; j < order; ++j) {
        cxv(j) = cxs[s][order - 1 - j], cyv(j) = cys[s][order - 1 - j], czv(j) = czs[s][order - 1 - j];
      }
      double ts = times[s];

      /* jerk matrix */
      Eigen::MatrixXd mat_jerk(order, order);
      mat_jerk.setZero();
      for (double i = 3; i < order; i += 1)
        for (double j = 3; j < order; j += 1) {
          mat_jerk(i, j) =
              i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) * pow(ts, i + j - 5) / (i + j - 5);
        }

      jerk += (cxv.transpose() * mat_jerk * cxv)(0, 0);
      jerk += (cyv.transpose() * mat_jerk * cyv)(0, 0);
      jerk += (czv.transpose() * mat_jerk * czv)(0, 0);
    }

    return jerk;
  }

  void getMeanAndMaxVel(double& mean_v, double& max_v) {
    int num = 0;
    mean_v = 0.0, max_v = -1.0;
    for (int s = 0; s < times.size(); ++s) {
      int order = cxs[s].size();
      Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);

      /* coef of vel */
      for (int i = 0; i < order - 1; ++i) {
        vx(i) = double(i + 1) * cxs[s][order - 2 - i];
        vy(i) = double(i + 1) * cys[s][order - 2 - i];
        vz(i) = double(i + 1) * czs[s][order - 2 - i];
      }
      double ts = times[s];

      double eval_t = 0.0;
      while (eval_t < ts) {
        Eigen::VectorXd tv(order - 1);
        for (int i = 0; i < order - 1; ++i)
          tv(i) = pow(ts, i);
        Eigen::Vector3d vel;
        vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
        double vn = vel.norm();
        mean_v += vn;
        if (vn > max_v) max_v = vn;
        ++num;

        eval_t += 0.01;
      }
    }

    mean_v = mean_v / double(num);
  }

  void getMeanAndMaxAcc(double& mean_a, double& max_a) {
    int num = 0;
    mean_a = 0.0, max_a = -1.0;
    for (int s = 0; s < times.size(); ++s) {
      int order = cxs[s].size();
      Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);

      /* coef of acc */
      for (int i = 0; i < order - 2; ++i) {
        ax(i) = double((i + 2) * (i + 1)) * cxs[s][order - 3 - i];
        ay(i) = double((i + 2) * (i + 1)) * cys[s][order - 3 - i];
        az(i) = double((i + 2) * (i + 1)) * czs[s][order - 3 - i];
      }
      double ts = times[s];

      double eval_t = 0.0;
      while (eval_t < ts) {
        Eigen::VectorXd tv(order - 2);
        for (int i = 0; i < order - 2; ++i)
          tv(i) = pow(ts, i);
        Eigen::Vector3d acc;
        acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
        double an = acc.norm();
        mean_a += an;
        if (an > max_a) max_a = an;
        ++num;

        eval_t += 0.01;
      }
    }

    mean_a = mean_a / double(num);
  }
};

#endif


#ifndef CORE_SCENE_HPP
#define CORE_SCENE_HPP

#include "object.hpp"

#include <graphics/color.hpp>
#include <lights/point_light.hpp>

#include <boost/optional.hpp>

#include <vector>

namespace math { struct ray3d; }

namespace core
{

struct intersection_info;

class scene
{
  std::vector<object> objects;
  std::vector<lights::point_light> lights;

public:

  graphics::color background_color;

  void add(const object& obj) { objects.push_back(obj); }
  void add(lights::point_light light) { lights.push_back(light); }

  const std::vector<lights::point_light>& get_lights() const { return lights; }

  friend bool intersects(const scene& scene, math::ray3d ray);
  friend boost::optional<core::intersection_info> closest_intersection(const scene& scene, math::ray3d ray);
};

bool intersects(const scene& scn, math::ray3d ray);
boost::optional<core::intersection_info> closest_intersection(const scene& scn, math::ray3d ray);

}

#endif


#ifndef CRADLE_GEOMETRY_DECODE_MATRIX_HPP
#define CRADLE_GEOMETRY_DECODE_MATRIX_HPP

#include <cradle/geometry/common.hpp>
#include <cradle/geometry/angle.hpp>
#include <boost/optional/optional.hpp>

// This file provides various functions for decoding transformation matrices.

namespace cradle {

// Determine if the given transformation matrix has a rotational component.
template<unsigned N, typename T>
bool has_rotation(matrix<N,N,T> const& m);

// Given a matrix that represents a 3D rotation about the X-axis (and only
// that), this returns the angle of the rotation.  For any other matrix,
// it returns an uninitialized value.
template<typename T>
optional<angle<T,radians> > decode_rotation_about_x(
    matrix<3,3,T> const& m);

// Decode a rotation about the Y-axis.
template<typename T>
optional<angle<T,radians> > decode_rotation_about_y(
    matrix<3,3,T> const& m);

// Decode a rotation about the Z-axis.
template<typename T>
optional<angle<T,radians> > decode_rotation_about_z(
    matrix<3,3,T> const& m);

}

#include <cradle/geometry/decode_matrix.ipp>

#endif


#include <algorithm>
#include <iostream>
#include <string>

#include <boost/range/irange.hpp>

using uintmax = std::uintmax_t;

auto next_sequence(const std::string& seq) {

  auto result  = std::string{};
  auto count   = uintmax{1};
  auto current = seq.front();

  const auto append = [&] {
    result.push_back('0' + count);
    result.push_back(current);
  };

  for(const auto i : boost::irange({1}, seq.size())) {
    const auto digit = seq[i];
    if(digit == current) {
      ++count;
    } else {
      append();
      count = 1;
      current = digit;
    }
  }
  append();

  return result;
}

auto final_length(const std::string& seq) {

    auto result = uintmax{1};

    std::adjacent_find(seq.begin(), seq.end(), [&result] (const auto a, const auto b) {
      result += (a != b);
      return false;
    });

    return (result * 2);
}

auto solution(std::string seq, uintmax num_iterations) {

  for(const auto i : boost::irange(num_iterations - 1)) {
    seq = next_sequence(seq);
  }

  return final_length(seq);
}

int main() {

    std::cout << solution("1321131112", 40) << std::endl;

}