#include <gtest/gtest.h>

#include "scheme/objective/ObjectiveFunction.hh"
#include "scheme/objective/ObjectiveFunction_io.hh"

#include "scheme/util/meta/ref_wrap.hh"

#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/size.hpp>
#include <boost/static_assert.hpp>

#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/include/io.hpp>

namespace scheme {
namespace objective {
namespace test {

namespace mpl = boost::mpl;
namespace bf = boost::fusion;
using std::cout;
using std::endl;

struct ScoreInt {
	typedef double Result;
	typedef int Interaction;
	double local_scale;
	ScoreInt():local_scale(1.0){}
	static std::string name(){ return "ScoreInt"; }
	template<class Config>
	Result operator()(Interaction a, Config const& c) const {
		return a*c.scale * local_scale;
	}
};
std::ostream & operator<<(std::ostream & out,ScoreInt const& si){ return out << si.name(); }

struct ScoreInt2 {
	typedef double Result;
	typedef int Interaction;
	static std::string name(){ return "ScoreInt2"; }
	template<class Config>
	Result operator()(Interaction const & a, Config const& c) const {
		return 2*a*c.scale;
	}
};
std::ostream & operator<<(std::ostream & out,ScoreInt2 const& si){ return out << si.name(); }
struct ScoreInt3 {
	typedef double Result;
	typedef int Interaction;
	static std::string name(){ return "ScoreInt3"; }
	template<class Config>
	Result operator()(Interaction const & a, Config const& c) const {
		return 3*a*c.scale;
	}
};
std::ostream & operator<<(std::ostream & out,ScoreInt3 const& si){ return out << si.name(); }

struct ScoreDouble {
	typedef double Result;
	typedef double Interaction;
	static std::string name(){ return "ScoreDouble"; }
	template<class Config>
	Result operator()(Interaction const & a, Config const& c) const {
		return a*c.scale;
	}
};
std::ostream & operator<<(std::ostream & out,ScoreDouble const& si){ return out << si.name(); }

struct ScoreIntDouble {
	typedef double Result;
	typedef std::pair<int,double> Interaction;
	static std::string name(){ return "ScoreIntDouble"; }
	template<class Config>
	Result operator()(int const & i,double const & a, Config const& c) const {
		return i*a*c.scale;
	}
	template<class Pair, class Config>
	Result operator()(Pair const & p, Config const& c) const {
		return this->operator()(p.first,p.second,c);
	}
};
std::ostream & operator<<(std::ostream & out,ScoreIntDouble const& si){ return out << si.name(); }

struct ConfigTest {
	double scale;
	ConfigTest():scale(1){}
};

TEST(ObjectiveFunction,detect_call_operator){
	BOOST_STATIC_ASSERT(( util::meta::has_const_call_oper_3<ScoreIntDouble,double,int const &,double const &,ConfigTest const &>::value ));
}

template<class Interactions >
struct SimpleInteractionSource {

	typedef util::meta::InstanceMap<Interactions,std::vector<mpl::_1> > MAP;

	MAP interactions_map_;

	typedef Interactions InteractionTypes;

	template<class Interaction>	std::vector<Interaction> &
	get_interactions() {
		return interactions_map_.template get<Interaction>();
	}
	template<class Interaction>	std::vector<Interaction> const &
	get_interactions() const {
		return interactions_map_.template get<Interaction>();
	}

	template<class Interaction>
	struct has_interaction : mpl::bool_<f::result_of::has_key<MAP,Interaction>::value> {};

	template<class Interaction>
	struct interaction_placeholder_type { typedef Interaction type; };

};
template<class I>
std::ostream & operator<<(std::ostream & out, SimpleInteractionSource<I> const & s){
	return out << s.interactions_map_;
}

TEST(ObjectiveFunction,basic_tests_local_and_global_config){
	typedef	ObjectiveFunction<
		mpl::list<
			ScoreInt,
			ScoreInt2,
			ScoreDouble,
			ScoreIntDouble
		>,
		ConfigTest
	> ObjFun;
	ObjFun score;

	typedef ObjFun::Results Results;
	typedef SimpleInteractionSource< mpl::vector<int,double,std::pair<int,double> > > InteractionSource;
	InteractionSource interaction_source;
	EXPECT_EQ( Results(0,0,0,0), score(interaction_source) );
	interaction_source.get_interactions<int>().push_back(1);
	EXPECT_EQ( Results(1,2,0,0), score(interaction_source) );
	interaction_source.get_interactions<double>().push_back(1.234);
	interaction_source.get_interactions<double>().push_back(2);
	interaction_source.get_interactions<double>().push_back(-2);
	EXPECT_EQ( Results(1,2,1.234,0), score(interaction_source) );
	interaction_source.get_interactions<std::pair<int,double> >().push_back(std::make_pair(1,1.5));
	EXPECT_EQ( Results(1,2,1.234,1.5), score(interaction_source) );

	score.default_config_.scale = 2.0;
	EXPECT_EQ( Results(2,4,2.468,3.0), score(interaction_source) );

	score.get_objective<ScoreInt>().local_scale = 2.0;
	EXPECT_EQ( Results(4,4,2.468,3.0), score(interaction_source) );

	std::ostringstream oss;
	oss << score << endl;
	// cout << oss.str();
}

TEST(ObjectiveFunction,test_results){
	typedef	ObjectiveFunction< mpl::vector< ScoreDouble, ScoreInt >, ConfigTest	> ObjFun;
	ObjFun score;
	// typedef util::meta::InstanceMap< mpl::vector<int,double>, std::vector<mpl::_1> > InteractionSource;
	typedef SimpleInteractionSource< mpl::vector<int,double> > InteractionSource;

	InteractionSource interaction_source;
	interaction_source.get_interactions<int>().push_back(1);
	interaction_source.get_interactions<int>().push_back(7);
	interaction_source.get_interactions<double>().push_back(1.2345);
	ObjFun::Results weights(2.0);
	ObjFun::Results results = score(interaction_source);
	float tot = results.sum();
	EXPECT_FLOAT_EQ( 9.2345f, tot );
	EXPECT_DOUBLE_EQ( 9.2345, results.sum() );
	EXPECT_DOUBLE_EQ( 18.469,  (results*weights).sum() );
	EXPECT_DOUBLE_EQ( 18.469,  (results+results).sum() );
	EXPECT_DOUBLE_EQ( 27.7035, (results*weights + results).sum() );
	EXPECT_DOUBLE_EQ( 9.2345,  (results*weights - results).sum() );
	EXPECT_DOUBLE_EQ( 9.2345,  (results*weights/weights).sum() );
}

template<class Interactions>
struct PlaceholderInteractionSource {
	typedef size_t Placeholder;
	typedef util::meta::InstanceMap<Interactions,std::vector<mpl::_1> > MAP;
	typedef util::meta::InstanceMap<Interactions,mpl::always<std::vector<Placeholder> > > MAP_int;

	MAP interactions_map_;
	MAP_int placeholder_map_;

	typedef Interactions InteractionTypes;

	template<class Interaction>
	void
	add_interaction(
		Interaction const & interaction
	){
		std::vector<Interaction> & vec = interactions_map_.template get<Interaction>();
		vec.push_back(interaction);
		placeholder_map_ .template get<Interaction>().push_back( vec.size()-1 );
		// cout << "add_interaction: " << interaction << " placeholder: " << vec.size()-1 << endl;
	}

	template<class Interaction>
	std::vector<Placeholder> const &
	get_interactions() const {
		return placeholder_map_.template get<Interaction>();
	}

	template<class Interaction>
	struct has_interaction : mpl::bool_<f::result_of::has_key<MAP,Interaction>::value> {};

	template<class Interaction>
	struct interaction_placeholder_type { typedef Placeholder type; };

	template<class Interaction>
	Interaction const &
	get_interaction_from_placeholder(
		Placeholder const & placeholder
	) const {
		// cout << "get_interaction_from_placeholder " << (interactions_map_.template get<Interaction>()[placeholder])
		//      << " placeholder " << placeholder << endl;
		return interactions_map_.template get<Interaction>()[placeholder];
	}

};
template<class I>
std::ostream operator<<(std::ostream & out, PlaceholderInteractionSource<I> const & s){
	return out << s.interactions_map_;
}

TEST(ObjectiveFunction,test_iteraction_placeholder){
	typedef	ObjectiveFunction<
		mpl::list<
			ScoreInt,
			ScoreInt2,
			ScoreInt3,
			ScoreDouble,
			ScoreIntDouble
		>,
		ConfigTest
	> ObjFun;
	ObjFun score;

	typedef ObjFun::Results Results;
	typedef PlaceholderInteractionSource< mpl::vector<int,double,std::pair<int,double> > > InteractionSource;
	InteractionSource interaction_source;
	EXPECT_EQ( Results(0,0,0,0,0), score(interaction_source) );
	interaction_source.add_interaction<int>(1);
	EXPECT_EQ( Results(1,2,3,0,0), score(interaction_source) );
	interaction_source.add_interaction<double>(1.234);
	interaction_source.add_interaction<double>(2);
	interaction_source.add_interaction<double>(-2);
	EXPECT_EQ( Results(1,2,3,1.234,0), score(interaction_source) );
	interaction_source.add_interaction<std::pair<int,double> >(std::make_pair(1,1.5));
	EXPECT_EQ( Results(1,2,3,1.234,1.5), score(interaction_source) );

	score.default_config_.scale = 2.0;
	EXPECT_EQ( Results(2,4,6,2.468,3.0), score(interaction_source) );

	score.get_objective<ScoreInt>().local_scale = 2.0;
	EXPECT_EQ( Results(4,4,6,2.468,3.0), score(interaction_source) );

}

TEST(ObjectiveFunction,ref_wrap_sanity){
	int i = 2;
	char c = 'b';
	std::pair<int,char> p(1,'a');
	std::pair<boost::reference_wrapper<int>,boost::reference_wrapper<char> > pr(boost::ref(i),boost::ref(c));
	EXPECT_NE( p.first, pr.first );	EXPECT_NE( p.second, pr.second );
	p = pr;
	EXPECT_EQ( p.first, pr.first );	EXPECT_EQ( p.second, pr.second );
	pr.first = boost::ref(i); pr.second = boost::ref(c);

	int & iref = pr.first;
	EXPECT_EQ(i,iref);
	iref = 4;
	EXPECT_EQ(i,4);

	// pr = p;
	// EXPECT_EQ( p.first, pr.first );

}

template<class IM_FROM, class IM_TO>
struct CopyIM {
	IM_FROM const & from_;
	IM_TO & to_;
	CopyIM(IM_FROM const & from,IM_TO & to) : from_(from),to_(to) {}
	template<class T>
	void operator()(util::meta::type2type<T>){
		typedef typename util::meta::recursive_remove_refwrap<T>::type Tnoref;
		// typedef std::pair<int,double> Tnoref;
		// cout << "FOO ";
		// util::meta::print_type<T>();
		// to_.template get<T>().resize(from_.template get<T>().size());
		// std::copy( from_.template get<T>().begin(), from_.template get<T>().end(), to_.template get<T>().begin() );
		// to_.template get<T>().clear();
		BOOST_FOREACH( Tnoref const & v, from_.template get<T>() ){
			to_.template get<T>().push_back( std::make_pair( boost::ref(v.first), boost::ref(v.second) ) );
		}
	}
};

struct vec_of_rem_refwrap {
	template<class T> struct apply { typedef
		std::vector<typename util::meta::recursive_remove_refwrap<T>::type> type; }; };

template<class Interactions >
struct RefInteractionSource {

	typedef util::meta::InstanceMap<Interactions,std::vector<mpl::_1> > MAPREF;
	typedef util::meta::InstanceMap<Interactions, vec_of_rem_refwrap > MAPVAL;

	MAPVAL imap_val_;
	MAPREF imap_ref_;

	// leave thes out here to test default behavoir
	// typedef Interactions InteractionTypes;

	void store_refs(){
		CopyIM<MAPVAL,MAPREF> cpim(imap_val_,imap_ref_);
		m::for_each<Interactions,util::meta::type2type<m::_1> >(cpim);
	}

	template<class Interaction>	std::vector<Interaction> const &
	get_interactions() const {
		return imap_ref_.template get<Interaction>();
	}

	template<class Interaction>	std::vector<typename util::meta::recursive_remove_refwrap<Interaction>::type > &
	get_interactions_vals() {
		return imap_val_.template get<Interaction>();
	}

	template<class Interaction>
	struct has_interaction : mpl::bool_<f::result_of::has_key<MAPREF,Interaction>::value> {};

	template<class Interaction>
	struct interaction_placeholder_type { typedef Interaction type; };

};
template<class I>
std::ostream & operator<<(std::ostream & out, RefInteractionSource<I> const & s){
	return out << s.imap_val_;
}

struct ScoreIntRefDoubleRef : ScoreIntDouble {
	typedef std::pair<boost::reference_wrapper<int const>, boost::reference_wrapper<double const> > Interaction;
	// template<class Config>
	// Result operator()(Interaction const & p, Config const& c) const {
	// 	int i = p.first;
	// 	double d = p.second;
	// 	this->template operator()<Config>(i,d,result,c);
	// }
};

TEST(ObjectiveFunction,tuple_of_ref_interactions){
	typedef	ObjectiveFunction<
		mpl::list<
			ScoreIntRefDoubleRef
		>,
		ConfigTest
	> ObjFun;
	ObjFun score;

	using boost::reference_wrapper;

	typedef ObjFun::Results Results;
	typedef std::pair<reference_wrapper<int const>,reference_wrapper<double const> > I1;
	typedef RefInteractionSource< mpl::vector<I1> > InteractionSource;
	InteractionSource interaction_source;

	EXPECT_EQ( Results(0), score(interaction_source) );
	interaction_source.get_interactions_vals<I1>().push_back(std::make_pair(1,1.5));
	interaction_source.store_refs();
	// std::vector< std::pair<int,double> > & tmp = interaction_source.get_interactions_vals<I1>();

	EXPECT_EQ( Results(1.5), score(interaction_source) );

	// std::ostringstream oss;
	// oss << score << endl;

}

struct ScoreIntWithPre {
	typedef double Result;
	typedef int Interaction;
	double local_scale;
	typedef m::true_ HasPre;
	ScoreIntWithPre():local_scale(1.0){}
	static std::string name(){ return "ScoreIntWithPre"; }
	template<class Config>
	Result operator()(Interaction a, Config const& c) const {
		return a*c.scale * local_scale;
	}
	template<class Scene, class Config>
	void pre( Scene const &, Result & r, Config const & c ) const {
		r += 1.0;
	}
};
std::ostream & operator<<(std::ostream & out,ScoreIntWithPre const& si){ return out << si.name(); }

TEST( ObjectiveFunction, test_pre )
{
	typedef	ObjectiveFunction<
		mpl::list<
			ScoreInt
		>,
		ConfigTest
	> ObjFunNoPre;
	ObjFunNoPre scorenopre;

	typedef	ObjectiveFunction<
		mpl::list<
			ScoreIntWithPre
		>,
		ConfigTest
	> ObjFunWithPre;
	ObjFunWithPre scorewithpre;

	typedef ObjFunNoPre::Results ResultsNoPre;
	typedef ObjFunWithPre::Results ResultsWithPre;
	typedef SimpleInteractionSource< mpl::vector<int,double,std::pair<int,double> > > InteractionSource;
	InteractionSource interaction_source;
	EXPECT_EQ( ResultsNoPre  (0), scorenopre  (interaction_source) );
	EXPECT_EQ( ResultsWithPre(1), scorewithpre(interaction_source) );
	interaction_source.get_interactions<int>().push_back(1);
	EXPECT_EQ( ResultsNoPre  (1), scorenopre  (interaction_source) );
	EXPECT_EQ( ResultsWithPre(2), scorewithpre(interaction_source) );

}

struct ScoreIntWithPost {
	typedef double Result;
	typedef int Interaction;
	double local_scale;
	typedef m::true_ HasPost;
	ScoreIntWithPost():local_scale(1.0){}
	static std::string name(){ return "ScoreIntWithPost"; }
	template<class Config>
	Result operator()(Interaction a, Config const& c) const {
		return a*c.scale * local_scale;
	}
	template<class Scene, class Config>
	void post( Scene const &, Result & r, Config const & c ) const {
		r += 1.0;
	}
};
std::ostream & operator<<(std::ostream & out,ScoreIntWithPost const& si){ return out << si.name(); }

TEST( ObjectiveFunction, test_post )
{
	typedef	ObjectiveFunction<
		mpl::list<
			ScoreInt
		>,
		ConfigTest
	> ObjFunNoPost;
	ObjFunNoPost scorenopost;

	typedef	ObjectiveFunction<
		mpl::list<
			ScoreIntWithPost
		>,
		ConfigTest
	> ObjFunWithPost;
	ObjFunWithPost scorewithpost;

	typedef ObjFunNoPost::Results ResultsNoPost;
	typedef ObjFunWithPost::Results ResultsWithPost;
	typedef SimpleInteractionSource< mpl::vector<int,double,std::pair<int,double> > > InteractionSource;
	InteractionSource interaction_source;
	EXPECT_EQ( ResultsNoPost  (0), scorenopost  (interaction_source) );
	EXPECT_EQ( ResultsWithPost(1), scorewithpost(interaction_source) );
	interaction_source.get_interactions<int>().push_back(1);
	EXPECT_EQ( ResultsNoPost  (1), scorenopost  (interaction_source) );
	EXPECT_EQ( ResultsWithPost(2), scorewithpost(interaction_source) );

}

struct ScoreIntWithScratch {
	typedef double Result;
	typedef int Interaction;
	double local_scale;
	typedef m::true_ HasPre;
	typedef m::true_ HasPost;
	typedef int Scratch;
	ScoreIntWithScratch():local_scale(1.0){}
	static std::string name(){ return "ScoreIntWithScratch"; }
	mutable Scratch * addr_of_scratch_should_stay_same;
	template<class Scene, class Config>
	void pre( Scene const &, Result & r, Scratch & s, Config const & c ) const {
		r += 1.0;
		addr_of_scratch_should_stay_same = &s;
	}
	template<class Config>
	Result operator()(Interaction a, Scratch & s, Config const& c ) const {
		if( addr_of_scratch_should_stay_same != &s ){
			std::cerr << "addr_of_scratch_should_stay_same" << std::endl;
			std::exit(-1);
		}
		return a*c.scale * local_scale;
	}
	template<class Scene, class Config>
	void post( Scene const &, Result & r, Scratch & s, Config const & c ) const {
		ASSERT_EQ( addr_of_scratch_should_stay_same, &s ); // verify that same scratch object is being passed
		r += 1.0;
	}
};
std::ostream & operator<<(std::ostream & out,ScoreIntWithScratch const& si){ return out << si.name(); }

TEST( ObjectiveFunction, test_scratch )
{
	typedef	ObjectiveFunction<
		mpl::list<
			ScoreInt
		>,
		ConfigTest
	> ObjFunNoScratch;
	ObjFunNoScratch scorenoscratch;

	typedef	ObjectiveFunction<
		mpl::list<
			ScoreIntWithScratch
		>,
		ConfigTest
	> ObjFunWithScratch;
	ObjFunWithScratch scorewithscratch;

	typedef ObjFunNoScratch::Results ResultsNoScratch;
	typedef ObjFunWithScratch::Results ResultsWithScratch;
	typedef SimpleInteractionSource< mpl::vector<int,double,std::pair<int,double> > > InteractionSource;
	InteractionSource interaction_source;
	EXPECT_EQ( ResultsNoScratch  (0), scorenoscratch  (interaction_source) );
	EXPECT_EQ( ResultsWithScratch(2), scorewithscratch(interaction_source) );
	interaction_source.get_interactions<int>().push_back(1);
	EXPECT_EQ( ResultsNoScratch  (1), scorenoscratch  (interaction_source) );
	EXPECT_EQ( ResultsWithScratch(3), scorewithscratch(interaction_source) );

}

}
}
}


#include "kfeaturelbp.h"
#include "kpicinfo.h"
#include "common.h"

#include "gdal_priv.h"
#include "cpl_conv.h" // for CPLMalloc()

#include <boost/dynamic_bitset.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>

#include <QCoreApplication>
#include <QFile>
#include <QDebug>

#include <map>
#include <utility>
#include <limits>

// test the extend function
//    KFeatureLBP *t= new KFeatureLBP();
//    float a[20]={1,2,3,4,
//             2,3,4,5,
//             3,4,5,6,
//             4,5,6,7,
//             5,6,7,8};
//    float *b = new float(110);
//    t->replicateExtend(a,b,10,11);
//    for(int i=0;i<11;++i)
//    {
//        for(int j=0;j<10;++j)
//        {
//            std::cout<<int(b[10*i+j])<<" ";
//        }
//        std::cout<<"\r\n";
//    }
//    std::cout<<"end"<<std::endl;
//    delete b;
KFeatureLBP::~KFeatureLBP()
{
    CPLFree(m_pHistogram);

    for(int bandIndex = 0;bandIndex<m_bandNum;++bandIndex){
        //qDebug()<<m_extDataBuff[bandIndex];
        CPLFree(m_extDataBuff[bandIndex]);
    }
    CPLFree(m_extDataBuff);
}

KFeatureLBP::KFeatureLBP(GDALDataset *piDataset, GDALDataset *poDataset, int sampleNum, int kernelRadius, bool improved)
    : m_piDataset(piDataset),
      m_poDataset(poDataset),
      m_kernelRadius(kernelRadius),
      m_sampleNum(sampleNum),
      m_beImproved(improved),
      m_fileName(""),
      refTable(NULL)
{

}

bool KFeatureLBP::calLBP(float *inBuff, GByte *outBuff, int width, int height,bool toBeNormarlized, KProgressBar * pProgressBar)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    boost::dynamic_bitset<> tempOne(m_sampleNum,1);
    boost::dynamic_bitset<> tempZero(m_sampleNum,0);

    GUIntBig maxValue = get2Power(m_sampleNum);

    for(int iYPos = m_kernelRadius,iYDes=0;iYPos<height-m_kernelRadius;++iYPos,++iYDes)
    {
        for(int iXPos = m_kernelRadius,iXDes=0;iXPos<width-m_kernelRadius;++iXPos,++iXDes)
        {
            boost::dynamic_bitset<> tempbin(m_sampleNum,0);
            // calculate anticlockwise
            for(int nsCnt=0;nsCnt<m_sampleNum;++nsCnt)
            {
                float fxAxis = iXPos + m_kernelRadius*boost::math::cos_pi(2.*nsCnt/m_sampleNum);
                float fyAxis = iYPos - m_kernelRadius*boost::math::sin_pi(2.*nsCnt/m_sampleNum);
                int ixAxis = static_cast<int>(fxAxis);
                int iyAxis = static_cast<int>(fyAxis);
                fxAxis = fxAxis-ixAxis;
                fyAxis = fyAxis-iyAxis;
                /** bilinear interpolation algorithm
                 * y x-------
                 * |(0,0)
                 * |  |a   b|
                 * |  | x,y | --> gray(x,y)=(1-x)*[(1-y)*a + y*c] + x*[(1-y)*b + y*d]
                 * |  |c   d|
                 *
                 *                  |f(0,0) f(1,0)||1-y|
                 * gray(x,y)=[1-x,x]|             ||   |
                 *                  |f(0,1) f(1,1)|| y |
                 */
                float tempValue = (1-fxAxis)*((1-fyAxis)*inBuff[iyAxis*width+ixAxis]
                        +fyAxis*inBuff[(iyAxis+1)*width+ixAxis])
                        +fxAxis*((1-fyAxis)*inBuff[iyAxis*width+ixAxis+1]
                        +fyAxis*inBuff[(iyAxis+1)*width+ixAxis+1]);

                tempbin <<= 1;
                tempbin |= tempValue>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
            }
            if(NULL != pProgressBar) pProgressBar->autoUpdate();
            if(toBeNormarlized==false)
            {
                if(m_beImproved) outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=refTable[tempbin.to_ulong()];
                else outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=tempbin.to_ulong()*255./maxValue;
            }
            else{
                if(m_beImproved) outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=refTable[tempbin.to_ulong()]*255./(m_sampleNum+2.);
                // here we suppose that the traditional lbp is normarlized anyway
                else outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=tempbin.to_ulong()*255./maxValue;
            }

//            tempbin=tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[iYPos*width+iXPos+1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos-1)*width+iXPos+1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos-1)*width+iXPos]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos-1)*width+iXPos-1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[iYPos*width+iXPos-1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos+1)*width+iXPos-1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos+1)*width+iXPos]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos+1)*width+iXPos+1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=tempbin.to_ulong();
        }
    }

    return true;
}

bool KFeatureLBP::run(Kapok::K_BorderTypes type,bool toBeNormarlized)
{
    int bandNum = KPicInfo::getInstance()->getBandNum();
    int nXSize = KPicInfo::getInstance()->getWidth();
    int nYSize = KPicInfo::getInstance()->getHeight();

    //GDALRasterBand * piBand = NULL;
    GDALRasterBand * poBand = NULL;
    //float *pafData = NULL;
    GByte *pafOutData = NULL;
    int err_code = 1 + bandNum;
    if(externDataSet(type)) err_code -= 1;

    if(m_beImproved){
        // create the reference table
        GUIntBig maxIndex = get2Power(m_sampleNum);
        GUIntBig minValue = 0;
        KProgressBar progressBar("Calculating LBP refTable",maxIndex,80);
        K_PROGRESS_START(progressBar);
        for(GUIntBig index = 0;index < maxIndex;++index)
        {
            // Achieving Rotation Invariance
            minValue = index;
            // leave the template args to be empty means determined by the compiler
            boost::dynamic_bitset<> bin(m_sampleNum, index);
            for(int pos = 0;pos<m_sampleNum;++pos){
                boost::dynamic_bitset<> tempbin=(bin>>pos)|(bin<<m_sampleNum-pos);
                if(minValue>tempbin.to_ulong()){
                    minValue = tempbin.to_ulong();
                }
            }
            // confirm the "Uniform" Patterns
            boost::dynamic_bitset<> temp(m_sampleNum,minValue);
            GByte cycleSum = abs(temp[0] - temp[m_sampleNum - 1]);
            // the default value is set to be the num of bits which are set
            refTable[index] = bin.count();
            for(GByte pos = 0;pos < m_sampleNum - 1;++pos)
            {
                cycleSum += abs(temp[pos]-temp[pos+1]);
                if(cycleSum > 2){
                    refTable[index] = m_sampleNum+1;
                    break;
                }
            }
            progressBar.autoUpdate();
        }
        K_PROGRESS_END(progressBar);
    }

    KProgressBar progressBar("Calculating LBP feature",bandNum*nXSize*nYSize,80);
    K_PROGRESS_START(progressBar);
    // get the LBP feature
    pafOutData = (GByte *) CPLMalloc(sizeof(GByte)*nXSize*nYSize);
    //pafData = (float *) CPLMalloc(sizeof(float)*(nXSize + 2*m_kernelRadius + 1)*(nYSize + 2*m_kernelRadius + 1));
    for(int index = 0; index < bandNum; ++index)
    {
        //piBand = m_piDataset->GetRasterBand(index + 1);
        poBand = m_poDataset->GetRasterBand(index + 1);

//        piBand->RasterIO( GF_Read, 0, 0, nXSize + 2*m_kernelRadius + 1, nYSize + 2*m_kernelRadius + 1
//                          , pafData, nXSize + 2*m_kernelRadius + 1, nYSize + 2*m_kernelRadius + 1
//                          , GDT_Float32, 0, 0 );

        if(calLBP(m_extDataBuff[index],pafOutData,nXSize + 2*m_kernelRadius,nYSize + 2*m_kernelRadius,toBeNormarlized,&progressBar)) err_code -= 1;

        poBand->RasterIO( GF_Write, 0, 0, nXSize , nYSize, pafOutData
                          , nXSize, nYSize, GDT_Byte, 0, 0 );
        poBand->FlushCache();

    }
    K_PROGRESS_END(progressBar);

    GDALClose(m_poDataset);

    std::map<int,long> histMap;
    int maxValue = (std::numeric_limits<int>::min)();

    if(!m_beImproved || toBeNormarlized) maxValue=256;
    else maxValue = m_sampleNum+2;

    for(int index = 0;index<maxValue;++index) histMap[index]=0;
    for(long index = 0;index<nXSize*nYSize;++index){ histMap[pafOutData[index]]++; }
    m_pHistogram = (long *) CPLMalloc(sizeof(long)*maxValue+sizeof(long));
    m_pHistogram[0]=maxValue;
    // key start at 0
    for(std::map<int,long>::iterator it = histMap.begin();it != histMap.end();++it){
        m_pHistogram[it->first+1]=it->second;
    }

    // release
    if(NULL != refTable) CPLFree(refTable);
    //CPLFree(pafData);
    CPLFree(pafOutData);
//    char ** filelist;// =m_piDataset->GetFileList();
//    GDALClose(m_piDataset);

//    // just fetch the first one
//    QString temp(*filelist);
//    CSLDestroy (filelist);
//    QFile file(temp);
//    if(!file.remove()) std::cout<<"KFeatureLBP:remove the temp file failed!"<<std::endl;
//    //qDebug()<<"close";

    return (0 == err_code);
}

GDALDataset *KFeatureLBP::build(QString fileName)
{
    m_fileName = fileName;
    if(KPicInfo::dataAttach(m_piDataset)) KPicInfo::getInstance()->build();
    //qDebug()<<"failed";qDebug()<<KPicInfo::dataAttach(m_piDataset);
    int bandNum = KPicInfo::getInstance()->getBandNum();
    int nXSize = KPicInfo::getInstance()->getWidth();
    int nYSize = KPicInfo::getInstance()->getHeight();

    if(m_sampleNum > 24){
        std::cout<<"KFeatureLBP:Sample points cannot more than 24!"<<std::endl;
        exit( 1 );
    }

    bool beSame = K_CheckDataSetEqu(m_piDataset,m_poDataset);

    if(!beSame){
        QString tempName=QCoreApplication::applicationDirPath()+"/tempImg%%KFeatureLBP";
        //QString tempInputName=QCoreApplication::applicationDirPath()+"/tempExtImg%%KFeatureLBP"+KPicInfo::getInstance()->getFileExtName();
        //QString tempName="D:/tempImg%%KFeatureLBP";
        //QString tempInputName="D:/tempExtImg%%KFeatureLBP"+KPicInfo::getInstance()->getFileExtName();
        if(!m_fileName.isEmpty()){ tempName = m_fileName; }

        const char *pszFormat = m_piDataset->GetDriverName();
        GDALDriver *poDriver = GetGDALDriverManager()->GetDriverByName(pszFormat);
        if( poDriver == NULL )
        {
            std::cout<<"KFeatureLBP:GetGDALDriverManager failed!"<<std::endl;
            exit( 1 );
        }
        if( CSLFetchBoolean( poDriver->GetMetadata(), GDAL_DCAP_CREATE, FALSE ) )
        {
            //qDebug( "KFeatureLBP:Driver %s supports Create() method.", pszFormat );
            m_realExtName = KPicInfo::getInstance()->getFileExtName();
        }
        else
        {
            poDriver = GetGDALDriverManager()->GetDriverByName("BMP");
            m_realExtName = ".bmp";
        }
        tempName += m_realExtName;
        if(NULL != m_poDataset) GDALClose(m_poDataset);
        // allocate the output Dataset
        m_poDataset = poDriver->Create(tempName.toUtf8().data(),nXSize,nYSize
                                       ,bandNum,GDT_Byte,0);
        // store the previous handle
        //m_piOrgDataset = m_piDataset;
        // +1 is to guarantee the success of bilinear interpolation algorithm
        // build the new handle to store the extended image
        //qDebug()<<"failed";qDebug()<<KPicInfo::getInstance()->getType();
        m_extDataBuff = (float **) CPLMalloc(sizeof(float *)*bandNum);
        m_bandNum = bandNum;
        for(int bandIndex = 0;bandIndex<bandNum;++bandIndex){
            m_extDataBuff[bandIndex] = (float *) CPLMalloc(sizeof(float)*(nXSize+2*m_kernelRadius + 1)*(nYSize+2*m_kernelRadius + 1));
        }
//        m_piDataset = poDriver->Create(tempInputName.toUtf8().data()
//                                       ,nXSize+2*m_kernelRadius + 1,nYSize+2*m_kernelRadius + 1
//                                       ,bandNum,KPicInfo::getInstance()->getType(),0);

    }else{
        std::cout<<"KFeatureLBP:the input and output cannot be same!"<<std::endl;
        m_poDataset = NULL;
    }

    assert(NULL != m_poDataset);

    if(NULL != refTable) CPLFree(refTable);
    if(m_beImproved) refTable = (unsigned char *) CPLMalloc(sizeof(unsigned char)*get2Power(m_sampleNum));
    else refTable=NULL;

    return m_poDataset;
}

bool KFeatureLBP::constExtend(float * inBuff, float * outBuff, int width, int height, float defaultValue)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    // fill the outbuff with the default value
    for(GIntBig index = 0;index < (width + 1) * (height + 1);++index)
    {
        outBuff[index] = defaultValue;
    }
    // copy the source to the outbuff
    for(int iYDes = m_kernelRadius,iYSrc = 0;iYDes < height - m_kernelRadius;++iYDes,++iYSrc)
    {
        for(int iXDes = m_kernelRadius,iXSrc = 0;iXDes < width - m_kernelRadius;++iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = inBuff[iYSrc*(width-2*m_kernelRadius)+iXSrc];
        }
    }
    return true;
}

bool KFeatureLBP::reflectExtend(float * inBuff, float * outBuff, int width, int height)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    if(width < 3 * m_kernelRadius || height < 3 * m_kernelRadius)
    {
        std::cout<<"KFeatureLBP:each line of the image cannot be shorter than KernelSize!"<<std::endl;
        exit( 1 );
    }
    // copy the source to the outbuff
    for(int iYDes = m_kernelRadius,iYSrc = 0;iYDes < height - m_kernelRadius;++iYDes,++iYSrc)
    {
        for(int iXDes = m_kernelRadius,iXSrc = 0;iXDes < width - m_kernelRadius;++iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = inBuff[iYSrc*(width-2*m_kernelRadius)+iXSrc];
        }
    }

    // fill horizontal
    for(int iYDes = m_kernelRadius;iYDes < height - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius+1;
        for(;iXDes >= 0 && iXSrc < width - m_kernelRadius;--iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc-1];
        }
        // fill the end of each line
        iXDes = width - m_kernelRadius,iXSrc = width - m_kernelRadius - 2;
        for(;iXDes < width + 1&& iXSrc >= m_kernelRadius;++iXDes,--iXSrc)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc+1];
        }
    }

    // fill vertical -- think rotate 90 degrees
    for(int iYDes = m_kernelRadius;iYDes < width - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius+1;
        for(;iXDes >= 0 && iXSrc < height - m_kernelRadius;--iXDes,++iXSrc)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[(iXSrc-1)*width+iYDes];
        }
        // fill the end of each line
        iXDes = height - m_kernelRadius,iXSrc = height - m_kernelRadius - 2;
        for(;iXDes < height + 1 && iXSrc >= m_kernelRadius;++iXDes,--iXSrc)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes < height + 1;++iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[(iXSrc+1)*width+iYDes];
        }
    }

    // fill four corners, please find the axis of symmetry carefully
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*m_kernelRadius-iYDes)*width+iXDes]+outBuff[iYDes*width+2*m_kernelRadius-iXDes])/2;
        }
    }
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = width-m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*m_kernelRadius-iYDes)*width+iXDes]+outBuff[iYDes*width+2*(width-m_kernelRadius-1)-iXDes])/2;
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*(height-m_kernelRadius-1)-iYDes)*width+iXDes]+outBuff[iYDes*width+2*m_kernelRadius-iXDes])/2;
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = width - m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*(height-m_kernelRadius-1)-iYDes)*width+iXDes]+outBuff[iYDes*width+2*(width-m_kernelRadius-1)-iXDes])/2;
        }
    }
    return true;
}

bool KFeatureLBP::replicateExtend(float * inBuff, float * outBuff, int width, int height)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    if(width < 3 * m_kernelRadius || height < 3 * m_kernelRadius)
    {
        std::cout<<"KFeatureLBP:each line of the image cannot be shorter than KernelSize!"<<std::endl;
        exit( 1 );
    }
    // copy the source to the outbuff
    for(int iYDes = m_kernelRadius,iYSrc = 0;iYDes < height - m_kernelRadius;++iYDes,++iYSrc)
    {
        for(int iXDes = m_kernelRadius,iXSrc = 0;iXDes < width - m_kernelRadius;++iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = inBuff[iYSrc*(width-2*m_kernelRadius)+iXSrc];
        }
    }

    // fill horizontal
    for(int iYDes = m_kernelRadius;iYDes < height - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius;
        for(;iXDes >= 0 && iXSrc < width - m_kernelRadius;--iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        // fill the end of each line
        iXDes = width - m_kernelRadius,iXSrc = width - m_kernelRadius - 1;
        for(;iXDes < width + 1 && iXSrc >= m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
    }

    // fill vertical -- think rotate 90 degrees
    for(int iYDes = m_kernelRadius;iYDes < width - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius;
        for(;iXDes >= 0 && iXSrc < height - m_kernelRadius;--iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        // fill the end of each line
        iXDes = height - m_kernelRadius,iXSrc = height - m_kernelRadius - 1;
        for(;iXDes < height + 1 && iXSrc >= m_kernelRadius;++iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes < height + 1;++iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
    }

    // fill four corners, please find the axis of symmetry carefully
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[m_kernelRadius*width+m_kernelRadius];
        }
    }
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = width-m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[m_kernelRadius*width+width-m_kernelRadius-1];
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[(height-m_kernelRadius-1)*width+m_kernelRadius];
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = width - m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[(height-m_kernelRadius-1)*width+width-m_kernelRadius-1];
        }
    }
    return true;
}

bool KFeatureLBP::externDataSet(Kapok::K_BorderTypes type,float defaultValue)
{
    int bandNum = KPicInfo::getInstance()->getBandNum();
    int nXSize = KPicInfo::getInstance()->getWidth();
    int nYSize = KPicInfo::getInstance()->getHeight();
    unsigned int err = bandNum;
    GDALRasterBand * piBand = NULL;
    //GDALRasterBand * poBand = NULL;
    float *pafData = NULL;
    //float *pafOutData = NULL;
    pafData = (float *) CPLMalloc(sizeof(float)*nXSize*nYSize);
    //pafOutData = (float *) CPLMalloc(sizeof(float)*(nXSize + 2*m_kernelRadius + 1)*(nYSize + 2*m_kernelRadius + 1));
    for(int index = 0; index < bandNum; ++index)
    {
        piBand = m_piDataset->GetRasterBand(index + 1);
        //poBand = m_piDataset->GetRasterBand(index + 1);

        piBand->RasterIO( GF_Read, 0, 0, nXSize, nYSize, pafData, nXSize, nYSize, GDT_Float32, 0, 0 );
        switch(type)
        {
        case Kapok::Border_Constant:
            err -= constExtend(pafData, m_extDataBuff[index], nXSize + 2*m_kernelRadius, nYSize + 2*m_kernelRadius, defaultValue);
            break;
        case Kapok::Border_Replicate:
            err -= replicateExtend(pafData, m_extDataBuff[index], nXSize + 2*m_kernelRadius, nYSize + 2*m_kernelRadius);
            break;
        case Kapok::Border_Reflect:
        default:
            err -= reflectExtend(pafData, m_extDataBuff[index], nXSize + 2*m_kernelRadius, nYSize + 2*m_kernelRadius);
            break;
        }

//        poBand->RasterIO( GF_Write, 0, 0, nXSize + 2*m_kernelRadius + 1
//                          , nYSize + 2*m_kernelRadius + 1, pafOutData
//                          , nXSize + 2*m_kernelRadius + 1, nYSize + 2*m_kernelRadius + 1, GDT_Float32, 0, 0 );
//        poBand->FlushCache();
    }
    CPLFree(pafData);
    //CPLFree(pafOutData);

    return !err;
}

QString KFeatureLBP::getSVMString(int start)
{
    int size = m_pHistogram[0];
    QString temp("");
    if(NULL==m_pHistogram) exit(1);
//    for(int index= start;index<start+size;++index){
//        temp+=QString("%1:%%2 ").arg(index).arg(index-start+1);
//    }
    for(int index = 1;index<size+1;++index){
        temp+=QString("%1:%2 ").arg(index-1+start).arg(m_pHistogram[index]);
        //temp=QString(temp).arg(m_pHistogram[index]);
        //qDebug()<<temp;
    }
    return temp;
}


#include "dofs.hpp"
#include "numpy.hpp"

#include <core/dofs.hpp>
#include <core/small_vector.hpp>

#include <boost/python.hpp>

namespace python {

  template<class T>
  static npy_intp size() {
    static_assert( std::is_same<real, scalar< deriv<T> > >::value, "need real scalar type");
    return sizeof(T) / sizeof(real);
  }

  template<class T>
  static boost::python::object pos(::dofs< T >* self) {
	real* data = reinterpret_cast<real*>(self->pos.begin());
	numpy::shape_type shape = {(long)self->size(), size<T>()  };
	return numpy::ndarray(data, shape);
  }

  template<class T>
  static boost::python::object vel(::dofs< T >* self) {
	real* data = reinterpret_cast<real*>(self->vel.begin());
	numpy::shape_type shape = {(long)self->size(), size<deriv<T>>()};
	return numpy::ndarray(data, shape);
  }

  template<class T>
  static boost::python::object mom(::dofs< T >* self) {
	real* data = reinterpret_cast<real*>(self->mom.begin());
	numpy::shape_type shape = {(long)self->size(), size<deriv<T>>()};
	return numpy::ndarray(data, shape);
  }

  template<class T, boost::python::object (*get)(::dofs< T >*) >
  static void set(::dofs<T>* self, boost::python::object obj) {
	get(self)[ boost::python::slice() ] = obj;
  }

  namespace impl {
    template<class U>
    static void bind_dofs(const std::string& tname) {
      using namespace boost::python;

      // static copy
      static std::string name = tname;

      // std::clog << name << std::endl;

      // TODO namespace
      using ::dofs;

      class_<dofs<U>, std::shared_ptr<dofs<U>>,
             bases<dofs_base>, boost::noncopyable >(name.c_str(), no_init)
        .add_property("pos", pos<U>, set<U, pos<U> >)
        .add_property("vel", vel<U>, set<U, vel<U> >)
        .def("size", &dofs<U>::size)

        ;

      // // warning: replace class name
      // class_<static_dofs<U>, std::shared_ptr<static_dofs<U>>,
      //        bases<dofs<U>>, boost::noncopyable >_(name.c_str()) // most vexing parse
      //   ;

      // warning: replace class name
      class_<dynamic_dofs<U>, std::shared_ptr<dynamic_dofs<U>>,
             bases<dofs<U>>, boost::noncopyable >(name.c_str() )
        .def("resize", &dynamic_dofs<U>::resize)
        ;

    }

  }

  template<class ... U>
  static void bind_dofs() {
    std::string prefix = "dofs_";
    const int expand[] = { (impl::bind_dofs<U>(prefix + traits<U>::name()), 0)... };
    (void) expand;
  }

  void dofs::module() {
	// TODO automatic instantitations
	// dofs

	using namespace boost::python;
	using ::dofs;
	class_<dofs_base, std::shared_ptr<dofs_base>, boost::noncopyable >("dofs_base", no_init);

    bind_dofs<real, vec3, rigid>();

  }

}


//---------------------------------------------------------------------------//
// Copyright (c) 2019 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// 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 CRYPTO3_STREAM_SALSA20_POLICY_HPP
#define CRYPTO3_STREAM_SALSA20_POLICY_HPP

#include <boost/endian/conversion.hpp>

#include <nil/crypto3/detail/inline_variable.hpp>

#include <nil/crypto3/stream/detail/basic_functions.hpp>

#define SALSA20_QUARTER_ROUND(x1, x2, x3, x4) \
    do {                                      \
        x2 ^= policy_type::rotl<7>(x1 + x4);  \
        x3 ^= policy_type::rotl<9>(x2 + x1);  \
        x4 ^= policy_type::rotl<13>(x3 + x2); \
        x1 ^= policy_type::rotl<18>(x4 + x3); \
    } while (0)

namespace nil {
    namespace crypto3 {
        namespace stream {
            namespace detail {
                template<std::size_t IVBits, std::size_t KeyBits, std::size_t Rounds>
                struct salsa20_policy : public basic_functions<32> {
                    typedef basic_functions<32> policy_type;

                    typedef typename policy_type::byte_type byte_type;
                    typedef typename policy_type::word_type word_type;

                    constexpr static const std::size_t rounds = Rounds;
                    BOOST_STATIC_ASSERT(Rounds % 2 == 0);

                    constexpr static const std::size_t value_bits = CHAR_BIT;
                    typedef byte_type value_type;

                    constexpr static const std::size_t block_size = 64;
                    constexpr static const std::size_t block_bits = block_size * value_bits;
                    typedef std::array<byte_type, block_size> block_type;

                    constexpr static const std::size_t min_key_bits = 16 * CHAR_BIT;
                    constexpr static const std::size_t max_key_bits = 32 * CHAR_BIT;
                    constexpr static const std::size_t key_bits = KeyBits;
                    constexpr static const std::size_t key_size = key_bits / CHAR_BIT;
                    BOOST_STATIC_ASSERT(min_key_bits <= KeyBits <= max_key_bits);
                    BOOST_STATIC_ASSERT(key_size % 16 == 0);
                    typedef std::array<byte_type, key_size> key_type;

                    constexpr static const std::size_t key_schedule_size = 16;
                    constexpr static const std::size_t key_schedule_bits = key_schedule_size * word_bits;
                    typedef std::array<word_type, key_schedule_size> key_schedule_type;

                    constexpr static const std::size_t round_constants_size = 4;
                    typedef std::array<word_type, round_constants_size> round_constants_type;

                    CRYPTO3_INLINE_VARIABLE(round_constants_type, tau,
                                            ({0x61707865, 0x3120646e, 0x79622d36, 0x6b206574}));
                    CRYPTO3_INLINE_VARIABLE(round_constants_type, sigma,
                                            ({0x61707865, 0x3320646e, 0x79622d32, 0x6b206574}));

                    constexpr static const std::size_t iv_bits = IVBits;
                    constexpr static const std::size_t iv_size = IVBits / CHAR_BIT;
                    typedef std::array<byte_type, iv_size> iv_type;

                    static void hsalsa20(word_type output[8], const key_schedule_type input) {
                        word_type x00 = input[0], x01 = input[1], x02 = input[2], x03 = input[3], x04 = input[4],
                                  x05 = input[5], x06 = input[6], x07 = input[7], x08 = input[8], x09 = input[9],
                                  x10 = input[10], x11 = input[11], x12 = input[12], x13 = input[13], x14 = input[14],
                                  x15 = input[15];

                        for (size_t i = 0; i != rounds / 2; ++i) {
                            SALSA20_QUARTER_ROUND(x00, x04, x08, x12);
                            SALSA20_QUARTER_ROUND(x05, x09, x13, x01);
                            SALSA20_QUARTER_ROUND(x10, x14, x02, x06);
                            SALSA20_QUARTER_ROUND(x15, x03, x07, x11);

                            SALSA20_QUARTER_ROUND(x00, x01, x02, x03);
                            SALSA20_QUARTER_ROUND(x05, x06, x07, x04);
                            SALSA20_QUARTER_ROUND(x10, x11, x08, x09);
                            SALSA20_QUARTER_ROUND(x15, x12, x13, x14);
                        }

                        output[0] = x00;
                        output[1] = x05;
                        output[2] = x10;
                        output[3] = x15;
                        output[4] = x06;
                        output[5] = x07;
                        output[6] = x08;
                        output[7] = x09;
                    }

                    static void salsa_core(block_type &block, const key_schedule_type &input) {
                        word_type x00 = input[0], x01 = input[1], x02 = input[2], x03 = input[3], x04 = input[4],
                                  x05 = input[5], x06 = input[6], x07 = input[7], x08 = input[8], x09 = input[9],
                                  x10 = input[10], x11 = input[11], x12 = input[12], x13 = input[13], x14 = input[14],
                                  x15 = input[15];

                        for (size_t i = 0; i != rounds / 2; ++i) {
                            SALSA20_QUARTER_ROUND(x00, x04, x08, x12);
                            SALSA20_QUARTER_ROUND(x05, x09, x13, x01);
                            SALSA20_QUARTER_ROUND(x10, x14, x02, x06);
                            SALSA20_QUARTER_ROUND(x15, x03, x07, x11);

                            SALSA20_QUARTER_ROUND(x00, x01, x02, x03);
                            SALSA20_QUARTER_ROUND(x05, x06, x07, x04);
                            SALSA20_QUARTER_ROUND(x10, x11, x08, x09);
                            SALSA20_QUARTER_ROUND(x15, x12, x13, x14);
                        }

                        boost::endian::store_little_u32(x00 + input[0], block[4 * 0]);
                        boost::endian::store_little_u32(x01 + input[1], block[4 * 1]);
                        boost::endian::store_little_u32(x02 + input[2], block[4 * 2]);
                        boost::endian::store_little_u32(x03 + input[3], block[4 * 3]);
                        boost::endian::store_little_u32(x04 + input[4], block[4 * 4]);
                        boost::endian::store_little_u32(x05 + input[5], block[4 * 5]);
                        boost::endian::store_little_u32(x06 + input[6], block[4 * 6]);
                        boost::endian::store_little_u32(x07 + input[7], block[4 * 7]);
                        boost::endian::store_little_u32(x08 + input[8], block[4 * 8]);
                        boost::endian::store_little_u32(x09 + input[9], block[4 * 9]);
                        boost::endian::store_little_u32(x10 + input[10], block[4 * 10]);
                        boost::endian::store_little_u32(x11 + input[11], block[4 * 11]);
                        boost::endian::store_little_u32(x12 + input[12], block[4 * 12]);
                        boost::endian::store_little_u32(x13 + input[13], block[4 * 13]);
                        boost::endian::store_little_u32(x14 + input[14], block[4 * 14]);
                        boost::endian::store_little_u32(x15 + input[15], block[4 * 15]);
                    }
                };
            }    // namespace detail
        }        // namespace stream
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_SALSA20_POLICY_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_MNORMEST_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_MNORMEST_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>

/*!
 * \ingroup algebra
 * \defgroup algebra_mnormest mnormest
 *
 * \par Description
 * norm estimate
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/mnormest.hpp>
 * \endcode
 *
 * normest name is here for Matlab naming compatibility
 * but mnormest is an estimate of mnorm (the 2-norm of the matrix).
 *
 * This function may be useful for large, full matrices.
 *
 * n = normest(a) returns an estimate of the 2-norm of the matrix S.
 *
 * n = normest(a,tol) uses relative error tol instead of the default tolerance 1.e-6.
 * The value of tol determines when the estimate is considered acceptable.
 *
 * normest(..., cnt) returns an estimate of the 2-norm and also gives
 *  in cnt the number of power iterations used.
 *
 * The tol default is sqrt(eps) for the current type used.
 **/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag mnormest_ of functor mnormest
     *        in namespace nt2::tag for toolbox algebra
     **/
    struct mnormest_ :  ext::abstract_<mnormest_> { typedef ext::abstract_<mnormest_> parent;   template<class... Args>   static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)   BOOST_AUTO_DECLTYPE_BODY( dispatching_mnormest_( ext::adl_helper(), static_cast<Args&&>(args)... ) ) };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::mnormest_, Site> dispatching_mnormest_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::mnormest_, Site>();
    }
    template<class... Args>
    struct impl_mnormest_;
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mnormest_, mnormest, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mnormest_, mnormest, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL(tag::mnormest_, mnormest,(A0 const&)(A1&),2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL(tag::mnormest_, mnormest,(A0 const&)(A1 const&)(A2&),3)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mnormest_, normest, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mnormest_, normest, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL(tag::mnormest_, normest,(A0 const&)(A1&),2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL(tag::mnormest_, normest,(A0 const&)(A1 const&)(A2&),3)

}

#endif


#ifndef VI_CONTROLLER_HPP
#define VI_CONTROLLER_HPP

// Component
#include "VehicleInterfaceConfig.hpp"
#include "VehicleInterfaceData.hpp"

// Libraries
#include <boost/cstdfloat.hpp>

// Ros
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>

// Standard
#include <cstdint>
#include <memory>
#include <string>

namespace vi
{

// Forward Declares
class PIDController;

using float64_t = boost::float64_t; ///< Alias for 64 bit float

/// @brief Class to store config values for vehicle interface
class Controller
{
public:
   /// @brief Default constructor
   Controller(const std::shared_ptr<VehicleInterfaceConfig>& cfg);

   /// @brief Destructor for forward declares
   ~Controller();

   /// @brief Main driving function
   /// @param now_s Current program time
   void update(const ros::Time& now_s);

   /// @brief Accessor for command
   /// @return Command
   const geometry_msgs::Twist& getCommand() const noexcept {return m_cmd;}

   /// @brief Mutator for current state
   /// @param val Val
   void setCurrentState(const geometry_msgs::Twist& val) noexcept {m_current_state = val;}

   /// @brief Mutator for command setpoint
   /// @param val Val
   void setCommandSetpoint(const geometry_msgs::Twist& val) noexcept {m_setpoint_cmd = val;}

private:
    std::unique_ptr<PIDController> m_linear_x_pid;  ///< PID Controller on linear x
    std::unique_ptr<PIDController> m_linear_y_pid;  ///< PID Controller on linear y
    std::unique_ptr<PIDController> m_angular_z_pid; ///< PID Controller on angular z

    geometry_msgs::Twist m_setpoint_cmd;  ///< Setpoint
    geometry_msgs::Twist m_current_state; ///< Current state;
    geometry_msgs::Twist m_cmd;           ///< Output command

    std::shared_ptr<VehicleInterfaceConfig> m_cfg; ///< VI Config
};

} // namespace vi

#endif // VI_CONTROLLER_HPP


//
// Created by jobic on 09/06/2021.
//

#include "cell.h"
#include <Eigen/Dense>

using namespace Eigen;

cell::cell() {
  border=-1;
  solide=-1;
  sdfMeanValue=-123456;
  ratioInsideOverTotal=-1;
}

cell::~cell() {
  border=-2;
  solide=-2;
  sdfMeanValue=-123456;
  ratioInsideOverTotal=-1;
}

void cell::setDims(std::vector<double> &another) {
  assert(another.size()==6);
  this->dims=another;
}

double cell::getDim(unsigned int i) {
  assert(i>0 && i<=6);
  return this->dims[i];
}

std::vector<double> cell::getDims() {
  return this->dims;
}

void cell::printDims() {
  assert(dims.size()==6);
  std::copy(std::begin(dims), std::end(dims), std::ostream_iterator<double>(std::cout, " "));
  std::cout << std::endl;
}

void cell::initSdfMeanValue(std::unique_ptr<Discregrid::DiscreteGrid> &sdf, unsigned int fields_id, std::vector<int> &samples) {
  int     nbInside;
  double  xwidths,ywidths,zwidths,dist,moyDist;
  bool initn=false,initp=false;
  auto evluatedPoint = Vector3d{};

  assert(samples.size()==3);
  assert(dims.size()==6);

  xwidths = (dims[XMAX]-dims[XMIN])/(samples[0]-1);
  ywidths = (dims[YMAX]-dims[YMIN])/(samples[1]-1);
  zwidths = (dims[ZMAX]-dims[ZMIN])/(samples[2]-1);

  nbInside=0;
  for(unsigned int i=0; i<samples[0]; ++i) {
    for(unsigned int j=0; j<samples[1]; ++j) {
      for(unsigned int k=0; k<samples[2]; ++k) {
        evluatedPoint(0) = dims[XMIN] + i * xwidths;
        evluatedPoint(1) = dims[YMIN] + j * ywidths;
        evluatedPoint(2) = dims[ZMIN] + k * zwidths;
        dist = sdf->interpolate(fields_id, evluatedPoint);
        if (dist == std::numeric_limits<double>::max())  {
          dist = 0.0;
        }
        if (!initp && dist > 0) {
          initp=true;
        }
        if (!initn && dist < 0) {
          initn=true;
        }
        moyDist += dist;
        if (dist>0) nbInside++;
      }
    }
  }
  unsigned int totalSamples = samples[0]*samples[1]*samples[2];
  sdfMeanValue = moyDist/totalSamples;
  //SDF is not very accurate for openBoundary domain. We know it's quite good
  //for closed one, which is the inside. Thus, computing the ratio of inside samples over
  //the total gives a better indicator of Solide/Fluide voxel.
  ratioInsideOverTotal = nbInside/(double)totalSamples;
  if (ratioInsideOverTotal>0.5) solide=1;
  else solide=0;
//  if (moyDist>0) solide=1;
//  else solide=0;
  if (initp && initn) border=1;
  else border=0;
  if (0 && border) {
    std::cout << "Cell border" << std::endl;
    printDims();
    std::cout << "nbInside " << nbInside << " ratioInsideOverTotal " << ratioInsideOverTotal << std::endl;
  }
}


#include <pcl/common/angles.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <sensor_msgs/PointCloud2.h>
#include <eigen_conversions/eigen_msg.h>
#include <pcl/common/transforms.h>
#include <pcl/common/time.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/random_sample.h>
#include <nav_msgs/Odometry.h>
#include <pcl/search/kdtree.h>
#include <Eigen/Core>
#include <cmath>
#include <math.h>
#include <tf/transform_datatypes.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/filter.h>

typedef pcl::PointNormal PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

PointCloudT::Ptr last_scan;
PointCloudT::Ptr built_map;
PointCloudT::Ptr full_map;

ros::Publisher odom_pub, built_map_pub, debug_pc_pub;
ros::Time stamp;

Eigen::Affine3d base_to_odom;
Eigen::Affine3d world_to_odom;
pcl::VoxelGrid<PointT> grid;
const float leaf = 0.005f;

tf::TransformListener* tf_listener;

int global_iters;

void icp_slam_iter(pcl::PointCloud<PointT>::Ptr &curr_scan);

void icp_global_map();

void pointcloud_callback(const sensor_msgs::PointCloud2ConstPtr &input_cloud){
  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
  pcl::RandomSample<PointT> rand_filt;
  tf::StampedTransform vel_to_base;
  pcl::NormalEstimationOMP<PointT, PointT> nest;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

  stamp = input_cloud->header.stamp;

  try {
    tf_listener->waitForTransform("/base_link",
                                 (*input_cloud).header.frame_id.c_str(),
                                 (*input_cloud).header.stamp, ros::Duration(1.0));
    tf_listener->lookupTransform("/base_link",
                                (*input_cloud).header.frame_id.c_str(),
                                (*input_cloud).header.stamp, vel_to_base);
  } catch (...) {
    return;
  }

  pcl::PCLPointCloud2::Ptr temp_cloud(new pcl::PCLPointCloud2);
  pcl::PointCloud<PointT>::Ptr cloudtemp(new pcl::PointCloud<PointT>);
  pcl_conversions::toPCL(*input_cloud, *temp_cloud);
  pcl::fromPCLPointCloud2(*temp_cloud, *cloudtemp);
  pcl_ros::transformPointCloud(*cloudtemp, *cloud, vel_to_base);
  cloud->header.frame_id = "base_link";

  // Uniformly randomly subsample pointcloud to have every 20 points
  rand_filt.setSample(input_cloud->width*input_cloud->height/10);
  rand_filt.filter(*cloud);

  nest.setSearchMethod(tree);
  nest.setKSearch(5);
  nest.setInputCloud(cloud);
  nest.compute(*cloud);

  if(last_scan->size() == 0){
    *last_scan = *cloud;
  }else{
    icp_slam_iter(cloud);
  }
}

void icp_slam_iter(pcl::PointCloud<PointT>::Ptr &curr_scan){
  sensor_msgs::PointCloud2 pub_cloud;
  pcl::toROSMsg(*last_scan, pub_cloud);
  pub_cloud.header.frame_id = "base_link";
  debug_pc_pub.publish(pub_cloud);

  pcl::IterativeClosestPointWithNormals<PointT, PointT> icp;
  pcl::PointCloud<PointT> transf_cloud;
  icp.setMaximumIterations(40);
  icp.setMaxCorrespondenceDistance(0.05);
  //ROS_INFO("0");
  icp.setInputSource(last_scan);
  icp.setInputTarget(curr_scan);
  //ROS_INFO("1");
  icp.align(transf_cloud);

  if(icp.hasConverged()){
    *last_scan = *curr_scan;
    //ROS_INFO("2");
    global_iters++;
    Eigen::Matrix4d relative_tf_m = icp.getFinalTransformation().cast<double>();
    Eigen::Affine3d relative_tf;
    relative_tf.matrix() = relative_tf_m.inverse();
    base_to_odom = relative_tf* base_to_odom;
    pcl::transformPointCloud<PointT>(*curr_scan, transf_cloud, base_to_odom.cast<float>());
    *built_map += transf_cloud;
    grid.setLeafSize(leaf,leaf,leaf);
    grid.setInputCloud(built_map);
    grid.filter(*built_map);
    //ROS_INFO("3");

    sensor_msgs::PointCloud2 pub_cloud;
    pcl::toROSMsg(*built_map, pub_cloud);
    pub_cloud.header.frame_id = "map";
    built_map_pub.publish(pub_cloud);
    //ROS_INFO("4");

    nav_msgs::Odometry odom;
    odom.header.frame_id = "map";
    tf::Pose tf_pose;
    tf::poseEigenToTF(base_to_odom, tf_pose);
    //ROS_INFO_STREAM("base to odom\n" << base_to_odom.matrix());
    tf::poseTFToMsg(tf_pose, odom.pose.pose);
    odom_pub.publish(odom);
    //ROS_INFO("5");

    static tf::TransformBroadcaster br;
    tf::Transform tf_odom;
    tf::transformEigenToTF(base_to_odom, tf_odom);
    br.sendTransform(tf::StampedTransform(tf_odom, ros::Time::now(), "map", "base_link"));
    //ROS_INFO("Sent transform for odom to base_link");
    if(global_iters>=10){
      //icp_global_map();
      global_iters=0;
    }

  }

}

void icp_global_map(){
  //ROS_INFO("Got here too");
  pcl::PointCloud<PointT>::Ptr map_down(new PointCloudT);
  PointCloudT::Ptr built_map_down(new PointCloudT);
  *built_map_down = *built_map;
  *map_down = *full_map;
  pcl::RandomSample<PointT> rand_filt;
  rand_filt.setSample(built_map->size()/4);
  rand_filt.filter(*built_map_down);
  rand_filt.setSample(full_map->size()/10);
  rand_filt.filter(*map_down);
  /*grid.setLeafSize(leaf*10,leaf*10,leaf*10);
  grid.setInputCloud(built_map);
  grid.filter(*built_map_down);
  grid.setInputCloud(full_map);
  grid.filter(*map_down);*/
  //ROS_INFO("6");
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*built_map_down, *built_map_down, indices);
  pcl::removeNaNNormalsFromPointCloud(*built_map_down,*built_map_down, indices);
  pcl::removeNaNFromPointCloud(*map_down, *map_down, indices);
  pcl::removeNaNNormalsFromPointCloud(*map_down,*map_down, indices);

  pcl::IterativeClosestPointWithNormals<PointT, PointT> icp;
  pcl::PointCloud<PointT> transf_cloud;
  icp.setMaximumIterations(10);
  icp.setMaxCorrespondenceDistance(0.05);
  icp.setInputTarget(map_down);
  icp.setInputSource(built_map_down);
  //ROS_INFO("7");
  icp.align(transf_cloud, world_to_odom.matrix().cast<float>());
  //ROS_INFO("8");

  if(icp.hasConverged()){
    //ROS_INFO("9");
    static tf::TransformBroadcaster br2;
    world_to_odom.matrix() = icp.getFinalTransformation().cast<double>().inverse();
    tf::Transform tf_odom;
    tf::transformEigenToTF(world_to_odom, tf_odom);
    br2.sendTransform(tf::StampedTransform(tf_odom, ros::Time::now(), "map", "odom"));
    //ROS_INFO("10");
  }
}

int main(int argc, char **argv){
  ros::init(argc, argv, "icp_slam");
  ros::NodeHandle nh("~");
  std::string pc_topic, template_path;
  base_to_odom.matrix().setIdentity();
  world_to_odom.matrix().setIdentity();

  tf_listener = new tf::TransformListener();

  if (!(nh.getParam("pc_topic", pc_topic))) {
    ROS_ERROR("pc_topic failed to get param");
  }

  if (!(nh.getParam("template", template_path))) {
    ROS_ERROR("Didn't get path for full object mesh");
  }

  full_map = boost::make_shared<PointCloudT>();
  built_map = boost::make_shared<PointCloudT>();
  last_scan = boost::make_shared<PointCloudT>();
  if (pcl::io::loadPCDFile<PointT>(template_path, *full_map) == -1) {
     ROS_ERROR_STREAM("Couldn't read full pcd file ");
     return -1;
  }
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*full_map, *full_map, indices);
  pcl::removeNaNNormalsFromPointCloud(*full_map,*full_map, indices);
  pcl::NormalEstimationOMP<PointT, PointT> nest;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
  nest.setSearchMethod(tree);
  nest.setKSearch(15);
  nest.setInputCloud(full_map);
  nest.compute(*full_map);
  pcl::removeNaNFromPointCloud(*full_map, *full_map, indices);
  pcl::removeNaNNormalsFromPointCloud(*full_map,*full_map, indices);
  full_map->is_dense=false;

  built_map_pub = nh.advertise<sensor_msgs::PointCloud2>("built_map", 1);
  odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 1);
  debug_pc_pub = nh.advertise<sensor_msgs::PointCloud2>("debug_cloud", 1);

  ros::Subscriber pc_sub = nh.subscribe(pc_topic.c_str(), 1, pointcloud_callback);

  ros::spin();
}


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 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.
 *
 * 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.
 *
 * ****************************************************************************
 * N-Sphere.
 * ****************************************************************************
 */

#ifndef VISIONCORE_TYPES_HYPERSPHERE_HPP
#define VISIONCORE_TYPES_HYPERSPHERE_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Dense>

// https://en.wikipedia.org/wiki/N-sphere

namespace vc
{

namespace types
{

template<typename _Scalar, int _Dimension = 0> class Hypersphere;

template <typename T> using CircleT = Hypersphere<T,1>;
template <typename T> using SphereT = Hypersphere<T,2>;

namespace internal
{

template<typename T, int Dimension>
struct helper_surface_area;

template<typename T, int Dimension>
struct helper_volume
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius)
    {
        return (radius / T(Dimension)) * helper_surface_area<T,Dimension-1>::calc(radius);
    }
};

template<typename T, int Dimension>
struct helper_surface_area
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius)
    {
        return T(2.0 * M_PI) * radius * helper_volume<T,Dimension-1>::calc(radius);
    }
};

template<typename T>
struct helper_surface_area<T,0>
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius) { return T(2.0); }
};

template<typename T>
struct helper_volume<T,0>
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius) { return T(1.0); }
};

}

}

}

namespace Eigen
{
    namespace internal
    {
        template<typename _Scalar, int _Dimension>
        struct traits<vc::types::Hypersphere<_Scalar,_Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Matrix<Scalar,_Dimension+1,1> CoeffType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
            : traits<vc::types::Hypersphere<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<Matrix<Scalar,_Dimension+1,1>, _Options> CoeffType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
            : traits<const vc::types::Hypersphere<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<const Matrix<Scalar,_Dimension+1,1>, _Options> CoeffType;
        };

    }
}

namespace vc
{

namespace types
{

template<typename Derived>
class HypersphereBase
{
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::CoeffType CoeffType;
    typedef Eigen::Matrix<Scalar,Dimension+1,1> VectorT;

    static inline constexpr int dimension() { return Dimension; }

    template<typename NewScalarType>
    EIGEN_DEVICE_FUNC inline Hypersphere<NewScalarType,Dimension> cast() const
    {
        return Hypersphere<NewScalarType,Dimension>(coeff().template cast<NewScalarType>(),
                                                    (NewScalarType)radius());
    }

    EIGEN_DEVICE_FUNC CoeffType& coeff()
    {
        return static_cast<Derived*>(this)->coeff_nonconst();
    }

    EIGEN_DEVICE_FUNC const CoeffType& coeff() const
    {
        return static_cast<const Derived*>(this)->coeff_const();
    }

    EIGEN_DEVICE_FUNC Scalar& radius()
    {
        return static_cast<Derived*>(this)->radius_nonconst();
    }

    EIGEN_DEVICE_FUNC const Scalar& radius() const
    {
        return static_cast<const Derived*>(this)->radius_const();
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline HypersphereBase<Derived>& operator=(const HypersphereBase<OtherDerived>& other)
    {
        coeff() = other.coeff();
        return *this;
    }

    EIGEN_DEVICE_FUNC inline Scalar volume()
    {
        using Eigen::numext::pow;
        return (pow(Scalar(M_PI), Scalar(Dimension+1)/Scalar(2.0)) / (tgamma(Scalar(Dimension+1)/Scalar(2.0) + Scalar(1.0)))) * pow(radius(), (Scalar)(Dimension+1) );
    }

    template<int OtherDimension>
    EIGEN_DEVICE_FUNC inline Scalar volume()
    {
        return internal::helper_volume<Scalar,OtherDimension+1>::calc(radius());
    }

    EIGEN_DEVICE_FUNC inline Scalar surfaceArea()
    {
        return internal::helper_surface_area<Scalar,Dimension>::calc(radius());
    }

    template<int OtherDimension>
    EIGEN_DEVICE_FUNC inline Scalar surfaceArea()
    {
        return internal::helper_surface_area<Scalar,OtherDimension>::calc(radius());
    }

    EIGEN_DEVICE_FUNC inline Scalar operator()(const VectorT& x) const
    {
        return evaluate(x);
    }

    EIGEN_DEVICE_FUNC inline Scalar evaluate(const VectorT& x) const
    {
        Scalar sum = Scalar(0.0);

        for(int i = 0 ; i < Dimension + 1 ; ++i)
        {
            const Scalar term = coeff()(i) - x(i);
            sum += (term * term);
        }

        return sqrt(sum);
    }

    EIGEN_DEVICE_FUNC inline bool isPointInside(const VectorT& x) const
    {
        return evaluate(x) < radius();
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        coeff().setZero();
        radius() = Scalar(0.0);
    }

#ifdef VISIONCORE_ENABLE_CEREAL
    template<typename Archive>
    void load(Archive & archive, std::uint32_t const version)
    {
        archive(cereal::make_nvp("Coefficients", coeff()));
        archive(cereal::make_nvp("Radius", radius()));
    }

    template<typename Archive>
    void save(Archive & archive, std::uint32_t const version) const
    {
        archive(cereal::make_nvp("Coefficients", coeff()));
        archive(cereal::make_nvp("Radius", radius()));
    }
#endif // VISIONCORE_ENABLE_CEREAL
};

/**
 * Generic Hypersphere.
 */
template<typename _Scalar, int _Dimension>
class Hypersphere : public HypersphereBase<Hypersphere<_Scalar,_Dimension>>
{
    typedef HypersphereBase<Hypersphere<_Scalar,_Dimension>> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Hypersphere>::Dimension;
    typedef typename Eigen::internal::traits<Hypersphere>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Hypersphere>::CoeffType CoeffType;

    friend class vc::types::HypersphereBase<Hypersphere<_Scalar,_Dimension>>;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Hypersphere)
    using Base::operator=;

    EIGEN_DEVICE_FUNC inline Hypersphere()
    {
    }

    EIGEN_DEVICE_FUNC inline Hypersphere(const Hypersphere<_Scalar,_Dimension>& other) : coeff_(other.coeff()), radius_(other.radius())
    {
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline Hypersphere(const HypersphereBase<OtherDerived>& other) : coeff_(other.coeff()), radius_(other.radius())
    {
    }

    EIGEN_DEVICE_FUNC inline Hypersphere(const CoeffType& coeff, const Scalar& r) : coeff_(coeff), radius_(r)
    {
    }

    EIGEN_DEVICE_FUNC inline ~Hypersphere()
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline const CoeffType& coeff_const() const { return coeff_; }
    EIGEN_DEVICE_FUNC inline CoeffType& coeff_nonconst() { return coeff_; }

    EIGEN_DEVICE_FUNC inline const Scalar& radius_const() const { return radius_; }
    EIGEN_DEVICE_FUNC inline Scalar& radius_nonconst() { return radius_; }

    CoeffType coeff_;
    Scalar radius_;
};

template<typename _Scalar, int _Dimension>
inline std::ostream& operator<<(std::ostream& os, const Hypersphere<_Scalar,_Dimension>& p)
{
    os << _Dimension << "-Sphere(r = " << p.radius() << ", " << p.coeff() << ")";
    return os;
}

}

}

namespace Eigen
{
/**
 * Specialisation of Eigen::Map for Hypersphere.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options>
    : public vc::types::HypersphereBase<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::HypersphereBase<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> > Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::CoeffType CoeffType;

    friend class vc::types::HypersphereBase<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)

    EIGEN_DEVICE_FUNC inline Map(Scalar* coeffs) : coeff_(coeffs), radius_(coeffs + Dimension + 1)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline const CoeffType& coeff_const() const { return coeff_; }
    EIGEN_DEVICE_FUNC inline CoeffType& coeff_nonconst() { return coeff_; }

    EIGEN_DEVICE_FUNC inline const Scalar& radius_const() const { return *radius_; }
    EIGEN_DEVICE_FUNC inline Scalar& radius_nonconst() { return *radius_; }

    CoeffType coeff_;
    Scalar* radius_;
};

/**
 * Specialisation of Eigen::Map for const Hypersphere.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options>
    : public vc::types::HypersphereBase<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::HypersphereBase<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> > Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::CoeffType CoeffType;

    friend class vc::types::HypersphereBase<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)

    EIGEN_DEVICE_FUNC inline Map(const Scalar* coeffs) : coeff_(coeffs), radius_(coeffs + Dimension + 1)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline const CoeffType& coeff_const() const { return coeff_; }
    EIGEN_DEVICE_FUNC inline const Scalar& radius_const() const { return *radius_; }

    const CoeffType coeff_;
    const Scalar* radius_;
};

}

#endif // VISIONCORE_TYPES_HYPERSPHERE_HPP


#pragma once

#include <boost/optional.hpp>

#include "netcode/detail/galois_field.hh"
#include "netcode/detail/square_matrix.hh"

namespace ntc { namespace detail {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Invert a matrix using a Galois field.
/// @attention @p mat will be overwritten
/// @note This is the algorithm provided by jerasure ( http://jerasure.org )
/// @related square_matrix
/// @return A initialized optional value if inversion failed. In this case, the value is the column
/// which made the inversion fail.
boost::optional<std::size_t>
invert(galois_field& gf, square_matrix& mat, square_matrix& inv)
noexcept;

/*------------------------------------------------------------------------------------------------*/

}} // namespace ntc::detail


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ACOS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing acos capabilities

    inverse cosine in radian.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = acos(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[0, \pi[\f$ such that <tt>cos(r) == x</tt>.
    If @c x is outside \f$[-1, 1[\f$ the result is Nan.

    @par Decorators

    std_ for floating entries

    @see acosd, acospi

  **/
  const boost::dispatch::functor<tag::acos_> acos = {};
} }
#endif

#include <boost/simd/function/scalar/acos.hpp>
#include <boost/simd/function/simd/acos.hpp>

#endif


#ifndef SCATTERING_H
#define SCATTERING_H

#include <iostream>
#include <cstdlib>
#include <boost/math/special_functions/bessel.hpp>
#include <complex>
using namespace std;

#include "potentials.hpp"
#include "numerov.hpp"
#include "writeout.hpp"
#include "constant.hpp"

// Calculation of the scattering amplitude for single-channel potential scattering.

complex<double> Smatrix(const CentralPotentials pot, const double energy, const int l, const double h2over2mu, const vector<double> & parameters,
	const int nmesh, const double mesh);
#endif


/*
  High Performance Astrophysical Reconstruction and Processing (HARP)
  (c) 2014-2015, The Regents of the University of California,
  through Lawrence Berkeley National Laboratory.  See top
  level LICENSE file for details.
*/

#include <harp/static_plugins.hpp>

#include <boost/random.hpp>

using namespace std;
using namespace harp;

static const char * image_sim_key_psf_type = "psf_type";
static const char * image_sim_key_spec_type = "spec_type";
static const char * image_sim_key_psf = "psf";
static const char * image_sim_key_spec = "spec";

harp::image_sim::image_sim ( ) : image () {
  rows_ = 0;
  cols_ = 0;
}

harp::image_sim::image_sim ( boost::property_tree::ptree const & props ) : image ( "sim", props ) {

  psf_type_ = props.get < string > ( image_sim_key_psf_type );
  spec_type_ = props.get < string > ( image_sim_key_spec_type );

  psf_props_ = props.get_child ( image_sim_key_psf );
  spec_props_ = props.get_child ( image_sim_key_spec );

  plugin_registry & reg = plugin_registry::get();

  spec_p child_spec ( reg.create_spec ( spec_type_, spec_props_ ) );

  size_t spec_nspec = child_spec->n_spec();
  size_t spec_nlambda = child_spec->n_lambda();

  psf_p child_psf ( reg.create_psf ( psf_type_, psf_props_ ) );

  size_t psf_nspec = child_psf->n_spec();
  size_t psf_nlambda = child_psf->n_lambda();

  if ( ( spec_nspec != psf_nspec ) || ( spec_nlambda != psf_nlambda ) ) {
    ostringstream o;
    o << "input spec size (" << spec_nspec << " x " << spec_nlambda << ") is not consistent with input PSF spectral size (" << psf_nspec << " x " << psf_nlambda << ")";
    HARP_THROW( o.str().c_str() );
  }

  rows_ = child_psf->img_rows();
  cols_ = child_psf->img_cols();

  size_t npix = rows_ * cols_;

  size_t nglobal = spec_nspec * spec_nlambda;

  // read spectra

  vector_double spec_data ( nglobal );
  vector_double spec_lambda ( spec_nlambda );

  child_spec->values ( spec_data );
  child_spec->lambda ( spec_lambda );

  // check wavelength solution against the one from the PSF

  vector_double psf_lambda = child_psf->lambda();

  for ( size_t i = 0; i < psf_nlambda; ++i ) {
    if ( fabs ( psf_lambda[i] - spec_lambda[i] ) / psf_lambda[i] > 0.001 ) {
      ostringstream o;
      o << "wavelength point " << i << " does not match between PSF (" << psf_lambda[i] << ") and spec (" << spec_lambda[i] << ")";
      HARP_THROW( o.str().c_str() );
    }
  }

  // get design matrix from PSF and project

  matrix_double_sparse AT;

  child_psf->project_transpose ( AT );

  // multiply to get signal data

  signal_.resize ( npix );

  sparse_mv_trans ( AT, spec_data, signal_ );

  // free up memory

  AT.clear();
  AT.resize ( 0, 0, false );

  // construct noise

  noise_.resize ( npix );
  invcov_.resize ( npix );

  typedef boost::ecuyer1988 base_generator_type;
  base_generator_type generator(42u);

  double rms;

  for ( size_t i = 0; i < npix; ++i ) {
    rms = sqrt( 16.0 + signal_[i] );
    invcov_[i] = 1.0 / (rms*rms);

    boost::normal_distribution < double > dist ( 0.0, rms );
    boost::variate_generator < base_generator_type&, boost::normal_distribution < double > > gauss ( generator, dist );

    noise_[i] = gauss();
  }

}

harp::image_sim::~image_sim ( ) {

}

void harp::image_sim::values ( vector_double & data ) const {

  data = signal_ + noise_;

  return;
}

void harp::image_sim::inv_variance ( vector_double & invvar ) const {

  invvar = invcov_;

  return;
}

BOOST_CLASS_EXPORT(harp::image_sim)

image * harp::image_sim_create ( boost::property_tree::ptree const & props ) {
  return new image_sim ( props );
}


#ifndef POTOK_HPACK_ENCODE_HPP_
#define POTOK_HPACK_ENCODE_HPP_

#include <potok/hpack/common.hpp>
#include <potok/hpack/error.hpp>

#include <potok/stdint.hpp>

#include <boost/asio/buffer.hpp>
#include <boost/asio/buffers_iterator.hpp>

#include <boost/assert.hpp>

#include <boost/throw_exception.hpp>

#include <stdexcept>

namespace potok {
namespace hpack {

// from the hpack rfc (https://datatracker.ietf.org/doc/html/rfc7541#section-5.1):
//  Pseudocode to represent an integer I is as follows:
//  if I < 2^N - 1, encode I on N bits
//  else
//      encode (2^N - 1) on N bits
//      I = I - (2^N - 1)
//      while I >= 128
//           encode (I % 128 + 128) on 8 bits
//           I = I / 128
//      encode I on 8 bits

struct integer_encoder {
  enum class state { first, continuation, final, done };

  u64      v_               = 0;
  state    state_           = state::first;
  u8 const num_prefix_bits_ = 0;

  integer_encoder()                       = delete;
  integer_encoder(integer_encoder const&) = default;
  integer_encoder(integer_encoder&&)      = default;

  integer_encoder(u64 const v, u8 const num_prefix_bits)
      : v_{v}
      , num_prefix_bits_{num_prefix_bits}
  {
  }

  template <class MutableBufferSequence>
  auto operator()(MutableBufferSequence      mutable_buf_seq,    //
                  boost::system::error_code& ec) -> usize
  {
    auto pos = boost::asio::buffers_begin(mutable_buf_seq);
    auto end = boost::asio::buffers_end(mutable_buf_seq);

    auto bytes_written = usize{0};

    if (pos == end) {
      ec = error::needs_more;
      return bytes_written;
    }

    u64 const max_prefix_value = get_max_prefix_value(num_prefix_bits_);

    switch (state_) {
      case state::first: {
        // clear the initial octet in preparation of writing the first N bits of the integer
        //
        *pos &= ~max_prefix_value;

        if (v_ < max_prefix_value) {
          *pos++ |= v_;
          ++bytes_written;

          state_ = state::done;

          break;
        }

        *pos++ |= max_prefix_value;
        ++bytes_written;

        state_ = state::continuation;

        v_ -= max_prefix_value;
      }

      case state::continuation: {
        for (; (pos != end) && (v_ >= 128); ++pos, ++bytes_written, v_ /= 128) {
          auto const v = (v_ % 128 + 128);

          *pos = v;
        }

        if (v_ >= 128) {
          ec     = error::needs_more;
          state_ = state::continuation;
          break;
        }

        if (pos == end) {
          ec     = error::needs_more;
          state_ = state::final;
          break;
        }
      }

      case state::final: {
        *pos++ = v_;
        ++bytes_written;

        state_ = state::done;
        break;
      }

      case state::done:
        break;
    }

    return bytes_written;
  }
};

}    // namespace hpack
}    // namespace potok

#endif    // POTOK_HPACK_ENCODE_HPP_


#pragma once

#ifdef BOOST_TEST_DYN_LINK
    #include <boost/test/unit_test.hpp>
#else
    #include <boost/test/included/unit_test.hpp>
#endif

#include <boost/mpl/list.hpp>
#include <boost/test/tools/output_test_stream.hpp>

#include <complex>
#include <functional>
#include <initializer_list>

template<typename, size_t, size_t> class Matrix;

boost::test_tools::output_test_stream output;
std::initializer_list<std::initializer_list<int>> initList3x3{{ 1,2,3 }, { 4,5,6 }, { 7,8,9 }};
typedef boost::mpl::list<short,int,long,double,std::complex<double>> test_types;

using M1 = Matrix<int,1,1>;
using M3 = Matrix<int,3,3>;

class coutRedirect {
    std::streambuf* old;
  public:
    coutRedirect(std::streambuf* buf) : old(std::cout.rdbuf(buf)) {}
    ~coutRedirect() { std::cout.rdbuf(old); }
};

#define CHECK_OUTPUT(msg) BOOST_CHECK(output.is_equal(msg))

template<typename T, size_t N, size_t M>
void TestConstructor(std::string msg)
{
    coutRedirect guard(output.rdbuf());
    Matrix<T,N,M> foo;
    CHECK_OUTPUT(msg + "\n");
    BOOST_CHECK_EQUAL(foo.numberOfRows(), N);
    BOOST_CHECK_EQUAL(foo.numberOfColumns(), M);
}

template<typename T, size_t N, size_t M>
void TestConstructorDyn(std::string msg)
{
    coutRedirect guard(output.rdbuf());
    Matrix<T,0,0> foo(N,M);
    CHECK_OUTPUT(msg + "\n");
    BOOST_CHECK_EQUAL(foo.numberOfRows(), N);
    BOOST_CHECK_EQUAL(foo.numberOfColumns(), M);
}

template<typename T, size_t N, size_t M>
struct TestBracket {
    Matrix<T,N,M> m;
    TestBracket()
    {
        BOOST_REQUIRE(m.numberOfRows() == N);
        BOOST_REQUIRE(m.numberOfColumns() == M);
        for (size_t i = 1; i <= N; ++i) {
            for (size_t j = 1; j <= M; ++j) {
                BOOST_CHECK_EQUAL(m(i,j), T{0});
            }
        }
    }
};

// vim: fen


#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include "ukf.h"

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::vector;

int main() {

	//Create a UKF instance
	UKF ukf;

/*******************************************************************************
* Programming assignment calls
*******************************************************************************/

    MatrixXd Xsig_aug = MatrixXd(15, 7);
    ukf.AugmentedSigmaPoints(&Xsig_aug);

	return 0;
}