#ifndef FSTCLASSIFIERSVM_H
#define FSTCLASSIFIERSVM_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    classifier_svm.hpp
   \brief   Wraps external Support Vector Machine implementation (in LibSVM) to serve as FST3 classifier
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <list>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "classifier.hpp"

#include "svm.h"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

// NOTE: this is the proxy to LIBSVM external library
//       therefore some FST coding conventions can not
//       be strictly kept

/*! \brief Wraps external Support Vector Machine implementation (in LibSVM) to serve as FST3 classifier
    \note clone() implementation assumes LibSVM's svm_model and svm_node is equivalent to that in LibSVM version 300

 \warning LIBSVM especially with LINEAR kernel seems to have occassional problems with certain C values on certain datasets and may freeze.
          (Other kernels are more stable but not completely immune to this problem.) This is a problem outside FST3.
*/

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Classifier_LIBSVM : public Classifier<RETURNTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> const PSubset;
	typedef typename DATAACCESSOR::PPattern PPattern;
	Classifier_LIBSVM();
	virtual ~Classifier_LIBSVM() {notify("Classifier_LIBSVM destructor."); cleanup();}

	void initialize(const PDataAccessor da); // must be called to pre-allocate data transfer buffers
protected:
	void allocate(); // to be called in initialize(), pre-allocates everything
	void cleanup(); // deallocates everything
public:
	void set_parameter_C(double newC) {parameters.C = newC;};
	void set_parameter_gamma(double newgamma) {parameters.gamma = newgamma;};
	void set_parameter_coef0(double newcoef0) {parameters.coef0 = newcoef0;};
	void set_kernel_type(int kernel_type) {parameters.kernel_type = kernel_type;}; // see svm.h for admissible values
	double get_parameter_C() const {return parameters.C;}
	double get_parameter_gamma() const {return parameters.gamma;}
	double get_parameter_coef0() const {return parameters.coef0;}
	int get_kernel_type() const {return parameters.kernel_type;}; // see svm.h for admissible values

	virtual bool classify(DIMTYPE &cls, const PPattern &pattern);  // classifies pattern, returns the respective class index
	virtual bool train(const PDataAccessor da, const PSubset sub); // learns from designated training part of data
	virtual bool test(RETURNTYPE &result, const PDataAccessor da); // estimates accuracy using designated test data

	bool optimize_parameters(const PDataAccessor da, const PSubset sub, const int max_points=100, const int max_throws=100, const double lgC_min=-5, const double lgC_max=9, const double lggamma_min=-15, const double lggamma_max=3, const double lgcoef0_min=-2, const double lgcoef0_max=5, std::ostream& os=std::cout);

	Classifier_LIBSVM* clone() const {return stateless_clone();} //! \note dirty workaround to enable easy usage in sequential_step ...
	Classifier_LIBSVM* sharing_clone() const {throw fst_error("Classifier_LIBSVM::sharing_clone() not supported, use Classifier_LIBSVM::stateless_clone() instead.");}
	Classifier_LIBSVM* stateless_clone() const;

	virtual std::ostream& print(std::ostream& os) const {os << "Classifier_LIBSVM(kernel="<<get_kernel_type()<<")"; return os;}
private:
	Classifier_LIBSVM(const Classifier_LIBSVM& csvm, int); // weak copy-constructor, does not copy LibSVM internal structures
protected:
	// the following to be initialized using initialize() based on dataaccessor info
	IDXTYPE _all_patterns;
	DIMTYPE _classes;
	DIMTYPE _features;
	struct svm_problem problem; // data to be stored in here
	struct svm_parameter parameters;
	struct svm_model *model;
	struct svm_node *onepattern;

	bool svm_class_weighing;

	//! Nested class to hold parameter candidates in the course of optimize_parameters() run
	class ParameterSet {
	public:
		ParameterSet(): _crit_value(0.0), _Cpar(1.0), _gamma(1.0), _coef0(1.0) {}
		ParameterSet(const RETURNTYPE crit_value, const double C, const double gamma, const double coef0) : _crit_value(crit_value), _Cpar(C), _gamma(gamma), _coef0(coef0) {}
		ParameterSet(const ParameterSet& ps) : _crit_value(ps._crit_value), _Cpar(ps._Cpar), _gamma(ps._gamma), _coef0(ps._coef0) {}
		void operator=(const ParameterSet& ps) {_crit_value=ps._crit_value; _Cpar=ps._Cpar; _gamma=ps._gamma; _coef0=ps._coef0;}
		RETURNTYPE _crit_value;
		double _Cpar; // _C is reserved
		double _gamma;
		double _coef0;
	};
	typedef list<ParameterSet> PARAMSETLIST;
	typename PARAMSETLIST::iterator iter;

private:
	boost::scoped_array<DIMTYPE> _index;
	DIMTYPE _subfeatures;
};

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_LIBSVM(const Classifier_LIBSVM& csvm, int) : // copy-constructor
	_all_patterns(csvm._all_patterns),
	_classes(csvm._classes),
	_features(csvm._features),
	svm_class_weighing(csvm.svm_class_weighing),
	_subfeatures(csvm._subfeatures)
{
	notify("Classifier_LIBSVM (weak) copy-constructor.");
	// does not copy LibSVM-defined structures, except for 'parameters'
	// other-than 'parameters' LibSVM structures are only pre-allocated using allocate()
	parameters.svm_type=csvm.parameters.svm_type;
	parameters.kernel_type=csvm.parameters.kernel_type;
	parameters.degree=csvm.parameters.degree;
	parameters.gamma=csvm.parameters.gamma;
	parameters.coef0=csvm.parameters.coef0;
	parameters.cache_size=csvm.parameters.cache_size;
	parameters.eps=csvm.parameters.eps;
	parameters.C=csvm.parameters.C;
	parameters.nr_weight=csvm.parameters.nr_weight;
	parameters.nu=csvm.parameters.nu;
	parameters.p=csvm.parameters.p;
	parameters.shrinking=csvm.parameters.shrinking;
	parameters.probability=csvm.parameters.probability;
	allocate();
	model=NULL;
	if(csvm.parameters.weight_label) memcpy((void *)parameters.weight_label,(void *)csvm.parameters.weight_label,(long)(_classes) * sizeof(int));
	if(csvm.parameters.weight) memcpy((void *)parameters.weight,(void *)csvm.parameters.weight,(long)(_classes) * sizeof(double));
	if(csvm._index) {_index.reset(new DIMTYPE[_features]); memcpy((void *)_index.get(),(void *)(csvm._index).get(),sizeof(DIMTYPE)*_features);}
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::stateless_clone() const
{
	Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this,(int)0);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_LIBSVM()
{
	notify("Classifier_LIBSVM constructor.");
	_all_patterns=0;
	_classes=0;
	_features=0;
	problem.x=NULL;
	problem.y=NULL;
	onepattern=NULL;
	parameters.weight_label=NULL;
	parameters.weight=NULL;
	model=NULL;
	_subfeatures=0;

	// default parameters
	parameters.svm_type=C_SVC;
	parameters.kernel_type=RBF; //LINEAR;
	parameters.degree=2;
	parameters.gamma=/*standard_gamma=*/1.0;
	parameters.coef0=1.0;
	parameters.cache_size=10;
	parameters.eps=0.001;
	parameters.C=/*standard_C=*/1.0;
	parameters.nr_weight=0;
	parameters.weight_label=NULL; // just sharing a pointer
	parameters.weight=NULL; // just sharing a pointer
	parameters.nu=1.0;
	parameters.p=1.0;
	parameters.shrinking=0;
	parameters.probability=0;
	svm_class_weighing=false;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::optimize_parameters(const PDataAccessor da, const PSubset sub, const int max_points, const int max_throws, const double lgC_min, const double lgC_max, const double lggamma_min, const double lggamma_max, const double lgcoef0_min, const double lgcoef0_max, std::ostream& os)
{
	// \note: assumes Classifier_LIBSVM::initialize() had been called for this da
	// assumes initial C, gamma, and coef0 have been set using set_parameter_*(), or that default values are OK
	// (the initial values are used as search starting point)
	/*! \warning LIBSVM LINEAR kernel seems to have problems with certain C values on certain datasets. If the optimization
	             process seemingly freezes, try narrower lgC_min and lgC_max. Especially the lower bound seems
	             important to be increased.
	*/
	assert(da);
	assert(sub);
	assert(sub->get_d()>0);
	assert(max_points>0);
	assert(max_throws>0);
	assert(lgC_max>lgC_min);
	assert(lggamma_max>lggamma_min);
	assert(lgcoef0_max>lgcoef0_min);
	const double lgCrange=lgC_max-lgC_min;
	const double lggammarange=lggamma_max-lggamma_min;
	const double lgcoef0range=lgcoef0_max-lgcoef0_min;
	PARAMSETLIST _paramlist; // _paramlist.clear();
	ParameterSet tmpset, bestset;
	// evaluate the initial (current) parameter set
	RETURNTYPE val, maxval, firstval;
	RETURNTYPE result=0.0;
	RETURNTYPE cnt=0.0;
	for(bool b=da->getFirstSplit();b==true;b=da->getNextSplit()) {
		if(!train(da,sub)) return false;
		if(!test(val,da)) return false;
		result+=val;
		cnt+=1.0;
	}
	firstval=maxval=val=result/(RETURNTYPE)cnt;
	{
		ostringstream sos;
		sos << "SVM before optimization: accuracy=" << val << ", C=" << get_parameter_C();
		switch(get_kernel_type()) {
			case LINEAR:  sos << std::endl; break;
			case RBF:     sos << ", gamma=" << get_parameter_gamma() << std::endl; break;
			case POLY:
			case SIGMOID: sos << ", gamma=" << get_parameter_gamma() << ", coef0=" << get_parameter_coef0() << std::endl; break;
		}
		syncout::print(os,sos);
	}
	_paramlist.push_back(ParameterSet(val,get_parameter_C(),get_parameter_gamma(),get_parameter_coef0()));
	for(int i=0;i<max_points;i++)
	{ // find and evaluate next parameter set candidate
		double maxdistmin=0.0;
		for(int j=0;j<max_throws;j++)
		{ // find next parameter set candidate
			tmpset._Cpar =exp(lgC_min+(((double)rand()*(double)(lgC_max-lgC_min))/(double)RAND_MAX));
			tmpset._gamma=exp(lggamma_min+(((double)rand()*(double)(lggamma_max-lggamma_min))/(double)RAND_MAX));
			tmpset._coef0=exp(lgcoef0_min+(((double)rand()*(double)(lgcoef0_max-lgcoef0_min))/(double)RAND_MAX));

			double distmin=-1.0; // potential new candidate (weighted) distance to already evaluated candidates
			for(iter=_paramlist.begin();iter!=_paramlist.end();iter++)
			{
				double dist=0.0;
				assert(get_kernel_type()==LINEAR || get_kernel_type()==POLY || get_kernel_type()==RBF || get_kernel_type()==SIGMOID);
				switch(get_kernel_type()) {
					case LINEAR:  dist=sqrt(((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)*((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)); break;
					case RBF:     dist=sqrt(((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)*((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)+((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)*((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)); break;
					case POLY:
					case SIGMOID: dist=sqrt(((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)*((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)+((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)*((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)+((log(tmpset._coef0)-log(iter->_coef0))/lgcoef0range)*((log(tmpset._coef0)-log(iter->_coef0))/lgcoef0range)); break;
				}
				// candidates more distant from all others are preferred
				// distances are weighed to penalize closeness to worse-performing candidates
				assert(iter->_crit_value>0.0);
				if(distmin<0 || dist*iter->_crit_value<distmin) distmin=dist*iter->_crit_value;
			}
			assert(distmin>=0.0);
			if(distmin>maxdistmin)
			{ // better (more distant from those already tested, esp. from the bad ones) parameter set candidate found
				maxdistmin=distmin;
				bestset=tmpset;
			}
		}
		// evaluate the chosen candidate and add it to list
		set_parameter_C(bestset._Cpar);
		set_parameter_gamma(bestset._gamma);
		set_parameter_coef0(bestset._coef0);
		assert(svm_check_parameter(&problem,&parameters)==NULL);
		result=0.0;
		cnt=0.0;
		for(bool b=da->getFirstSplit();b==true;b=da->getNextSplit()) {
			if(!train(da,sub)) return false;
			if(!test(val,da)) return false;
			result+=val;
			cnt+=1.0;
		}
		val=result/(RETURNTYPE)cnt;
		bestset._crit_value=val;
		_paramlist.push_back(bestset);
		if(true)
		{
			ostringstream sos;
			sos << i+1 << ". ";
			if(val>maxval) {maxval=val; sos << "BEST accuracy=";} else sos << "test accuracy=";
			sos << bestset._crit_value << ", C=" << get_parameter_C();
			switch(get_kernel_type()) {
				case LINEAR:  sos << std::endl; break;
				case RBF:     sos << ", gamma=" << get_parameter_gamma() << std::endl; break;
				case POLY:
				case SIGMOID: sos << ", gamma=" << get_parameter_gamma() << ", coef0=" << get_parameter_coef0() << std::endl; break;
			}
			syncout::print(os,sos);
		}
	}
	// identify the best parameter set among all tested ones
	assert(!_paramlist.empty());
	iter=_paramlist.begin();
	bestset=(*iter);
	while(iter!=_paramlist.end())
	{
		if(iter->_crit_value>bestset._crit_value) bestset=(*iter);
		iter++;
	}
	set_parameter_C(bestset._Cpar);
	set_parameter_gamma(bestset._gamma);
	set_parameter_coef0(bestset._coef0);
	{
		ostringstream sos;
		sos << "SVM after optimization: accuracy=" << bestset._crit_value << ", C=" << get_parameter_C();
		switch(get_kernel_type()) {
			case LINEAR:  sos << std::endl; break;
			case RBF:     sos << ", gamma=" << get_parameter_gamma() << std::endl; break;
			case POLY:
			case SIGMOID: sos << ", gamma=" << get_parameter_gamma() << ", coef0=" << get_parameter_coef0() << std::endl; break;
		}
		sos << "Accuracy difference=" << bestset._crit_value-firstval << std::endl << std::endl;
		syncout::print(os,sos);
	}
	return false;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::allocate()
{
	try {
		//cleanup(); - not to be called here or it would destroy original data in copy-constructor
		assert(_all_patterns>0);
		assert(_classes>1);
		assert(_features>0);

		problem.y =               (double *)          malloc(_all_patterns * sizeof(double));
		problem.x =               (struct svm_node **)malloc((long)(_all_patterns) * sizeof(struct svm_node *));
		parameters.weight_label = (int *)             malloc((long)(_classes) * sizeof(int));
		parameters.weight =       (double *)          malloc((long)(_classes) * sizeof(double));
		onepattern =              (struct svm_node *) malloc((long)(_features+1) * sizeof(struct svm_node));
		for(IDXTYPE p=0;p<_all_patterns;p++) problem.x[p]=NULL;
		// pre-allocate all problem.x[] arrays to max size, to prevent re-allocation speed efficiency problems
		// - WARNING! this leads to uneconomical use of memory
		for(IDXTYPE p=0;p<_all_patterns;p++)
			problem.x[p] = (struct svm_node *) malloc((long)(_features+1) * sizeof(struct svm_node));

		// class penalty weighing disabled
		parameters.nr_weight=0;
		problem.l = _all_patterns;
		assert(svm_check_parameter(&problem,&parameters)==NULL);
	}
	catch (...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM allocation problem.");
	}
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(const PDataAccessor da)
{
	assert(da);
	try {
		cleanup();
		_all_patterns = da->getClassSizeSum(); assert(_all_patterns>0);
		_classes = da->getNoOfClasses(); assert(_classes>1);
		_features = da->getNoOfFeatures(); assert(_features>0);
		allocate();
	}
	catch (...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM initialization problem.");
	}
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::cleanup()
{
	// NOTE: called from destructor
		if(problem.x!=NULL) {
			for(IDXTYPE p=0;p<_all_patterns;p++) if(problem.x[p]!=NULL) {free(problem.x[p]); problem.x[p]=NULL;}
			free(problem.x); problem.x=NULL; // the array only, not the actual pointers
		}
		if(problem.y!=NULL) free(problem.y); problem.y=NULL;
		if(onepattern!=NULL) free(onepattern); onepattern=NULL;
		if(parameters.weight_label!=NULL) free(parameters.weight_label); parameters.weight_label=NULL;
		if(parameters.weight!=NULL) free(parameters.weight); parameters.weight=NULL;
		if(model!=NULL) {svm_free_and_destroy_model(&model); model=NULL;}

		_all_patterns=0;
		_classes=0;
		_features=0;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::train(const PDataAccessor da, const PSubset sub)
{
	assert(da);
	assert(_classes==da->getNoOfClasses());
	assert(_features==da->getNoOfFeatures());
	assert(_all_patterns==da->getClassSizeSum());
	assert(problem.x!=NULL);
	assert(problem.y!=NULL);
	assert(onepattern!=NULL);
	assert(parameters.weight_label!=NULL);
	assert(parameters.weight!=NULL);
	assert(svm_check_parameter(&problem,&parameters)==NULL);
	try {
		// feature index buffering to accelerate data transfer
		DIMTYPE f,ftmp;
		bool b;
		if(!_index || _features<sub->get_n_raw()) _index.reset(new DIMTYPE[_features]);
		for(b=sub->getFirstFeature(f),_subfeatures=0;b==true;b=sub->getNextFeature(f),_subfeatures++) {assert(_subfeatures<_features); _index[_subfeatures]=f;}
#ifdef DEBUG
		{
			ostringstream sos;
			sos << "index: "; for(f=0;f<_subfeatures;f++) sos << _index[f] << " "; //sos << std::endl;
			syncout::print(std::cout,sos);
		}
#endif
		assert(_subfeatures==sub->get_d_raw());

		typename DATAACCESSOR::PPattern p,ptmp;
		IDXTYPE s,i;//,ifeatures;
		IDXTYPE count;
		DIMTYPE nzfeat;
		double tmp;
		const DIMTYPE da_train_loop=0; // to avoid mixup of get*Block() loops of different types

		count=0;
		for(DIMTYPE c_train=0;c_train<_classes;c_train++)
		{
			da->setClass(c_train);
			for(b=da->getFirstBlock(TRAIN,p,s,da_train_loop);b==true;b=da->getNextBlock(TRAIN,p,s,da_train_loop)) for(i=0;i<s;i++)
			{
				problem.y[count]=c_train; // current pattern class info
				nzfeat=0; // no. of nonzero features processed in current pattern
				ptmp=&p[i*_features]; // temp pointer to current pattern
				for(f=0;f<_subfeatures;f++)
				{
					ftmp=_index[f]; // (full set) feature index
					tmp=ptmp[ftmp]; // feature value
					if(tmp!=0.0) {
						problem.x[count][nzfeat].index=ftmp+1;
						problem.x[count][nzfeat].value=tmp;
						nzfeat++;
					}
				}
				problem.x[count][nzfeat].index=-1;
				count++;
			}
		}
		assert(count>0);
		problem.l=count;
		if(model!=NULL) svm_free_and_destroy_model(&model);
		// train SVM model
		model=svm_train(&problem, &parameters);
	}
	catch(...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM::train() error.");
	}
	if(model==NULL) return false;
	return true;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::classify(DIMTYPE &cls, const PPattern &pattern)  // classifies pattern, returns the respective class index
{
	assert(model!=NULL);
	assert(svm_check_parameter(&problem,&parameters)==NULL);
	try {
		// assumes feature index buffering to accelerate data transfer
		assert(_index);
		assert(_subfeatures>0 && _subfeatures<=_features);

		DIMTYPE nzfeat;
		double tmp;
		RETURNTYPE tmpval;
		DIMTYPE ftmp;

		nzfeat=0; // no. of nonzero features processed in current pattern
		for(DIMTYPE f=0;f<_subfeatures;f++)
		{
			ftmp=_index[f]; // (full set) feature index
			tmp=pattern[ftmp]; // feature value
			if(tmp!=0.0) {
				onepattern[nzfeat].index=ftmp+1;
				onepattern[nzfeat].value=tmp;
				nzfeat++;
			}
		}
		onepattern[nzfeat].index=-1;
		// now classify the pattern
		tmpval=svm_predict(model, onepattern);
		cls=(DIMTYPE)tmpval;
	}
	catch(...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM::classify() error.");
	}
	return true;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::test(RETURNTYPE &result, const PDataAccessor da)
{
	assert(da);
	assert(_classes==da->getNoOfClasses());
	assert(_features==da->getNoOfFeatures());
	assert(_all_patterns==da->getClassSizeSum());
	assert(model!=NULL);
	assert(svm_check_parameter(&problem,&parameters)==NULL);
	try {
		// assumes feature index buffering to accelerate data transfer
		assert(_index);
		assert(_subfeatures>0);

		typename DATAACCESSOR::PPattern p; //,ptmp;
		IDXTYPE s,i;
		IDXTYPE count, correct;
		DIMTYPE clstmp;
		bool b;
		const DIMTYPE da_test_loop=1; // to avoid mixup of get*Block() loops of different types

		count=0;
		correct=0;
		for(DIMTYPE c_test=0;c_test<_classes;c_test++)
		{
			da->setClass(c_test);
			for(b=da->getFirstBlock(TEST,p,s,da_test_loop);b==true;b=da->getNextBlock(TEST,p,s,da_test_loop)) for(i=0;i<s;i++)
			{
				if(!classify(clstmp,&p[i*_features])) return false;
				if(clstmp==c_test) correct++;
				count++;
			}
		}
		assert(count>0);
		result=(RETURNTYPE)correct/(RETURNTYPE)count;
#ifdef DEBUG
		{
			ostringstream sos;
			sos << " result=" << result << std::endl;
			syncout::print(std::cout,sos);
		}
#endif
	}
	catch(...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM::test() error.");
	}
	return true;
}

} // namespace
#endif // FSTCLASSIFIERSVM_H ///:~


/*
 *  Copyright (c) 2009, Rene Wagner
 *  Copyright (c) 2010, 2011 DFKI GmbH
 *  All rights reserved.
 *
 *  Author: Rene Wagner <rene.wagner@dfki.de>
 *
 *  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 DFKI GmbH 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 OWNER 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.
 */

#ifndef __UKFOM_UKF_HPP__
#define __UKFOM_UKF_HPP__

#include <vector>
#include <algorithm>
#include <numeric>

#include <boost/bind.hpp>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

#include <Eigen/QR>

#include "lapack/cholesky.hpp"
#include "traits/dof.hpp"
#include "util.hpp"

namespace ukfom {

// import most common Eigen types
using namespace Eigen;

template <typename state>
class ukf {
	typedef ukf self;

	enum {
		n = state::DOF
	};

public:

	typedef typename state::scalar_type scalar_type;
	typedef typename state::vectorized_type vectorized_state;
	typedef Matrix<scalar_type, int(state::DOF), int(state::DOF)> cov;
	typedef std::vector<state> state_vector;

	ukf(const state &mu,
		const cov &sigma)
		: mu_(mu),
		  sigma_(sigma)
	{
	}

	template<typename ProcessModel>
	void predict(ProcessModel g, const cov &R)
	{
		predict(g, boost::bind(id<cov>, R));
	}

	template<typename ProcessModel, typename ProcessNoiseCovariance>
	void predict(ProcessModel g, ProcessNoiseCovariance R)
	{
		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, sigma_, X);

		std::transform(X.begin(), X.end(), X.begin(), g);

		mu_ = sigma_points_mean(X);

		//std::cout << "mu':" << std::endl << mu_ << std::endl;

		sigma_ = sigma_points_cov<state::DOF>(mu_, X) + R();
	}

	template<typename Measurement,
			 typename MeasurementModel,
			 typename MeasurementNoiseCovariance>
	void update(const Measurement &z,
				MeasurementModel h,
				MeasurementNoiseCovariance Q)
	{
		update(z, h, Q,
			   accept_any_mahalanobis_distance<scalar_type>);
	}

	template<typename Measurement,
			 typename MeasurementModel>
	void update(const Measurement &z,
				MeasurementModel h,
				const Eigen::Matrix<scalar_type, dof<Measurement>::value, dof<Measurement>::value> &Q)
	{
		typedef Eigen::Matrix<scalar_type, dof<Measurement>::value, dof<Measurement>::value> measurement_cov;
		update(z, h,
			   boost::bind(id<measurement_cov>, Q),
			   accept_any_mahalanobis_distance<scalar_type>);
	}

	template<typename Measurement,
			 typename MeasurementModel,
			 typename MeasurementNoiseCovariance,
			 typename MahalanobisTest>
	void update(const Measurement &z,
				MeasurementModel h,
				MeasurementNoiseCovariance Q,
				MahalanobisTest mt)
	{
		const static int measurement_rows = dof<Measurement>::value;
		typedef Measurement measurement;
		typedef Eigen::Matrix<scalar_type, measurement_rows, 1> vectorized_measurement;
		typedef Matrix<scalar_type, measurement_rows, measurement_rows> measurement_cov;
		typedef Matrix<scalar_type, state::DOF, measurement_rows> cross_cov;

		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, sigma_, X);

		std::vector<measurement> Z(X.size());
		std::transform(X.begin(), X.end(), Z.begin(), h);

		const measurement meanZ = sigma_points_mean(Z);
		const measurement_cov S = sigma_points_cov<measurement_rows>(meanZ, Z) + Q();
		const cross_cov covXZ = sigma_points_cross_cov<measurement_rows>(mu_, meanZ, X, Z);

		measurement_cov S_inverse(S.inverse());

		const cross_cov K = covXZ * S_inverse;

		const vectorized_measurement innovation = z - meanZ;

		const scalar_type mahalanobis2 = (innovation.transpose() * S_inverse * innovation)(0);

		if (mt(mahalanobis2))
		{
			sigma_ -= K * S * K.transpose();
			apply_delta(K * innovation);
		}
	}

	const state &mu() const
	{
		return mu_;
	}

	const cov &sigma() const
	{
		return sigma_;
	}

private:

	void generate_sigma_points(const state &mu,
							   const vectorized_state &delta,
							   const cov &sigma,
							   state_vector &X) const
	{
		assert(X.size() == 2 * n + 1);

		MatrixXd L(sigma.llt().matrixL());

		/*if (!L.isSPD())
		{
			std::cerr << std::endl << "sigma is not SPD:" << std::endl
					  << sigma << std::endl
					  << "---" << std::endl;
			Eigen::EigenSolver<cov> eig(sigma);
			std::cerr << "eigen values: " << eig.eigenvalues().transpose() << std::endl;
		}

		assert(L.isSPD());*/

		/*
		std::cout << ">> L" << std::endl
				  << L.getL() << std::endl
				  << "<< L" << std::endl;
		*/

		X[0] = mu + delta;
		for (std::size_t i = 1, j = 0; j < n; ++j)
		{
			//std::cout << "L.col(" << j << "): " << L.getL().col(j).transpose() << std::endl;
			X[i++] = mu + (delta + L.col(j));
			X[i++] = mu + (delta - L.col(j));
		}
		//print_sigma_points(X);
	}

	void generate_sigma_points(const state &mu,
							   const cov &sigma,
							   state_vector &X) const
	{
		generate_sigma_points(mu, vectorized_state::Zero(), sigma, X);
	}

	// manifold mean
	template<typename manifold>
	manifold
	sigma_points_mean(const std::vector<manifold> &X) const
	{
		manifold reference = X[0];
		typename manifold::vectorized_type mean_delta;
		const static std::size_t max_it = 10000;

		std::size_t i = 0;
		do {
			mean_delta.setZero();
			for (typename std::vector<manifold>::const_iterator Xi = X.begin(); Xi != X.end(); ++Xi)
			{
				mean_delta += *Xi - reference;
			}
			mean_delta /= X.size();
			reference += mean_delta;
		} while (mean_delta.norm() > 1e-6
				 && ++i < max_it);

		if (i >= max_it)
		{
			std::cerr << "ERROR: sigma_points_mean() did not converge. norm(mean_delta)=" << mean_delta.norm() << std::endl;
			assert(false);
		}

		return reference;
	}

	// vector mean
	template<int measurement_rows>
	Matrix<scalar_type, measurement_rows, 1>
	sigma_points_mean(const std::vector<Matrix<scalar_type, measurement_rows, 1> > &Z) const
	{
		typedef Matrix<scalar_type, measurement_rows, 1> measurement;

		return std::accumulate(Z.begin(), Z.end(), measurement(measurement::Zero())) / Z.size();
	}

#ifdef VECT_H_
	// MTK vector mean
	template<int measurement_rows>
	MTK::vect<measurement_rows, scalar_type>
	sigma_points_mean(const std::vector<MTK::vect<measurement_rows, scalar_type> > &Z) const
	{
		typedef MTK::vect<measurement_rows, scalar_type> measurement;

		return std::accumulate(Z.begin(), Z.end(), measurement(measurement::Zero())) / Z.size();
	}
#endif // VECT_H_

	template<int cov_size, typename T>
	Matrix<scalar_type, -1, -1>
	sigma_points_cov(const T &mean, const std::vector<T> &V) const
	{
		typedef Matrix<scalar_type, -1, -1> cov_mat;
		typedef Matrix<scalar_type, cov_size, 1> cov_col;

		cov_mat c(cov_mat::Zero(cov_size,cov_size));

		for (typename std::vector<T>::const_iterator Vi = V.begin(); Vi != V.end(); ++Vi)
		{
			cov_col d = *Vi - mean;
			c += d * d.transpose();
		}

		return 0.5 * c;
	}

	template<int measurement_rows, typename Measurement>
	Matrix<scalar_type, state::DOF, measurement_rows>
	sigma_points_cross_cov(const state &meanX,
						   const Measurement &meanZ,
						   const state_vector &X,
						   const std::vector<Measurement> &Z) const
	{
		assert(X.size() == Z.size());

		typedef Matrix<scalar_type, state::DOF, measurement_rows> cross_cov;

		cross_cov c(cross_cov::Zero());

		{
			typename state_vector::const_iterator Xi = X.begin();
			typename std::vector<Measurement>::const_iterator Zi = Z.begin();
			for (;Zi != Z.end(); ++Xi, ++Zi)
			{
				c += (*Xi - meanX) * (*Zi - meanZ).transpose();
			}
		}

		return 0.5 * c;
	}

	void apply_delta(const vectorized_state &delta)
	{
		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, delta, sigma_, X);

		mu_ = sigma_points_mean(X);
		sigma_ = sigma_points_cov<state::DOF>(mu_, X);
	}

public: state mu_;
public: cov sigma_;

	// for debugging only

	void print_sigma_points(const state_vector &X) const
	{
		std::cout << "generated sigma points:" << std::endl;
		for (typename state_vector::const_iterator Xi = X.begin(); Xi != X.end(); ++Xi)
		{
			std::cout << *Xi << std::endl << "***" << std::endl;
		}
	}

public:
	void check_sigma_points()
    {
		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, sigma_, X);

		state muX = sigma_points_mean(X);

		cov sigma_test = sigma_points_cov<state::DOF>(muX, X);
		if((sigma_test - sigma_).cwise().abs().maxCoeff()>1e-6){
			std::cerr << sigma_test << "\n\n" << sigma_;
			assert(false);
		}

		if (mu_ != muX)
		{
//			std::cout << "mu_:" << mu_ << std::endl;
//			std::cout << "muX:" << muX << std::endl;
			std::cout << "norm:" << ((mu_ - muX).norm() > 0. ? ">" : "=") << std::endl;
		}
		assert (mu_ == muX);
	}

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace ukfom
#endif // __UKFOM_UKF_HPP__


#ifndef WAVE_SENSOR_MODEL_HPP
#define WAVE_SENSOR_MODEL_HPP

#include <vector>
#include <Eigen/Core>
#include "wave/utils/math.hpp"

namespace wave {

/**
 * Parameters struct specifies the beam number and spacing
 */
struct RangeSensorParams {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /** Covariance of noise in spherical coordinates
    * Noise should be in linear, angular, angular units
     * on range, elevation, azimuth in that order
    */
    Mat3 sigma_spherical = Mat3::Identity();
};

/**
 * Class to generate covariance matrices in Cartesian sensor frame given
 * a sensor variance supplied in Spherical Coordinates
 */
class RangeSensor {
 private:
    RangeSensor() = delete;
    const RangeSensorParams param;

 public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    RangeSensor(RangeSensorParams params);

    void getEuclideanCovariance(const double *point, Mat3& covar_euclid) const;
};
}

#endif  // WAVE_SENSOR_MODEL_HPP


#include "PCE.h"
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <tchar.h>
#include <iostream>

using namespace PartDesignGui;

PCEPosition::PCEPosition()
{
	t = Vector2d(0.0, 0.0);
	style = FreeStyle;
	score = 0;
}

void PCEPosition::Clear()
{
	//transform->Clear();
	std::vector<int>().swap(edge_indexes);
	t = Vector2d(0.0, 0.0);
	style = FreeStyle;
	score = 0;
}

void PCEPosition::ComputeM()
{
	M = Math::Functs::TranslationMatrix(Vector3d(t.x, t.y, 0.));
	glm::vec4 v(Vector3d(transform->bounding_box.center_2d.x, transform->bounding_box.center_2d.y, 0.0), 1.0);
	Vector3d t_v = Vector3d(M * v);
	center = Vector2d(t_v.x, t_v.y);
}

void PCEPosition::UpdateEdges(std::vector<PCEEdge>& edges_)
{
	for (auto edge : edge_indexes)
		edges_[edge].cut.Translate(t);
}


#ifndef __TADPOLE_HPP__
#define __TADPOLE_HPP__

#include "types.hpp"
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/depth_first_search.hpp>

// class dummy_visitor:public default_dfs_visitor
// {
//   // typedef typename property_traits < TimeMap >::value_type T;
// public:

//   dummy_visitor(int t)
//     :  m_time(t)
//   {
//   }

//   template < typename Vertex, typename Graph >
//     void discover_vertex(Vertex u, const Graph & g) const
//   {
//     std::cout << "Discovered vertex " << u << std::endl;
//     // put(m_dtimemap, u, m_time++);
//   }

//   template < typename Vertex, typename Graph >
//     void finish_vertex(Vertex u, const Graph & g) const
//   {
//     // put(m_ftimemap, u, m_time++);
//   }
//   int & m_time;
// };

namespace boost
{
  struct edge_component_t
  {
    enum
      { num = 555 };
    typedef edge_property_tag kind;
  }
    edge_component;
}

typedef boost::adjacency_list<multisetS, vecS, undirectedS,
                              property<vertex_name_t, int>,
                              property < edge_index_t, int, property<edge_component_t, int> >
                              > Graph_t;

typedef graph_traits<Graph_t>::edge_descriptor Edge;

//
// This type of graph when created contains information
// about full set of tadpole subgraph i.e. connected through
// the singular bridge(zero momentum) ")-O" or snail type ")O".
//
class TadpoleGraph
{
public:

  // Boolean flag "NotATadpole" set if negative edges present
  typedef std::vector<std::pair<std::vector<int>, bool> > SubgraphIds;

private:
  Graph_t g;

  size_t snails;
  std::vector<int> vnames;

  // List of one-loop tadpole edge numbers
  std::vector<size_t> self_edges;

  SubgraphIds subgraphs;

  // std::vector<size_t> count_loops;
  std::vector<std::set<int>> count_verts;

public:
  TadpoleGraph(const DiagramRecord& dr)
  {

    // First we add legs, connecting all legs on single vertex "0"

    property_map<Graph_t, edge_index_t>::type eim = get(edge_index, g);
    property_map<Graph_t, edge_component_t>::type ecm = get(edge_component, g);

    for(std::vector<Leg>::const_iterator li = dr.legs.begin(); li != dr.legs.end(); ++li)
      {

        // U - is negative, eternal
        // V - is positive, internal
        VD u,v;

        // We use single external vertex for all external legs
        if(vnames.size() == 0 )
          {
            u = add_vertex(IndexedGraph::vertex_property_type(0), g);
            vnames.push_back(0);
            assert(u == vnames.size() - 1);
          }

        std::vector<int>::iterator vit = std::find(vnames.begin(), vnames.end(), li->v);
        if( vit != vnames.end())
          {
            v = (vit - vnames.begin());
            std::cout << "v From vec " << v <<std::endl;
          }
        else
          {
            v = add_vertex(IndexedGraph::vertex_property_type(li->v), g);
            std::cout << "v To   vec " << v <<std::endl;
            vnames.push_back(li->v);
            assert(v == vnames.size() - 1);
          }

        Edge ee = add_edge (u, v, (-1)*(li->id), g).first;

        // Set to negative to mark parallel edges not included in map
        ecm[ee] = -1;

        std::cout << "Added edge with index " << eim[ee] << " and component " << ecm[ee] << std::endl;

        std::cout << "Leg " << li->type << " added" << std::endl;

      }

    for(std::vector<Prop>::const_iterator pi = dr.props.begin(); pi != dr.props.end(); ++pi)
      {
        VD u,v;
        std::vector<int>::iterator vit = std::find(vnames.begin(), vnames.end(), pi->u);

        // One-loop tadpoles not added to graph
        // They are stored in list "self_edges"
        if(pi->u == pi->v)
          self_edges.push_back(pi->id);
        // All other edges added normally
        else
          {
            if(vit  != vnames.end())
              {
                u = (vit - vnames.begin());

                std::cout << "u From vec " << u <<std::endl;
              }
            else
              {
                u = add_vertex(Graph_t::vertex_property_type(pi->u), g);

                std::cout << "u To   vec " << u <<std::endl;
                vnames.push_back(pi->u);
                assert(u == vnames.size() - 1);
              }

            vit = std::find(vnames.begin(), vnames.end(), pi->v);
            if( vit != vnames.end())
              {
                v = (vit - vnames.begin());
                std::cout << "v From vec " << v <<std::endl;
              }
            else
              {
                v = add_vertex(Graph_t::vertex_property_type(pi->v), g);
                std::cout << "v To   vec " << v <<std::endl;
                vnames.push_back(pi->v);
                assert(v == vnames.size() - 1);
              }

            Edge ee = add_edge (u, v, pi->id, g).first;

            // Set to negative to mark parallel edges not included in map
            ecm[ee] = -1;

            std::cout << "Edge " << pi->type << " added" << std::endl;
            // }
          }
      }

    // ==================================================================
    //
    //    Tadpole Graph prepared filling subgraph information
    //

    property_map < Graph_t, edge_component_t >::type component = get(edge_component, g);

    std::size_t num_comps = biconnected_components(g, component);

    subgraphs.resize(num_comps, std::pair<std::vector<int>, bool>(std::vector<int>(),false));
    // By default all graphs are tree
    // count_loops.resize(num_comps, 0);
    // std::vector<size_t> count_edges;

    count_verts.resize(num_comps);
    // count_edges.resize(num_comps, 0);

    std::cerr << "Found " << num_comps << " biconnected components.\n";

    graph_traits < Graph_t >::edge_iterator ei, ei_end;
    graph_traits < Graph_t >::out_edge_iterator pei, pei_end;

    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
      {
        std::cout << get(edge_index, g, *ei) << " ::: ";
        std::cout << (char)(source(*ei, g) + 'A') << " -- "
                  << (char)(target(*ei, g) + 'A');

        if (component[*ei] < 0) // Edge has parallel
          {
            int par_comp = -1;
            for (tie(pei, pei_end) = edge_range(source(*ei, g), target(*ei, g), g); pei != pei_end; ++pei)
              {
                std::cout << " par to (" << source(*pei, g) << "," << target(*pei, g) << ")" << get(edge_index, g, *pei) << std::endl;
                par_comp = std::max(par_comp,component[*pei]);
              }
            component[*ei] = par_comp;
          }
        std::cout << "[label=\"" << component[*ei] << "\"]\n";

        // If components are numerated from 0 to num_components
        // we add edges to [i-th] vector
        std::cout << "Edge id " << get(edge_index, g, *ei) << std::endl;
        subgraphs[component[*ei]].first.push_back(get(edge_index, g, *ei));

        count_verts[component[*ei]].insert(source(*ei, g));
        count_verts[component[*ei]].insert(target(*ei, g));

        // If external legs in subgraph it is not a tadpole
        if(get(edge_index, g, *ei) < 0 ) subgraphs[component[*ei]].second = true;

      }

    //
    //
    // ========================================================================================

    std::vector<VD> art_points;
    articulation_points(g, std::back_inserter(art_points));
    std::cerr << "Found " << art_points.size() << " articulation points.\n";

    snails = art_points.size();

    for (std::size_t i = 0; i < art_points.size(); ++i) {
      std::cout << (char)(art_points[i] + 'A')
                << " [ style=\"filled\", fillcolor=\"red\" ];"
                << std::endl;
    }

    // dummy_visitor vis(3);

    // depth_first_search(g, visitor(vis));

  }

  size_t numTadpoles() const
  {
    return snails;
  }

  const std::vector<size_t>& tad1l() const
  {
    return self_edges;
  }

  const SubgraphIds& getSubgraphs() const
  {
    return subgraphs;
  }

  const std::set<int>& verts(size_t i) const
  {
    return count_verts[i];
  }
};

#endif  // __TADPOLE_HPP__


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  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_ARCH_X86_SSE1_SIMD_FUNCTION_SUM_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE1_SIMD_FUNCTION_SUM_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/splatted.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( sum_
                          , (typename A0)
                          , bs::sse1_
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE float operator() ( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      // We optimize this by saving a movaps and 2-3 uops by using movehl instead of second shuffle
      __m128  shuf  = _mm_shuffle_ps(a0, a0, _MM_SHUFFLE(2, 3, 0, 1));
      __m128  sums  = _mm_add_ps(a0, shuf);

      return _mm_cvtss_f32(_mm_add_ss(sums, _mm_movehl_ps(shuf, sums)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sum_
                          , (typename A0)
                          , bs::sse1_
                          , bs::splatted_tag
                          , bs::pack_<bd::arithmetic_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(bs::splatted_tag const&, const A0 & a0 ) const BOOST_NOEXCEPT
    {
      return A0(sum(a0));
    }
  };
} } }

#endif


#pragma once

#include <descartes_light/test/utils.h>

#include <descartes_light/descartes_macros.h>
DESCARTES_IGNORE_WARNINGS_PUSH
#include <Eigen/Geometry>
#include <memory>
#include <numeric>
#include <random>
#include <vector>
DESCARTES_IGNORE_WARNINGS_POP

namespace descartes_light
{
template <typename FloatType>
typename State<FloatType>::Ptr generateRandomState(Eigen::Index dof, std::mt19937& rand_gen)
{
  std::normal_distribution<FloatType> dist;
  Eigen::Matrix<FloatType, Eigen::Dynamic, 1> sol(dof);
  for (Eigen::Index i = 0; i < dof; ++i)
    sol(i) = dist(rand_gen);

  return std::make_shared<State<FloatType>>(sol);
}

template <typename FloatType>
RandomStateSampler<FloatType>::RandomStateSampler(const Eigen::Index dof,
                                                  const std::size_t n_samples,
                                                  const FloatType state_cost,
                                                  const std::size_t zero_state_idx,
                                                  std::shared_ptr<std::mt19937> rand_gen)
  : dof_(dof)
  , n_samples_(n_samples)
  , state_cost_(state_cost)
  , zero_state_idx_(zero_state_idx)
  , rand_gen_(std::move(rand_gen))
{
}

template <typename FloatType>
typename std::vector<StateSample<FloatType>> RandomStateSampler<FloatType>::sample() const
{
  // Generate some random joint states
  typename std::vector<StateSample<FloatType>> waypoints;
  waypoints.reserve(n_samples_);
  std::generate_n(std::back_inserter(waypoints), n_samples_, [this]() {
    return StateSample<FloatType>{ generateRandomState<FloatType>(dof_, *rand_gen_), state_cost_ };
  });

  // Set one of the joint states to all zeros
  waypoints.at(zero_state_idx_) = StateSample<FloatType>{
    std::make_shared<State<FloatType>>(Eigen::Matrix<FloatType, Eigen::Dynamic, 1>::Zero(this->dof_)), state_cost_
  };

  return waypoints;
}

template <typename FloatType>
NaiveEdgeEvaluator<FloatType>::NaiveEdgeEvaluator(const bool valid) : valid_(valid)
{
}

template <typename FloatType>
std::pair<bool, FloatType> NaiveEdgeEvaluator<FloatType>::evaluate(const State<FloatType>&,
                                                                   const State<FloatType>&) const
{
  return std::make_pair(valid_, 0.0);
}

template <typename FloatType>
NaiveStateEvaluator<FloatType>::NaiveStateEvaluator(const bool valid, const FloatType cost) : valid_(valid), cost_(cost)
{
}

template <typename FloatType>
std::pair<bool, FloatType> NaiveStateEvaluator<FloatType>::evaluate(const State<FloatType>&) const
{
  return std::make_pair(valid_, cost_);
}

template <class FloatType>
std::vector<typename WaypointSampler<FloatType>::ConstPtr>
createSamplers(std::size_t dof, std::size_t n_waypoints, std::size_t samples_per_waypoint, FloatType state_cost)
{
  std::vector<typename WaypointSampler<FloatType>::ConstPtr> samplers;
  samplers.reserve(n_waypoints);

  auto rand_gen = std::make_shared<std::mt19937>(0);
  std::uniform_int_distribution<std::size_t> dist(0, samples_per_waypoint - 1);

  // Create waypoint samplers
  for (std::size_t i = 0; i < n_waypoints; ++i)
  {
    const std::size_t zero_state_idx = dist(*rand_gen);
    samplers.push_back(std::make_shared<RandomStateSampler<FloatType>>(
        dof, samples_per_waypoint, state_cost, zero_state_idx, rand_gen));
  }

  return samplers;
}

}  // namespace descartes_light


//==================================================================================================
/*!
    @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_ARCH_X86_SSE2_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/gt.hpp>
#include <boost/simd/function/simd/if_add.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/if_one_else_zero.hpp>
#include <boost/simd/function/simd/is_gez.hpp>
#include <boost/simd/function/simd/is_gtz.hpp>
#include <boost/simd/function/simd/ge.hpp>
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/simd/function/simd/lt.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/shift_right.hpp>
#include <boost/simd/function/simd/tofloat.hpp>
#include <boost/simd/function/simd/toint.hpp>
#include <boost/simd/function/simd/touint.hpp>
#include <boost/simd/constant/four.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/assert_utils.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
       return _mm_sqrt_pd(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
// TODO     BOOST_ASSERT_MSG(assert_all(is_gez(a0)), "sqrt input is negative");
      using uint_type = bd::as_integer_t<A0,unsigned>;
      return simd::bitwise_cast<A0>(sqrt( simd::bitwise_cast<uint_type>(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
//  TODO     BOOST_ASSERT_MSG(assert_all(is_gez(a0)), "sqrt input is negative");
      return bs::toint(bs::sqrt(bs::tofloat(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na  = is_nez(a0);
      A0 n   = plus(shift_right(a0, 4), Four<A0>());
      A0 n1  = shift_right(n+a0/n, 1);

      auto ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n  = if_add( gt(n*n,a0), n, Mone<A0>());
      return n+if_one_else_zero(na);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na = is_nez(a0);
      A0 const  z1 = plus(shift_right(a0, 6), Ratio<A0, 16>());
      A0 const  z2 = plus(shift_right(a0,10), Ratio<A0, 256>());
      A0 const  C1 = Ratio<A0, 31679>();
      // choose a proper starting point for approximation
      A0 n  = if_else(lt(a0, C1), z1, z2);
      auto ok =  is_gtz(n);
      n  = if_else(ok, n, One<A0>());

      A0 n1 = if_else(ok, shift_right(n+a0/n, 1), One<A0>());

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);
      n  = if_add( gt(n*n,a0), n, Mone<A0>());

     return if_add(na, Zero<A0>(), n);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na = is_nez(a0);
      A0 const z1 = plus(shift_right(a0, 6),    Ratio<A0,16>());
      A0 const z2 = plus(shift_right(a0,10),   Ratio<A0,256>());
      A0 const z3 = plus(shift_right(a0,13),  Ratio<A0,2048>());
      A0 const z4 = plus(shift_right(a0,16), Ratio<A0,16384>());
      A0 n  = if_else( gt(a0, Ratio<A0,177155824>())
                  , z4
                  , if_else( gt(a0, Ratio<A0,4084387>())
                        , z3
                        , if_else( gt(a0, Ratio<A0,31679>())
                                , z2
                                , z1
                                )
                        )
                  );
      auto ok =  is_gtz(n);
      n = if_else(ok, n, One<A0>());
      A0 n1 = if_else(ok, shift_right(n+a0/n, 1), One<A0>());

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);

      A0 tmp = minus(n*minus(n, One<A0>()), One<A0>());
      n  = if_add( ge(tmp+n,a0), n, Mone<A0>());
      n =  if_add(na, Zero<A0>(), n);

      return n;
     }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return bs::touint(bs::sqrt(bs::tofloat(a0)));
    }
  };

} } }

#endif


#pragma once

#include <string>
#include <boost/graph/adjacency_list.hpp>
#include "Node.hpp"
#include "Edge.hpp"

class GraphData {
  public:
  std::string name;
};

typedef boost::adjacency_list<boost::setS,
                              boost::vecS,
                              boost::bidirectionalS,
                              Node,
                              Edge,
                              GraphData>
    DiGraph;


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

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

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_ASSIGN_PARENTS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_ASSIGN_PARENTS_HPP

#include <boost/range.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/detail/partition.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_ring.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/geometries/box.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace overlay
{

template
<
    typename Item,
    typename Geometry1, typename Geometry2,
    typename RingCollection
>
static inline bool within_selected_input(Item const& item2, ring_identifier const& ring_id,
        Geometry1 const& geometry1, Geometry2 const& geometry2,
        RingCollection const& collection)
{
    typedef typename geometry::tag<Geometry1>::type tag1;
    typedef typename geometry::tag<Geometry2>::type tag2;

    switch (ring_id.source_index)
    {
        case 0 :
            return geometry::within(item2.point,
                get_ring<tag1>::apply(ring_id, geometry1));
            break;
        case 1 :
            return geometry::within(item2.point,
                get_ring<tag2>::apply(ring_id, geometry2));
            break;
        case 2 :
            return geometry::within(item2.point,
                get_ring<void>::apply(ring_id, collection));
            break;
    }
    return false;
}

template <typename Point>
struct ring_info_helper
{
    typedef typename geometry::default_area_result<Point>::type area_type;

    ring_identifier id;
    area_type real_area;
    area_type abs_area;
    model::box<Point> envelope;

    inline ring_info_helper()
        : real_area(0), abs_area(0)
    {}

    inline ring_info_helper(ring_identifier i, area_type a)
        : id(i), real_area(a), abs_area(geometry::math::abs(a))
    {}
};

struct ring_info_helper_get_box
{
    template <typename Box, typename InputItem>
    static inline void apply(Box& total, InputItem const& item)
    {
        geometry::expand(total, item.envelope);
    }
};

struct ring_info_helper_ovelaps_box
{
    template <typename Box, typename InputItem>
    static inline bool apply(Box const& box, InputItem const& item)
    {
        return ! geometry::detail::disjoint::disjoint_box_box(box, item.envelope);
    }
};

template <typename Geometry1, typename Geometry2, typename Collection, typename RingMap>
struct assign_visitor
{
    typedef typename RingMap::mapped_type ring_info_type;

    Geometry1 const& m_geometry1;
    Geometry2 const& m_geometry2;
    Collection const& m_collection;
    RingMap& m_ring_map;
    bool m_check_for_orientation;

    inline assign_visitor(Geometry1 const& g1, Geometry2 const& g2, Collection const& c,
                RingMap& map, bool check)
        : m_geometry1(g1)
        , m_geometry2(g2)
        , m_collection(c)
        , m_ring_map(map)
        , m_check_for_orientation(check)
    {}

    template <typename Item>
    inline void apply(Item const& outer, Item const& inner, bool first = true)
    {
        if (first && outer.abs_area < inner.abs_area)
        {
            // Apply with reversed arguments
            apply(inner, outer, false);
            return;
        }

        if (m_check_for_orientation
         || (math::larger(outer.real_area, 0)
          && math::smaller(inner.real_area, 0)))
        {
            ring_info_type& inner_in_map = m_ring_map[inner.id];

            if (geometry::within(inner_in_map.point, outer.envelope)
               && within_selected_input(inner_in_map, outer.id, m_geometry1, m_geometry2, m_collection)
               )
            {
                // Assign a parent if there was no earlier parent, or the newly
                // found parent is smaller than the previous one
                if (inner_in_map.parent.source_index == -1
                    || outer.abs_area < inner_in_map.parent_area)
                {
                    inner_in_map.parent = outer.id;
                    inner_in_map.parent_area = outer.abs_area;
                }
            }
        }
    }
};

template
<
    typename Geometry1, typename Geometry2,
    typename RingCollection,
    typename RingMap
>
inline void assign_parents(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            RingCollection const& collection,
            RingMap& ring_map,
            bool check_for_orientation = false)
{
    typedef typename geometry::tag<Geometry1>::type tag1;
    typedef typename geometry::tag<Geometry2>::type tag2;

    typedef typename RingMap::mapped_type ring_info_type;
    typedef typename ring_info_type::point_type point_type;
    typedef model::box<point_type> box_type;

    typedef typename RingMap::iterator map_iterator_type;

    {
        typedef ring_info_helper<point_type> helper;
        typedef std::vector<helper> vector_type;
        typedef typename boost::range_iterator<vector_type const>::type vector_iterator_type;

        std::size_t count_total = ring_map.size();
        std::size_t count_positive = 0;
        std::size_t index_positive = 0; // only used if count_positive>0
        std::size_t index = 0;

        // Copy to vector (with new approach this might be obsolete as well, using the map directly)
        vector_type vector(count_total);

        for (map_iterator_type it = boost::begin(ring_map);
            it != boost::end(ring_map); ++it, ++index)
        {
            vector[index] = helper(it->first, it->second.get_area());
            helper& item = vector[index];
            switch(it->first.source_index)
            {
                case 0 :
                    geometry::envelope(get_ring<tag1>::apply(it->first, geometry1),
                            item.envelope);
                    break;
                case 1 :
                    geometry::envelope(get_ring<tag2>::apply(it->first, geometry2),
                            item.envelope);
                    break;
                case 2 :
                    geometry::envelope(get_ring<void>::apply(it->first, collection),
                            item.envelope);
                    break;
            }
            if (item.real_area > 0)
            {
                count_positive++;
                index_positive = index;
            }
        }

        if (! check_for_orientation)
        {
            if (count_positive == count_total)
            {
                // Optimization for only positive rings
                // -> no assignment of parents or reversal necessary, ready here.
                return;
            }

            if (count_positive == 1)
            {
                // Optimization for one outer ring
                // -> assign this as parent to all others (all interior rings)
                // In unions, this is probably the most occuring case and gives
                //    a dramatic improvement (factor 5 for star_comb testcase)
                ring_identifier id_of_positive = vector[index_positive].id;
                ring_info_type& outer = ring_map[id_of_positive];
                index = 0;
                for (vector_iterator_type it = boost::begin(vector);
                    it != boost::end(vector); ++it, ++index)
                {
                    if (index != index_positive)
                    {
                        ring_info_type& inner = ring_map[it->id];
                        inner.parent = id_of_positive;
                        outer.children.push_back(it->id);
                    }
                }
                return;
            }
        }

        assign_visitor
            <
                Geometry1, Geometry2,
                RingCollection, RingMap
            > visitor(geometry1, geometry2, collection, ring_map, check_for_orientation);

        geometry::partition
            <
                box_type
            >::apply(vector, visitor, ring_info_helper_get_box(),
                     ring_info_helper_ovelaps_box());
    }

    if (check_for_orientation)
    {
        for (map_iterator_type it = boost::begin(ring_map);
            it != boost::end(ring_map); ++it)
        {
            if (geometry::math::equals(it->second.get_area(), 0))
            {
                it->second.discarded = true;
            }
            else if (it->second.parent.source_index >= 0
                    && math::larger(it->second.get_area(), 0))
            {
                const ring_info_type& parent = ring_map[it->second.parent];

                if (math::larger(parent.area, 0))
                {
                    // Discard positive inner ring with positive parent
                    it->second.discarded = true;
                }
                // Remove parent ID from any positive inner ring
                it->second.parent.source_index = -1;
            }
            else if (it->second.parent.source_index < 0
                    && math::smaller(it->second.get_area(), 0))
            {
                // Reverse negative ring without parent
                it->second.reversed = true;
            }
        }
    }

    // Assign childlist
    for (map_iterator_type it = boost::begin(ring_map);
        it != boost::end(ring_map); ++it)
    {
        if (it->second.parent.source_index >= 0)
        {
            ring_map[it->second.parent].children.push_back(it->first);
        }
    }
}

// Version for one geometry (called by buffer)
template
<
    typename Geometry,
    typename RingCollection,
    typename RingMap
>
inline void assign_parents(Geometry const& geometry,
            RingCollection const& collection,
            RingMap& ring_map,
            bool check_for_orientation)
{
    // Call it with an empty geometry as second geometry (source_id == 1)
    // (ring_map should be empty for source_id==1)

    Geometry empty;
    assign_parents(geometry, empty, collection, ring_map, check_for_orientation);
}

}} // namespace detail::overlay
#endif // DOXYGEN_NO_DETAIL

}} // namespace geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_ASSIGN_PARENTS_HPP