#ifndef FSTMODELMULTINOM_H
#define FSTMODELMULTINOM_H

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

   \file    model_multinom.hpp
   \brief   Implements multinomial model
   \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 <limits>
#include <cmath>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "model.hpp"
#include "indexed_vector.hpp"
#include "indexed_matrix.hpp"

/*============== 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 {

//! Implements multinomial model
//
// \note only non-negative integers assumed as data
template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Model_Multinomial : public Model<SUBSET,DATAACCESSOR> {
public:
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Model_Multinomial() : _classes(0), _n_max(0), _n(0), _d(0), _allpatterns(0), _doc_avg_length(0) {notify("Model_Multinomial constructor");}
	Model_Multinomial(const Model_Multinomial& mm); // copy-constructor
	virtual ~Model_Multinomial() {notify("Model_Multinomial destructor");}

	virtual void learn(PDataAccessor da); // learn model of full dimensionality
	virtual void learn(PDataAccessor da, const PSubset sub); // learn model of lower dimensionality

	void narrow_to(const PSubset sub); // fill _index[] -> access to data is then mapped to sub-matrix
	void denarrow(); // reset _index[] -> access is reset to the full matrix

	void compute_theta();
	void compute_MI(); // Mutual Information of individual features
	void compute_IB(); // Individual Bhattacharyya of individual features

	DIMTYPE get_n_max() const {return _n_max;}
	DIMTYPE get_n() const {return _n;}
	DIMTYPE get_d() const {return _d;}
	DIMTYPE get_classes() const {return _classes;}

	REALTYPE get_doc_avg_length() const {return _doc_avg_length;}
	// NOTE: caller must ensure any access takes place only during _theta, _IB, _MI and _Pc existence
	REALTYPE get_Pc(const DIMTYPE c) const {assert(c>=0 && c<_classes); return _Pc[c];}
protected:
	template<class, class, class, class, class, class, class> friend class Criterion_Multinomial_Bhattacharyya;
	template<class, class, class, class, class, class, class> friend class Classifier_Multinomial_NaiveBayes;
	const boost::scoped_array<REALTYPE>& get_theta() const {return _theta;}
	const boost::scoped_array<REALTYPE>& get_IB() const {return _IB;}
	const boost::scoped_array<REALTYPE>& get_MI() const {return _MI;}
protected:
	DIMTYPE _classes; // initialized in learn(), WARNING: int sufficient ? note expressions like _classes*_n*_n
	// NOTE: matrix size is defined by:
	//       _n_max - hard constant representing maximum allocated space of size _n_max*_n_max
	//       _n     - "full feature set" dimensionality, _n<=_n_max. If Indexed_Matrix servers as output buffer
	//                to store matrices of various (reduced) dimensionalites, this is needed for correct functionality
	//                of at(), at_raw() etc. (both "raw" and "non-raw" matrix item access methods)
	//       _d     - "virtual subset" dimensionality, _d<=_n. Assuming the matrix holds _n*_n values (_n consecutively stored
	//                rows of _n values each), a virtual sub-matrix can be defined and accessed using at() ("non-raw" methods)
	DIMTYPE _n_max;
	DIMTYPE _n; // initialized in learn(), WARNING: int sufficient ? note expressions like _classes*_n*_n
	void compute_Nsuminclass(PDataAccessor da);
	boost::scoped_array<DATATYPE> _Nsuminclass;
	boost::scoped_array<REALTYPE> _theta;// actually P(term|class)
	boost::scoped_array<REALTYPE> _Pc;
	boost::scoped_array<REALTYPE> _Pc_d; // for compute_MI
	boost::scoped_array<REALTYPE> _Pv;
	boost::scoped_array<REALTYPE> _MI;
	boost::scoped_array<REALTYPE> _IB;

	DIMTYPE _d;
	// NOTE: _index defines a sub-space of subs-space defined in _learn_index
	boost::scoped_array<DIMTYPE> _index; //!< maps feature subset indexes to raw (full set) feature indexes, used for narrow()ing

	IDXTYPE _allpatterns; // initialized in learn()
	REALTYPE _doc_avg_length; // initialized in compute__theta(), needed in compute_IB()

	// NOTE: _learn_index here is not used for narrow()ing, but for learn(da,sub)
	boost::scoped_array<DIMTYPE> _learn_index; // maps virtual sub-space _n dimensions to original _n_max dimensions
};

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Model_Multinomial(const Model_Multinomial& mm) : // copy-constructor
	Model<SUBSET,DATAACCESSOR>(mm),
	_classes(mm._classes),
	_n_max(mm._n_max),
	_n(mm._n),
	_d(mm._d),
	_allpatterns(mm._allpatterns),
	_doc_avg_length(mm._doc_avg_length)
{
	notify("Model_Multinomial copy-constructor.");
	if(_classes>0) {
		_Pc.reset(new REALTYPE[_classes]); memcpy((void *)_Pc.get(),(void *)(mm._Pc).get(),sizeof(REALTYPE)*_classes);
		_Pc_d.reset(new REALTYPE[_classes]); memcpy((void *)_Pc_d.get(),(void *)(mm._Pc_d).get(),sizeof(REALTYPE)*_classes);
		if(_n_max>0) {
			_Nsuminclass.reset(new DATATYPE[_n_max*_classes]); memcpy((void *)_Nsuminclass.get(),(void *)(mm._Nsuminclass).get(),sizeof(DATATYPE)*_n_max*_classes);
			_theta.reset(new REALTYPE[_n_max*_classes]); memcpy((void *)_theta.get(),(void *)(mm._theta).get(),sizeof(REALTYPE)*_n_max*_classes);
		}
	}
	if(_n_max>0)
	{
		_Pv.reset(new REALTYPE[_n_max]); memcpy((void *)_Pv.get(),(void *)(mm._Pv).get(),sizeof(REALTYPE)*_n_max);
		_MI.reset(new REALTYPE[_n_max]); memcpy((void *)_MI.get(),(void *)(mm._MI).get(),sizeof(REALTYPE)*_n_max);
		_IB.reset(new REALTYPE[_n_max]); memcpy((void *)_IB.get(),(void *)(mm._IB).get(),sizeof(REALTYPE)*_n_max);
		// to store subsubspace info of the (possibly subspace-learned) learned model
		_index.reset(new DIMTYPE[_n_max]); memcpy((void *)_index.get(),(void *)(mm._index).get(),sizeof(DIMTYPE)*_n_max);
		// to store subspace info for the to-be-learned model
		_learn_index.reset(new DIMTYPE[_n_max]); memcpy((void *)_learn_index.get(),(void *)(mm._learn_index).get(),sizeof(DIMTYPE)*_n_max);
	}
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::learn(PDataAccessor da)
{
	assert(da);
	assert(da->getNoOfClasses()>0);
	assert(da->getNoOfFeatures()>0);
	PSubset fullset(new SUBSET(da->getNoOfFeatures()));
	fullset->select_all();
#ifdef DEBUG
	{std::ostringstream sos; sos << "fullset = "<< *fullset << std::endl; syncout::print(std::cout,sos);}
#endif
	learn(da,fullset);
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::learn(PDataAccessor da, const PSubset sub)
{
#ifdef DEBUG
	{std::ostringstream sos; sos <<"Model_Multinomial::learn():"<<std::endl; syncout::print(std::cout,sos);}
#endif
	assert(sub);
	assert(da->getNoOfClasses()>0);
	assert(da->getNoOfFeatures()>0);
	assert(sub->get_n_raw()==da->getNoOfFeatures());

	if(_classes!=da->getNoOfClasses() || sub->get_n_raw()>_n_max) // insufficient buffers - reset constants and reallocate
	{
		_classes=da->getNoOfClasses();
		_n_max=sub->get_n_raw();

		_learn_index.reset(new DIMTYPE[_n_max]); // to store subspace info about the to-be-learned model
		_index.reset(new DIMTYPE[_n_max]); for(DIMTYPE f=0;f<_n_max;f++) _index[f]=f; // to store subsubspace info of the (possibly subspace-learned) learned model
		_Nsuminclass.reset(new DATATYPE[_n_max*_classes]);
		_theta.reset(new REALTYPE[_n_max*_classes]);
		_Pv.reset(new REALTYPE[_n_max]);
		_MI.reset(new REALTYPE[_n_max]);
		_IB.reset(new REALTYPE[_n_max]);
		_Pc.reset(new REALTYPE[_classes]);
		_Pc_d.reset(new REALTYPE[_classes]);
		// NOTE: class priors are estimated from complete data available through da.
		//       Note that when only parts of data are processed (i.e., using data splits), the
		//       estimates below would possibly not precisely depict the relative frequency of documents
		//       in stratified class parts, esp. if class sizes are small
		IDXTYPE _classsizesum=da->getClassSizeSum();
		assert(_classsizesum>0);
		if(_classsizesum==0) throw FST::fst_error("Model_Multinomial::learn: zero _classsizesum");
		for(DIMTYPE c=0;c<_classes;c++) _Pc[c]=(REALTYPE)da->getClassSize(c)/(REALTYPE)_classsizesum;
	}
	_n=sub->get_d_raw(); // actual to-be-learned model dimensionality is to be the one defined by the subset defined in sub
	_d=_n; // access to learned model is initially not narrow()ed
	DIMTYPE f, fi=0;
	for(bool b=sub->getFirstFeature(f);b==true;b=sub->getNextFeature(f)) {_learn_index[fi++]=f;}
	assert(fi==_n);

#ifdef DEBUG
	{
		std::ostringstream sos; sos << "_Pc:"<<std::endl;
		for(DIMTYPE c=0;c<_classes;c++) sos << _Pc[c] << " "; sos << std::endl;
		syncout::print(std::cout,sos);
	}
#endif
	compute_Nsuminclass(da); // sub info passed in form of _learn_index

	DATATYPE total_sum_length=0;
	DIMTYPE wCV=0;
	for(DIMTYPE c=0;c<_classes;c++) {
		for(DIMTYPE f=0;f<_n;f++) total_sum_length+=_Nsuminclass[wCV+f];
		wCV+=_n;
	}
	if(total_sum_length==0) {
		for(DIMTYPE c=0;c<_classes;c++) _Pc_d[c]=1.0/(REALTYPE)_classes; // workaround to prevent division by zero below
	} else {
		wCV=0;
		for(DIMTYPE c=0;c<_classes;c++) {
			DATATYPE class_sum_length=0;
			for(DIMTYPE f=0;f<_n;f++) class_sum_length+=_Nsuminclass[wCV+f]; // in subset case this is done only for selected features
			_Pc_d[c]=(REALTYPE)class_sum_length/(REALTYPE)total_sum_length;
			wCV+=_n;
		}
	}
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::compute_Nsuminclass(PDataAccessor da)
{
	assert(_learn_index);
	assert(_Nsuminclass);
	for(DIMTYPE i=0;i<_classes*_n;i++) _Nsuminclass[i]=0;
	typename DATAACCESSOR::PPattern p;
	IDXTYPE cnt;
	assert(da->getSplitIndex()>0); //
	DIMTYPE wCV=0;
	DIMTYPE _features=da->getNoOfFeatures();
	_allpatterns=0;
	for(DIMTYPE c=0;c<_classes;c++) {
		da->setClass(c);
		for(bool b=da->getFirstBlock(TRAIN,p,cnt);b!=false;b=da->getNextBlock(TRAIN,p,cnt)) {
			for(IDXTYPE i=0;i<cnt;i++) {
				for(DIMTYPE f=0;f<_n;f++) {_Nsuminclass[wCV+f]+=p[i*_features+_learn_index[f]];}
			}
			_allpatterns+=cnt;
		}
		wCV+=_n;
	}
#ifdef DEBUG
	{
		std::ostringstream sos; sos <<"_Nsuminclass:"<<std::endl;
		for(DIMTYPE c=0;c<_classes;c++) {for(DIMTYPE f=0;f<_n;f++) sos << _Nsuminclass[c*_n+f] << " "; sos << std::endl;}
		syncout::print(std::cout,sos);
	}
#endif
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::compute_theta()
{
	assert(_classes>0);
	assert(_n>0);
	assert(_d>0 || _d<=_n);
	assert(_index);
	assert(_allpatterns>0);
	assert(_Nsuminclass);
	assert(_theta);
	DATATYPE total_sum_length=0;
	DIMTYPE wCV=0, wCd=0;
	for(DIMTYPE c=0;c<_classes;c++) {
		DATATYPE class_sum_length=0;
		for(DIMTYPE f=0;f<_d;f++) class_sum_length+=_Nsuminclass[wCV+_index[f]]; // in subset case this is done only for selected features
		total_sum_length+=class_sum_length;
		for(DIMTYPE f=0;f<_d;f++) { // in subset case this is done only for selected features
			_theta[wCd++]=(1.0+(REALTYPE)_Nsuminclass[wCV+_index[f]])/((REALTYPE)_d+(REALTYPE)class_sum_length); // in subset case substitute d (subset size) for _n
		}
		wCV+=_n;
	}
	if(_allpatterns==0) throw FST::fst_error("Model_Multinomial::compute_theta: division by zero _allpatterns");
	_doc_avg_length=(REALTYPE)total_sum_length/(REALTYPE)_allpatterns;
#ifdef DEBUG
	{
		std::ostringstream sos; sos <<"_theta:"<<std::endl;
		for(DIMTYPE c=0;c<_classes;c++) {
			REALTYPE tmp=0.0;
			for(DIMTYPE f=0;f<_d;f++) {tmp+=_theta[c*_d+f]; sos << _theta[c*_d+f] << " "; sos << std::endl;}
			sos << "theta sum over class<"<<c<<">: "<<tmp<<std::endl;
		}
		sos << "total_sum_length="<<total_sum_length<<", doc_avg_length="<<_doc_avg_length<<std::endl;
		syncout::print(std::cout,sos);
	}
#endif
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::narrow_to(const PSubset sub)
{
	assert(_index);
	assert(sub);
	assert(sub->get_n_raw()==_n); // NOTE: sub should represent subspace of the learned subspace ! not of the full space
	assert(sub->get_d_raw()>0);
	DIMTYPE f;
	_d=0;
	for(bool b=sub->getFirstFeature(f);b!=false;b=sub->getNextFeature(f)) _index[_d++]=f;
	assert(_d>0 && _d<=_n);
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::denarrow()
{
	assert(_index);
	for(DIMTYPE i=0;i<_n;i++) _index[i]=i;
	_d=_n;
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::compute_MI()
{
	// NOTE: needs _theta computed for full feature set
	assert(_classes>0);
	assert(_n>0);
	assert(_Nsuminclass);
	assert(_theta);
	assert(_Pc);
	assert(_Pv);
	assert(_MI);
	for(DIMTYPE f=0;f<_n;f++) _MI[f]=0.0;
	DIMTYPE wCV=0;
	DATATYPE sum=0;
	for(DIMTYPE c=0;c<_classes;c++) {
		for(DIMTYPE f=0;f<_n;f++) {
			_MI[f]+=_Nsuminclass[wCV+f];
			sum+=_Nsuminclass[wCV+f];
		}
		wCV+=_n;
	}
	for(DIMTYPE f=0;f<_n;f++) {if(sum>0.0) _Pv[f]=(_MI[f])/((REALTYPE)sum); else _Pv[f]=0.0;}

	for(DIMTYPE f=0;f<_n;f++) {
		_MI[f]=0.0;
		if(_Pv[f]>0.0) {
			wCV=0;
			for(DIMTYPE c=0;c<_classes;c++) {
				//_MI[f]+=_theta[wCV+f]*_Pc_d[c]*fabs( (REALTYPE)log(_theta[wCV+f]/_Pv[f]) ); // IG abs
				//_MI[f]+=_theta[wCV+f]*_Pc_d[c]*(REALTYPE)log(_theta[wCV+f]/_Pv[f]); // IG
				//_MI[f]+=_Pc_d[c]*fabs( (REALTYPE)log(_theta[wCV+f]/_Pv[f]) ); // MI abs
				_MI[f]+=_Pc_d[c]*(REALTYPE)log(_theta[wCV+f]/_Pv[f]); // MI
				wCV+=_n;
			}
		}
	}
#ifdef DEBUG
	{
		std::ostringstream sos; sos <<"_MI:"<<std::endl;
		for(DIMTYPE f=0;f<_n;f++) sos << _MI[f] << " "; sos << std::endl;
		syncout::print(std::cout,sos);
	}
#endif
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::compute_IB()
{
	// NOTE: needs _theta computed for full feature set
	assert(_classes>0);
	assert(_n>0);
	assert(_doc_avg_length>0);
	assert(_theta);
	assert(_Pc);
	assert(_IB);
	for(DIMTYPE f=0;f<_n;f++) _IB[f]=0.0;
	DIMTYPE wCV1,wCV2;
	DIMTYPE combs;
	REALTYPE _thetasum, value;
	for(DIMTYPE f=0;f<_n;f++) {
		value=0.0; combs=0;
		wCV1=0;
		for(DIMTYPE c1=0;c1<_classes;c1++) {
			wCV2=wCV1+_n;
			for(DIMTYPE c2=c1+1;c2<_classes;c2++) {
				_thetasum=sqrt(_theta[wCV1+f]*_theta[wCV2+f])+sqrt((1.0-_theta[wCV1+f])*(1.0-_theta[wCV2+f]));
#ifdef DEBUG
				{std::ostringstream sos; sos << _thetasum<< std::endl; syncout::print(std::cout,sos);}
#endif
				value+=((-_doc_avg_length)*(REALTYPE)log(_thetasum))*_Pc_d[c1]*_Pc_d[c2];
				combs++;
				wCV2+=_n;
			}
			wCV1+=_n;
		}
		assert(combs>0);
		if(combs==0) throw FST::fst_error("Model_Multinomial::compute_IB: division by zero combs");
		_IB[f]=value/(REALTYPE)combs;
	}
#ifdef DEBUG
	{
		std::ostringstream sos; sos <<"_IB:"<<std::endl;
		for(DIMTYPE f=0;f<_n;f++) sos << _IB[f] << " "; sos << std::endl;
		syncout::print(std::cout,sos);
	}
#endif
}

} // namespace
#endif // FSTMODELMULTINOM_H ///:~


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_BLOCK_CIPHERS_DETAIL_MD4_POLICY_HPP
#define CRYPTO3_BLOCK_CIPHERS_DETAIL_MD4_POLICY_HPP

#include <array>

#include <boost/crypto3/block/detail/md4/md4_functions.hpp>

namespace boost {
    namespace crypto3 {
        namespace block {
            namespace detail {
                struct md4_policy : md4_functions {

                    constexpr static const std::size_t block_bits = 128;
                    constexpr static const std::size_t block_words = block_bits / word_bits;
                    typedef std::array<word_type, block_words> block_type;

                    constexpr static const std::size_t key_words = 16;
                    constexpr static const std::size_t key_bits = key_words * word_bits;
                    typedef std::array<word_type, key_words> key_type;

                    constexpr static const std::size_t rounds = 48;
                    typedef std::array<unsigned, rounds> key_indexes_type;

                    constexpr static const key_indexes_type key_indexes = {{
                        0, 1, 2, 3,  4, 5,  6, 7,  8, 9, 10, 11, 12, 13, 14, 15,

                        0, 4, 8, 12, 1, 5,  9, 13, 2, 6, 10, 14, 3,  7,  11, 15,

                        0, 8, 4, 12, 2, 10, 6, 14, 1, 9, 5,  13, 3,  11, 7,  15,
                    }};
                };

                constexpr md4_policy::key_indexes_type const md4_policy::key_indexes;
            }    // namespace detail
        }        // namespace block
    }            // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_CIPHERS_DETAIL_MD4_POLICY_HPP


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_DetailedWHIncoherentPhotonScatteringDistribution.hpp
//! \author Alex Robinson
//! \brief  The detailed Waller-Hartree incoherent photon scat. dist. decl.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_DETAILED_WH_INCOHERENT_PHOTON_SCATTERING_DISTRIBUTION_HPP
#define MONTE_CARLO_DETAILED_WH_INCOHERENT_PHOTON_SCATTERING_DISTRIBUTION_HPP

// Boost Includes
#include <boost/scoped_ptr.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>

// FRENSIE Includes
#include "MonteCarlo_WHIncoherentPhotonScatteringDistribution.hpp"

namespace MonteCarlo{

/*! The detailed Waller-Hartree incoherent photon scattering distribution class
 * \details This class will sample an interaction subshell for atomic
 * relaxation eventhough the Waller-Hartree model doesn't directly provide
 * that information. The subshell occupancies are used instead of the
 * more accurate subshell incoherent cross sections.
 */
class DetailedWHIncoherentPhotonScatteringDistribution : public WHIncoherentPhotonScatteringDistribution
{

public:

  //! Constructor
  DetailedWHIncoherentPhotonScatteringDistribution(
      const Teuchos::RCP<const Utility::OneDDistribution>& scattering_function,
      const Teuchos::Array<double>& subshell_occupancies,
      const Teuchos::Array<SubshellType>& subshell_order,
      const double kahn_sampling_cutoff_energy = 3.0 );

  //! Destructor
  ~DetailedWHIncoherentPhotonScatteringDistribution()
  { /* ... */ }

  //! Randomly scatter the photon and return the shell that was interacted with
  void scatterPhoton( PhotonState& photon,
		      ParticleBank& bank,
		      SubshellType& shell_of_interaction ) const;

private:

  //! Sample the subshell that is interacted with
  void sampleInteractionSubshell( SubshellType& shell_of_interaction ) const;

  // The shell interaction probabilities
  boost::scoped_ptr<const Utility::TabularOneDDistribution>
  d_subshell_occupancy_distribution;

  // The subshell ordering
  Teuchos::Array<SubshellType> d_subshell_order;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_DETAILED_WH_INCOHERENT_PHOTON_SCATTERING_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_DetailedWHIncoherentPhotonScatteringDistribution.hpp
//---------------------------------------------------------------------------//


#include <iostream>

#include <boost/program_options.hpp>

#include "graph.h"
#include "magnusson.h"
#include "flowgraph.h"
#include "jsongraphreader.h"
#include "flowgraphbuilder.h"
#include "magnussongraphbuilder.h"

using namespace dpct;

int main(int argc, char** argv) {
	namespace po = boost::program_options;

	std::string modelFilename;
	std::string weightsFilename;
	std::string outputFilename;
	std::string method("flow");
	bool swap = true;
	bool useOrderedNodeListInBF = true;
	bool partialBFUpdates = true;
	size_t maxNumPaths = 0;

	// Declare the supported options.
	po::options_description description("Allowed options");
	description.add_options()
	    ("help", "produce help message")
	    ("model,m", po::value<std::string>(&modelFilename), "filename of model stored as Json file")
	    ("weights,w", po::value<std::string>(&weightsFilename), "filename of the weights stored as Json file")
	    ("output,o", po::value<std::string>(&outputFilename), "filename where the resulting tracking (as links) will be stored as Json file")
	    ("method,e", po::value<std::string>(&method), "method to use for tracking: 'flow' (default), 'flow-flow', 'magnusson-flow' or 'magnusson'")
	    ("swap,s", po::value<bool>(&swap), "whether swap arcs are enabled (default=true)")
	    ("maxNumPaths,n", po::value<size_t>(&maxNumPaths), "maximum number of paths to find, default=0=no limit")
	    ("orderNodes", po::value<bool>(&useOrderedNodeListInBF), "use ordered node list in BF? flow only. (default=true)")
	    ("partialBF", po::value<bool>(&partialBFUpdates), "check which parts of the graph were influenced by last path and only update there? flow only. (default=true)")
	;

	po::variables_map variableMap;
	po::store(po::parse_command_line(argc, argv, description), variableMap);
	po::notify(variableMap);

	if (variableMap.count("help"))
	{
	    std::cout << description << std::endl;
	    return 1;
	}

	if (!variableMap.count("model") || !variableMap.count("output") || !variableMap.count("weights"))
	{
	    std::cout << "Model, Weights and Output filenames have to be specified!" << std::endl;
	    std::cout << description << std::endl;
	}
	else
	{
		if(method == "flow")
		{
		    FlowGraph graph;
		    FlowGraphBuilder graphBuilder(&graph);
		    JsonGraphReader jsonReader(modelFilename, weightsFilename, &graphBuilder);
		    jsonReader.createGraphFromJson();
		    std::cout << "Model has state zero energy: " << jsonReader.getInitialStateEnergy() << std::endl;
		    graph.maxFlowMinCostTracking(jsonReader.getInitialStateEnergy(), swap, maxNumPaths, useOrderedNodeListInBF, partialBFUpdates);
		    jsonReader.saveResultJson(outputFilename);
		}
		else if(method == "flow-flow")
		{
		    FlowGraph graph;
		    FlowGraphBuilder graphBuilder(&graph);
		    JsonGraphReader jsonReader(modelFilename, weightsFilename, &graphBuilder);
		    jsonReader.createGraphFromJson();
		    std::cout << "Model has state zero energy: " << jsonReader.getInitialStateEnergy() << std::endl;
		    double energy = graph.maxFlowMinCostTracking(jsonReader.getInitialStateEnergy(), false, maxNumPaths, useOrderedNodeListInBF, partialBFUpdates);
		    graph.maxFlowMinCostTracking(energy, true, maxNumPaths, useOrderedNodeListInBF, partialBFUpdates);
		    jsonReader.saveResultJson(outputFilename);
		}
		else if(method == "magnusson")
		{
			Graph::Configuration config(true, true, true);
    		Graph graph(config);
		    MagnussonGraphBuilder graphBuilder(&graph);
		    JsonGraphReader jsonReader(modelFilename, weightsFilename, &graphBuilder);
		    jsonReader.createGraphFromJson();
		    std::cout << "Model has state zero energy: " << jsonReader.getInitialStateEnergy() << std::endl;

		    Magnusson tracker(&graph, swap, true, false);
		    if(maxNumPaths > 0)
		    	tracker.setMaxNumberOfPaths(maxNumPaths);

		    std::vector<TrackingAlgorithm::Path> paths;
		    double score = tracker.track(paths);
		    std::cout << "\nTracking finished in " << tracker.getElapsedSeconds() << " secs with score "
		    		  << jsonReader.getInitialStateEnergy() - score << std::endl;
		    graphBuilder.getSolutionFromPaths(paths);
		    jsonReader.saveResultJson(outputFilename);
		}
		else if(method == "magnusson-flow")
		{
			double zeroEnergy, score;

			// set up magnusson
			Graph::Configuration config(true, true, true);
    		Graph graph(config);
		    MagnussonGraphBuilder graphBuilder(&graph);
		    std::vector<TrackingAlgorithm::Path> paths;

		    { // scope needed due to weird model scores that show up otherwise
			    JsonGraphReader jsonReader(modelFilename, weightsFilename, &graphBuilder);
			    jsonReader.createGraphFromJson();
			    zeroEnergy = jsonReader.getInitialStateEnergy();
			    std::cout << "Model has state zero energy: " << zeroEnergy << std::endl;

			    // track magnusson
			    Magnusson tracker(&graph, swap, true, false);
			    if(maxNumPaths > 0)
			    	tracker.setMaxNumberOfPaths(maxNumPaths);

			    score = tracker.track(paths);
			    std::cout << "\nTracking finished in " << tracker.getElapsedSeconds() << " secs with score "
			    		  << zeroEnergy - score << std::endl;

			    std::cout << "Extracting solution" << std::endl;
			}

		    // set up flow
		    FlowGraph flowGraph;
		    FlowGraphBuilder flowGraphBuilder(&flowGraph);
		    JsonGraphReader flowJsonReader(modelFilename, weightsFilename, &flowGraphBuilder);
		    flowJsonReader.createGraphFromJson();
		    std::cout << "Model has state zero energy: " << flowJsonReader.getInitialStateEnergy() << std::endl;

		    // initialize flow with magnusson's result
		    std::cout << "initializing flow solver" << std::endl;
		    flowGraph.initializeResidualGraph(true, useOrderedNodeListInBF);

		    std::vector<FlowGraph::Path> flowPaths = graphBuilder.translateSolution(paths, flowGraphBuilder);
		    for(auto p : flowPaths)
		    {
		    	flowGraph.augmentUnitFlow(p);
		    	flowGraph.updateEnabledArcs(p);
		    }
		    flowGraph.synchronizeDivisionDuplicateArcFlows();

		    // track flow
		    std::cout << "beginning tracking" << std::endl;
			flowGraph.maxFlowMinCostTracking(zeroEnergy - score, true, maxNumPaths, useOrderedNodeListInBF, partialBFUpdates);
		    flowJsonReader.saveResultJson(outputFilename);
		}
		else
			throw std::runtime_error("Unknown tracking method selected");
	}
}


#include <string>

#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Geometry>
#include <Eigen/SVD>

namespace
{

constexpr auto pi {3.141592653589793};

}

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

  int target_number {1};
  std::string root_name {"openni_coordinater"};
  std::string body_name {"torso"};
  {
    ros::NodeHandle pn {"~"};
    pn.getParam("target_number", target_number);
    pn.getParam("root", root_name);
    pn.getParam("body", body_name);
  }

  const auto body_frame_name {body_name + '_' + std::to_string(target_number)};

  ros::Rate r {5};
  tf2_ros::Buffer tfBuffer {};
  tf2_ros::TransformListener tfListener {tfBuffer};
  tf2_ros::TransformBroadcaster br {};

  geometry_msgs::TransformStamped ts {};
  ts.header.frame_id = root_name;
  ts.child_frame_id = "sentry_frame_" + std::to_string(target_number);
  while (ros::ok()) {
    try {
      auto body_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, body_frame_name, ros::Time{0}))};
      body_pos.translation() = Eigen::Vector3d::Zero();
      const auto body_ypr {body_pos.linear().eulerAngles(2, 0, 1)};
      constexpr auto trim_half_rotation {[](double angle) {
        if (angle < -pi / 2)
          return angle + pi;
        if (angle > pi / 2)
          return angle - pi;
        return angle;
      }};
      const auto yaw_angle {body_ypr(2)};

      ts.header.stamp = ros::Time::now();
      ts.transform.rotation = tf2::toMsg(Eigen::Quaterniond{Eigen::AngleAxisd{yaw_angle, Eigen::Vector3d::UnitY()}});
      //ts.transform.rotation = tf2::toMsg(Eigen::Quaterniond{body_pos.rotation()});

      br.sendTransform(ts);
    } catch (tf2::TransformException &e) {
      ROS_WARN("%s", e.what());
    }

    r.sleep();
  }
}


#include "gyuanxd_key.h"
#include "utils.hpp"

#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include <sodium.h>

#include <exception>
#include <fstream>
#include <iterator>

namespace fs = boost::filesystem;

namespace gyuanx {

private_key_t gyuanxdKeyFromHex(const std::string& private_key_hex) {
    if (private_key_hex.size() != KEY_LENGTH * 2)
        throw std::runtime_error("Gyuanxd key data is invalid: expected " +
                                 std::to_string(KEY_LENGTH) + " bytes not " +
                                 std::to_string(private_key_hex.size()) +
                                 " bytes");

    const auto bytes = util::hex_to_bytes(private_key_hex);
    private_key_t private_key;
    std::copy(bytes.begin(), bytes.end(), private_key.begin());

    return private_key;
}

private_key_ed25519_t
private_key_ed25519_t::from_hex(const std::string& sc_hex) {
    if (sc_hex.size() != private_key_ed25519_t::LENGTH * 2)
        throw std::runtime_error("Gyuanxd key data is invalid: expected " +
                                 std::to_string(private_key_ed25519_t::LENGTH) +
                                 " bytes not " + std::to_string(sc_hex.size()) +
                                 " bytes");

    private_key_ed25519_t key;

    const auto bytes = util::hex_to_bytes(sc_hex);
    std::copy(bytes.begin(), bytes.end(), key.data.begin());

    return key;
}

public_key_t derive_pubkey_legacy(const private_key_t& private_key) {
    public_key_t publicKey;
    crypto_scalarmult_ed25519_base_noclamp(publicKey.data(),
                                           private_key.data());

    return publicKey;
}

public_key_t derive_pubkey_x25519(const private_key_t& seckey) {

    public_key_t pubkey;
    crypto_scalarmult_curve25519_base(pubkey.data(), seckey.data());

    return pubkey;
}

public_key_t derive_pubkey_ed25519(const private_key_ed25519_t& seckey) {

    public_key_t pubkey;
    crypto_sign_ed25519_sk_to_pk(pubkey.data(), seckey.data.data());

    return pubkey;
}

std::string key_to_string(const std::array<uint8_t, gyuanx::KEY_LENGTH>& key) {
    auto pk = reinterpret_cast<const char*>(&key);
    return std::string{pk, gyuanx::KEY_LENGTH};
}

} // namespace gyuanx


#pragma once
#include <Eigen/Core>

#include "srrg_hbst/types/binary_matchable.hpp"

namespace srrg_hbst {

  template <typename ObjectType_,
            uint32_t descriptor_size_bits_ = 256,
            typename real_precision_       = double>
  class ProbabilisticMatchable : public BinaryMatchable<ObjectType_, descriptor_size_bits_> {
    // ds template forwarding
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef ObjectType_ ObjectType;
    typedef typename BinaryMatchable<ObjectType_, descriptor_size_bits_>::Descriptor Descriptor;
    typedef Eigen::Matrix<real_precision_, descriptor_size_bits_, 1> BitStatisticsVector;

    // ds ctor/dtor
  public:
    ProbabilisticMatchable(ObjectType object_,
                           const Descriptor& descriptor_,
                           const uint64_t& image_identifier_ = 0) :
      BinaryMatchable<ObjectType_, descriptor_size_bits_>(object_, descriptor_, image_identifier_),
      bit_probabilities(BitStatisticsVector()),
      bit_volatility(BitStatisticsVector()) {
    }

    ProbabilisticMatchable(ObjectType object_,
                           const Descriptor& descriptor_,
                           const BitStatisticsVector& bit_probabilities_,
                           const BitStatisticsVector& bit_volatility_,
                           const uint64_t& image_identifier_ = 0) :
      BinaryMatchable<ObjectType_, descriptor_size_bits_>(object_, descriptor_, image_identifier_),
      bit_probabilities(bit_probabilities_),
      bit_volatility(bit_volatility_) {
    }

    // ds wrapped constructors - only available if OpenCV is present on building system
#ifdef SRRG_HBST_HAS_OPENCV
    ProbabilisticMatchable(ObjectType object_,
                           const cv::Mat& descriptor_,
                           const uint64_t& image_identifier_ = 0) :
      BinaryMatchable<ObjectType_, descriptor_size_bits_>(object_, descriptor_, image_identifier_),
      bit_probabilities(BitStatisticsVector()),
      bit_volatility(BitStatisticsVector()) {
    }

    ProbabilisticMatchable(ObjectType object_,
                           const cv::Mat& descriptor_,
                           const BitStatisticsVector& bit_probabilities_,
                           const BitStatisticsVector& bit_volatility_,
                           const uint64_t& image_identifier_ = 0) :
      BinaryMatchable<ObjectType_, descriptor_size_bits_>(object_, descriptor_, image_identifier_),
      bit_probabilities(bit_probabilities_),
      bit_volatility(bit_volatility_) {
    }
#endif

    ~ProbabilisticMatchable() {
    }

    // ds attributes
  public:
    // ds statistical data: bit probabilities
    BitStatisticsVector bit_probabilities;

    // ds statistical data: bit volatity info, maximum number of appearances where the bit was
    // stable - currently not used
    BitStatisticsVector bit_volatility;
  };

} // namespace srrg_hbst


#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <cmath>
#include <memory>
#include <vector>
#include <limits>
#include <algorithm>
#include <utility>
#include <iterator>

#include "statespace.hpp"

using namespace boost::numeric::ublas;

CompactSupportSSM::CompactSupportSSM(\
       const vector<int> & start_idxs, const vector <int > & end_idxs,
       const vector<int> & identities, const matrix<double> & basis_prototypes,
       const vector<double> & coef_means,  const vector<double> & coef_vars,
       double obs_noise, double bias) : start_idxs(start_idxs), end_idxs(end_idxs),
					identities(identities), basis_prototypes(basis_prototypes),
					coef_means(coef_means), coef_vars(coef_vars),
					obs_noise(obs_noise), bias(bias) {

  this->is_cssm = true;
  this->n_basis = start_idxs.size();
  this->n_steps = *(std::max_element(end_idxs.begin(), end_idxs.end()));

  if (coef_means.size() != this->n_basis) {
    printf("ERROR: coef prior mean length must match size of basis\n");
    exit(-1);
  }
  if (coef_vars.size() != this->n_basis) {
    printf("ERROR: coef prior vars length must match size of basis\n");
    exit(-1);
  }

  std::vector< std::vector<int> > tmp_active_basis(this->n_steps);

  this->active_indices.set_empty_key(std::make_pair(-1,-1) );

  unsigned int i;
  vector<int>::const_iterator st;
  vector<int>::const_iterator et;
  for( i = 0, st = this->start_idxs.begin(), et = this->end_idxs.begin() ;
       i < this->n_basis && st < this->start_idxs.end() && et < this->end_idxs.end();
       ++i, ++st, ++et ) {

    for (unsigned j= std::max(*st, 0); j < std::min((unsigned int)*et, this->n_steps); ++j) {
      tmp_active_basis[j].push_back(i);
      (this->active_indices)[std::make_pair(i,j)] = int(tmp_active_basis[j].size()-1);
    }
  }

  int max_dimension = 0;
  for (unsigned i=0; i < this->n_steps; ++i) {
    max_dimension = std::max(max_dimension, int(tmp_active_basis[i].size()));
  }
  this->max_dimension = max_dimension;

  this->active_basis = matrix<int>(this->n_steps, max_dimension);
  for (unsigned i=0; i < this->n_steps; ++i) {
    int j = 0;
    for(j=0; j < int(tmp_active_basis[i].size()); ++j) {
      this->active_basis(i,j) = tmp_active_basis[i][j];
    }
    if (j < max_dimension) {
      this->active_basis(i,j) = -1;
    }
  }

  // printf("identities len %d\n", this->identities.size());

}

CompactSupportSSM::~CompactSupportSSM() {
  return;
};

int CompactSupportSSM::apply_transition_matrix( const double * x, int k, double * result) {
  /* compute F_k*x for the given x, where
     F_k is the transition matrix from time
     k-1 to k. The result is stored in
     the provided vector x_new. If x_new
     has extra dimensions, they are not
     touched (so may still contain garbage).*/

  if (k <= 0) {
    printf("error: applying CSSM transition at invalid index %d.\n", k);
    exit(-1);
  }

  for (int i=0; i < this->max_dimension; ++i) {
    result[i] = 0;
  }

  if (k >= this->n_steps) {
    return 0;
  }

  const matrix_row< matrix<int> > active = row(this->active_basis, k);
  matrix_row< matrix<int> >::const_iterator idx;
  int i = 0;
  for (i=0, idx = active.begin();
       idx < active.end() && *idx >= 0;
       ++i, ++idx) {
    // if this basis is new at this timestep, initialize to zero.
    // otherwise, use its previous value.
    std::pair<int,int> key = std::make_pair(int(*idx), k-1);
    dense_hash_map< std::pair<int, int>, int, boost::hash< std::pair< int,int> >  >::iterator contains = this->active_indices.find(key);
    if (contains == this->active_indices.end()) {
      result[i] = 0;
      D(printf("transition k %d basis %d idx %d prev_idx %d x_new[i] %.4f\n", k, *idx, i, -1, 0.0);)
    } else {
      int prev_idx = this->active_indices[key];
      result[i] =x[prev_idx];
      D(printf("transition k %d basis %d idx %d prev_idx %d x_new[i] %.4f\n", k, *idx, i, prev_idx, x[prev_idx]);)
    }
  }
  return i;
}

/* Each column of the input X gives a column of the result matrix */
int CompactSupportSSM::apply_transition_matrix( const matrix<double> &X,
						unsigned int x_row_offset,
						int k,
						matrix<double> &result,
						unsigned int r_row_offset,
						unsigned int n) {
  if (k <= 0) {
    printf("error: applying CSSM transition at invalid index %d.\n", k);
    exit(-1);
  }

  for (int i=r_row_offset; i < r_row_offset+this->max_dimension; ++i) {
    for (int j =0; j < n; ++j) {
      result(i,j) = 0;
    }
  }

  if (k >= this->n_steps) {
    return 0;
  }

  const matrix_row< matrix<int> > active = row(this->active_basis, k);
  matrix_row< matrix<int> >::const_iterator idx;
  unsigned i = r_row_offset;

  for (i=r_row_offset, idx = active.begin();
       idx < active.end() && *idx >= 0;
       ++i, ++idx) {

    // if this basis is new at this timestep, initialize to zero.
    // otherwise, use its previous value.
    std::pair<int,int> key = std::make_pair(int(*idx), k-1);
    dense_hash_map< std::pair<int, int>, int, boost::hash< std::pair< int,int> >  >::iterator contains = this->active_indices.find(key);

    if (contains == this->active_indices.end()) {
      for (unsigned j=0; j < n; ++j) result(i, j) = 0;
      // printf("MATR0 transition k %d i %d idx %d prev_idx %d x_new[i,i] %.4f\n", k, i, *idx, -1, 0.0);
    } else {
      int prev_idx = this->active_indices[key] + x_row_offset;
      for (unsigned j=0; j < n; ++j) result(i, j) = X(prev_idx, j);
      //printf("MATR1 transition k %d i %d idx %d prev_idx %d x_new[i, i] %.4f\n", k, i, *idx, prev_idx, X(prev_idx, prev_idx));
    }
  }
  return i-r_row_offset;
}

void CompactSupportSSM::transition_bias(int k, double * result) {
  if (k >= this->n_steps) {
    return;
  }

  const matrix_row< matrix<int> > active = row(this->active_basis, k);
  matrix_row< matrix<int> >::const_iterator idx;
  int i = 0;
  for (i=0, idx = active.begin();
       idx < active.end() && *idx >= 0;
       ++i, ++idx) {

    // if this basis is new at this timestep, initialize to zero.
    // otherwise, use its previous value.
    std::pair<int,int> key = std::make_pair(*idx, k-1);
    dense_hash_map< std::pair<int, int>, int, boost::hash< std::pair< int,int> >  >::iterator contains = this->active_indices.find(key);
    if (contains == this->active_indices.end()) {
      result[i] += this->coef_means(*idx);
    }
  }
}

void CompactSupportSSM::transition_noise_diag(int k, double * result) {
  if (k >= this->n_steps) {
    return;
  }

  unsigned int i = 0;
  for (i; i < this->max_dimension; ++i) {
    result[i] = 0.0;
  }

  const matrix_row< matrix<int> > active = row(this->active_basis, k);
  matrix_row< matrix<int> >::const_iterator idx;
  for (i=0, idx = active.begin();
       idx < active.end() && *idx >= 0;
       ++i, ++idx) {
    std::pair<int,int> key = std::make_pair(*idx, k-1);
    dense_hash_map< std::pair<int, int>, int, boost::hash< std::pair< int,int> >  >::iterator contains = this->active_indices.find(key);
    if (contains == this->active_indices.end()) {
      result[i] = this->coef_vars(*idx);
      //D(printf("time %d instantiating coef %d into state %d with noise variance %f\n", k, *idx, i, result[i]);)
    } else {
      result[i] = 0.0;
    }
  }
}

double CompactSupportSSM::apply_observation_matrix(const double * x, int k) {
  // const matrix_row< matrix<int> > active = row(this->active_basis, k);

  double result = 0;

  if (k >= this->n_steps) {
    return 0;
  }

  matrix_row< matrix<int> >::const_iterator idx;
  unsigned int i = 0;
  for (i=0; i < this->active_basis.size2() && this->active_basis(k, i) >= 0; ++i) {
    int basis = this->active_basis(k, i);

    int prototype = (this->identities)(basis);
    int st = (this->start_idxs)(basis);

    result += x[i] * (this->basis_prototypes)(prototype, k-st);
  }

  return result;
}

void CompactSupportSSM::apply_observation_matrix(const matrix<double> &X,
						 unsigned int row_offset, int k,
						 double *result, double *result_tmp, unsigned int n) {
  //const matrix_row< matrix<int> > active = row(this->active_basis, k);
  //matrix_row< matrix<int> >::const_iterator idx;

  for (unsigned j=0; j < n; ++j) {
    result[j] = 0;
  }

  if (k >= this->n_steps) {
    return;
  }

  unsigned int i = 0;
  for (i=0; i < this->active_basis.size2() && this->active_basis(k, i) >= 0; ++i) {
    int basis = this->active_basis(k, i);

    int prototype = this->identities(basis);
    int st = this->start_idxs(basis);

    for (unsigned j=0; j < n; ++j) {
      result[j] += X(row_offset+i,j) * this->basis_prototypes(prototype, k-st);
    }
  }
}

double CompactSupportSSM::observation_bias(int k) {
  return this->bias;
}

double CompactSupportSSM::observation_noise(int k) {
  return this->obs_noise;
}

bool CompactSupportSSM::stationary(int k) {
  return k >= this->n_steps;
}

int CompactSupportSSM::prior_mean(double *result) {
  const matrix_row< matrix<int> > active = row(this->active_basis, 0);
  matrix_row< matrix<int> >::const_iterator idx;
  int i = 0;

  for (i=0, idx = active.begin();
       idx < active.end() && *idx >= 0;
       ++i, ++idx) {
    result[i] = this->coef_means(*idx);
  }
  for (unsigned ii=i; ii < this->max_dimension; ++ii) {
    result[ii] = 0.0;
  }
  return i;
}

int CompactSupportSSM::prior_vars(double *result) {
  const matrix_row< matrix<int> > active = row(this->active_basis, 0);
  matrix_row< matrix<int> >::const_iterator idx;
  int i = 0;
  for (i=0, idx = active.begin();
       idx < active.end() && *idx >= 0;
       ++i, ++idx) {
    result[i] = this->coef_vars(*idx);
  }
  for (unsigned ii=i; ii < this->max_dimension; ++ii) {
    result[ii] = 0.0;
  }

  return i;
}

void CompactSupportSSM::extract_coefs(const vector<double> &x,
				      const matrix<double> &P,
				      unsigned int state_offset,
				      int k,
				      vector<double> & coef_means,
				      vector<double> & coef_vars) {

  if (k >= this->n_steps) {
    return;
  }

  /* given a state estimate at some time k, extract marginals for
   * whatever coefficients we can reasonably do so for at the
   * current time. Earlier estimates will always be overwritten
   * by later ones. */
  const matrix_row< matrix<int> > active = row(this->active_basis, k);
  matrix_row< matrix<int> >::const_iterator idx;
  unsigned int i;

  for (i=0, idx = active.begin();
       idx < active.end() && *idx >= 0;
       ++i, ++idx) {
    int basis_idx = *idx;
    std::pair<int,int> key = std::make_pair(basis_idx, k);

    dense_hash_map< std::pair<int, int>, int, boost::hash< std::pair< int,int> >  >::iterator contains = this->active_indices.find(key);
    if (contains == this->active_indices.end()) {
      printf("\nWAAT, no key found for basis %d\n", basis_idx);
      exit(-1);
    } else {
      int state_idx = this->active_indices[key] + state_offset;
      coef_means(basis_idx) = x(state_idx);
      coef_vars(basis_idx) = P(state_idx,state_idx);

      //printf("extracted coef %d var %f from state %d at cssm-local time %d\n", basis_idx, coef_vars(basis_idx), state_idx, k);
      /*if (coef_vars(basis_idx) == 0) {
	printf("WARNING: posterior coef var is zero for basis %d at state %d at time %d, this doesn't seem right....\n", basis_idx, state_idx, k);
	//exit(-1);
	}*/
    }
  }
}


// The MIT License (MIT)

// Copyright (c) 2019 Caili Li

//  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.

/* code */

#ifndef __SPHEREWORLDNAVIGATIONFUNCTION__
#define __SPHEREWORLDNAVIGATIONFUNCTION__

#include <memory>
#include <utility>
#include <unordered_map>

#include <Eigen/Core>

#include "navigation_function/NavigationFunction.hpp"
#include "navigation_function/math/Sphere.hpp"
#include "navigation_function/math/ScalarObject.hpp"

namespace roboflow
{
namespace navigation_function
{

class SphereWorldNavigationFunction final : public NavigationFunction
{
    std::unordered_map<uint64_t, std::weak_ptr<math::Sphere>> obstacles_;
    std::weak_ptr<math::Sphere> zeroth_obstacle_;
    std::weak_ptr<math::ScalarObject> destination_;
    double kappa_ = 1.0;
    DestinationFunctionType d_type_;

public:
    // insert specific obstacle; return: successful or not
    bool setObstacle(std::shared_ptr<math::Sphere>);
    // set the obstacle defining the workspace
    void setZerothObstacle(std::shared_ptr<math::Sphere>);
    // set destination function
    void setDestination(std::shared_ptr<math::ScalarObject>, DestinationFunctionType);
    // clear all obstacles
    void clear();
    // delete specific obstcle; return: successful or not
    bool eraseObstacle(uint64_t);
    // set kappa
    void setKappa(double);
    // get kappa
    double getKappa() const;

    // point
    std::pair<double, bool> evaluate(Eigen::Vector2d) override;
    // point: x, y
    std::pair<double, bool> evaluate(double, double) override;
    // get all obstacles, including the zeroth obsatcle
    std::vector<std::shared_ptr<math::StarObject>> getObstacles() const override;
    // get destination function and its type
    std::pair<std::shared_ptr<math::ScalarObject>, DestinationFunctionType>
    getDestinationFunction() const override;
};

} // namespace navigation_function
} // namespace roboflow

#endif


///////////////////////////////////////////////////////////////////////////////
// times2_iterator.hpp
//
//  Copyright 2006 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_TIMES2_ITERATOR_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_TIMES2_ITERATOR_HPP_DE_01_01_2006

#include <functional>
#include <boost/detail/workaround.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/permutation_iterator.hpp>

namespace boost { namespace accumulators
{

namespace detail
{
    typedef transform_iterator<
        std::binder1st<std::multiplies<std::size_t> >
      , counting_iterator<std::size_t>
    > times2_iterator;

    inline times2_iterator make_times2_iterator(std::size_t i)
    {
        return make_transform_iterator(
            make_counting_iterator(i)
          , std::bind1st(std::multiplies<std::size_t>(), 2)
        );
    }

    ///////////////////////////////////////////////////////////////////////////////
    // lvalue_index_iterator
    template<typename Base>
    struct lvalue_index_iterator
      : Base
    {
        lvalue_index_iterator()
          : Base()
        {}

        lvalue_index_iterator(Base base)
          : Base(base)
        {
        }

        typename Base::reference operator [](typename Base::difference_type n) const
        {
            return *(*this + n);
        }
    };
} // namespace detail

}}

#endif


#include <iostream>
#include <fstream>
#include <boost/smart_ptr/shared_ptr.hpp>
#include <gng_node.h>
#include <logger.h>

#include <threading.h>
#include <utils.h>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstring>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

using namespace std;
using namespace gmum;

template<class Node, class Edge, class Mutex>
RAMGNGGraph<Node, Edge, Mutex>::RAMGNGGraph(Mutex * mutex, unsigned int dim,
		int initial_pool_size, GNGDistanceFunction dist_fnc,
		boost::shared_ptr<Logger> logger) :
		maximum_index(-1), mutex(mutex), gng_dim(dim), first_free(-1), nodes(0), dist_fnc(
				dist_fnc), m_logger(logger) {

	positions.resize(initial_pool_size * gng_dim);

	//Initialize graph data structures
	g.resize(initial_pool_size);

	for (int i = 0; i < initial_pool_size; ++i)
		g[i].reserve(gng_dim);

	occupied.resize(initial_pool_size);

	for (int i = 0; i < initial_pool_size; ++i)
		occupied[i] = false;
	next_free.resize(initial_pool_size);

	for (int i = 0; i < initial_pool_size - 1; ++i)
		next_free[i] = i + 1;
	next_free[initial_pool_size - 1] = -1;
	first_free = 0;

}

template<class Node, class Edge, class Mutex>
RAMGNGGraph<Node, Edge, Mutex>::~RAMGNGGraph() {
	for (int i = 0; i < g.size(); ++i) {
		if (occupied[i]) {
			BOOST_FOREACH(GNGEdge * edg, g[i])
				delete edg;
		}
	}

}

template<class Node, class Edge, class Mutex>
unsigned int RAMGNGGraph<Node, Edge, Mutex>::get_maximum_index() const {
	return this->maximum_index;
}

template<class Node, class Edge, class Mutex>
bool RAMGNGGraph<Node, Edge, Mutex>::existsNode(unsigned i) const {
	return i < nodes && occupied[i];
}

template<class Node, class Edge, class Mutex>
bool RAMGNGGraph<Node, Edge, Mutex>::isEdge(int a, int b) const {

	BOOST_FOREACH(GNGEdge * edg, g[a]) {
		if ((edg)->nr == b)
			return true;
	}
	return false;
}

template<class Node, class Edge, class Mutex>
const double *RAMGNGGraph<Node, Edge, Mutex>::getPosition(int nr) const {
	return g[nr].position;
}

template<class Node, class Edge, class Mutex>
unsigned int RAMGNGGraph<Node, Edge, Mutex>::get_number_nodes() const {
	return this->nodes;
}

template<class Node, class Edge, class Mutex>
Node &
RAMGNGGraph<Node, Edge, Mutex>::operator[](int i) {
	return g[i];
}

template<class Node, class Edge, class Mutex>
double RAMGNGGraph<Node, Edge, Mutex>::get_dist(int a, int b) {
	return get_dist(g[a].position, g[b].position);
}

template<class Node, class Edge, class Mutex>
double RAMGNGGraph<Node, Edge, Mutex>::get_euclidean_dist(const double *pos_a,
		const double *pos_b) const {
	double distance = 0;
	for (int i = 0; i < this->gng_dim; ++i)
		distance += (pos_a[i] - pos_b[i]) * (pos_a[i] - pos_b[i]);

	return distance;
}

template<class Node, class Edge, class Mutex>
double RAMGNGGraph<Node, Edge, Mutex>::get_dist(const double *pos_a,
		const double *pos_b) const {
	if (dist_fnc == Euclidean) {
		double distance = 0;
		for (size_t i = 0; i < this->gng_dim; ++i)
			distance += (pos_a[i] - pos_b[i]) * (pos_a[i] - pos_b[i]);

		return distance;
	} else if (dist_fnc == Cosine) {
		double norm_1 = 0, norm_2 = 0, distance = 0;

		for (size_t i = 0; i < this->gng_dim; ++i) {
			norm_1 += (pos_a[i]) * (pos_a[i]);
			norm_2 += (pos_b[i]) * (pos_b[i]);
			distance += pos_a[i] * pos_b[i];
		}

		norm_1 = sqrt(norm_1);
		norm_2 = sqrt(norm_2);
		return 1.0 - distance / (norm_1 * norm_2);
	}
}

template<class Node, class Edge, class Mutex>
int RAMGNGGraph<Node, Edge, Mutex>::newNode(const double *position) {
	if (first_free == -1) {
		DBG(m_logger,10, "RAMGNGGraph::newNode() growing pool");
		this->resizeGraph();

	}

	int createdNode = first_free; //taki sam jak w g_node_pool

	maximum_index = createdNode > maximum_index ? createdNode : maximum_index;

	//Assuming it is clear here
#ifdef GMUM_DEBUG
	assert(g[createdNode].size() == 0);
#endif

	// Initialize node
	g[createdNode].position = &positions[createdNode * gng_dim];
	occupied[createdNode] = true;
	g[createdNode].nr = createdNode;
	g[createdNode].edgesCount = 0;
	g[createdNode].utility = 0.0;
	g[createdNode]._position_owner = false;
	g[createdNode].dim = gng_dim;
	g[createdNode].extra_data = 0.0;

	first_free = next_free[createdNode];

	//zwiekszam licznik wierzcholkow //na koncu zeby sie nie wywalil przypadkowo
	++this->nodes;
	memcpy(&(g[createdNode].position[0]), position,
			sizeof(double) * (this->gng_dim));        //param

	//TODO: this should be tracked by GNGAlgorithm
	g[createdNode].error = 0.0;
	g[createdNode].error_cycle = 0;

	return createdNode;

}

template<class Node, class Edge, class Mutex>
bool RAMGNGGraph<Node, Edge, Mutex>::deleteNode(int x) {

	this->lock();
	if (existsNode(x)) {
		//TODO: add automatic erasing edges
		assert(g[x].size() == 0);

		--nodes;
		if (maximum_index == x)
			maximum_index = maximum_index - 1;

		occupied[x] = false;
		next_free[x] = first_free;
		first_free = x;
		this->unlock();
		return true;

	}

	this->unlock();
	return false;

}

template<class Node, class Edge, class Mutex>
typename RAMGNGGraph<Node, Edge, Mutex>::EdgeIterator RAMGNGGraph<Node, Edge,
		Mutex>::removeUDEdge(int a, int b) {

	this->lock();

	for (typename Node::iterator edg = g[a].begin(); edg != g[a].end(); ++edg) {
		if ((*edg)->nr == b) {
			Edge *ptr_rev = (Edge *) ((**edg).rev);
			Edge *ptr = (Edge *) (&(**edg));

			g[b].erase(find(g[b].begin(), g[b].end(), (*edg)->rev));

			edg = g[a].erase(edg);

			delete ptr;
			delete ptr_rev;

			g[a].edgesCount--;
			g[b].edgesCount--;
			this->unlock();
			return edg;
		}
	}

	this->unlock();
	DBG(m_logger,10, "ExtGraphNodeManager()::removeEdge Not found edge!");
	return g[a].end();

}

template<class Node, class Edge, class Mutex>
void RAMGNGGraph<Node, Edge, Mutex>::addUDEdge(int a, int b) {

	this->lock();

	if (a == b)
		throw "Added loop to the graph";

	g[a].push_back(new Edge(b));
	g[b].push_back(new Edge(a));

	g[a].back()->rev = g[b].back();
	g[b].back()->rev = g[a].back();

	g[a].edgesCount++;
	g[b].edgesCount++;
	this->unlock();

}

template<class Node, class Edge, class Mutex>
void RAMGNGGraph<Node, Edge, Mutex>::addDEdge(int a, int b) {
	throw BasicException("Not implemented");
}

template<class Node, class Edge, class Mutex>
std::string RAMGNGGraph<Node, Edge, Mutex>::reportPool() {
	std::stringstream ss;
	for (unsigned int i = 0; i < g.size(); ++i) {
		string tmp = "";
		if (occupied[i]) {
			tmp = tmp + to_str(g[i]) + ":";
			BOOST_FOREACH(GNGEdge * it2, g[i]) {
				tmp += to_str((it2)->nr) + "[" + to_str((((it2)->rev))->nr)
						+ "],";
			}
			tmp = tmp + "\n";
		}
		ss << tmp;
	}
	return ss.str();
}

template<class Node, class Edge, class Mutex>
int RAMGNGGraph<Node, Edge, Mutex>::get_dim() const {
	return gng_dim;
}
template<class Node, class Edge, class Mutex>
void RAMGNGGraph<Node, Edge, Mutex>::lock() {
	mutex->lock();
}

template<class Node, class Edge, class Mutex>
void RAMGNGGraph<Node, Edge, Mutex>::unlock() {
	mutex->unlock();
}

template<class Node, class Edge, class Mutex>
void RAMGNGGraph<Node, Edge, Mutex>::serialize(std::ostream & output) {
	this->lock();

	vector<double> S;
	S.reserve(10000);

	//Header
	S.push_back((double) (g.size()));
	S.push_back((double) (maximum_index + 1));
	S.push_back((double) gng_dim);
	S.push_back((double) first_free);
	S.push_back((double) nodes);

	DBG(m_logger,7, "GNGGraph::Serializing nodes");
	//Nodes
	for (int i = 0; i < g.size(); ++i) {
		if (existsNode(i)) {
			S.push_back((double) 1);
			vector<double> serialized_node = g[i].dumpVertexData();

			std::copy(serialized_node.begin(), serialized_node.end(),
					std::back_inserter(S));
		} else {
			S.push_back((double) 0);
		}
	}DBG(m_logger,7, "GNGGraph::Serializing edges");
	//Edges
	for (int i = 0; i < g.size(); ++i) {
		if (existsNode(i)) {
			vector<double> serialized_node = g[i].dumpEdges();
			std::copy(serialized_node.begin(), serialized_node.end(),
					std::back_inserter(S));
		} else {
			S.push_back((double) 0);
		}
	}DBG(m_logger,7, "GNGGraph::Serializing nextFree");
	//NextFree
	for (int i = 0; i < g.size(); ++i) {
		S.push_back((double) next_free[i]);
	}DBG(m_logger,7, "GNGGraph::Serialize;:writing out");

	_write_bin_vect(output, S);

	this->unlock();
}

template<class Node, class Edge, class Mutex>
void RAMGNGGraph<Node, Edge, Mutex>::load(std::istream & input) {
	this->lock();

	DBG(m_logger,7, "GNGGraph:: loading ");

	vector<double> S = _load_bin_vector(input);
	vector<double>::iterator itr = S.begin();
	//Header
	unsigned int bufor_size = (int) *itr;
	maximum_index = (int) *(++itr) - 1;
	gng_dim = (int) *(++itr);
	first_free = (int) *(++itr);
	nodes = (int) *(++itr);

	DBG(m_logger,5, "Read in "+to_str(bufor_size) +" sized graph with "+
			" max_index="+to_str(maximum_index)+" gng_dim="+to_str(gng_dim)+" "+
			"first_free="+to_str(first_free)+" nodes="+to_str(nodes)
	);

	positions.clear();
	g.clear();
	next_free.clear();
	occupied.clear();

	occupied.resize(bufor_size);
	g.resize(bufor_size);
	next_free.resize(bufor_size);
	positions.resize((bufor_size + 1) * gng_dim);

	for (size_t i = 0; i < bufor_size; ++i) {
		occupied[i] = false;
		g[i].reserve(gng_dim + 2);
	}

	//Deserialize nodes
	for (size_t i = 0; i < g.size(); ++i) {
		int tmp = (int) *(++itr);
		occupied[i] = (bool) tmp;
		if (occupied[i])
			g[i].loadVertexData(itr, gng_dim, &positions[i * gng_dim]);

	}

	//Deserialize edges
	for (size_t i = 0; i < g.size(); ++i) {
		int edges_length = (int) *(++itr);

		for (int j = 0; j < edges_length; ++j) {
			int gng_endpoint_index = (int) *(++itr);
			if (gng_endpoint_index > i)
				this->addUDEdge(i, gng_endpoint_index);
		}
	}

	//Deserialize nextFree
	for (size_t i = 0; i < g.size(); ++i) {
		next_free[i] = (int) *(++itr);
	}

	this->unlock();
}

template<class Node, class Edge, class Mutex>
void RAMGNGGraph<Node, Edge, Mutex>::resizeGraph() {
	//DBG(m_logger,5, "GNGGraph::resizing graph from "+to_string(g.size()));
	DBG(m_logger,5, "GNGGraph::resizing");
	unsigned int previous_size = g.size();
	//Grow positions pool

	positions.resize(2 * previous_size * gng_dim);

	//Reassign memory pointers
	for (size_t i = 0; i < previous_size; ++i) {
		g[i].position = &positions[i * gng_dim];

	}

	g.resize(2 * previous_size);

	for (size_t i = 0; i < previous_size; ++i) {
		g[i].position = &positions[i * gng_dim];
	}

	occupied.resize(2 * previous_size);
	for (int i = previous_size; i < 2 * previous_size; ++i) {
		//                        g[i].reset();
		//                        g[i].reserve(gng_dim); //for speed purposes
		occupied[i] = false;
	}

	next_free.resize(2 * previous_size);
	for (size_t i = previous_size - 1; i < 2 * previous_size - 1; ++i) {
		next_free[i] = i + 1;
	}
	next_free[g.size() - 1] = -1;
	first_free = previous_size;

	DBG(m_logger,5, "GNGGraph::resizing done");DBG(m_logger,5, to_str(first_free));DBG(m_logger,5, to_str(next_free[previous_size]));
}

static void writeToGraphML(GNGGraph &g, std::ostream & out) {

	g.lock();

	out << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n";
	out
			<< "<graphml xmlns=\"http://graphml.graphdrawing.org/xmlns\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xsi:schemaLocation=\"http://graphml.graphdrawing.org/xmlns http://graphml.graphdrawing.org/xmlns/1.0/graphml.xsd\">\n";
	out
			<< "<key id=\"key0\" for=\"edge\" attr.name=\"dist\" attr.type=\"double\" />\n";
	out
			<< "<key id=\"key1\" for=\"node\" attr.name=\"error\" attr.type=\"double\" />\n";
	out
			<< "<key id=\"key2\" for=\"node\" attr.name=\"label\" attr.type=\"double\" />\n";
	out
			<< "<key id=\"key3\" for=\"node\" attr.name=\"gng_index\" attr.type=\"int\" />\n";
	out
			<< "<key id=\"key4\" for=\"node\" attr.name=\"utility\" attr.type=\"double\" />\n";
	out
			<< "<key id=\"key5\" for=\"node\" attr.name=\"v0\" attr.type=\"double\" />\n";
	out
			<< "<key id=\"key6\" for=\"node\" attr.name=\"v1\" attr.type=\"double\" />\n";
	out
			<< "<key id=\"key7\" for=\"node\" attr.name=\"v2\" attr.type=\"double\" />\n";
	out
			<< "<graph id=\"G\" edgedefault=\"undirected\" parse.nodeids=\"canonical\" parse.edgeids=\"canonical\" parse.order=\"nodesfirst\">\n";

	std::map<int, int> gng_index_to_graph_index;

	unsigned int k = 0;
	for (int i = 0; i <= g.get_maximum_index(); ++i) {

		if (g.existsNode(i)) {
			gng_index_to_graph_index[g[i].nr] = k; //TODO:To be honest I dnt remember purpose of g[i].nr..

			out << "<node id=\"n" << k++ << "\">\n";
			out << "<data key=\"key1\">" << g[i].error << "</data>\n";
			out << "<data key=\"key2\">" << g[i].extra_data << "</data>\n";
			out << "<data key=\"key3\">" << g[i].nr << "</data>\n";
			out << "<data key=\"key4\">" << g[i].utility << "</data>\n";
			out << "<data key=\"key5\">" << g[i].position[0] << "</data>\n";
			out << "<data key=\"key6\">" << g[i].position[1] << "</data>\n";
			out << "<data key=\"key7\">" << g[i].position[2] << "</data>\n";
			out << "</node>\n";
		}
	}

	unsigned int l = 0;

	for (unsigned int i = 0; i <= g.get_maximum_index(); ++i)
		if (g.existsNode(i)) {
			FOREACH(GNGEdge * edg, g[i]) {
				if (g[i].nr > (edg)->nr) { //directed!
					out << "<edge id=\"e" << l++ << "\" source=\"n"
							<< gng_index_to_graph_index[(edg)->nr]
							<< "\" target=\"n"
							<< gng_index_to_graph_index[g[i].nr] << "\">\n";
					out << "<data key=\"key0\">" << g.get_dist(i, (edg)->nr)
							<< "</data>";
					out << "</edge>\n";
				}
			}

		}

	out << "</graph>\n</graphml>\n";
	g.unlock();

}

static std::string writeToGraphML(GNGGraph &g, string filename) {

	if (filename == "") {
		std::stringstream ss;
		writeToGraphML(g, ss);
		return ss.str();
	} else {
		ofstream myfile(filename.c_str());
		writeToGraphML(g, myfile);
		myfile.close();
		return "";
	}

}


#pragma once
#include "criteria.hpp"
#include <boost/heap/pairing_heap.hpp>
#include <functional>
#include <unordered_set>

namespace sssp {

using relaxation_heuristic = std::function<double(size_t node)>;

// Uses a heuristic to find nodes to relax.
// The heuristic has to *underestimate* the real cost, i.e., est_fn(node) <= distance(start, node).
class heuristic : public criteria {
  public:
    heuristic(const sssp::graph* graph, size_t start_node, const relaxation_heuristic& heuristic);
    virtual void relaxable_nodes(todo_output& output) const override;
    virtual void changed_predecessor(size_t node, size_t predecessor, double distance) override;
    virtual void relaxed_node(size_t node) override;
    virtual bool is_complete() const override { return false; }

  private:
    struct pred_info {
        pred_info(size_t index, double est_distance) : index(index), estimated_distance(est_distance) {}
        size_t index;
        double estimated_distance;
    };

    struct node_info {
        node_info(const sssp::graph& graph, const relaxation_heuristic& h, size_t index);
        size_t index;
        std::vector<pred_info> unsettled_predecessors;
        double tentative_distance = INFINITY;
        bool settled = false;
    };

    relaxation_heuristic m_heuristic;
    std::unordered_set<size_t, std::hash<size_t>, std::equal_to<size_t>> m_safe_to_relax;
    node_map<node_info> m_node_info;
};

} // namespace sssp


///////////////////////////////////////////////////////////////
//  Copyright 2012-21 John Maddock.
//  Copyright 2021 Iskandarov Lev. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MP_INTEGER_HPP
#define BOOST_MP_INTEGER_HPP

#include <type_traits>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/detail/bitscan.hpp>
#include <boost/multiprecision/detail/no_exceptions_support.hpp>
#include <boost/multiprecision/detail/standalone_config.hpp>

namespace boost {
namespace multiprecision {

template <class Integer, class I2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value && boost::multiprecision::detail::is_integral<I2>::value, Integer&>::type
multiply(Integer& result, const I2& a, const I2& b)
{
   return result = static_cast<Integer>(a) * static_cast<Integer>(b);
}
template <class Integer, class I2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value && boost::multiprecision::detail::is_integral<I2>::value, Integer&>::type
add(Integer& result, const I2& a, const I2& b)
{
   return result = static_cast<Integer>(a) + static_cast<Integer>(b);
}
template <class Integer, class I2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value && boost::multiprecision::detail::is_integral<I2>::value, Integer&>::type
subtract(Integer& result, const I2& a, const I2& b)
{
   return result = static_cast<Integer>(a) - static_cast<Integer>(b);
}

template <class Integer>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value>::type divide_qr(const Integer& x, const Integer& y, Integer& q, Integer& r)
{
   q = x / y;
   r = x % y;
}

template <class I1, class I2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<I1>::value && boost::multiprecision::detail::is_integral<I2>::value, I2>::type integer_modulus(const I1& x, I2 val)
{
   return static_cast<I2>(x % val);
}

namespace detail {
//
// Figure out the kind of integer that has twice as many bits as some builtin
// integer type I.  Use a native type if we can (including types which may not
// be recognised by boost::int_t because they're larger than long long),
// otherwise synthesize a cpp_int to do the job.
//
template <class I>
struct double_integer
{
   static constexpr const unsigned int_t_digits =
       2 * sizeof(I) <= sizeof(long long) ? std::numeric_limits<I>::digits * 2 : 1;

   using type = typename std::conditional<
       2 * sizeof(I) <= sizeof(long long),
       typename std::conditional<
           boost::multiprecision::detail::is_signed<I>::value && boost::multiprecision::detail::is_integral<I>::value,
           typename boost::multiprecision::detail::int_t<int_t_digits>::least,
           typename boost::multiprecision::detail::uint_t<int_t_digits>::least>::type,
       typename std::conditional<
           2 * sizeof(I) <= sizeof(double_limb_type),
           typename std::conditional<
               boost::multiprecision::detail::is_signed<I>::value && boost::multiprecision::detail::is_integral<I>::value,
               signed_double_limb_type,
               double_limb_type>::type,
           number<cpp_int_backend<sizeof(I) * CHAR_BIT * 2, sizeof(I) * CHAR_BIT * 2, (boost::multiprecision::detail::is_signed<I>::value ? signed_magnitude : unsigned_magnitude), unchecked, void> > >::type>::type;
};

} // namespace detail

template <class I1, class I2, class I3>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<I1>::value && boost::multiprecision::detail::is_unsigned<I2>::value && boost::multiprecision::detail::is_integral<I3>::value, I1>::type
powm(const I1& a, I2 b, I3 c)
{
   using double_type = typename detail::double_integer<I1>::type;

   I1          x(1), y(a);
   double_type result(0);

   while (b > 0)
   {
      if (b & 1)
      {
         multiply(result, x, y);
         x = integer_modulus(result, c);
      }
      multiply(result, y, y);
      y = integer_modulus(result, c);
      b >>= 1;
   }
   return x % c;
}

template <class I1, class I2, class I3>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<I1>::value && boost::multiprecision::detail::is_signed<I2>::value && boost::multiprecision::detail::is_integral<I2>::value && boost::multiprecision::detail::is_integral<I3>::value, I1>::type
powm(const I1& a, I2 b, I3 c)
{
   if (b < 0)
   {
      BOOST_MP_THROW_EXCEPTION(std::runtime_error("powm requires a positive exponent."));
   }
   return powm(a, static_cast<typename boost::multiprecision::detail::make_unsigned<I2>::type>(b), c);
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, std::size_t>::type lsb(const Integer& val)
{
   if (val <= 0)
   {
      if (val == 0)
      {
         BOOST_MP_THROW_EXCEPTION(std::domain_error("No bits were set in the operand."));
      }
      else
      {
         BOOST_MP_THROW_EXCEPTION(std::domain_error("Testing individual bits in negative values is not supported - results are undefined."));
      }
   }
   return detail::find_lsb(val);
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, std::size_t>::type msb(Integer val)
{
   if (val <= 0)
   {
      if (val == 0)
      {
         BOOST_MP_THROW_EXCEPTION(std::domain_error("No bits were set in the operand."));
      }
      else
      {
         BOOST_MP_THROW_EXCEPTION(std::domain_error("Testing individual bits in negative values is not supported - results are undefined."));
      }
   }
   return detail::find_msb(val);
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, bool>::type bit_test(const Integer& val, std::size_t index)
{
   Integer mask = 1;
   if (index >= sizeof(Integer) * CHAR_BIT)
      return 0;
   if (index)
      mask <<= index;
   return val & mask ? true : false;
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, Integer&>::type bit_set(Integer& val, std::size_t index)
{
   Integer mask = 1;
   if (index >= sizeof(Integer) * CHAR_BIT)
      return val;
   if (index)
      mask <<= index;
   val |= mask;
   return val;
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, Integer&>::type bit_unset(Integer& val, std::size_t index)
{
   Integer mask = 1;
   if (index >= sizeof(Integer) * CHAR_BIT)
      return val;
   if (index)
      mask <<= index;
   val &= ~mask;
   return val;
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, Integer&>::type bit_flip(Integer& val, std::size_t index)
{
   Integer mask = 1;
   if (index >= sizeof(Integer) * CHAR_BIT)
      return val;
   if (index)
      mask <<= index;
   val ^= mask;
   return val;
}

namespace detail {

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR Integer karatsuba_sqrt(const Integer& x, Integer& r, size_t bits)
{
   //
   // Define the floating point type used for std::sqrt, in our tests, sqrt(double) and sqrt(long double) take
   // about the same amount of time as long as long double is not an emulated 128-bit type (ie the same type
   // as __float128 from libquadmath).  So only use long double if it's an 80-bit type:
   //
#ifndef __clang__
   typedef typename std::conditional<(std::numeric_limits<long double>::digits == 64), long double, double>::type real_cast_type;
#else
   // clang has buggy __int128 -> long double conversion:
   typedef double real_cast_type;
#endif
   //
   // As per the Karatsuba sqrt algorithm, the low order bits/4 bits pay no part in the result, only in the remainder,
   // so define the number of bits our argument must have before passing to std::sqrt is safe, even if doing so
   // looses a few bits:
   //
   constexpr std::size_t cutoff = (std::numeric_limits<real_cast_type>::digits * 4) / 3;
   //
   // Type which can hold at least "cutoff" bits:
   //
#ifdef BOOST_HAS_INT128
   using cutoff_t = typename std::conditional<(cutoff > 64), uint128_type, std::uint64_t>::type;
#else
   using cutoff_t = std::uint64_t;
#endif
   //
   // See if we can take the fast path:
   //
   if (bits <= cutoff)
   {
      constexpr cutoff_t half_bits = (cutoff_t(1u) << ((sizeof(cutoff_t) * CHAR_BIT) / 2)) - 1;
      cutoff_t       val = static_cast<cutoff_t>(x);
      real_cast_type real_val = static_cast<real_cast_type>(val);
      cutoff_t       s64 = static_cast<cutoff_t>(std::sqrt(real_val));
      // converting to long double can loose some precision, and `sqrt` can give eps error, so we'll fix this
      // this is needed
      while ((s64 > half_bits) || (s64 * s64 > val))
         s64--;
      // in my tests this never fired, but theoretically this might be needed
      while ((s64 < half_bits) && ((s64 + 1) * (s64 + 1) <= val))
         s64++;
      r = static_cast<Integer>(val - s64 * s64);
      return static_cast<Integer>(s64);
   }
   // https://hal.inria.fr/file/index/docid/72854/filename/RR-3805.pdf
   std::size_t b = bits / 4;
   Integer q = x;
   q >>= b * 2;
   Integer s = karatsuba_sqrt(q, r, bits - b * 2);
   Integer t = 0u;
   bit_set(t, static_cast<unsigned>(b * 2));
   r <<= b;
   t--;
   t &= x;
   t >>= b;
   t += r;
   s <<= 1;
   divide_qr(t, s, q, r);
   r <<= b;
   t = 0u;
   bit_set(t, static_cast<unsigned>(b));
   t--;
   t &= x;
   r += t;
   s <<= (b - 1); // we already <<1 it before
   s += q;
   q *= q;
   // we substract after, so it works for unsigned integers too
   if (r < q)
   {
      t = s;
      t <<= 1;
      t--;
      r += t;
      s--;
   }
   r -= q;
   return s;
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR Integer bitwise_sqrt(const Integer& x, Integer& r)
{
   //
   // This is slow bit-by-bit integer square root, see for example
   // http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Binary_numeral_system_.28base_2.29
   // There are better methods such as http://hal.inria.fr/docs/00/07/28/54/PDF/RR-3805.pdf
   // and http://hal.inria.fr/docs/00/07/21/13/PDF/RR-4475.pdf which should be implemented
   // at some point.
   //
   Integer s = 0;
   if (x == 0)
   {
      r = 0;
      return s;
   }
   std::ptrdiff_t g = msb(x);
   if (g == 0)
   {
      r = 1;
      return s;
   }

   Integer t = 0;
   r = x;
   g /= 2;
   bit_set(s, g);
   bit_set(t, 2 * g);
   r = x - t;
   --g;
   do
   {
      t = s;
      t <<= g + 1;
      bit_set(t, 2 * g);
      if (t <= r)
      {
         bit_set(s, g);
         r -= t;
      }
      --g;
   } while (g >= 0);
   return s;
}

} // namespace detail

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, Integer>::type sqrt(const Integer& x, Integer& r)
{
#ifndef BOOST_MP_NO_CONSTEXPR_DETECTION
   // recursive Karatsuba sqrt can cause issues in constexpr context:
   if (BOOST_MP_IS_CONST_EVALUATED(x))
      return detail::bitwise_sqrt(x, r);
#endif
   if (x == 0u) {
      r = 0u;
      return 0u;
   }

   return detail::karatsuba_sqrt(x, r, msb(x) + 1);
}

template <class Integer>
BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<Integer>::value, Integer>::type sqrt(const Integer& x)
{
   Integer r(0);
   return sqrt(x, r);
}

}} // namespace boost::multiprecision

#endif


#include <boost/range/adaptors.hpp>
namespace ba = boost::adaptors;

template<class P>
template<class Filtration, class ReportPair>
void
dionysus::StandardReduction<P>::
operator()(const Filtration& filtration, const ReportPair& report_pair)
{
    using Cell = typename Filtration::Cell;
    (*this)(filtration, [](const Cell&) { return false; }, report_pair, no_progress);
}

template<class P>
template<class Filtration, class Relative, class ReportPair, class Progress>
void
dionysus::StandardReduction<P>::
operator()(const Filtration& filtration, const Relative& relative, const ReportPair& report_pair, const Progress& progress)
{
    persistence_.reserve(filtration.size());

    typedef     typename Filtration::Cell                       Cell;
    typedef     ChainEntry<Field, Cell>                         CellChainEntry;
    typedef     ChainEntry<Field, Index>                        ChainEntry;

    unsigned i = 0;
    for(auto& c : filtration)
    {
        progress();

        if (relative(c))
        {
            ++i;
            persistence_.add_skip();
            continue;
        }

        //std::cout << "Adding: " << c << " : " << boost::distance(c.boundary(persistence_.field())) << std::endl;
        Index pair = persistence_.add(c.boundary(persistence_.field()) |
                                                 ba::filtered([relative](const CellChainEntry& e) { return !relative(e.index()); }) |
                                                 ba::transformed([this,&filtration](const CellChainEntry& e)
                                                 { return ChainEntry(e.element(), filtration.index(e.index())); }));
        if (pair != persistence_.unpaired())
            report_pair(c.dimension(), pair, i);
        ++i;
    }
}


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2015 Bryce Adelstein Lelbach aka wash
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#include <mdspan>

#include <boost/core/lightweight_test.hpp>

using std::is_same;
using std::array;

using std::experimental::dyn;
using std::experimental::detail::make_dynamic_dims_array_t;

int main()
{
    // See notes in the definition of make_dynamic_dims_array, when there are no
    // dynamic extents we change the element type to unsigned char to ensure
    // that the size of the array is 1.
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<             >, array<unsigned char, 0>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<3            >, array<unsigned char, 0>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<3,   5       >, array<unsigned char, 0>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<3,   5,   9  >, array<unsigned char, 0>
    >::value));

    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<dyn          >, array<std::size_t, 1>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<dyn, dyn     >, array<std::size_t, 2>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<dyn, dyn, dyn>, array<std::size_t, 3>
    >::value));

    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<dyn, 5,   9  >, array<std::size_t, 1>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<3,   dyn, 9  >, array<std::size_t, 1>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<3,   5,   dyn>, array<std::size_t, 1>
    >::value));

    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<3,   dyn, dyn>, array<std::size_t, 2>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<dyn, 5,   dyn>, array<std::size_t, 2>
    >::value));
    BOOST_TEST((is_same<
        make_dynamic_dims_array_t<dyn, dyn, 9  >, array<std::size_t, 2>
    >::value));

    return boost::report_errors();
}