#include <emmintrin.h>
#include <smmintrin.h>
#include <immintrin.h>

#include <iterator>
#include <set>
#include <vector>
#include <iomanip>
#include <cctype>
#include <ostream>
#include <map>
#include <algorithm>
#include <boost/config.hpp>
#include <boost/timer/timer.hpp>
#include <iostream>
#include <boost/range.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/function.hpp>
#include <boost/range/any_range.hpp>
#include <boost/bind/bind.hpp>
#include <boost/iterator/indirect_iterator.hpp>
#include <boost/shared_container_iterator.hpp>
#include <boost/mem_fn.hpp>
#include <boost/log/trivial.hpp>
#include <boost/timer/timer.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>

struct NamedFunction{
        NamedFunction(std::string const& name, boost::function<size_t()> f)
                :name_(name),
                f_(f)
        {}
        std::string const& Name()const{ return name_; }
        size_t Execute()const{
                return f_();
        }
private:
        std::string name_;
        boost::function<size_t()> f_;
};
typedef boost::any_range<
        NamedFunction
        , boost::forward_traversal_tag
        , NamedFunction
        , std::ptrdiff_t
> FunctionRange;
struct Test{
        virtual ~Test(){}
        virtual std::string Name()const=0;
        virtual FunctionRange Subs()const=0;
};
#define ADD_PROFILE(NAME) \
        builder.Add(#NAME , boost::bind(boost::mem_fn(&self_type::NAME          ), this));
struct SubBuilder{
        typedef std::vector<NamedFunction>    VectorType;
        typedef boost::shared_ptr<VectorType> SharedVectorType;
        SubBuilder(){
                Restart();
        }
        void Restart(){
                vec_ = boost::make_shared<VectorType>();
        }
        template<class F>
        void Add(std::string const& name, F const& f){
                vec_->push_back(NamedFunction(name, f));
        }
        FunctionRange Make(){
                SharedVectorType V = vec_;
                Restart();
                return FunctionRange(boost::make_shared_container_iterator(V->begin(), V),
                                     boost::make_shared_container_iterator(V->end(), V));
        }
private:
        boost::shared_ptr<std::vector<NamedFunction> > vec_;
};
namespace Detail{
        template<class _FwdIt> inline
	_FwdIt _Max_element(_FwdIt _First, _FwdIt _Last)
	{	// find largest element, using operator<
	_FwdIt _Found = _First;
	if (_First != _Last)
		for (; ++_First != _Last; )
			if (*_Found < *_First)
				_Found = _First;
	return (_Found);
	}
        template<class T>
        T _Max(T left, T right){
                return ( left < right ? right : left );
        }
} // end namespace Detail

struct MaxElement : Test{
        typedef MaxElement self_type;
        explicit MaxElement(size_t n){
                vec_.resize(n);
                for(size_t idx=0;idx!=n;++idx){
                        vec_[idx] = (idx * idx )  % 762 + ( idx * 34 ) % 493  + idx / 1000;
                }
                size_t sz = n * sizeof(unsigned);
                alligned_vec_ = (unsigned*)_mm_malloc(sz, 16);
                std::memcpy(alligned_vec_, &vec_[0], sz);
        }
        ~MaxElement(){
                _mm_free(alligned_vec_);
        }
        unsigned ExecuteStl()const{
                return * Detail::_Max_element(vec_.begin(), vec_.end());
        }
        unsigned ExecuteStlRaw()const{
                return * Detail::_Max_element(&vec_[0], &vec_[0] + vec_.size());
        }
        unsigned ExecuteNaive()const{
                unsigned working = vec_[0];
                for(size_t idx=0;idx!=vec_.size();++idx){
                        working = Detail::_Max(working, vec_[idx]);
                }
                return working;
        }
        unsigned ExecuteNaiveRaw()const{
                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                unsigned working = *first;
                ++first;
                for(;first!=last;++first){
                        working = Detail::_Max(working, *first);
                }
                return working;
        }
        unsigned ExecuteStride()const{
                enum{ Stride = 4 };
                unsigned working = vec_[0];
                size_t idx=0;
                for(;idx + Stride < vec_.size();idx+=Stride){
                        unsigned A = Detail::_Max(vec_[idx+0], vec_[idx+1]);
                        unsigned B = Detail::_Max(vec_[idx+2], vec_[idx+3]);

                        unsigned E = Detail::_Max(A,B);

                        working = Detail::_Max(working, E);
                }
                for(;idx!=vec_.size();++idx){
                        working = Detail::_Max(working, vec_[idx]);
                }
                return working;
        }
        unsigned ExecuteStrideRaw()const{
                enum{ Stride = 4 };
                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                unsigned working = *first;
                ++first;
                for(;first + Stride < last;first+=Stride){
                        unsigned A = Detail::_Max(*(first+0), *(first+1));
                        unsigned B = Detail::_Max(*(first+2), *(first+3));

                        unsigned E = Detail::_Max(A,B);

                        working = Detail::_Max(working, E);
                }

                for(;first!=last;++first){
                        working = Detail::_Max(working, *first);
                }
                return working;
        }
        unsigned ExecuteSSE()const{
                enum{ Stride = 4 };
                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                __m128i M = _mm_set_epi32(*(first+0),
                                          *(first+1),
                                          *(first+2),
                                          *(first+3));
                for(;first + Stride < last;first+=Stride){
                        __m128i A = _mm_set_epi32(*(first+0),
                                                  *(first+1),
                                                  *(first+2),
                                                  *(first+3));
                        M = _mm_max_epu32(A,M);
                }
                unsigned working = Detail::_Max(
                        Detail::_Max( _mm_extract_epi32(M, 0), _mm_extract_epi32(M, 1)),
                        Detail::_Max( _mm_extract_epi32(M, 2), _mm_extract_epi32(M, 3))
                );
                for(;first!=last;++first){
                        working = Detail::_Max(working, *first);
                }
                return working;
        }
        unsigned ExecuteSSELoad()const{
                enum{ Stride = 4 };
                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();

                __m128i M = _mm_load_si128((__m128i const*)first);
                for(;first + Stride < last;first+=Stride){
                        __m128i A = _mm_load_si128((__m128i const*)first);
                        M = _mm_max_epu32(A,M);
                }
                unsigned working = Detail::_Max(
                        Detail::_Max( _mm_extract_epi32(M, 0), _mm_extract_epi32(M, 1)),
                        Detail::_Max( _mm_extract_epi32(M, 2), _mm_extract_epi32(M, 3))
                );
                for(;first!=last;++first){
                        working = Detail::_Max(working, *first);
                }
                return working;
        }
        unsigned ExecuteSSELoadOneReg()const{
                enum{ Stride = 4 };
                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();

                __m128i M = _mm_load_si128((__m128i const*)first);
                for(;first + Stride < last;first+=Stride){
                        M = _mm_max_epu32(_mm_load_si128((__m128i const*)first),M);
                }
                unsigned working = Detail::_Max(
                        Detail::_Max( _mm_extract_epi32(M, 0), _mm_extract_epi32(M, 1)),
                        Detail::_Max( _mm_extract_epi32(M, 2), _mm_extract_epi32(M, 3))
                );
                for(;first!=last;++first){
                        working = Detail::_Max(working, *first);
                }
                return working;
        }
        unsigned ExecuteSSEStrideRaw()const{
                enum{ Stride = 16 };
                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                __m128i M = _mm_set_epi32(*(first+0),
                                          *(first+1),
                                          *(first+2),
                                          *(first+3));
                for(;first + Stride < last;){
                        __m128i A = _mm_set_epi32(*(first+0),
                                                  *(first+1),
                                                  *(first+2),
                                                  *(first+3));
                        first += Stride;
                        __m128i B = _mm_set_epi32(*(first+0),
                                                  *(first+1),
                                                  *(first+2),
                                                  *(first+3));
                        first += Stride;
                        __m128i C = _mm_set_epi32(*(first+0),
                                                  *(first+1),
                                                  *(first+2),
                                                  *(first+3));
                        first += Stride;
                        __m128i D = _mm_set_epi32(*(first+0),
                                                  *(first+1),
                                                  *(first+2),
                                                  *(first+3));

                        __m128i E = _mm_max_epu32(A,B);
                        __m128i F = _mm_max_epu32(C,D);

                        __m128i G = _mm_max_epu32(E,F);

                        M = _mm_max_epu32(A,M);
                }
                unsigned working = Detail::_Max(
                        Detail::_Max( _mm_extract_epi32(M, 0), _mm_extract_epi32(M, 1)),
                        Detail::_Max( _mm_extract_epi32(M, 2), _mm_extract_epi32(M, 3))
                );
                for(;first!=last;++first){
                        working = Detail::_Max(working, *first);
                }
                return working;
        }
        unsigned ExecuteSSEStide()const{
                __m128i M = _mm_set_epi32(vec_[0], vec_[1], vec_[2], vec_[3]);
                enum{ Stride = 16 };
                size_t idx=0;
                for(;idx + Stride < vec_.size();idx+=Stride){
                        __m128i A = _mm_set_epi32(vec_[idx+ 0+0], vec_[idx+ 0+1], vec_[idx+ 0+2], vec_[idx+ 0+3]);
                        __m128i B = _mm_set_epi32(vec_[idx+ 4+0], vec_[idx+ 4+1], vec_[idx+ 4+2], vec_[idx+ 4+3]);
                        __m128i C = _mm_set_epi32(vec_[idx+ 8+0], vec_[idx+ 8+1], vec_[idx+ 8+2], vec_[idx+ 8+3]);
                        __m128i D = _mm_set_epi32(vec_[idx+12+0], vec_[idx+12+1], vec_[idx+12+2], vec_[idx+12+3]);

                        __m128i E = _mm_max_epu32(A,B);
                        __m128i F = _mm_max_epu32(C,D);

                        __m128i G = _mm_max_epu32(E,F);

                        M = _mm_max_epu32(A,M);
                }
                unsigned working = Detail::_Max(
                        Detail::_Max( _mm_extract_epi32(M, 0), _mm_extract_epi32(M, 1)),
                        Detail::_Max( _mm_extract_epi32(M, 2), _mm_extract_epi32(M, 3))
                );

                for(;idx!=vec_.size();++idx){
                        working = Detail::_Max(working, vec_[idx]);
                }
                return working;
        }
        unsigned ExecuteBoostRange()const{
                return *boost::max_element(vec_);
        }

        virtual FunctionRange Subs()const{
                SubBuilder builder;
                ADD_PROFILE(ExecuteStl);
                ADD_PROFILE(ExecuteStlRaw);
                ADD_PROFILE(ExecuteNaive);
                ADD_PROFILE(ExecuteNaiveRaw);
                ADD_PROFILE(ExecuteStride);
                ADD_PROFILE(ExecuteStrideRaw);
                ADD_PROFILE(ExecuteSSE);
                ADD_PROFILE(ExecuteSSELoad);
                ADD_PROFILE(ExecuteSSELoadOneReg);
                ADD_PROFILE(ExecuteSSEStide);
                ADD_PROFILE(ExecuteBoostRange);
                // bad
                //ADD_PROFILE(ExecuteSSEStrideRaw);

                return builder.Make();
        }
        virtual std::string Name()const{ return "MaxElement"; }
private:
        std::vector<unsigned> vec_;
        unsigned* alligned_vec_;
};

struct CountDisjoint : Test{
        typedef CountDisjoint self_type;
        explicit CountDisjoint(size_t n){
                vec_.resize(n);
                for(size_t idx=0;idx!=n;++idx){
                        vec_[idx] = (idx * idx )  % 762 + ( idx * 34 ) % 493  + idx / 1000 + ( idx % 5 == 0 ? 1 : 0 );
                }
                size_t sz = n * sizeof(unsigned);
                alligned_vec_ = (unsigned*)_mm_malloc(sz, 16);
                std::memcpy(alligned_vec_, &vec_[0], sz);
        }
        unsigned ExecuteNaive()const{
                unsigned mask = 0x4587au;

                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                unsigned count = 0;
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        unsigned ExecuteNaiveUnrolled()const{
                unsigned mask = 0x4587au;

                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                unsigned count = 0;
                enum{ Stride = 8 };
                for(;first + Stride < last;){

                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;

                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                }
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        unsigned ExecuteSSE()const{
                unsigned mask = 0x4587au;

                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                enum{ Stride = 4 };

                __m128i M       = _mm_set1_epi32(mask);
                __m128i Counter = _mm_setzero_si128();
                __m128i Mask    = _mm_set1_epi32(1);

                for(;first + Stride < last;first+=Stride){
                        // load into A = [v0,v1,v2,v3]
                        __m128i A = _mm_load_si128((__m128i const*)first);
                        // bit and with mask [v0,v1,v2,v3] & [mask,mask,mask,mask]
                        //                 = [v0 & mask, ... ]
                        __m128i B = _mm_and_si128(M, A);
                        // now compare with zero,  maps to {0,~0}
                        __m128i C = _mm_cmpeq_epi32(B, _mm_setzero_si128());
                        // map ~0 to 1, which corresponds to [c0,c1,c2,c3}, where ci \in {0,1}
                        __m128i D = _mm_and_si128(C,Mask);
                        // add to the summing sum
                        Counter = _mm_add_epi32(D, Counter);
                }
                unsigned count = 0;
                count += _mm_extract_epi32(Counter, 0);
                count += _mm_extract_epi32(Counter, 1);
                count += _mm_extract_epi32(Counter, 2);
                count += _mm_extract_epi32(Counter, 3);
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }

        unsigned ExecuteSSEPopcount()const{
                unsigned mask = 0x4587au;

                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                enum{ Stride = 4 };

                __m128i M       = _mm_set1_epi32(mask);
                __m128i Mask    = _mm_set1_epi32(1);
                unsigned count = 0;

                for(;first + Stride < last;first+=Stride){
                        // load into A = [v0,v1,v2,v3]
                        __m128i A = _mm_load_si128((__m128i const*)first);
                        // bit and with mask [v0,v1,v2,v3] & [mask,mask,mask,mask]
                        //                 = [v0 & mask, ... ]
                        __m128i B = _mm_and_si128(M, A);
                        // now compare with zero,  maps to {0,~0}
                        __m128i C = _mm_cmpeq_epi32(B, _mm_setzero_si128());

                        #ifdef BOOST_OS_LINUX
                        count += __builtin_popcount(_mm_movemask_epi8(C))/4;
                        #else
                        count += __popcnt16(_mm_movemask_epi8(C))/4;
                        #endif

                        #if 0
                        // map ~0 to 1, which corresponds to [c0,c1,c2,c3}, where ci \in {0,1}
                        __m128i D = _mm_and_si128(C,Mask);
                        // add to the summing sum
                        Counter = _mm_add_epi32(D, Counter);
                        #endif
                }
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }

        unsigned ExecuteSSEStream()const{
                unsigned mask = 0x4587au;

                unsigned const* first = &vec_[0];
                unsigned const* last  = first + vec_.size();
                enum{ Stride = 4 };

                __m128i M       = _mm_set1_epi32(mask);
                __m128i Counter = _mm_setzero_si128();
                __m128i Mask    = _mm_set1_epi32(1);

                for(;first + Stride < last;first+=Stride){
                        __m128i A = _mm_stream_load_si128((__m128i*)first);
                        __m128i B = _mm_and_si128(M, A);
                        __m128i C = _mm_and_si128(_mm_cmpeq_epi32(B, _mm_setzero_si128()),Mask);
                        Counter = _mm_add_epi32(C, Counter);
                }
                unsigned count = 0;
                count += _mm_extract_epi32(Counter, 0);
                count += _mm_extract_epi32(Counter, 1);
                count += _mm_extract_epi32(Counter, 2);
                count += _mm_extract_epi32(Counter, 3);
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        virtual std::string Name()const{ return "CountDisjoint"; }

        virtual FunctionRange Subs()const{
                SubBuilder builder;
                ADD_PROFILE(ExecuteNaive);
                ADD_PROFILE(ExecuteNaiveUnrolled);
                ADD_PROFILE(ExecuteSSE);
                ADD_PROFILE(ExecuteSSEStream);
                ADD_PROFILE(ExecuteSSEPopcount);

                return builder.Make();
        }
private:
        std::vector<unsigned> vec_;
        unsigned* alligned_vec_;
};

struct CountDisjoint64 : Test{
        typedef CountDisjoint64 self_type;
        #ifdef BOOST_OS_LINUX
        typedef std::int64_t int_type;
        #else
        typedef unsigned __int64 int_type;
        #endif
        explicit CountDisjoint64(size_t sz):sz_(sz){
                alligned_vec_ = (int_type*)_mm_malloc(sz_ * sizeof(int_type), 16);
                int_type* out = alligned_vec_;
                for(size_t idx=0;idx!=sz_;++idx, ++out){
                        *out = (idx * idx )  % 762 + ( idx * 34 ) % 493  + idx / 1000 + ( idx % 5 == 0 ? 1 : 0 ) + ( 3 << ( idx  % 43 ) );
                }
        }
        ~CountDisjoint64(){
                _mm_free(alligned_vec_);
        }
        int_type const* begin()const{ return alligned_vec_; }
        int_type const* end()const{ return alligned_vec_ + sz_; }

        size_t ExecuteNaive()const{
                //              <64-bit><32-bit>
                //            0xFFFFFFFFFFFFFFFF
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                size_t count = 0;
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        size_t ExecuteNaiveUnrolled()const{
                //              <64-bit><32-bit>
                //            0xFFFFFFFFFFFFFFFF
                int_type mask = 0x4587a8292388988aull;

                enum{ Stride = 16 };

                int_type const* first = begin();
                int_type const* last  = end();
                size_t count = 0;
                for(;first + Stride!=last;){

                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;

                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;

                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;
                        if( ( *first++ & mask ) == 0 ) ++count;

                        if( ( *first++ & mask ) == 0 ) ++count;
                }
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        unsigned ExecuteSSE128()const{
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                enum{ Stride = 2 };

                // don't always have X_epi64(), so we operator on hi and lo half

                __m128i M       = _mm_set_epi32( static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff),
                                                 static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff));
                __m128i Counter = _mm_setzero_si128();
                __m128i Mask    = _mm_set_epi32(0, 1, 0, 1);

                for(;first + Stride < last;first+=Stride){
                        __m128i A = _mm_load_si128((__m128i const*)first);
                        __m128i B = _mm_and_si128(M, A);
                        __m128i C = _mm_cmpeq_epi64(B, _mm_setzero_si128());
                        __m128i D = _mm_and_si128(C,Mask);
                        Counter = _mm_add_epi64(D, Counter);
                }
                size_t count = 0;
                count += _mm_extract_epi32(Counter, 0);
                count += _mm_extract_epi32(Counter, 1) * 0xffffffff;
                count += _mm_extract_epi32(Counter, 2);
                count += _mm_extract_epi32(Counter, 3) * 0xffffffff;
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        unsigned ExecuteSSE128WithZero()const{
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                enum{ Stride = 2 };

                // don't always have X_epi64(), so we operator on hi and lo half

                __m128i M       = _mm_set_epi32( static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff),
                                                 static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff));
                __m128i Zero    = _mm_setzero_si128();
                __m128i Counter = _mm_setzero_si128();
                __m128i Mask    = _mm_set_epi32(0, 1, 0, 1);

                for(;first + Stride < last;first+=Stride){
                        __m128i A = _mm_load_si128((__m128i const*)first);
                        __m128i B = _mm_and_si128(M, A);
                        __m128i C = _mm_cmpeq_epi64(B, Zero);
                        __m128i D = _mm_and_si128(C,Mask);
                        Counter = _mm_add_epi64(D, Counter);
                }
                size_t count = 0;
                count += _mm_extract_epi32(Counter, 0);
                count += _mm_extract_epi32(Counter, 1) * 0xffffffff;
                count += _mm_extract_epi32(Counter, 2);
                count += _mm_extract_epi32(Counter, 3) * 0xffffffff;
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        unsigned ExecuteSSE128Unrolled()const{
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                enum{ Stride = 8, SubStride = 2 };

                // don't always have X_epi64(), so we operator on hi and lo half

                __m128i M       = _mm_set_epi32( static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff),
                                                 static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff));
                __m128i Counter = _mm_setzero_si128();
                __m128i Mask    = _mm_set_epi32(0, 1, 0, 1);

                for(;first + Stride < last;){
                        __m128i A = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_load_si128((__m128i const*)first)), _mm_setzero_si128()),Mask);
                        first += 2;
                        __m128i B = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_load_si128((__m128i const*)first)), _mm_setzero_si128()),Mask);
                        first += 2;
                        __m128i C = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_load_si128((__m128i const*)first)), _mm_setzero_si128()),Mask);
                        first += 2;
                        __m128i D = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_load_si128((__m128i const*)first)), _mm_setzero_si128()),Mask);
                        first += 2;

                        Counter = _mm_add_epi64(Counter, _mm_add_epi64(_mm_add_epi64(A,B),_mm_add_epi64(C,D)));
                }
                size_t count = 0;
                count += _mm_extract_epi32(Counter, 0);
                count += _mm_extract_epi32(Counter, 1) * 0xffffffff;
                count += _mm_extract_epi32(Counter, 2);
                count += _mm_extract_epi32(Counter, 3) * 0xffffffff;
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        unsigned ExecuteSSE128UnrolledStream()const{
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                enum{ Stride = 8, SubStride = 2 };

                // don't always have X_epi64(), so we operator on hi and lo half

                __m128i M       = _mm_set_epi32( static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff),
                                                 static_cast<int>(mask >> 32 ),
                                                 static_cast<int>(mask & 0xffffffff));
                __m128i Counter = _mm_setzero_si128();
                __m128i Mask    = _mm_set_epi32(0, 1, 0, 1);

                for(;first + Stride < last;){
                        __m128i A = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_stream_load_si128((__m128i*)first)), _mm_setzero_si128()),Mask);
                        first += 2;
                        __m128i B = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_stream_load_si128((__m128i*)first)), _mm_setzero_si128()),Mask);
                        first += 2;
                        __m128i C = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_stream_load_si128((__m128i*)first)), _mm_setzero_si128()),Mask);
                        first += 2;
                        __m128i D = _mm_and_si128(_mm_cmpeq_epi64(_mm_and_si128(M, _mm_stream_load_si128((__m128i*)first)), _mm_setzero_si128()),Mask);
                        first += 2;

                        Counter = _mm_add_epi64(Counter, _mm_add_epi64(_mm_add_epi64(A,B),_mm_add_epi64(C,D)));
                }
                size_t count = 0;
                count += _mm_extract_epi32(Counter, 0);
                count += _mm_extract_epi32(Counter, 1) * 0xffffffff;
                count += _mm_extract_epi32(Counter, 2);
                count += _mm_extract_epi32(Counter, 3) * 0xffffffff;
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }

        unsigned ExecuteSSE256()const{
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                enum{ Stride = 4 };

                // don't always have X_epi64(), so we operator on hi and lo half

                __m256i M = _mm256_set_epi32( static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff) );
                __m256i Counter = _mm256_setzero_si256();
                __m256i Mask    = _mm256_set_epi32(0, 1, 0, 1, 0, 1, 0, 1);

                for(;first + Stride < last;first+=Stride){
                        __m256i A = _mm256_load_si256((__m256i const*)first);
                        __m256i B = _mm256_and_si256(M, A);
                        __m256i C = _mm256_cmpeq_epi64(B, _mm256_setzero_si256());
                        __m256i D = _mm256_and_si256(C,Mask);
                        Counter = _mm256_add_epi64(D, Counter);
                }
                size_t count = 0;
                count += _mm256_extract_epi32(Counter, 0);
                count += _mm256_extract_epi32(Counter, 1) * 0xffffffff;
                count += _mm256_extract_epi32(Counter, 2);
                count += _mm256_extract_epi32(Counter, 3) * 0xffffffff;
                count += _mm256_extract_epi32(Counter, 4);
                count += _mm256_extract_epi32(Counter, 5) * 0xffffffff;
                count += _mm256_extract_epi32(Counter, 6);
                count += _mm256_extract_epi32(Counter, 7) * 0xffffffff;
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        unsigned ExecuteSSE256Mask()const{
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                enum{ Stride = 4 };

                // don't always have X_epi64(), so we operator on hi and lo half

                __m256i M = _mm256_set_epi32( static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff) );

                std::array<__mmask8, 256> mask2count;
                for(size_t x = 0; x < 256; ++x ){
                        mask2count[x] = __builtin_popcount(x);
                }

                size_t count = 0;

                for(;first + Stride < last;first+=Stride){
                        __m256i A = _mm256_load_si256((__m256i const*)first);
                        __m256i B = _mm256_and_si256(M, A);
                        __mmask8 C = _mm256_cmpeq_epi64_mask(B, _mm256_setzero_si256());
                        count += mask2count[C];
                }
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        #if 0
        unsigned ExecuteSSE512()const{
                int_type mask = 0x4587a8292388988aull;

                int_type const* first = begin();
                int_type const* last  = end();
                enum{ Stride = 8 };

                // don't always have X_epi64(), so we operator on hi and lo half

                __m512i M = _mm512_set_epi32( static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff),
                                              static_cast<int>(mask >> 32 ),
                                              static_cast<int>(mask & 0xffffffff) );
                __m512i Mask    = _mm512_set_epi32(0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1);

                size_t count = 0;
                std::array<__mmask8, 256> mask2count;
                for(size_t x = 0; x < 256; ++x ){
                        mask2count[x] = __builtin_popcount(x);
                }

                for(;first + Stride < last;first+=Stride){
                        __m512i A = _mm512_load_si512((__m512i const*)first);
                        __m512i B = _mm512_and_si512(M, A);
                        __mmask8 C = _mm512_cmpeq_epi64_mask(B, _mm512_setzero_si512());
                        count += mask2count[C];
                }
                for(;first!=last;++first){
                        if( ( *first & mask ) == 0 )
                                ++count;
                }
                return count;
        }
        #endif

        virtual std::string Name()const{ return "CountDisjoint64"; }

        virtual FunctionRange Subs()const{
                SubBuilder builder;
                ADD_PROFILE(ExecuteNaive);
                ADD_PROFILE(ExecuteNaiveUnrolled);
                ADD_PROFILE(ExecuteSSE128);
                ADD_PROFILE(ExecuteSSE128WithZero);
                ADD_PROFILE(ExecuteSSE128Unrolled);
                ADD_PROFILE(ExecuteSSE128UnrolledStream);
                ADD_PROFILE(ExecuteSSE256);

                return builder.Make();
        }
private:
        size_t sz_;
        int_type* alligned_vec_;
};

void ReportAvailabe(){
        #if 0
        struct Feature{
                Feature(std::string const& Name_, std::int64_t Mask_)
                        :Name(Name_),
                        Mask(Mask_)
                {}
                std::string Name;
                std::int64_t Mask;
        };
        std::vector<Feature> features;

        features.push_back(Feature("_FEATURE_GENERIC_IA32", _FEATURE_GENERIC_IA32));
        features.push_back(Feature("_FEATURE_FPU"         , _FEATURE_FPU));
        features.push_back(Feature("_FEATURE_CMOV"        , _FEATURE_CMOV));
        features.push_back(Feature("_FEATURE_MMX"         , _FEATURE_MMX));
        features.push_back(Feature("_FEATURE_FXSAVE"      , _FEATURE_FXSAVE));
        features.push_back(Feature("_FEATURE_SSE"         , _FEATURE_SSE));
        features.push_back(Feature("_FEATURE_SSE2"        , _FEATURE_SSE2));
        features.push_back(Feature("_FEATURE_SSE3"        , _FEATURE_SSE3));
        features.push_back(Feature("_FEATURE_SSSE3"       , _FEATURE_SSSE3));
        features.push_back(Feature("_FEATURE_SSE4_1"      , _FEATURE_SSE4_1));
        features.push_back(Feature("_FEATURE_SSE4_2"      , _FEATURE_SSE4_2));
        features.push_back(Feature("_FEATURE_MOVBE"       , _FEATURE_MOVBE));
        features.push_back(Feature("_FEATURE_POPCNT"      , _FEATURE_POPCNT));
        features.push_back(Feature("_FEATURE_PCLMULQDQ"   , _FEATURE_PCLMULQDQ));
        features.push_back(Feature("_FEATURE_AES"         , _FEATURE_AES));
        features.push_back(Feature("_FEATURE_F16C"        , _FEATURE_F16C));
        features.push_back(Feature("_FEATURE_AVX"         , _FEATURE_AVX));
        features.push_back(Feature("_FEATURE_RDRND"       , _FEATURE_RDRND));
        features.push_back(Feature("_FEATURE_FMA"         , _FEATURE_FMA));
        features.push_back(Feature("_FEATURE_BMI"         , _FEATURE_BMI));
        features.push_back(Feature("_FEATURE_LZCNT"       , _FEATURE_LZCNT));
        features.push_back(Feature("_FEATURE_HLE"         , _FEATURE_HLE));
        features.push_back(Feature("_FEATURE_RTM"         , _FEATURE_RTM));
        features.push_back(Feature("_FEATURE_AVX2"        , _FEATURE_AVX2));
        features.push_back(Feature("_FEATURE_KNCNI"       , _FEATURE_KNCNI));
        features.push_back(Feature("_FEATURE_AVX512F"     , _FEATURE_AVX512F));
        features.push_back(Feature("_FEATURE_ADX"         , _FEATURE_ADX));
        features.push_back(Feature("_FEATURE_RDSEED"      , _FEATURE_RDSEED));
        features.push_back(Feature("_FEATURE_AVX512ER"    , _FEATURE_AVX512ER));
        features.push_back(Feature("_FEATURE_AVX512PF"    , _FEATURE_AVX512PF));
        features.push_back(Feature("_FEATURE_AVX512CD"    , _FEATURE_AVX512CD));
        features.push_back(Feature("_FEATURE_SHA"         , _FEATURE_SHA));
        features.push_back(Feature("_FEATURE_MPX"         , _FEATURE_MPX));

        BOOST_FOREACH( Feature const& f , features){
                BOOST_LOG_TRIVIAL(trace) << std::setw(32) << f.Name << " is " << ( _may_i_use_cpu_feature(f.Mask) ? "ON" : "OFF" );
        }
        #endif
}

int main(){
        enum{ Dp = 8 };
        enum{ Count = 10 };
        enum{ Width = 50 };
        enum{ AlwaysPrint = 0 };
        std::vector<boost::shared_ptr<Test> > T;
        T.push_back(boost::make_shared<MaxElement>(100000000));
        T.push_back(boost::make_shared<CountDisjoint>  (100000000));
        T.push_back(boost::make_shared<CountDisjoint64>(100000000));

        ReportAvailabe();

        BOOST_FOREACH(boost::shared_ptr<Test> test, T){
                std::map<std::string, boost::timer::nanosecond_type> profile;
                for(size_t idx=0;idx!=Count;++idx){
                        std::set<unsigned> check;
                        std::vector<std::pair<std::string, unsigned> > ledger;
                        BOOST_FOREACH(NamedFunction const& F, test->Subs()){
                                boost::timer::cpu_timer tmr;
                                unsigned result = F.Execute();
                                std::string token = test->Name() + "." + F.Name();
                                profile[token] += tmr.elapsed().wall;
                                ledger.emplace_back(token, result);
                                check.insert(result);
                                //BOOST_LOG_TRIVIAL(trace) << std::left << std::setw(Width) << token << boost::timer::format(tmr.elapsed(), Dp, " took %w seconds") << " ( " << result << ")";
                        }
                        std::string error_msg = (check.size() >= 2 ? "ERROR: " : "");
                        if( AlwaysPrint || error_msg.size() != 0  ){
                                for(auto const& p : ledger ){
                                        BOOST_LOG_TRIVIAL(trace)  << error_msg << std::setw(Width) << std::left << p.first  << " | " << p.second;
                                }
                        }
                }
                typedef std::pair<std::string, boost::timer::nanosecond_type>  pair_type;
                std::vector<pair_type> view(profile.begin(), profile.end());
                struct sorter{
                        bool operator()(pair_type const& l, pair_type const& r)const{
                                return l.second > r.second;
                        }
                };
                boost::sort(view, sorter());
                BOOST_FOREACH(pair_type& p, view){
                        p.second /= Count;
                        BOOST_LOG_TRIVIAL(trace) << std::left << std::setw(Width) << p.first << std::setprecision(Dp) << (p.second / 1000000000.0) << " seconds";
                }
        }
}


/*
    Copyright (c) 2017 Mobile Robots Laboratory at Poznan University of Technology:
    -Jan Wietrzykowski name.surname [at] put.poznan.pl

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

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

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

#ifndef INCLUDE_OBJINSTANCE_HPP_
#define INCLUDE_OBJINSTANCE_HPP_

class ObjInstance;

#include <vector>
#include <string>

#include <boost/serialization/vector.hpp>

#include <opencv2/opencv.hpp>

#include <Eigen/Eigen>

#include <pcl/segmentation/supervoxel_clustering.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/surface/convex_hull.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "PlaneSeg.hpp"
#include "LineSeg.hpp"
#include "ConcaveHull.hpp"
#include "Serialization.hpp"
#include "PlaneEstimator.hpp"

// only planes in a current version
class ObjInstance{
public:
	enum class ObjType{
		Plane,
		Unknown
	};

    ObjInstance();

    /**
     *
     * @param iid
     * @param itype
     * @param ipoints
     * @param isvs
     */
	ObjInstance(int iid,
				ObjType itype,
				pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr ipoints,
				const vectorPlaneSeg& isvs,
                int ieol = 4);

    ObjInstance(const ObjInstance &other);

	void merge(const ObjInstance &other);

	inline int getId() const {
		return id;
	}

    inline void setId(int nid){
        id = nid;
    }

	inline ObjType getType() const {
		return type;
	}

	inline const pcl::PointCloud<pcl::PointXYZRGB>::Ptr getPoints() const {
		return points;
	}

	inline const vectorPlaneSeg& getSvs() const {
		return svs;
	}

	inline Eigen::Vector4d getParamRep() const {
		return paramRep;
	}

    inline Eigen::Vector4d getNormal() const {
        return normal;
    }

    inline const vectorVector3d getPrincComp() const {
        return princComp;
    }

    inline const std::vector<double> &getPrincCompLens() const {
        return princCompLens;
    }

    inline double getShorterComp() const {
        return shorterComp;
    }

	inline float getCurv() const {
		return curv;
	}

    inline const ConcaveHull &getHull() const {
        return *hull;
	}

    inline const vectorLineSeg &getLineSegs() const {
        return lineSegs;
    }

//    inline void getQuatAndCovar(Eigen::Quaterniond &q,
//                                Eigen::Matrix4d &covar) const
//    {
//        q = quat;
//        covar = covarQuat;
//    }

//    inline const EKFPlane &getEkf() const {
//        return ekf;
//    }

    inline const PlaneEstimator &getPlaneEstimator() const {
        return planeEstimator;
    }

    int getEolCnt() const {
        return eolCnt;
    }

    void setEolCnt(int eolCnt) {
        ObjInstance::eolCnt = eolCnt;
    }

    void increaseEolCnt(int eolAdd){
        ObjInstance::eolCnt += eolAdd;
    }

    void decreaseEolCnt(int eolSub){
        ObjInstance::eolCnt -= eolSub;
    }

    int getObsCnt() const {
        return obsCnt;
    }

    void setObsCnt(int obsCnt) {
        ObjInstance::obsCnt = obsCnt;
    }

    bool isTrial() const {
        return trial;
    }

    void setTrial(bool trial) {
        ObjInstance::trial = trial;
    }

    void transform(const Vector7d &transform);

    inline void addLineSeg(const LineSeg &newLineSeg){
        lineSegs.push_back(newLineSeg);
    }

    cv::Mat getColorHist() const {
        return colorHist;
    }

    bool isMatching(const ObjInstance &other,
                    pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
                    int viewPort1 = -1,
                    int viewPort2 = -1) const;

    static double compHistDist(cv::Mat hist1, cv::Mat hist2);

    void display(pcl::visualization::PCLVisualizer::Ptr viewer,
                 int vp,
                 double shading = 1.0) const ;

    void cleanDisplay(pcl::visualization::PCLVisualizer::Ptr viewer,
                      int vp) const;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    void correctOrient();

    void compColorHist();

	int id;

	ObjType type;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr points;

	vectorPlaneSeg svs;

	Eigen::Vector4d paramRep;

    /**
     * Normal oriented towards observable side.
     */
	Eigen::Vector4d normal;

	vectorVector3d princComp;

    std::vector<double> princCompLens;

    double shorterComp;

    float curv;

    cv::Mat colorHist;

    std::shared_ptr<ConcaveHull> hull;

	vectorLineSeg lineSegs;

//    EKFPlane ekf;
    PlaneEstimator planeEstimator;

    int eolCnt;

    int obsCnt;

    bool trial;

    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & id;
        ar & type;
        ar & points;
        ar & svs;
        ar & paramRep;
        ar & normal;
        ar & princComp;
        ar & princCompLens;
        ar & shorterComp;
        ar & curv;
        ar & colorHist;
        ar & hull;
        ar & lineSegs;
//        ar & ekf;
        ar & planeEstimator;
        ar & eolCnt;
        ar & obsCnt;
        ar & trial;
    }
};

#endif /* INCLUDE_OBJINSTANCE_HPP_ */


#include "NumberClRA.h"

#include "NumberMpq.h"
#include "NumberMpz.h"

#include <boost/numeric/interval/interval.hpp>
#include <boost/numeric/interval/policies.hpp>
#include <boost/numeric/interval/arith2.hpp>
#include <boost/numeric/interval/checking.hpp>

#include <limits>

namespace carl {

#ifdef USE_CLN_NUMBERS

		//constructs a/b:
		//(this looks hacky.. seems to be the only really functioning way though: take the integers as strings, put the sign at the front and construct
		//cl_RA from the string "[-]a/b")
		Number<cln::cl_RA>::Number(const Number<cln::cl_I>& a, const Number<cln::cl_I>& b):
			Number(cln::cl_RA(a) / b) {}

		Number<cln::cl_RA>::Number(const Number<cln::cl_I>& n): Number(n.getValue()) {}
		//Number(const cln::cl_I& n) { mData = cln::cl_RA(n); }

		Number<cln::cl_RA>::Number(const Number<mpq_class>& n) : Number(cln::cl_RA(n.toString().c_str())) {}
		Number<cln::cl_RA>::Number(const Number<mpz_class>& n) : Number(cln::cl_RA(n.toString().c_str())) {}

		//this can maybe be done such that it's the same as for mpq_class
    	   Number<cln::cl_RA>::Number(const std::string& s) {
		//here, we need to distinguish two cases: we want to deal with normal floating point inputs (as "3.5") but also fractions ("7/2")
		if (s.find_first_of('/') == std::string::npos) { //this is the floating point case

			std::vector<std::string> strs;
			boost::split(strs, s, boost::is_any_of("."));

			if(strs.size() > 2)
			{
			    throw std::invalid_argument("More than one delimiter in the string.");
			}
			cln::cl_RA result(0);
			if(!strs.front().empty())
			{
			    result += cln::cl_RA(strs.front().c_str());
			}
			if(strs.size() > 1)
			{
			    result += (cln::cl_RA(strs.back().c_str())/carl::pow(cln::cl_RA(10),static_cast<unsigned>(strs.back().size())));
			}
			mData = cln::cl_RA(result);
		} else { //the case where we have a fraction as input
			cln::cl_read_flags flags = {cln::syntax_rational, cln::lsyntax_all, 10, {cln::default_float_format, cln::default_float_format, false}};
			std::istringstream istr(s);
			mData = cln::cl_RA(cln::read_rational(istr,flags));
		}
	    }
	 /*   Number<cln::cl_RA>::Number(double n) {
		switch (std::fpclassify(n)) {
		    case FP_NORMAL: // normalized are fully supported
		        mData = cln::rationalize(convert<mpq_class, cln::cl_RA>(n));
		    case FP_SUBNORMAL: { // subnormals result in underflows, hence the value of the double is 0.f, where f is the significand precision
					static_assert(sizeof(n) == 8, "double is assumed to be eight bytes wide.");
		        sint significandBits = reinterpret_cast<sint>(&n);
		        significandBits = (significandBits << 12) >> 12;
		        if( n < 0 )
		            significandBits = -significandBits;
		        mData = cln::cl_RA( significandBits ) * ONE_DIVIDED_BY_10_TO_THE_POWER_OF_52;
		}
		case FP_ZERO:
		    mData = cln::cl_RA(0);
		case FP_NAN: // NaN and infinite are not supported
		case FP_INFINITE:
		    assert(false);
		    break;
		}
		mData = cln::cl_RA(0);
	}

	Number<cln::cl_RA>::Number(float n) {
		switch (std::fpclassify(n))
		{
		    case FP_NORMAL: // normalized are fully supported
		        mData = cln::rationalize(convert<mpq_class, cln::cl_RA>(n));
		    case FP_SUBNORMAL: { // subnormals result in underflows, hence the value of the double is 0.f, where f is the significand precision
					static_assert(sizeof(n) == 4, "float is assumed to be four bytes wide.");
		        sint significandBits = reinterpret_cast<sint>(&n);
		        significandBits = (significandBits << 9) >> 9;
		        if( n < 0 )
		            significandBits = -significandBits;
		        mData = cln::cl_RA( significandBits ) * ONE_DIVIDED_BY_10_TO_THE_POWER_OF_23;
		}
		case FP_ZERO:
		    mData = cln::cl_RA(0);
		case FP_NAN: // NaN and infinite are not supported
		case FP_INFINITE:
		    assert(false);
		    break;
		}
		mData = cln::cl_RA(0);
	} */

	//TODO: why not use the standard output of cln here?! Surely that works?
	    std::string Number<cln::cl_RA>::toString(bool _infix) const
	    {

		std::stringstream s;
		bool negative = (mData < cln::cl_RA(0));
		if(negative) s << "(-" << (_infix ? "" : " ");
		if(_infix) s << this->abs().mData;
		else
		{
		    cln::cl_I d = cln::denominator(mData);
		    if(mData != carl::constant_one<cln::cl_I>().get()) s << "(/ " << cln::abs(cln::numerator(mData)) << " " << cln::abs(d) << ")";
		    else s << this->abs().getValue();
		}
		if(negative)
		    s << ")";
		return s.str();
	    }

	 bool Number<cln::cl_RA>::sqrt_exact(Number<cln::cl_RA>& b) const
	 {
		if( mData < 0 ) return false;
		cln::cl_RA result;
		bool boolResult = cln::sqrtp( mData, &result );
		b = Number(result);
		return boolResult;
	 }

	//this is the same as for mpq_class
	 Number<cln::cl_RA> Number<cln::cl_RA>::sqrt() const
	 {
		std::pair<Number<cln::cl_RA>, Number<cln::cl_RA>> r = this->sqrt_safe();
		return Number((r.first.getValue() + r.second.getValue()) / 2); //TODO: remove the getValue once operators are implemented!!
	 }

	cln::cl_RA Number<cln::cl_RA>::scaleByPowerOfTwo(const cln::cl_RA& a, int exp) const {
		if (exp > 0) {
			return cln::cl_RA(cln::numerator(a) << exp) / cln::denominator(a);
		} else if (exp < 0) {
			return cln::cl_RA(cln::numerator(a)) / (cln::denominator(a) << -exp);
		}
		return a;
	}

	    std::pair<Number<cln::cl_RA>, Number<cln::cl_RA>> Number<cln::cl_RA>::sqrt_safe() const
	    {
		assert( mData >= 0 );
		cln::cl_RA exact_root;
		if (cln::sqrtp(mData, &exact_root)) {
		    // root can be computed exactly.
		    return std::make_pair(Number(exact_root), Number(exact_root));
		} else {
				auto factor = int(cln::integer_length(cln::denominator(mData))) - int(cln::integer_length(cln::numerator(mData)));
				if (cln::oddp(factor)) factor += 1;
				cln::cl_RA n = scaleByPowerOfTwo(mData, factor);
				double dn = cln::double_approx(n);
				cln::cl_RA nra = cln::rationalize(dn);
				boost::numeric::interval<double> i;
				if (nra > n) {
					i.assign(cln::double_approx(2*n-nra), dn);
					assert(2*n-nra <= n);
				} else {
					i.assign(dn, cln::double_approx(2*n-nra));
					assert(n <= 2*n-nra);
				}
				i = boost::numeric::sqrt(i);
				i.assign(
					std::nexttoward(i.lower(), -std::numeric_limits<double>::infinity()),
					std::nexttoward(i.upper(), std::numeric_limits<double>::infinity())
				);
				factor = factor / 2;
				cln::cl_RA lower = scaleByPowerOfTwo(cln::rationalize(i.lower()), -factor);
				cln::cl_RA upper = scaleByPowerOfTwo(cln::rationalize(i.upper()), -factor);
				assert(lower*lower <= mData);
				assert(mData <= upper*upper);
				return std::make_pair(Number(lower), Number(upper));
		}
	    }

	    std::pair<Number<cln::cl_RA>, Number<cln::cl_RA>> Number<cln::cl_RA>::sqrt_fast() const
	    {
			assert(mData >= 0);
			cln::cl_RA exact_root;
			if (cln::sqrtp(mData, &exact_root)) {
				// root can be computed exactly.
				return std::make_pair(Number(exact_root), Number(exact_root));
			} else {
				// compute an approximation with sqrt(). we can assume that the surrounding integers contain the actual root.
				//auto factor = cln::integer_length(cln::denominator(a)) - cln::integer_length(cln::numerator(a));
				//if (cln::oddp(factor)) factor += 1;
				cln::cl_I lower = cln::floor1(cln::sqrt(toLF(mData)));
			        cln::cl_I upper = lower + 1;
			        assert(cln::expt_pos(lower,2) < mData);
			        assert(cln::expt_pos(upper,2) > mData);
			        return std::make_pair(Number(lower), Number(upper));
			}
	    }

#endif

}


/**

Copyright (c) 2016, Hanselmann Fabian, Heller Florian, Heizmann Heinrich, Kübler Marcel, Mehlhaus Jonas, Meißner Pascal, Qattan Mohamad, Reckling Reno, Stroh Daniel
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "DirectionOrientationRelationHeuristic.hpp"

#include <vector>
#include <boost/math/constants/constants.hpp>
#include <algorithm>
#include "utility/GeometryHelper.hpp"

namespace ISM {
    DirectionOrientationRelationHeuristic::DirectionOrientationRelationHeuristic(const double pStaticBreakRatio, const double pTogetherRatio, const double pMaxAngleDeviation) :
      Heuristic("DirectionOrientationRelationHeuristic"), mStaticBreakRatio(pStaticBreakRatio), mTogetherRatio(pTogetherRatio), mMaxAngleDeviation(pMaxAngleDeviation) {}

    void
    DirectionOrientationRelationHeuristic::applyHeuristic(const TracksPtr& tracks) {
        typedef GeometryHelper GH;
        typedef Eigen::Vector3d Vector;

        double bestDistance;
        for (TrackPtr& first : tracks->tracks) {
            TrackPtr currentBest;
            double currentClosestDistance;
            int currentBestBreaks = 1;
            int currentBestCommonPositions = 1;

            for (TrackPtr& second : tracks->tracks) {
                if (first == second) {
                    continue;
                }

                /*
                 * What we do:
                 * Calculate a direction Vector from first to second for every Frame.
                 * Check in every frame the angle between the reference vector (first vector) and the current vector.
                 * If the misalignment is more than mMaxAngleDeviation degrees, increase staticBreaks and
                 * recalculate the reference vote.
                 * Also calculate the average distance between first and second.
                 *
                 * At the end, if the staticBreaks are below mStaticBreakRatio of the sample range,
                 * and they appear together in more than mTogetherRatio of the frames,
                 * and the second is closer to first than the current closest track,
                 * replace the current closest track with second.
                 *
                 * At the end, choose the first<->second combination with the lowest rate of static breaks.
                 *
                 * This will always create a cluster of two tracks.
                 */

                int commonPositions = 0;
                double averageDistance = 0;

                for (size_t i = 0; i < first->objects.size(); i++) {
                    ObjectPtr firstObject = first->objects[i];
                    ObjectPtr secondObject = second->objects[i];
                    if (!firstObject || !secondObject) {
                        continue;
                    }

                    averageDistance += GH::getDistanceBetweenPoints(firstObject->pose->point,
                            secondObject->pose->point);
                    commonPositions++;
                }
                if (commonPositions < (double) first->objects.size() * mTogetherRatio) {
                    continue;
                }

                averageDistance /= (double) commonPositions;

                int staticBreaks = 0;
                Vector directionVector;
                double orientationDifference;
                bool firstRun = true;

                for (size_t i = 0; i < first->objects.size(); i++)
                {
                    ObjectPtr firstObject = first->objects[i];
                    ObjectPtr secondObject = second->objects[i];
                    if (!firstObject || !secondObject) {
                        continue;
                    }

                    if (firstRun) {
              directionVector = GH::getDirectionVector(firstObject->pose, secondObject->pose);
                        orientationDifference = GH::getAngleBetweenQuats(
                            firstObject->pose->quat, secondObject->pose->quat
                        );
                        firstRun = false;
                        continue;
                    }

                    Vector currentDirection = GH::getDirectionVector(firstObject->pose, secondObject->pose);
                    double currentOrientationDiff = GH::getAngleBetweenQuats(firstObject->pose->quat,
                                                                           secondObject->pose->quat);

		    double deviation = GH::getAngleBetweenAxes(directionVector, currentDirection);

                    double orientationDeviation = fabs(orientationDifference - currentOrientationDiff);

                    if (deviation > mMaxAngleDeviation || orientationDeviation > mMaxAngleDeviation)
                    {
                        staticBreaks++;
                        directionVector = currentDirection;
                        orientationDifference = currentOrientationDiff;
                    }
                }

                if (
                    ((double) staticBreaks < ((double) commonPositions) * mStaticBreakRatio) &&
                    (!currentBest || (currentClosestDistance > averageDistance))
                    ) {
                    currentBest = second;
                    currentClosestDistance = averageDistance;
                    currentBestBreaks = staticBreaks;
                    currentBestCommonPositions = commonPositions;
                }
            }

            if (currentBest) {
                double conf = 1 - (double) currentBestBreaks / (double) currentBestCommonPositions;
                if (!this->cluster
                    || (conf > this->confidence
                        || (conf == this->confidence && bestDistance > currentClosestDistance))) {
                    std::vector<TrackPtr> cluster;
                    cluster.push_back(first);
                    cluster.push_back(currentBest);
                    this->cluster = TracksPtr(new Tracks(cluster));
                    this->confidence = conf;
                    bestDistance = currentClosestDistance;
                }
            }
        }
    }

}


//  ========================================================================
//  Author: Youhua Xu
//  Date:   Jan-7, 2017
//
//  Supernovae data structures and nuisance parameters
//
//  JLA is defined in another independent source file
//  ========================================================================

#ifndef __SNE_UNION__
#define __SNE_UNION__

#include <string>
#include <armadillo>
#include <imcmc/imcmc.hpp>
#include <imcmc/parser++.hpp>

#include "ParamList.hpp"

struct SNe_UNION{

    DataInfo    data_info;

    int         sne_num;    //  580
    arma::vec   z;          //  redshifts of SNe
    arma::vec   mu, dmu;    //  distance modulus and uncertainties
    arma::vec   P;          //  probability of low mass
    arma::mat   icov;       //  covariance matrix, two are provided, nosys / sys, has aready been inverted
    bool        systematic; //  true = sys, false = nosys

    SNe_UNION(){};  // do nothing
    ~SNe_UNION() {}; // do nothing

    double      max_z();
    void        Init( std::string& dataset );
};

double Likelihood_SNe_UNION(imcmc::imcmc_double&    param,
                            double&                 lndet,
                            double&                 chisq,
                            void*                   model,
                            void*                   data,
                            imcmc::istate&          state );

#endif  //  __SNE_UNION__


#pragma once

#include <Eigen/Dense>

namespace RRT {

/**
 * @brief Handles a grid of obstacles laid over a continuous 2d plane.
 * @details The state space is broken up into a grid with the given discrete
 * height and widths.
 */
class ObstacleGrid {
public:
    ObstacleGrid(float width, float height, int discretizedWidth,
                 int discretizedHeight);
    ~ObstacleGrid();

    Eigen::Vector2i gridSquareForLocation(const Eigen::Vector2f& loc) const;

    /**
     * Finds the distance from state to its neareset obstacle. Only searches up
     *to
     * maxDist around
     * state so as to not waste time checking far away and irrelevant obstacles.
     *
     * @param state The location to search with respect to for the nearest
     * obstacle dist
     * @param maxDist The maximum vertical and horizontal distance from state to
     * search for obstacles
     */
    float nearestObstacleDist(const Eigen::Vector2f& state,
                              float maxDist) const;
    void clear();
    bool& obstacleAt(int x, int y);
    bool obstacleAt(int x, int y) const;
    bool& obstacleAt(const Eigen::Vector2i& gridLoc);
    bool obstacleAt(const Eigen::Vector2i& gridLoc) const;

    int discretizedWidth() const;
    int discretizedHeight() const;
    float width() const;
    float height() const;

private:
    int _discretizedWidth, _discretizedHeight;
    float _width, _height;

    /// 2d array of obstacles
    bool* _obstacles;
};

}  // namespace RRT


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(cylinder_test)
{

  {
    geometry::Cylinder<V> const A;

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.center()(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(A.center()(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(A.center()(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(A.axis()(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(A.axis()(1), 0.0, 0.01);
    BOOST_CHECK_CLOSE(A.axis()(2), 1.0, 0.01);

    BOOST_CHECK_CLOSE(A.radius(), 1.0, 0.01);
    BOOST_CHECK_CLOSE(A.height(), 1.0, 0.01);

  }

  {

    V const center = V::make(1.0, 2.0, 3.0);
    V const axis   = V::make(0.0, 5.0, 0.0);
    T const radius = 2.0;
    T const height = 4.0;

    geometry::Cylinder<V> const A = geometry::make_cylinder(radius, height, axis, center);

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.center()(0), 1.0, 0.01);
    BOOST_CHECK_CLOSE(A.center()(1), 2.0, 0.01);
    BOOST_CHECK_CLOSE(A.center()(2), 3.0, 0.01);

    BOOST_CHECK_CLOSE(A.axis()(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(A.axis()(1), 1.0, 0.01);
    BOOST_CHECK_CLOSE(A.axis()(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(A.radius(), 2.0, 0.01);
    BOOST_CHECK_CLOSE(A.height(), 4.0, 0.01);
  }

  {
    V const center = V::make(1.0, 2.0, 3.0);
    V const axis   = V::make(0.0, 5.0, 0.0);
    T const radius = 2.0;
    T const height = 4.0;

    geometry::Cylinder<V> A;

    A.center() = center;
    A.set_axis(axis);
    A.set_radius(radius);
    A.set_height(height);

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.center()(0), 1.0, 0.01);
    BOOST_CHECK_CLOSE(A.center()(1), 2.0, 0.01);
    BOOST_CHECK_CLOSE(A.center()(2), 3.0, 0.01);

    BOOST_CHECK_CLOSE(A.axis()(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(A.axis()(1), 1.0, 0.01);
    BOOST_CHECK_CLOSE(A.axis()(2), 0.0, 0.01);

    BOOST_CHECK_CLOSE(A.radius(), 2.0, 0.01);
    BOOST_CHECK_CLOSE(A.height(), 4.0, 0.01);
  }

  {

    V const center = V::make(1.0, 2.0, 3.0);
    V const axis   = V::make(0.0, 5.0, 0.0);
    T const radius = 2.0;
    T const height = 4.0;

    geometry::Cylinder<V> const A = geometry::make_cylinder(radius, height, axis, center);
    geometry::Cylinder<V> const B(A);
    geometry::Cylinder<V> const C = B;

    BOOST_CHECK( geometry::is_valid(B) == true );

    BOOST_CHECK_CLOSE(B.center()(0), 1.0, 0.01);
    BOOST_CHECK_CLOSE(B.center()(1), 2.0, 0.01);
    BOOST_CHECK_CLOSE(B.center()(2), 3.0, 0.01);
    BOOST_CHECK_CLOSE(B.axis()(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(B.axis()(1), 1.0, 0.01);
    BOOST_CHECK_CLOSE(B.axis()(2), 0.0, 0.01);
    BOOST_CHECK_CLOSE(B.radius(), 2.0, 0.01);
    BOOST_CHECK_CLOSE(B.height(), 4.0, 0.01);

    BOOST_CHECK_CLOSE(C.center()(0), 1.0, 0.01);
    BOOST_CHECK_CLOSE(C.center()(1), 2.0, 0.01);
    BOOST_CHECK_CLOSE(C.center()(2), 3.0, 0.01);
    BOOST_CHECK_CLOSE(C.axis()(0), 0.0, 0.01);
    BOOST_CHECK_CLOSE(C.axis()(1), 1.0, 0.01);
    BOOST_CHECK_CLOSE(C.axis()(2), 0.0, 0.01);
    BOOST_CHECK_CLOSE(C.radius(), 2.0, 0.01);
    BOOST_CHECK_CLOSE(C.height(), 4.0, 0.01);

  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <Python.h>
#include <omp.h>
#include <vector>
#include <set>
#include <map>
#include <string>
#include <iostream>
#include <boost/algorithm/minmax_element.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

const double machine_epsilon = 1e-8;

using namespace std;
using namespace boost;
using namespace boost::numeric;

struct DATUM_X_T
{
    wstring x;
    double proba;
};
typedef vector<DATUM_X_T> DATA_X_T;
struct DATUM_XY_T
{
    wstring x;
    wstring y;
    double proba;
};
typedef vector<DATUM_XY_T> DATA_XY_T;
struct FEATURE1_T
{
    wchar_t prev_label;
    wchar_t label;
};
struct FEATURE2_T
{
    vector<int> positions;
    vector<wchar_t> values;
    wchar_t label;
};
typedef pair<vector<FEATURE1_T>, vector<FEATURE2_T>> FEATURES_T;
typedef map<wchar_t, size_t> YSET_T;

struct X_MS_ITEM_T
{
    wstring x;
    double proba;
    vector<ublas::matrix<double>> ms;
    ublas::matrix<double> alpha;
    ublas::matrix<double> beta;
    double logz;
};

inline double logsumexp(ublas::vector<double>& x )
{
    double maxval = *max_element(x.begin(), x.end());
    double val = 0;
    for(ublas::vector<double>::iterator it=x.begin(); it<x.end(); ++it)
        val += exp(*it - maxval);
    val = maxval + log(val);
    return val;
}

YSET_T pyyset_as_cpp(PyObject* pyyset)
{
    YSET_T yset;
    PyObject *key, *value;
    Py_ssize_t pos = 0;
    while (PyDict_Next(pyyset, &pos, &key, &value))
    {
        wchar_t c;
        PyUnicode_AsWideChar(key, &c, 1);
        size_t i = PyLong_AsSize_t(value);
        yset.insert(make_pair(c, i));
    }
    return yset;
}

DATA_X_T pydata_x_as_cpp(PyObject* pydata_x)
{
    DATA_X_T result;
    for(Py_ssize_t i=0, size = PyList_Size(pydata_x); i<size; ++i)
    {
        PyObject* pyxp = PyList_GetItem(pydata_x, i);
        wstring x( PyUnicode_AsUnicode( PyTuple_GetItem( pyxp, 0) ) );
        double proba = PyFloat_AsDouble( PyTuple_GetItem( pyxp, 1) );
        result.push_back({x, proba});
    }
    return result;
}

DATA_XY_T pydata_xy_as_cpp(PyObject* pydata_xy)
{
    DATA_XY_T result;
    for(Py_ssize_t i=0, size=PyList_Size(pydata_xy); i<size; ++i)
    {
        PyObject* pyxyp = PyList_GetItem( pydata_xy, i );
        PyObject* pyxy = PyTuple_GetItem( pyxyp, 0 );
        wstring x( PyUnicode_AsUnicode( PyTuple_GetItem( pyxy, 0 ) ) );
        wstring y( PyUnicode_AsUnicode( PyTuple_GetItem( pyxy, 1 ) ) );
        double proba = PyFloat_AsDouble( PyTuple_GetItem( pyxyp, 1 ) );
        assert( x.length() == y.length() );
        //wcout << x << y << proba << endl;
        result.push_back({x, y, proba});
    }
    return result;
}

vector<ublas::matrix<double>> xmatrices(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const wstring& x);
ublas::matrix<double> xalphas(const vector<ublas::matrix<double>>& ms);
ublas::matrix<double> xbetas(const vector<ublas::matrix<double>>& ms);

FEATURES_T pyfeatures_as_cpp(PyObject* pyfeatures)
{
    vector<FEATURE1_T> features1;
    vector<FEATURE2_T> features2;
    Py_ssize_t K = PyList_Size(pyfeatures);
    for(Py_ssize_t k=0; k<K; ++k)
    {
        PyObject* pyfeature = PyList_GetItem(pyfeatures,k);
        Py_ssize_t t = PyTuple_Size(pyfeature);
        if(t==2)
        {
            wchar_t prev_label = *PyUnicode_AsUnicode(PyTuple_GetItem(pyfeature, 0));
            wchar_t label = *PyUnicode_AsUnicode(PyTuple_GetItem(pyfeature, 1));
            features1.push_back({prev_label, label});
        }
        else if(t==3)
        {
            wchar_t label = *PyUnicode_AsUnicode(PyTuple_GetItem(pyfeature, 2));
            PyObject* pytemp = PyTuple_GetItem(pyfeature, 0);
            PyObject* pyValue = PyTuple_GetItem(pyfeature, 1);
            assert( PyTuple_Size(pytemp) == PyTuple_Size(pyValue) );
            vector<int> positions;
            vector<wchar_t> values;
            for(Py_ssize_t i=0; i<PyTuple_Size(pytemp); ++i)
            {
                int pos = PyLong_AsLong(PyTuple_GetItem(pytemp, i));
                wchar_t value = *PyUnicode_AsUnicode(PyTuple_GetItem(pyValue, i));
                positions.push_back(pos);
                values.push_back(value);
            }
            features2.push_back({positions, values, label});
        }
        else
        {
            cout << "feature size error! " << t << endl;
            exit(-1);
        }
    }
    return make_pair(features1, features2);
}

inline bool fkyyxi(const FEATURES_T& features, const size_t k,const wchar_t prev_label,const wchar_t label,const wstring& x,const size_t i)
{
    size_t K1 = features.first.size();
    if ( k<K1 )
        return features.first[k].prev_label == prev_label and features.first[k].label == label ? 1 : 0;
    if( features.second[k-K1].label == label )
    {
        size_t k3 = k-K1, n = x.length();
        for(size_t t=0, T=features.second[k3].positions.size(); t<T; ++t)
        {
            int pos = (int)i+features.second[k3].positions[t];
            if(((pos>=0 && pos<(int)n) ? x[pos] : L'\0') != features.second[k3].values[t]) return 0;
        }
        return 1;
    }
    return 0;
}

inline double featurekxy(const FEATURES_T& features,const  size_t k, const wstring& x,const wstring& y)
{
    assert( x.length() == y.length() );
    double value = 0.0;
    wchar_t prev_label = L'^';
    for(size_t i=0, n = x.length(); i<=n; ++i)
    {
        wchar_t label = i < n? y.at(i) : L'$';
        value += fkyyxi(features, k, prev_label, label, x, i);
        prev_label = label;
    }
    return value;
}

ublas::vector<double> stat_all_xy_features_values(const FEATURES_T& features, const DATA_XY_T& data_xy)
{
    size_t K1 = features.first.size();
    size_t K2 = features.second.size();
    ublas::vector<double> values(K1+K2, 0);
    #pragma omp parallel for
    for(size_t k=0; k<K1+K2; ++k)
    {
        double value = 0;
        for(DATA_XY_T::const_iterator it=data_xy.begin(); it<data_xy.end(); ++it)
            value += it->proba * featurekxy(features, k, it->x, it->y);
        values(k) = value;
    }
    return values;
}

ublas::vector<double> doublelist_as_cpp(PyObject* pydoublelist)
{
    ublas::vector<double> vec(PyList_Size(pydoublelist));
    for(Py_ssize_t i=0; i<PyList_Size(pydoublelist); ++i)
    {
        vec(i) = PyFloat_AsDouble(PyList_GetItem(pydoublelist, i));
    }
    return vec;
}

static PyObject* stat_all_xy_features_values(PyObject* self, PyObject* args)
{
    PyObject* pyfeatures = NULL;
    PyObject* pydata_xy  = NULL;
    if (!PyArg_ParseTuple(args, "OO", &pyfeatures, &pydata_xy))
        return NULL;
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    DATA_XY_T data_xy = pydata_xy_as_cpp(pydata_xy);
    ublas::vector<double> values = stat_all_xy_features_values(features, data_xy);
    ublas::vector<double>::size_type k=0, K=values.size();
    PyObject* pyvalues = PyList_New(K);
    for(; k<K; ++k) PyList_SetItem(pyvalues, k, PyFloat_FromDouble( values(k) ) );
    return pyvalues;
}

double model_values(const ublas::vector<double>& weights,const YSET_T& yset,const FEATURES_T& features, const DATA_X_T& data_x)
{
    double val = 0;
    #pragma omp parallel for reduction(+:val)
    for( DATA_X_T::const_iterator it=data_x.begin(); it<data_x.end(); ++it )
    {
        vector<ublas::matrix<double>> ms = xmatrices(yset, features, weights, it->x);
        ublas::matrix<double> alpha = xalphas(ms);
        //ublas::matrix<double> beta = xbetas(ms);
        ublas::vector<double> alphalastrow = ublas::matrix_row<ublas::matrix<double>>(alpha, alpha.size1()-1);
        //ublas::vector<double> betalastrow = ublas::matrix_row<ublas::matrix<double>>(beta, 0);
        double logz1 = logsumexp(alphalastrow);
        //double logz2 = logsumexp(betalastrow);
        //cout << logz1 << " , " << logz2 << " abs:" << abs(logz1 - logz2) << endl;
        //assert( abs(logz1 - logz2) < machine_epsilon );
        val += it->proba * logz1;
    }
    return val;
}

static PyObject* model_values(PyObject* self, PyObject* args)
{
    PyObject* pyweights = NULL;
    PyObject* pyyset = NULL;
    PyObject* pyfeatures = NULL;
    PyObject* pydata_x  = NULL;
    if (!PyArg_ParseTuple(args, "OOOO", &pyweights, &pyyset, &pyfeatures, &pydata_x))
        return NULL;
    ublas::vector<double> weights = doublelist_as_cpp(pyweights);
    YSET_T yset = pyyset_as_cpp(pyyset);
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    DATA_X_T data_x = pydata_x_as_cpp(pydata_x);
    return PyFloat_FromDouble(model_values(weights, yset, features, data_x));
}

inline double yyxi(const FEATURES_T& features,const ublas::vector<double>& weights,const wchar_t prev_label,const wchar_t label,const wstring& x,const size_t i)
{
    static map<tuple<wchar_t, wchar_t, wstring, size_t>, vector<size_t>> yyxi_cache_helper;
    double val=0;
    auto key = make_tuple(prev_label, label, x, i);
    if( yyxi_cache_helper.find(key) != yyxi_cache_helper.end() )
    {
        vector<size_t> ks = yyxi_cache_helper[key];
        for(vector<size_t>::iterator it=ks.begin(); it<ks.end(); ++it)
            val += weights[*it];
        return val;
    }
    for(size_t k=0; k<weights.size(); ++k)
    {
        if( fkyyxi(features, k, prev_label, label, x, i) > 0.5 )
        {
            if ( yyxi_cache_helper.find(key) == yyxi_cache_helper.end() )
            {
                #pragma omp critical
                yyxi_cache_helper.insert(make_pair(key, vector<size_t>()));
            }
            #pragma omp critical
            yyxi_cache_helper[key].push_back(k);
            val += weights[k];
        }
    }
    return val;
}

inline ublas::matrix<double> xmatrixi(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const wstring& x,const size_t i)
{
    ublas::matrix<double> m(yset.size(), yset.size());
    for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
    {
        for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
        {
            if(i==0)
            {
                m(y0it->second, y1it->second) = (y0it->second==0) ? yyxi(features, weights, L'^', y1it->first, x, i) : 0;
            }
            else if(i==x.length())
            {
                m(y0it->second, y1it->second) = (y1it->second==0) ? yyxi(features, weights, y0it->first, L'$', x, i) : 0;
            }
            else
            {
                m(y0it->second, y1it->second) = yyxi(features, weights, y0it->first, y1it->first, x, i);
            }
        }
    }
    return m;
}

inline vector<ublas::matrix<double>> xmatrices(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const wstring& x)
{
    size_t n = x.length();
    vector<ublas::matrix<double>> ms(n+1);
    for(size_t i=0; i<=n; ++i)
        ms.at(i) = xmatrixi(yset, features, weights, x, i);
    return ms;
}

inline ublas::matrix<double> xalphas(const vector<ublas::matrix<double>>& ms)
{
    size_t mslen = ms.size();
    ublas::matrix<double> alpha(mslen, ms[0].size1());
    ublas::row<ublas::matrix<double>>(alpha, 0) = ublas::row<ublas::matrix<double>>(ms[0], 0);
    for(size_t i=1; i<mslen-1; ++i)
    {
        for(size_t j=0; j<ms[i].size2(); ++j)
        {
            ublas::vector<double> values = ublas::row<ublas::matrix<double>>(alpha, i-1)
                                           + ublas::column<ublas::matrix<double>>(ms[i], j);
            alpha(i, j) = logsumexp( values );
        }
    }
    ublas::row<ublas::matrix<double>>(alpha, mslen-1) = ublas::row<ublas::matrix<double>>(alpha, mslen-2)
                                   + ublas::column<ublas::matrix<double>>(ms[mslen-1], 0);
    return alpha;
}

inline ublas::matrix<double> xbetas(const vector<ublas::matrix<double>>& ms)
{
    size_t mslen = ms.size();
    ublas::matrix<double> beta(mslen, ms[0].size1());
    ublas::row<ublas::matrix<double>>(beta, mslen-1) = ublas::column<ublas::matrix<double>>(ms[mslen-1], 0);
    for(size_t i=mslen-2; i>0; --i)
    {
        for(size_t j=0; j<ms[i].size1(); ++j)
        {
            ublas::vector<double> values = ublas::row<ublas::matrix<double>>(ms[i], j)
                                           + ublas::row<ublas::matrix<double>>(beta, i+1);
            beta(i, j) = logsumexp( values );
        }
    }
    ublas::row<ublas::matrix<double>>(beta, 0) = ublas::row<ublas::matrix<double>>(ms[0], 0)
                                   + ublas::row<ublas::matrix<double>>(beta, 1);
    return beta;
}

inline double xyiiproba(const YSET_T& yset,const vector<ublas::matrix<double>>& ms,const ublas::matrix<double>& alpha,const ublas::matrix<double>& beta,const size_t s0,const size_t s1,const size_t i,const double logz)
{
    //assert( 0<=i and i<ms.size() );
    double p = -logz;
    if (i==0)
    {
        p += ms[i](0, s1) + beta(i+1, s1);
    }
    else if(i==ms.size()-1)
    {
        p += alpha(i-1, s0) + ms[i](s0, 0);
    }
    else
    {
        p += alpha(i-1, s0) + ms[i](s0, s1) + beta(i+1, s1);
    }
    return exp( p );
}

inline double xyproba(const YSET_T& yset, const vector<ublas::matrix<double>>& ms, const double logz,const wstring& y)
{
    double v=0;
    v += ms.front()(0, yset.at(y.front()));
    for(size_t i=1; i<y.size(); ++i)
        v+=ms.at(i)(yset.at(y.at(i-1)), yset.at(y.at(i)));
    v += ms.back()( yset.at(y.back()), 0);
    return exp( v - logz );
}

inline double kx(const YSET_T& yset,const FEATURES_T& features,const size_t k,const wstring& x,const vector<ublas::matrix<double>>& ms,const ublas::matrix<double>& alpha,const ublas::matrix<double>& beta,const double logz)
{
    //assert( x.length()+1 == ms.size() );
    double v = 0;
    size_t n = x.length();

    for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
        if( fkyyxi(features, k, L'^', y1it->first, x, 0) )
            v += xyiiproba(yset, ms, alpha, beta, 0, y1it->second, 0, logz);

    for(size_t i=1; i<n; ++i)
        for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
            for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
                if( fkyyxi(features, k, y0it->first, y1it->first, x, i) )
                    v += xyiiproba(yset, ms, alpha, beta, y0it->second, y1it->second, i, logz);

    for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
        if( fkyyxi(features, k, y0it->first, L'$', x, n) )
            v += xyiiproba(yset, ms, alpha, beta, y0it->second, 0, n, logz);

    return v;
}

inline double allxv(const YSET_T& yset, const FEATURES_T& features,const size_t k,const vector<X_MS_ITEM_T>& malphabetaz)
{
    double grad=0;
    for(vector<X_MS_ITEM_T>::const_iterator it=malphabetaz.cbegin(); it<malphabetaz.cend(); ++it)
        grad += it->proba * kx( yset, features, k, it->x, it->ms, it->alpha, it->beta, it->logz );
    return grad;
}

ublas::vector<double> model_grads(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const DATA_X_T& data_x)
{
    size_t K = weights.size();
    ublas::vector<double> grads(K);
    vector<X_MS_ITEM_T> malphabetaz( data_x.size() );
    #pragma omp parallel for
    for(DATA_X_T::size_type i=0; i<data_x.size(); ++i)
    {
        vector<ublas::matrix<double>> ms = xmatrices( yset, features, weights, data_x.at(i).x );
        ublas::matrix<double> alpha = xalphas(ms);
        ublas::matrix<double> beta = xbetas(ms);
        ublas::vector<double> alphalastrow = ublas::matrix_row<ublas::matrix<double>>(alpha, alpha.size1()-1);
        ublas::vector<double> betafirstrow = ublas::matrix_row<ublas::matrix<double>>(beta, 0);
        double logz1 = logsumexp( alphalastrow );
        double logz2 = logsumexp( betafirstrow );
        assert( abs(logz1 - logz2) < machine_epsilon );
        assert( data_x.at(i).x.length()+1 == ms.size() );
        malphabetaz.at(i) = {data_x.at(i).x, data_x.at(i).proba, ms, alpha, beta, logz1};
    }
    #pragma omp parallel for
    for(size_t k=0; k<K; ++k)
        grads(k) = allxv(yset, features, k, malphabetaz);
    return grads;
}

ublas::vector<double> model_grads1(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const DATA_X_T& data_x)
{
    size_t K = weights.size();
    ublas::vector<double> grads(K);
    vector<X_MS_ITEM_T> malphabetaz( data_x.size() );
    #pragma omp parallel for
    for(DATA_X_T::size_type i=0; i<data_x.size(); ++i)
    {
        vector<ublas::matrix<double>> ms = xmatrices( yset, features, weights, data_x.at(i).x );
        ublas::matrix<double> alpha = xalphas(ms);
        ublas::matrix<double> beta = xbetas(ms);
        ublas::vector<double> alphalastrow = ublas::matrix_row<ublas::matrix<double>>(alpha, alpha.size1()-1);
        ublas::vector<double> betafirstrow = ublas::matrix_row<ublas::matrix<double>>(beta, 0);
        double logz1 = logsumexp( alphalastrow );
        double logz2 = logsumexp( betafirstrow );
        assert( abs(logz1 - logz2) < machine_epsilon );
        assert( data_x.at(i).x.length()+1 == ms.size() );
        malphabetaz.at(i) = {data_x.at(i).x, data_x.at(i).proba, ms, alpha, beta, logz1};
    }
    struct XS0S1I_T
    {
        size_t s0;
        size_t s1;
        size_t xi;
    };
    typedef vector<XS0S1I_T> XS0S1I_LIST_T;
    struct KX_T
    {
        size_t xnum;
        XS0S1I_LIST_T pxs;
    };
    typedef vector<KX_T> KX_LIST_T;
    static vector<KX_LIST_T> kxtablecache(K);
    static bool kxtablecache_flag = 0;
    if ( !kxtablecache_flag )
    {
        #pragma omp parallel for
        for(size_t k=0; k<K; ++k)
        {
            KX_LIST_T kx;
            for(size_t xnum=0; xnum<data_x.size(); ++xnum)
            {
                XS0S1I_LIST_T xs0s1i;
                wstring x = data_x.at(xnum).x;
                size_t n = x.length();
                for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
                    if( fkyyxi(features, k, L'^', y1it->first, x, 0) )
                        xs0s1i.push_back({ 0, y1it->second, 0 });
                for(size_t i=1; i<n; ++i)
                    for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
                        for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
                            if( fkyyxi(features, k, y0it->first, y1it->first, x, i) )
                                xs0s1i.push_back({ y0it->second, y1it->second, i });
                for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
                    if( fkyyxi(features, k, y0it->first, L'$', x, n) )
                        xs0s1i.push_back( {y0it->second, 0, n});
                if ( xs0s1i.size() > 0 )
                    kx.push_back({xnum, xs0s1i});
            }
            kxtablecache.at(k) = kx;
        }
        kxtablecache_flag = 1;
    }

    #pragma omp parallel for
    for(size_t k=0; k<K; ++k)
    {
        const KX_LIST_T& kx = kxtablecache.at(k);
        for( KX_LIST_T::const_iterator kxit=kx.begin(); kxit<kx.end(); ++kxit  )
        {
            const X_MS_ITEM_T& xitem = malphabetaz.at(kxit->xnum);
            const XS0S1I_LIST_T& xs0s1i = kxit->pxs;
            double v=0;
            for( XS0S1I_LIST_T::const_iterator it=xs0s1i.begin(); it<xs0s1i.end(); ++it )
            {
                v += xyiiproba(yset, xitem.ms, xitem.alpha, xitem.beta, it->s0, it->s1, it->xi, xitem.logz );
            }
            grads(k) += xitem.proba * v;
        }
    }
    return grads;
}

static PyObject* model_grads(PyObject* self, PyObject* args)
{
    PyObject* pyyset = NULL;
    PyObject* pyfeatures = NULL;
    PyObject* pyweights = NULL;
    PyObject* pydata_x  = NULL;
    if (!PyArg_ParseTuple(args, "OOOO", &pyyset, &pyfeatures, &pyweights, &pydata_x))
        return NULL;
    YSET_T yset = pyyset_as_cpp(pyyset);
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    ublas::vector<double> weights = doublelist_as_cpp(pyweights);
    DATA_X_T data_x = pydata_x_as_cpp(pydata_x);
    ublas::vector<double> grads = model_grads(yset, features, weights, data_x);
    PyObject* pygrads = PyList_New(grads.size());
    for(size_t i=0; i<grads.size(); ++i)
        PyList_SetItem(pygrads, i, PyFloat_FromDouble( grads(i) ) );
    return pygrads;
}

static PyObject* model_grads1(PyObject* self, PyObject* args)
{
    PyObject* pyyset = NULL;
    PyObject* pyfeatures = NULL;
    PyObject* pyweights = NULL;
    PyObject* pydata_x  = NULL;
    if (!PyArg_ParseTuple(args, "OOOO", &pyyset, &pyfeatures, &pyweights, &pydata_x))
        return NULL;
    YSET_T yset = pyyset_as_cpp(pyyset);
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    ublas::vector<double> weights = doublelist_as_cpp(pyweights);
    DATA_X_T data_x = pydata_x_as_cpp(pydata_x);
    ublas::vector<double> grads = model_grads1(yset, features, weights, data_x);
    PyObject* pygrads = PyList_New(grads.size());
    for(size_t i=0; i<grads.size(); ++i)
        PyList_SetItem(pygrads, i, PyFloat_FromDouble( grads(i) ) );
    return pygrads;
}

static PyMethodDef crfextMethods[] =
{
    {"model_grads", model_grads, METH_VARARGS, "grads"},
    {"model_grads1", model_grads1, METH_VARARGS, "grads"},
    {"model_values", model_values, METH_VARARGS, ""},
    {"stat_all_xy_features_values", stat_all_xy_features_values, METH_VARARGS, "stat all features values on all data"},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

static struct PyModuleDef crfextmodule =
{
    PyModuleDef_HEAD_INIT,
    "crfext",
    NULL,
    -1,
    crfextMethods
};

PyMODINIT_FUNC PyInit_crfext(void)
{
    return PyModule_Create(&crfextmodule);
};