// gcc -O2 -march=native -DNDEBUG -o sfact sfact.cpp -std=c++11 -lstdc++ -lm -I../.. -I/usr/include/QtGui/ ../../libs/spacegroups/spacegroup.cpp ../../libs/spacegroups/crystalsys.cpp ../../libs/globals.cpp ../../libs/formfactors/formfact.cpp ../../tlibs/log/log.cpp -DNO_QT -lboost_system -lboost_filesystem -lboost_iostreams
/**
 * generates structure factors
 * @author Tobias Weber <tobias.weber@tum.de>
 * @date nov-2015
 * @license GPLv2
 */

#include <vector>
#include <unordered_map>
#include <tuple>
#include <algorithm>
#include <sstream>
#include "tlibs/math/linalg_ops.h"
#include "tlibs/phys/atoms.h"
#include "tlibs/phys/mag.h"
#include "tlibs/phys/lattice.h"
#include "tlibs/phys/neutrons.h"
#include "tlibs/string/string.h"
#include "tlibs/file/prop.h"
#include "libs/spacegroups/spacegroup.h"
#include "libs/formfactors/formfact.h"
#include "libs/globals.h"

namespace ublas = tl::ublas;

using t_real = t_real_glob;
typedef ublas::vector<t_real> t_vec;
typedef ublas::matrix<t_real> t_mat;

const std::string g_strXmlRoot("taz/");

// --------------------------------------------------------------------------------------------
// Lattice
// --------------------------------------------------------------------------------------------
static inline tl::Lattice<t_real> enter_lattice()
{
	std::cout << "\n----------------------------------------------------------------------\n" << std::endl;
	t_real a,b,c, alpha,beta,gamma;
	std::cout << "Enter unit cell lattice constants [a b c (in A)]: ";
	std::cin >> a >> b >> c;
	std::cout << "Enter unit cell angles [alpha beta gamma (in deg)]: ";
	std::cin >> alpha >> beta >> gamma;

	alpha = tl::d2r(alpha);
	beta = tl::d2r(beta);
	gamma = tl::d2r(gamma);

	return tl::Lattice<t_real>(a,b,c, alpha,beta,gamma);
}

static tl::Lattice<t_real> load_lattice(const tl::Prop<>& file)
{
	t_real a = file.Query<t_real>((g_strXmlRoot + "sample/a").c_str(), 0.);
	t_real b = file.Query<t_real>((g_strXmlRoot + "sample/b").c_str(), 0.);
	t_real c = file.Query<t_real>((g_strXmlRoot + "sample/c").c_str(), 0.);
	t_real alpha = file.Query<t_real>((g_strXmlRoot + "sample/alpha").c_str(), 0.);
	t_real beta = file.Query<t_real>((g_strXmlRoot + "sample/beta").c_str(), 0.);
	t_real gamma = file.Query<t_real>((g_strXmlRoot + "sample/gamma").c_str(), 0.);

	alpha = tl::d2r(alpha);
	beta = tl::d2r(beta);
	gamma = tl::d2r(gamma);

	return tl::Lattice<t_real>(a,b,c, alpha,beta,gamma);
}
// --------------------------------------------------------------------------------------------

// --------------------------------------------------------------------------------------------
// Space group
// --------------------------------------------------------------------------------------------
static inline std::string enter_spacegroup()
{
	std::cout << "\n----------------------------------------------------------------------\n" << std::endl;
	std::string strSg;
	std::cout << "Enter spacegroup: ";
	std::cin.ignore();
	std::getline(std::cin, strSg);

	return strSg;
}

static std::string load_spacegroup(const tl::Prop<>& file)
{
	std::string strSG = file.Query<std::string>((g_strXmlRoot + "sample/spacegroup").c_str(), "");
	tl::trim(strSG);

	return strSG;
}
// --------------------------------------------------------------------------------------------

// --------------------------------------------------------------------------------------------
// Atom positions
// --------------------------------------------------------------------------------------------
static inline
std::tuple<std::vector<std::string>, std::vector<t_vec>, std::unordered_map<std::string, t_real>>
enter_atoms()
{
	std::cout << "\n----------------------------------------------------------------------\n" << std::endl;
	int iAtom=0;
	std::vector<std::string> vecElems;
	std::vector<t_vec> vecAtoms;
	std::unordered_map<std::string, t_real> mapMag;

	while(1)
	{
		std::cout << "Enter element name " << (++iAtom) << " name (or <Enter> to finish): ";
		std::string strElem;
		std::getline(std::cin, strElem);
		tl::trim(strElem);
		if(strElem == "")
			break;

		std::cout << "Enter atom position " << (iAtom) << " [x y z (in frac. units)]: ";
		std::string strAtom;
		std::getline(std::cin, strAtom);
		tl::trim(strAtom);
		if(strAtom == "")
			break;

		std::cout << "Enter atom " << (iAtom) << " effective magnetic moment [muB]: ";
		std::string strMag;
		std::getline(std::cin, strMag);
		tl::trim(strMag);
		if(strMag == "")
			strMag = "0";

		vecElems.push_back(strElem);

		t_vec vec(4);
		std::istringstream istrAtom(strAtom);
		istrAtom >> vec[0] >> vec[1] >> vec[2];
		vec[3] = 1.;
		vecAtoms.push_back(vec);

		t_real dMag;
		std::istringstream istrMag(strMag);
		istrMag >> dMag;
		dMag = tl::mag_scatlen_eff(dMag);
		mapMag[strElem] = dMag;
	}

	return std::make_tuple(vecElems, vecAtoms, mapMag);
}

static inline
std::tuple<std::vector<std::string>, std::vector<t_vec>, std::unordered_map<std::string, t_real>>
load_atoms(const tl::Prop<>& file)
{
	std::vector<std::string> vecElems;
	std::vector<t_vec> vecAtoms;
	std::unordered_map<std::string, t_real> mapMag;

	std::size_t iNumAtoms = file.Query<std::size_t>((g_strXmlRoot + "sample/atoms/num").c_str(), 0);
	vecElems.reserve(iNumAtoms);
	vecAtoms.reserve(iNumAtoms);

	for(std::size_t iAtom=0; iAtom<iNumAtoms; ++iAtom)
	{
		std::string strNr = tl::var_to_str(iAtom);

		std::string strAtomName =
			file.Query<std::string>((g_strXmlRoot + "sample/atoms/" + strNr + "/name").c_str(), "");

		t_vec vec(4);
		vec[0] = file.Query<t_real>((g_strXmlRoot + "sample/atoms/" + strNr + "/x").c_str(), 0.);
		vec[1] = file.Query<t_real>((g_strXmlRoot + "sample/atoms/" + strNr + "/y").c_str(), 0.);
		vec[2] = file.Query<t_real>((g_strXmlRoot + "sample/atoms/" + strNr + "/z").c_str(), 0.);
		vec[3] = 1.;

		vecElems.push_back(strAtomName);
		vecAtoms.push_back(vec);
	}

	return std::make_tuple(vecElems, vecAtoms, mapMag);
}
// --------------------------------------------------------------------------------------------

void gen_atoms_sfact(const char *pcFile = nullptr)
{
	bool bHasFile = (pcFile != nullptr);
	tl::Prop<> file;
	if(bHasFile)
	{
		bHasFile = file.Load(pcFile, tl::PropType::XML);
		if(!bHasFile)
			std::cerr << "Error: Invalid file \"" << pcFile << "\"." << std::endl;
	}

	// --------------------------------------------------------------------------------------------
	// Tables
	// --------------------------------------------------------------------------------------------
	std::shared_ptr<const xtl::ScatlenList<t_real>> lst = xtl::ScatlenList<t_real>::GetInstance();
	std::shared_ptr<const xtl::FormfactList<t_real>> lstff = xtl::FormfactList<t_real>::GetInstance();
	std::shared_ptr<const xtl::MagFormfactList<t_real>> lstmff = xtl::MagFormfactList<t_real>::GetInstance();
	std::shared_ptr<const xtl::SpaceGroups<t_real>> sgs = xtl::SpaceGroups<t_real>::GetInstance();
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Lattice
	// --------------------------------------------------------------------------------------------
	tl::Lattice<t_real> lattice;
	if(bHasFile)
		lattice = load_lattice(file);
	else
		lattice = enter_lattice();

	const t_real dVol = lattice.GetVol();
	const t_mat matA = lattice.GetBaseMatrixCov();
	const t_mat matB = lattice.GetRecip().GetBaseMatrixCov();
	std::cout << "Unit cell volume: " << dVol << " A^3" << std::endl;
	std::cout << "A = " << matA << std::endl;
	std::cout << "B = " << matB << std::endl;
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Space group
	// --------------------------------------------------------------------------------------------
	std::string strSg;
	if(bHasFile)
		strSg = load_spacegroup(file);
	else
		strSg = enter_spacegroup();

	const auto pSg = sgs->Find(strSg);
	if(!pSg)
	{
		std::cerr << "Error: Unknown spacegroup." << std::endl;
		return;
	}
	std::cout << "Spacegroup " << pSg->GetNr() << ": " << pSg->GetName() << "." << std::endl;

	const std::vector<t_mat>& vecTrafos = pSg->GetTrafos();
	std::cout << vecTrafos.size() << " symmetry operations in spacegroup." << std::endl;
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Atom positions
	// --------------------------------------------------------------------------------------------
	int iAtom=0;
	std::vector<std::string> vecElems;
	std::vector<t_vec> vecAtoms;
	std::unordered_map<std::string, t_real> mapMag;

	if(bHasFile)
		std::tie(vecElems, vecAtoms, mapMag) = load_atoms(file);
	else
		std::tie(vecElems, vecAtoms, mapMag) = enter_atoms();
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Unit cell & Scattering lengths (assuming magnetic unit cell is the same as structural one)
	// --------------------------------------------------------------------------------------------
	std::vector<unsigned int> vecNumAtoms;
	std::vector<t_vec> vecAllAtoms;
	std::vector<std::complex<t_real>> vecScatlens;
	std::vector<std::complex<t_real>> vecMagScatlens;
	std::vector<int> vecAtomIndices;

	t_real dsigCoh = 0.;
	t_real dsigInc = 0.;
	t_real dsigScat = 0.;
	t_real dsigAbs = 0.;

	t_real dSigAbs = 0.;
	t_real dSigScat = 0.;

	for(int iAtom=0; iAtom<int(vecAtoms.size()); ++iAtom)
	{
		// generate symmetry-equivalent atom positions
		const t_vec& vecAtom = vecAtoms[iAtom];
		std::vector<t_vec> vecPos = tl::generate_atoms<t_mat, t_vec, std::vector>(vecTrafos, vecAtom);
		vecNumAtoms.push_back(vecPos.size());
		std::cout << "Generated " << vecPos.size() << " " << vecElems[iAtom] << " atoms." << std::endl;
		for(const t_vec& vec : vecPos)
			std::cout << vec << std::endl;

		// get scattering lengths
		const xtl::ScatlenList<t_real>::elem_type* pElem = lst->Find(vecElems[iAtom]);
		if(pElem == nullptr)
		{
			std::cerr << "Error: cannot get scattering length for "
				<< vecElems[iAtom] << "." << std::endl;
			return;
		}
		std::complex<t_real> b = pElem->GetCoherent();

		// microscopic cross-sections
		dsigCoh += pElem->GetXSecCoherent().real()*vecNumAtoms[iAtom];
		dsigInc += pElem->GetXSecIncoherent().real()*vecNumAtoms[iAtom];
		dsigScat += pElem->GetXSecScatter().real()*vecNumAtoms[iAtom];
		dsigAbs += pElem->GetXSecAbsorption().real()*vecNumAtoms[iAtom];

		// get magnetic scattering lengths
		std::complex<t_real> p(0.);
		auto iterMag = mapMag.find(vecElems[iAtom]);
		if(iterMag == mapMag.end())
		{
			std::cerr << "Warning: cannot get effective magnetic scattering length for "
				<< vecElems[iAtom] << "." << std::endl;
		}
		else
		{
			p = iterMag->second;
		}

		// macroscopic cross-sections
		dSigScat += tl::macro_xsect(pElem->GetXSecScatter().real()*tl::get_one_femtometer<t_real>()*tl::get_one_femtometer<t_real>(),
			vecNumAtoms[iAtom],
			dVol*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()) * tl::get_one_centimeter<t_real>();

		dSigAbs += tl::macro_xsect(pElem->GetXSecAbsorption().real()*tl::get_one_femtometer<t_real>()*tl::get_one_femtometer<t_real>(),
			vecNumAtoms[iAtom],
			dVol*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()) * tl::get_one_centimeter<t_real>();

		// store calculations
		for(t_vec vecThisAtom : vecPos)
		{
			vecThisAtom.resize(3,1);
			vecAllAtoms.push_back(tl::mult<t_mat, t_vec>(matA, vecThisAtom));
			vecScatlens.push_back(b);
			vecMagScatlens.push_back(p);
			vecAtomIndices.push_back(iAtom);
		}
	}

	std::cout << "\nMicroscopic coherent cross-section: " << dsigCoh << " fm^2." << std::endl;
	std::cout << "Microscopic incoherent cross-section: " << dsigInc << " fm^2." << std::endl;
	std::cout << "Microscopic total scattering cross-section: " << dsigScat << " fm^2." << std::endl;
	std::cout << "Microscopic absorption cross-section: " << dsigAbs << " fm^2." << std::endl;

	const t_real dLam0 = 1.8;	// thermal
	const t_real dLam = 4.5;
	std::cout << "\nMacroscopic total scattering cross-section for lambda = 4.5 A: "
		<< dSigScat*dLam/dLam0 << " / cm." << std::endl;
	std::cout << "Macroscopic absorption cross-section for lambda = 4.5 A: "
		<< dSigAbs*dLam/dLam0 << " / cm." << std::endl;

	//for(const t_vec& vecAt : vecAllAtoms) std::cout << vecAt << std::endl;
	//for(const std::complex<t_real>& cb : vecScatlens) std::cout << cb << std::endl;
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Bragg peaks
	// --------------------------------------------------------------------------------------------
	std::vector<t_real> vecFormfacts;
	std::vector<t_real> vecMagFormfacts;

	while(1)
	{
		std::cout << "\n----------------------------------------------------------------------\n" << std::endl;

		t_real h=0., k=0., l=0.;
		std::cout << "Enter h k l [rlu]: ";
		std::cin >> h >> k >> l;

		t_vec vecG = tl::mult<t_mat, t_vec>(matB, tl::make_vec({h,k,l}));
		t_real dG = ublas::norm_2(vecG);
		std::cout << "G = " << dG << " / A" << std::endl;

		vecFormfacts.clear();
		vecMagFormfacts.clear();
		for(unsigned int iAtom=0; iAtom<vecAllAtoms.size(); ++iAtom)
		{
			//const t_vec& vecAtom = vecAllAtoms[iAtom];
			const xtl::FormfactList<t_real>::elem_type* pElemff = lstff->Find(vecElems[vecAtomIndices[iAtom]]);
			const xtl::MagFormfactList<t_real>::elem_type* pElemMff = lstmff->Find(vecElems[vecAtomIndices[iAtom]]);

			if(pElemff == nullptr)
			{
				std::cerr << "Error: cannot get form factor for "
					<< vecElems[vecAtomIndices[iAtom]] << "." << std::endl;
				return;
			}
			/*if(pElemMff == nullptr)
			{
				std::cerr << "Warning: cannot get magnetic form factor for "
					<< vecElems[vecAtomIndices[iAtom]] << "." << std::endl;
			}*/

			t_real dFF = pElemff ? pElemff->GetFormfact(dG) : 0.;
			vecFormfacts.push_back(dFF);

			t_real dMFF = pElemMff ? pElemMff->GetFormfact(dG) : 0.;
			vecMagFormfacts.push_back(dMFF);
		}

		std::vector<std::complex<t_real>> vecMag(vecMagScatlens.size());
		if(vecMagScatlens.size() != vecMagFormfacts.size())
		{
			std::cerr << "Size mismatch in magnetic scattering lengths and form factors."
				<< std::endl;
		}
		else
		{
			std::transform(vecMagScatlens.begin(), vecMagScatlens.end(), vecMagFormfacts.begin(),
				vecMag.begin(), [](const std::complex<t_real>& c, t_real d) -> std::complex<t_real>
			{
				// multiply eff. mag. scattering lengths with respective form factors
				return c * d;
			});
		}

		std::complex<t_real> F = tl::structfact<t_real, std::complex<t_real>, ublas::vector<t_real>, std::vector>
			(vecAllAtoms, vecG, vecScatlens);
			std::complex<t_real> Fm = tl::structfact<t_real, std::complex<t_real>, ublas::vector<t_real>, std::vector>
			(vecAllAtoms, vecG, vecMag);
		std::complex<t_real> Fx = tl::structfact<t_real, t_real, ublas::vector<t_real>, std::vector>
			(vecAllAtoms, vecG, vecFormfacts);

		std::cout << std::endl;
		std::cout << "Neutron nuclear structure factor: " << std::endl;
		t_real dFsq = (std::conj(F)*F).real();
		std::cout << "F = " << F << " fm" << std::endl;
		std::cout << "|F| = " << std::sqrt(dFsq) << " fm" << std::endl;
		std::cout << "|F|^2 = " << dFsq << " fm^2" << std::endl;

		std::cout << std::endl;
		std::cout << "Neutron magnetic structure factor: " << std::endl;
		t_real dFmsq = (std::conj(Fm)*Fm).real();
		std::cout << "Fm = " << Fm << " fm" << std::endl;
		std::cout << "|Fm| = " << std::sqrt(dFmsq) << " fm" << std::endl;
		std::cout << "|Fm|^2 = " << dFmsq << " fm^2" << std::endl;

		std::cout << std::endl;
		std::cout << "X-ray atomic structure factor: " << std::endl;
		t_real dFxsq = (std::conj(Fx)*Fx).real();
		std::cout << "Fx = " << Fx << std::endl;
		std::cout << "|Fx| = " << std::sqrt(dFxsq) << std::endl;
		std::cout << "|Fx|^2 = " << dFxsq << std::endl;
	}
	// --------------------------------------------------------------------------------------------
}

#include "libs/version.h"
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

int main(int argc, char** argv)
{
#ifdef NO_TERM_CMDS
	tl::Log::SetUseTermCmds(0);
#endif

	// plain C locale
	/*std::*/setlocale(LC_ALL, "C");
	std::locale::global(std::locale::classic());

	// Header
	tl::log_info("Libcrystal structure factor calculator.");
	tl::log_info("Written by Tobias Weber <tobias.weber@tum.de>, 2014-2017.");
	tl::log_info("License: GPLv2.");

	// possible resource paths
	const char* pcProg = argv[0];
	fs::path path(pcProg);
	path.remove_filename();
	tl::log_info("Program path: ", path.string());

	add_resource_path(path.string());
	add_resource_path((path / "..").string());
	add_resource_path((path / "resources").string());
	add_resource_path((path / "Resources").string());
	add_resource_path((path / ".." / "resources").string());
	add_resource_path((path / ".." / "Resources").string());

	// load a taz file if given
	const char *pcFile = nullptr;
	if(argc >= 2)
	{
		pcFile = argv[1];
		std::cout << "Using crystal file \"" << pcFile << "\"." << std::endl;
	}

	try
	{
		gen_atoms_sfact(pcFile);
	}
	catch(const std::exception& err)
	{
		std::cerr << err.what() << std::endl;
	}

	return 0;
}


#include "singular.h"

#include <boost/random/poisson_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/beta_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

namespace statiskit
{
    SingularDistribution::~SingularDistribution()
    {}

    double SingularDistribution::loglikelihood(const MultivariateData& data) const
    {
        double llh = 0.;
        std::unique_ptr< MultivariateData::Generator > generator = data.generator();
        while(generator->is_valid() && boost::math::isfinite(llh))
        {
            double weight = generator->weight();
            if(weight > 0.)
            { llh += weight * probability(generator->event(), true); }
            ++(*generator);
        }
        return llh;
    }

    MultinomialSingularDistribution::MultinomialSingularDistribution(const Eigen::VectorXd& pi)
    {
        _pi = Eigen::VectorXd::Zero(pi.size());
        set_pi(pi);
    }

    MultinomialSingularDistribution::MultinomialSingularDistribution(const MultinomialSingularDistribution& splitting)
    { _pi = splitting._pi; }

    MultinomialSingularDistribution::~MultinomialSingularDistribution()
    {}

    Index MultinomialSingularDistribution::get_nb_components() const
    { return _pi.size(); }

    unsigned int MultinomialSingularDistribution::get_nb_parameters() const
    { return _pi.size() - 1; }

    double MultinomialSingularDistribution::probability(const MultivariateEvent* event, const bool& logarithm) const
    {
        double p;
        if(event && event->size() == get_nb_components())
        {
            try
            {
                p = 0.;
                int sum = 0;
                for(Index component = 0, max_component = get_nb_components(); component < max_component; ++component)
                {
                    const UnivariateEvent* uevent = event->get(component);
                    if(uevent)
                    {
                        if(uevent->get_outcome() == DISCRETE && uevent->get_event() == ELEMENTARY)
                        {
                            int value = static_cast< const DiscreteElementaryEvent* >(uevent)->get_value();
                            if(!(_pi[component] <= 0. && value == 0))
                            {
                                p += value * log(_pi[component]) - boost::math::lgamma(value + 1);
                                sum += value;
                            }
                        }
                        else
                        { throw std::exception(); }
                    }
                }
                p += boost::math::lgamma(sum + 1);
            }
            catch(const std::exception& error)
            { p = log(0.); }
        }
        else
        { p = log(0.); }
        if(!logarithm)
        { p = exp(p); }
        return p;
    }

    std::unique_ptr< MultivariateEvent > MultinomialSingularDistribution::simulate(unsigned int sum) const
    {
        double pi = 0.;
        Index component = 0, max_component = get_nb_components() - 1;
        std::unique_ptr< VectorEvent > event = std::make_unique< VectorEvent >(max_component + 1);
        while(component < max_component && sum > 0)
        {
            boost::binomial_distribution<> dist(sum, _pi[component] / (1 - pi));
            boost::variate_generator<boost::mt19937&, boost::binomial_distribution<> > simulator(__impl::get_random_generator(), dist);
            int value = simulator();
            pi += _pi[component];
            event->set(component, DiscreteElementaryEvent(value));
            sum -= value;
            ++component;
        }
        for(; component < max_component; ++component)
        { event->set(component, DiscreteElementaryEvent(0)); }
        event->set(max_component, DiscreteElementaryEvent(sum));
        return std::move(event);
    }

    const Eigen::VectorXd& MultinomialSingularDistribution::get_pi() const
    { return _pi; }

    void MultinomialSingularDistribution::set_pi(const Eigen::VectorXd& pi)
    {
        if(pi.rows() == _pi.size() - 1)
        {
            Index j = 0;
            while(j < pi.rows() && pi[j] >= 0.)
            { ++j; }
            if(j < pi.rows())
            { throw parameter_error("pi", "contains negative values"); }
            double sum = pi.sum();
            if(sum < 1)
            {
                _pi.block(0, 0, _pi.size() - 1, 1) = pi / sum;
                _pi[_pi.size()-1] = 1 - sum;
            }
            else
            { throw parameter_error("pi", "last category values"); }
        }
        else if(pi.rows() == _pi.size())
        {
            Index j = 0;
            while(j < pi.rows() && pi[j] >= 0.)
            { ++j; }
            if(j < pi.rows())
            { throw parameter_error("pi", "contains negative values"); }
            _pi = pi / pi.sum();
        }
        else
        { throw parameter_error("pi", "number of parameters"); }
    }

    DirichletMultinomialSingularDistribution::DirichletMultinomialSingularDistribution(const Eigen::VectorXd& alpha)
    {
        _alpha = Eigen::VectorXd::Zero(alpha.size());
        set_alpha(alpha);
    }

    DirichletMultinomialSingularDistribution::DirichletMultinomialSingularDistribution(const DirichletMultinomialSingularDistribution& splitting)
    { _alpha = splitting._alpha; }

    DirichletMultinomialSingularDistribution::~DirichletMultinomialSingularDistribution()
    {}

    Index DirichletMultinomialSingularDistribution::get_nb_components() const
    { return _alpha.size(); }

    unsigned int DirichletMultinomialSingularDistribution::get_nb_parameters() const
    { return _alpha.size(); }

    double DirichletMultinomialSingularDistribution::probability(const MultivariateEvent* event, const bool& logarithm) const
    {
        double p;
        if(event && event->size() == get_nb_components())
        {
            try
            {
                p = 0.;
                int sum = 0;
                for(Index component = 0, max_component = get_nb_components(); component < max_component; ++component)
                {
                    const UnivariateEvent* uevent = event->get(component);
                    if(uevent)
                    {
                        if(uevent->get_outcome() == DISCRETE && uevent->get_event() == ELEMENTARY)
                        {
                            int value = static_cast< const DiscreteElementaryEvent* >(uevent)->get_value();
                            if(!(_alpha[component] <= 0. && value == 0))
                            {
                                p += boost::math::lgamma(_alpha[component] + value);
                                p -= boost::math::lgamma(_alpha[component]) + boost::math::lgamma(value + 1);
                                sum += value;
                            }
                        }
                        else
                        { throw std::exception(); }
                    }
                }
                double alpha = _alpha.sum();
                p += boost::math::lgamma(sum + 1) + boost::math::lgamma(alpha) - boost::math::lgamma(alpha + sum);
            }
            catch(const std::exception& error)
            { p = log(0.); }
        }
        else
        { p = log(0.); }
        if(!logarithm)
        { p = exp(p); }
        return p;
    }

    std::unique_ptr< MultivariateEvent > DirichletMultinomialSingularDistribution::simulate(unsigned int sum) const
    {
        Eigen::VectorXd _pi = Eigen::VectorXd::Zero(get_nb_components());
        for(Index component = 0, max_component = get_nb_components(); component < max_component; ++component)
        {
            boost::random::gamma_distribution<> dist(_alpha(component), 1.);
            boost::variate_generator<boost::mt19937&, boost::random::gamma_distribution<> > simulator(__impl::get_random_generator(), dist);
            _pi(component) = simulator();
        }
        _pi /= _pi.sum();
        double pi = 0.;
        Index component = 0, max_component = get_nb_components() - 1;
        std::unique_ptr< VectorEvent > event = std::make_unique< VectorEvent >(max_component + 1);
        while(component < max_component && sum > 0)
        {
            boost::binomial_distribution<> dist(sum, _pi[component] / (1 - pi));
            boost::variate_generator<boost::mt19937&, boost::binomial_distribution<> > simulator(__impl::get_random_generator(), dist);
            int value = simulator();
            pi += _pi[component];
            event->set(component, DiscreteElementaryEvent(value));
            sum -= value;
            ++component;
        }
        for(; component < max_component; ++component)
        { event->set(component, DiscreteElementaryEvent(0)); }
        event->set(max_component, DiscreteElementaryEvent(sum));
        return std::move(event);
    }

    const Eigen::VectorXd& DirichletMultinomialSingularDistribution::get_alpha() const
    { return _alpha; }

    void DirichletMultinomialSingularDistribution::set_alpha(const Eigen::VectorXd& alpha)
    {
        if(alpha.rows() == _alpha.size())
        {
            Index j = 0;
            while(j < alpha.rows() && alpha[j] >= 0.)
            { ++j; }
            if(j < alpha.rows())
            { throw parameter_error("alpha", "contains negative values"); }
            _alpha = alpha;
        }
        else
        { throw parameter_error("alpha", "number of parameters"); }
    }
}


#pragma once
#include <Eigen/Dense>
#include <unsupported/Eigen/KroneckerProduct>

#include <vector>

#include "../utils.hpp"

namespace yavque
{
enum class Pauli : char
{
	X = 'X',
	Y = 'Y',
	Z = 'Z'
};

namespace detail
{
	class CompressedPauliString
	{
	private:
		const std::vector<Pauli> pstring_;
		Eigen::MatrixXcd mat_;
		mutable bool diagonalized_ = false;
		mutable Eigen::MatrixXcd evecs_;
		mutable Eigen::MatrixXcd evals_;

		static std::vector<Pauli> construct_pauli(const std::string& str)
		{
			std::vector<Pauli> res;
			for(auto c : str)
			{
				res.emplace_back(Pauli(c));
			}
			return res;
		}

		static Eigen::MatrixXcd get_pauli(Pauli p)
		{
			switch(p)
			{
			case Pauli::X:
				return pauli_x().cast<cx_double>();
			case Pauli::Y:
				return pauli_y();
			case Pauli::Z:
				return pauli_z().cast<cx_double>();
			}
			__builtin_unreachable();
			return Eigen::MatrixXcd();
		}

		void diagonalize() const
		{
			if(!diagonalized_)
			{
				Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> es(mat_);
				evecs_ = es.eigenvectors();
				evals_ = es.eigenvalues();
				diagonalized_ = true;
			}
		}

		static uint32_t change_bits(const std::vector<uint32_t>& indices,
		                            uint32_t bitstring, uint32_t bits_to_change)
		{
			for(uint32_t n = 0; n < indices.size(); ++n)
			{
				uint32_t b = (bits_to_change >> n) & 1U;
				bitstring = (bitstring & (~(1U << indices[n]))) | (b << indices[n]);
			}
			return bitstring;
		}

		static uint32_t bits(const std::vector<uint32_t>& indices, uint32_t bitstring)
		{
			uint32_t b = 0U;

			for(uint32_t k = 0; k < indices.size(); ++k)
			{
				b |= ((bitstring >> indices[k]) & 1U) << k;
			}
			return b;
		}

		void construct_matrix()
		{
			mat_ = Eigen::MatrixXcd::Ones(1, 1);
			for(auto iter = pstring_.rbegin(); iter != pstring_.rend(); ++iter)
			{
				mat_ = Eigen::kroneckerProduct(mat_, get_pauli(*iter)).eval();
			}
		}

	public:
		explicit CompressedPauliString(const std::string& str)
			: pstring_{construct_pauli(str)}
		{
			construct_matrix();
		}

		explicit CompressedPauliString(std::vector<Pauli> pvec)
			: pstring_{std::move(pvec)}
		{
			construct_matrix();
		}

		Pauli at(uint32_t idx) const { return pstring_[idx]; }

		Eigen::VectorXcd apply(const std::vector<uint32_t>& indices,
		                       const Eigen::VectorXcd& vec) const
		{
			assert(pstring_.size() == indices.size());
			uint32_t dim = 1U << pstring_.size();
			Eigen::VectorXcd res = Eigen::VectorXcd::Zero(vec.size());

			if(indices.size() == 1)
			{
				return apply_single_qubit(vec, mat_, indices[0]);
			}
			if(indices.size() == 2)
			{
				return apply_two_qubit(vec, mat_, {indices[0], indices[1]});
			}
			if(indices.size() == 3)
			{
				return apply_three_qubit(vec, mat_, {indices[0], indices[1], indices[2]});
			}

			for(uint32_t k = 0; k < vec.size(); ++k)
			{
				cx_double v = 0.0;
				uint32_t row = bits(indices, k);
				for(uint32_t col = 0; col < dim; ++col)
				{
					uint32_t l = change_bits(indices, k, col);
					v += mat_(row, col) * vec(l);
				}
				res(k) = v;
			}
			return res;
		}

		/* return exp(t*P) applied to indices */
		Eigen::VectorXcd apply_exp(cx_double t, const std::vector<uint32_t>& indices,
		                           const Eigen::VectorXcd& vec) const
		{
			assert(pstring_.size() == indices.size());
			uint32_t dim = 1U << pstring_.size();
			Eigen::VectorXcd res = Eigen::VectorXcd::Zero(vec.size());

			if(!diagonalized_)
			{
				diagonalize();
			}

			Eigen::VectorXcd p = (t * evals_.array()).exp();
			Eigen::MatrixXcd exp_mat = evecs_ * p.asDiagonal() * evecs_.adjoint();

			if(indices.size() == 1)
			{
				return apply_single_qubit(vec, exp_mat, indices[0]);
			}
			if(indices.size() == 2)
			{
				return apply_two_qubit(vec, exp_mat, {indices[0], indices[1]});
			}
			if(indices.size() == 3)
			{
				return apply_three_qubit(vec, exp_mat,
				                         {indices[0], indices[1], indices[2]});
			}

			for(uint32_t k = 0; k < vec.size(); ++k)
			{
				cx_double v = 0.0;
				uint32_t row = bits(indices, k);
				for(uint32_t col = 0; col < dim; ++col)
				{
					uint32_t l = change_bits(indices, k, col);
					v += exp_mat(row, col) * vec(l);
				}
				res(k) = v;
			}
			return res;
		}
	};
} // namespace detail

class CPSFactory
{
private:
	std::map<std::string, std::weak_ptr<detail::CompressedPauliString>> map_;

	CPSFactory() = default;

public:
	CPSFactory(const CPSFactory&) = delete;
	CPSFactory& operator=(const CPSFactory&) = delete;

	CPSFactory(CPSFactory&&) = delete;
	CPSFactory& operator=(CPSFactory&&) = delete;
	~CPSFactory() = default;

	std::shared_ptr<detail::CompressedPauliString>
	get_pauli_string_for(const std::string& pstr)
	{
		auto iter = map_.find(pstr);
		if((iter == map_.end()) || iter->second.expired())
		{
			auto p = std::make_shared<detail::CompressedPauliString>(pstr);
			map_[pstr] = p;
			return p;
		}
		return std::shared_ptr<detail::CompressedPauliString>(map_[pstr]);
	}

	static CPSFactory& get_instance()
	{
		static CPSFactory instance;
		return instance;
	}
};
} // namespace yavque


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/function/jacobi_zeta.hpp>
#include <boost/math/special_functions/jacobi_zeta.hpp>
#include <eve/function/next.hpp>
#include <eve/function/prev.hpp>
#include <eve/function/is_denormal.hpp>
#include <eve/constant/nan.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/minlog.hpp>
#include <eve/platform.hpp>

#include <cmath>

TTS_CASE_TPL("Check eve::jacobi_zeta return type", EVE_TYPE)
{
  TTS_EXPR_IS(eve::jacobi_zeta(T(0), T(0)), T);
}

TTS_CASE_TPL("Check eve::jacobi_zeta behavior two parameter", EVE_TYPE)
{
  using v_t = eve::element_type_t<T>;
  using eve::as;
  if constexpr( eve::platform::supports_invalids )
  {
   TTS_IEEE_EQUAL(eve::jacobi_zeta(eve::pio_4(as<T>()), eve::nan(eve::as<T>())) , eve::nan(eve::as<T>()) );
   TTS_ULP_EQUAL(eve::jacobi_zeta(eve::pio_2(as<T>()), T(1)) , eve::zero(eve::as<T>()), 0.5);
   TTS_ULP_EQUAL(eve::jacobi_zeta(eve::pio_2(as<T>()), T(-1)), eve::zero(eve::as<T>()), 0.5);
  }

  TTS_ULP_EQUAL( eve::jacobi_zeta(eve::pio_2(as<T>()), T( 0.)),  eve::zero(eve::as<T>()), 0.5);
  TTS_ULP_EQUAL( eve::jacobi_zeta(eve::pio_2(as<T>()), T( 0.5)), eve::zero(eve::as<T>()), 4);
  TTS_ULP_EQUAL( eve::jacobi_zeta(eve::pio_2(as<T>()), T( 0.9)), eve::zero(eve::as<T>()), 4.55);

  TTS_ULP_EQUAL( eve::jacobi_zeta(eve::pio_4(as<T>()), T( 0.)),  T(boost::math::jacobi_zeta(v_t(0)  , eve::pio_4(as<v_t>()))), 0.5);
  TTS_ULP_EQUAL( eve::jacobi_zeta(eve::pio_4(as<T>()), T( 0.5)), T(boost::math::jacobi_zeta(v_t(0.5), eve::pio_4(as<v_t>()))), 1.0);
  TTS_ULP_EQUAL( eve::jacobi_zeta(eve::pio_4(as<T>()), T( 0.9)), T(boost::math::jacobi_zeta(v_t(0.9), eve::pio_4(as<v_t>()))), 1.0);
  TTS_ULP_EQUAL( eve::jacobi_zeta(3*eve::pio_4(as<T>()), T( 0.)),  T(boost::math::jacobi_zeta(v_t(0)  , 3*eve::pio_4(as<v_t>()))), 0.5);
  TTS_ULP_EQUAL( eve::jacobi_zeta(T(100), T( 0.)),  T(boost::math::jacobi_zeta(v_t(0)  , v_t(100))), 0.5);
  TTS_ULP_EQUAL( eve::jacobi_zeta(T(1.5), T(1)),    T(boost::math::jacobi_zeta(v_t(1)  , v_t(1.5))), 1.0);
  TTS_ULP_EQUAL( eve::jacobi_zeta(3*eve::pio_4(as<T>()), T( 0.5)), T(boost::math::jacobi_zeta(v_t(0.5), 3*eve::pio_4(as<v_t>()))), 0.5);
  TTS_ULP_EQUAL( eve::jacobi_zeta(3*eve::pio_4(as<T>()), T( 0.9)), T(boost::math::jacobi_zeta(v_t(0.9), 3*eve::pio_4(as<v_t>()))), 0.5);
  TTS_ULP_EQUAL( eve::jacobi_zeta(T(100), T( 0.5)), T(boost::math::jacobi_zeta(v_t(0.5), v_t(100))), 1.0);
  TTS_ULP_EQUAL( eve::jacobi_zeta(T(100), T( 0.9)), T(boost::math::jacobi_zeta(v_t(0.9), v_t(100))), 1.0);
}


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

    @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_FUNCTION_LOG1P_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOG1P_HPP_INCLUDED

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

  /*!
    @ingroup group-exponential
    This function object computes \f$\log(1+x)\f$ with good accuracy even for small
    \f$x\f$ values.

    @par Header <boost/simd/function/log1p.hpp>

    @par Decorators

      - std_ for floating entries calls @c std::log1p

    @see log, exp, expm1

    @par Example:

      @snippet log1p.cpp log1p

    @par Possible output:

      @snippet log1p.txt log1p

  **/
  IEEEValue log1p(IEEEValue const& x);
} }
#endif

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

#endif


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

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

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing divround capabilities

    Computes the round of the division.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = divround(x, y);
    @endcode

    The code is similar to:

    @code
    T r = round(x/y);
    @endcode

    for integral types, if y is null, it returns @ref Valmax or @ref Valmin
    if x is positive (resp. negative) and 0 if x is null.
    Take also care that dividing @ref Valmin by -1 for signed integral types has
    undefined behaviour.

    @see  divides, rec, divs, divfloor,
    divceil, divround2even, divfix

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

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

#endif


// ds THIS CODE WAS CREATED BASED ON: http://kitti.is.tue.mpg.de/kitti/devkit_odometry.zip
// ds minimally modified to avoid C++11 warnings and provided a brief result dump to stdout

#include <Eigen/Geometry>
#include <fstream>
#include <iostream>
#include <limits>
#include <set>
#include <srrg_system_utils/system_utils.h>
#include <vector>

// ds readability
typedef Eigen::Matrix<float, 4, 4> Matrix4f;
typedef std::vector<Matrix4f, Eigen::aligned_allocator<Matrix4f>> Matrix4fVector;

// static parameter
float lengths_small[] = {5, 10, 25, 50, 75, 100, 150, 200};
float lengths_large[] = {100, 200, 300, 400, 500, 600, 700, 800};
float* lengths        = lengths_large;
int32_t num_lengths   = 8;

constexpr size_t minimum_number_of_samples_per_length = 3;

struct errors {
  int32_t first_frame;
  float r_err;
  float t_err;
  float len;
  float speed;
  errors(int32_t first_frame, float r_err, float t_err, float len, float speed) :
    first_frame(first_frame),
    r_err(r_err),
    t_err(t_err),
    len(len),
    speed(speed) {
  }
};

Matrix4fVector loadPoses(std::string file_name) {
  Matrix4fVector poses;

  // ds going modern
  std::ifstream pose_file(file_name, std::ifstream::in);

  // ds grab a line from the ground truth
  std::string buffer_line;
  while (std::getline(pose_file, buffer_line)) {
    // ds get it to a std::stringstream
    std::istringstream buffer_stream(buffer_line);

    // ds information fields (KITTI format)
    Matrix4f pose(Matrix4f::Identity());
    for (uint8_t u = 0; u < 3; ++u) {
      for (uint8_t v = 0; v < 4; ++v) {
        buffer_stream >> pose(u, v);
      }
    }
    poses.push_back(pose);
  }

  pose_file.close();
  return poses;
}

std::vector<float> trajectoryDistances(Matrix4fVector& poses) {
  std::vector<float> dist;
  dist.push_back(0);
  for (std::size_t i = 1; i < poses.size(); i++) {
    Matrix4f P1 = poses[i - 1];
    Matrix4f P2 = poses[i];
    float dx    = P1(0, 3) - P2(0, 3);
    float dy    = P1(1, 3) - P2(1, 3);
    float dz    = P1(2, 3) - P2(2, 3);
    dist.push_back(dist[i - 1] + std::sqrt(dx * dx + dy * dy + dz * dz));
  }
  return dist;
}

int32_t lastFrameFromSegmentLength(std::vector<float>& dist, int32_t first_frame, float len) {
  for (std::size_t i = first_frame; i < dist.size(); i++)
    if (dist[i] > dist[first_frame] + len)
      return i;
  return -1;
}

inline float rotationError(Matrix4f& pose_error) {
  float a = pose_error(0, 0);
  float b = pose_error(1, 1);
  float c = pose_error(2, 2);
  float d = 0.5 * (a + b + c - 1.0);
  return std::acos(std::max(std::min(d, 1.0f), -1.0f));
}

inline float translationError(Matrix4f& pose_error) {
  float dx = pose_error(0, 3);
  float dy = pose_error(1, 3);
  float dz = pose_error(2, 3);
  return std::sqrt(dx * dx + dy * dy + dz * dz);
}

std::vector<errors> calcSequenceErrors(Matrix4fVector& poses_gt, Matrix4fVector& poses_result) {
  // error std::vector
  std::vector<errors> error_statistics;

  // parameters
  int32_t step_size = 10; // every second

  // pre-compute distances (from ground truth as reference)
  std::vector<float> dist = trajectoryDistances(poses_gt);

  // for all start positions do
  std::multiset<size_t> evaluated_lengths;
  for (std::size_t first_frame = 0; first_frame < poses_gt.size(); first_frame += step_size) {
    // for all segment lengths do
    for (int32_t i = 0; i < num_lengths; i++) {
      // current length
      float len = lengths[i];

      // compute last frame
      int32_t last_frame = lastFrameFromSegmentLength(dist, first_frame, len);

      // continue, if sequence not long enough
      if (last_frame == -1) {
        continue;
      } else {
        evaluated_lengths.insert(len);
      }

      // compute rotational and translational errors
      Matrix4f pose_delta_gt     = poses_gt[first_frame].inverse() * poses_gt[last_frame];
      Matrix4f pose_delta_result = poses_result[first_frame].inverse() * poses_result[last_frame];
      Matrix4f pose_error        = pose_delta_result.inverse() * pose_delta_gt;
      float r_err                = rotationError(pose_error);
      float t_err                = translationError(pose_error);

      // compute speed
      float num_frames = (float) (last_frame - first_frame + 1);
      float speed      = len / (0.1 * num_frames);

      // write to file
      error_statistics.push_back(errors(first_frame, r_err / len, t_err / len, len, speed));
    }
  }

  // ds check sample distribution
  size_t number_of_valid_lengths = 0;
  for (size_t i = 0; i < static_cast<size_t>(num_lengths); ++i) {
    if (evaluated_lengths.count(static_cast<size_t>(lengths[i])) > 6) {
      std::cerr << lengths[i] << " : " << evaluated_lengths.count(static_cast<size_t>(lengths[i]))
                << std::endl;
      ++number_of_valid_lengths;
    }
  }

  // ds if we did not manage to sample enough lengths and are not running on small samples already
  if (number_of_valid_lengths < 3 && lengths != lengths_small) {
    std::cerr << "calcSequenceErrors|not enough samples, switching to smaller intervals"
              << std::endl;
    lengths = lengths_small;
    return calcSequenceErrors(poses_gt, poses_result);
  } else {
    return error_statistics;
  }
}

void saveSequenceErrors(std::vector<errors>& err, std::string file_name) {
  // open file
  FILE* fp;
  fp = fopen(file_name.c_str(), "w");

  // write to file
  for (std::vector<errors>::iterator it = err.begin(); it != err.end(); it++)
    fprintf(fp, "%d %f %f %f %f\n", it->first_frame, it->r_err, it->t_err, it->len, it->speed);

  // close file
  fclose(fp);
  std::cerr << "saveSequenceErrors|wrote to: '" << file_name << "'" << std::endl;
}

void savePathPlot(Matrix4fVector& poses_gt, Matrix4fVector& poses_result, std::string file_name) {
  // parameters
  int32_t step_size = 3;

  // open file
  FILE* fp = fopen(file_name.c_str(), "w");

  // save x/z coordinates of all frames to file
  for (std::size_t i = 0; i < poses_gt.size(); i += step_size)
    fprintf(fp,
            "%f %f %f %f\n",
            poses_gt[i](0, 3),
            poses_gt[i](2, 3),
            poses_result[i](0, 3),
            poses_result[i](2, 3));

  // close file
  fclose(fp);
  std::cerr << "savePathPlot|wrote to: '" << file_name << "'" << std::endl;
}

std::vector<int32_t> computeRoi(Matrix4fVector& poses_gt, Matrix4fVector& poses_result) {
  float x_min = std::numeric_limits<int32_t>::max();
  float x_max = std::numeric_limits<int32_t>::min();
  float z_min = std::numeric_limits<int32_t>::max();
  float z_max = std::numeric_limits<int32_t>::min();

  for (Matrix4fVector::iterator it = poses_gt.begin(); it != poses_gt.end(); it++) {
    float x = (*it)(0, 3);
    float z = (*it)(2, 3);
    if (x < x_min)
      x_min = x;
    if (x > x_max)
      x_max = x;
    if (z < z_min)
      z_min = z;
    if (z > z_max)
      z_max = z;
  }

  for (Matrix4fVector::iterator it = poses_result.begin(); it != poses_result.end(); it++) {
    float x = (*it)(0, 3);
    float z = (*it)(2, 3);
    if (x < x_min)
      x_min = x;
    if (x > x_max)
      x_max = x;
    if (z < z_min)
      z_min = z;
    if (z > z_max)
      z_max = z;
  }

  float dx = 1.1 * (x_max - x_min);
  float dz = 1.1 * (z_max - z_min);
  float mx = 0.5 * (x_max + x_min);
  float mz = 0.5 * (z_max + z_min);
  float r  = 0.5 * std::max(dx, dz);

  std::vector<int32_t> roi;
  roi.push_back((int32_t)(mx - r));
  roi.push_back((int32_t)(mx + r));
  roi.push_back((int32_t)(mz - r));
  roi.push_back((int32_t)(mz + r));
  return roi;
}

int32_t plotPathPlot(std::string dir, std::vector<int32_t>& roi, int32_t idx) {
  // gnuplot file name
  char command[1024];
  char file_name[256];
  sprintf(file_name, "%02d.gp", idx);
  std::string full_name = dir + "/" + file_name;

  // ds system calls
  int32_t result = 0;

  // create png + eps
  for (int32_t i = 0; i < 1 /*PNG only*/; i++) {
    // open file
    FILE* fp = fopen(full_name.c_str(), "w");

    // save gnuplot instructions
    if (i == 0) {
      fprintf(fp, "set term png size 900,900\n");
      fprintf(fp, "set output \"%02d.png\"\n", idx);
    } else {
      fprintf(fp, "set term postscript eps enhanced color\n");
      fprintf(fp, "set output \"%02d.eps\"\n", idx);
    }

    fprintf(fp, "set size ratio -1\n");
    fprintf(fp, "set xrange [%d:%d]\n", roi[0], roi[1]);
    fprintf(fp, "set yrange [%d:%d]\n", roi[2], roi[3]);
    fprintf(fp, "set xlabel \"x [m]\"\n");
    fprintf(fp, "set ylabel \"z [m]\"\n");
    fprintf(
      fp, "plot \"%02d.txt\" using 1:2 lc rgb \"#FF0000\" title 'Ground Truth' w lines,", idx);
    fprintf(fp, "\"%02d.txt\" using 3:4 lc rgb \"#0000FF\" title 'Visual Odometry' w lines,", idx);
    fprintf(fp,
            "\"< head -1 %02d.txt\" using 1:2 lc rgb \"#000000\" pt 4 ps 1 lw 2 title 'Sequence "
            "Start' w points\n",
            idx);

    // close file
    fclose(fp);
    std::cerr << "plotPathPlot|wrote to: '" << full_name << "'" << std::endl;

    // run gnuplot => create png + eps
    sprintf(command, "cd %s; gnuplot %s", dir.c_str(), file_name);
    result = system(command);
    std::cerr << "plotPathPlot|wrote to: '" << file_name << "'" << std::endl;
  }

  // create pdf and crop TODO re-enable
  //  sprintf(command, "cd %s; ps2pdf %02d.eps %02d_large.pdf", dir.c_str(), idx, idx);
  //  result = system(command);
  //  sprintf(command, "cd %s; pdfcrop %02d_large.pdf %02d.pdf", dir.c_str(), idx, idx);
  //  result = system(command);
  //  sprintf(command, "cd %s; rm %02d_large.pdf", dir.c_str(), idx);
  //  result = system(command);
  return result;
}

void saveErrorPlots(std::vector<errors>& seq_err, std::string plot_error_dir, const char* prefix) {
  // file names
  char file_name_tl[1024];
  sprintf(file_name_tl, "%s/%s_tl.txt", plot_error_dir.c_str(), prefix);
  char file_name_rl[1024];
  sprintf(file_name_rl, "%s/%s_rl.txt", plot_error_dir.c_str(), prefix);
  //  char file_name_ts[1024];
  //  sprintf(file_name_ts, "%s/%s_ts.txt", plot_error_dir.c_str(), prefix);
  //  char file_name_rs[1024];
  //  sprintf(file_name_rs, "%s/%s_rs.txt", plot_error_dir.c_str(), prefix);

  // open files
  FILE* fp_tl = fopen(file_name_tl, "w");
  FILE* fp_rl = fopen(file_name_rl, "w");
  //  FILE* fp_ts = fopen(file_name_ts, "w");
  //  FILE* fp_rs = fopen(file_name_rs, "w");

  // for each segment length do
  std::cerr << "---------------------------- ERROR STATISTICS ----------------------------"
            << std::endl;
  float total_error_rotation    = 0;
  float total_error_translation = 0;
  uint32_t number_of_lengths    = 0;
  for (int32_t i = 0; i < num_lengths; i++) {
    float t_err = 0;
    float r_err = 0;
    float num   = 0;

    // for all errors do
    for (std::vector<errors>::iterator it = seq_err.begin(); it != seq_err.end(); it++) {
      if (fabs(it->len - lengths[i]) < 1.0) {
        t_err += it->t_err;
        r_err += it->r_err;
        num++;
      }
    }

    // ds we require at least 2 lengths to be evaluated
    if (num > 1) {
      fprintf(fp_tl, "%f %f\n", lengths[i], t_err / num);
      fprintf(fp_rl, "%f %f\n", lengths[i], r_err / num);
      total_error_rotation += r_err / num * (180 / M_PI) * 100;
      total_error_translation += t_err / num * 100;
      ++number_of_lengths;

      // ds info
      std::printf("length: %f error rotation (deg/100m): %9.6f error translation (%%): %9.6f\n",
                  lengths[i],
                  r_err / num * (180 / M_PI) * 100,
                  t_err / num * 100);
    }
  }
  std::cerr << "---------------------------- ---------------- ----------------------------"
            << std::endl;
  std::printf("average error rotation (deg/100m): %9.6f error translation (%%): %9.6f\n",
              total_error_rotation / number_of_lengths,
              total_error_translation / number_of_lengths);
  std::cerr << "---------------------------- ---------------- ----------------------------"
            << std::endl;

  //  // for each driving speed do (in m/s)
  //  for (float speed = 2; speed < 25; speed += 2) {
  //    float t_err = 0;
  //    float r_err = 0;
  //    float num   = 0;
  //
  //    // for all errors do
  //    for (std::vector<errors>::iterator it = seq_err.begin(); it != seq_err.end(); it++) {
  //      if (fabs(it->speed - speed) < 2.0) {
  //        t_err += it->t_err;
  //        r_err += it->r_err;
  //        num++;
  //      }
  //    }
  //
  //    // ds we require at least 2 lengths to be evaluated
  //    if (num > 1) {
  //      fprintf(fp_ts, "%f %f\n", speed, t_err / num);
  //      fprintf(fp_rs, "%f %f\n", speed, r_err / num);
  //    }
  //  }

  // close files
  fclose(fp_tl);
  fclose(fp_rl);
  //  fclose(fp_ts);
  //  fclose(fp_rs);
}

int32_t plotErrorPlots(std::string dir, const char* prefix) {
  char command[1024];

  // ds system calls
  int32_t result = 0;

  // for all four error plots do
  for (int32_t i = 0; i < 2; i++) {
    // create suffix
    char suffix[16];
    switch (i) {
      case 0:
        sprintf(suffix, "tl");
        break;
      case 1:
        sprintf(suffix, "rl");
        break;
      case 2:
        sprintf(suffix, "ts");
        break;
      case 3:
        sprintf(suffix, "rs");
        break;
    }

    // gnuplot file name
    char file_name[1024];
    char full_name[1024];
    sprintf(file_name, "%s_%s.gp", prefix, suffix);
    sprintf(full_name, "%s/%s", dir.c_str(), file_name);

    // create png + eps
    for (int32_t j = 0; j < 1 /*PNG only*/; j++) {
      // open file
      FILE* fp = fopen(full_name, "w");

      // save gnuplot instructions
      if (j == 0) {
        fprintf(fp, "set term png size 500,250 font \"Helvetica\" 11\n");
        fprintf(fp, "set output \"%s_%s.png\"\n", prefix, suffix);
      } else {
        fprintf(fp, "set term postscript eps enhanced color\n");
        fprintf(fp, "set output \"%s_%s.eps\"\n", prefix, suffix);
      }

      // start plot at 0
      fprintf(fp, "set size ratio 0.5\n");
      fprintf(fp, "set yrange [0:*]\n");

      // x label
      if (i <= 1)
        fprintf(fp, "set xlabel \"Path Length [m]\"\n");
      else
        fprintf(fp, "set xlabel \"Speed [km/h]\"\n");

      // y label
      if (i == 0 || i == 2)
        fprintf(fp, "set ylabel \"Translation Error [%%]\"\n");
      else
        fprintf(fp, "set ylabel \"Rotation Error [deg/m]\"\n");

      // plot error curve
      fprintf(fp, "plot \"%s_%s.txt\" using ", prefix, suffix);
      switch (i) {
        case 0:
          fprintf(fp, "1:($2*100) title 'Translation Error'");
          break;
        case 1:
          fprintf(fp, "1:($2*57.3) title 'Rotation Error'");
          break;
        case 2:
          fprintf(fp, "($1*3.6):($2*100) title 'Translation Error'");
          break;
        case 3:
          fprintf(fp, "($1*3.6):($2*57.3) title 'Rotation Error'");
          break;
      }
      fprintf(fp, " lc rgb \"#0000FF\" pt 4 w linespoints\n");

      // close file
      fclose(fp);

      // run gnuplot => create png + eps
      sprintf(command, "cd %s; gnuplot %s", dir.c_str(), file_name);
      result = system(command);
      std::cerr << "plotErrorPlots|wrote to: '" << dir + "/" + file_name << "'" << std::endl;
    }

    // create pdf and crop TODO re-enable
    //    sprintf(command,
    //            "cd %s; ps2pdf %s_%s.eps %s_%s_large.pdf",
    //            dir.c_str(),
    //            prefix,
    //            suffix,
    //            prefix,
    //            suffix);
    //    result = system(command);
    //    sprintf(command,
    //            "cd %s; pdfcrop %s_%s_large.pdf %s_%s.pdf",
    //            dir.c_str(),
    //            prefix,
    //            suffix,
    //            prefix,
    //            suffix);
    //    result = system(command);
    //    sprintf(command, "cd %s; rm %s_%s_large.pdf", dir.c_str(), prefix, suffix);
    //    result = system(command);
  }

  return result;
}

void saveStats(std::vector<errors> err, std::string dir) {
  float t_err = 0;
  float r_err = 0;

  // for all errors do => compute sum of t_err, r_err
  for (std::vector<errors>::iterator it = err.begin(); it != err.end(); it++) {
    t_err += it->t_err;
    r_err += it->r_err;
  }

  // open file
  FILE* fp = fopen((dir + "/stats.txt").c_str(), "w");

  // save errors
  float num = err.size();
  fprintf(fp, "%f %f\n", t_err / num, r_err / num);

  // close file
  fclose(fp);
  std::cerr << "saveStats|wrote to '" << dir + "/stats.txt"
            << "'" << std::endl;
}

int eval(const std::string& file_trajectory_test_,
         const std::string& file_trajectory_ground_truth_,
         const std::string& file_sequence_) {
  // ground truth and result directories
  const std::string result_dir     = "results";
  const std::string error_dir      = result_dir + "/errors";
  const std::string plot_path_dir  = result_dir + "/plot_path";
  const std::string plot_error_dir = result_dir + "/plot_error";

  // create output directories TODO evil evil
  int32_t result = system(("mkdir -p " + result_dir).c_str());
  result         = system(("mkdir -p " + error_dir).c_str());
  result         = system(("mkdir -p " + plot_path_dir).c_str());
  result         = system(("mkdir -p " + plot_error_dir).c_str());
  if (result < 0) {
    std::cerr << "eval|ERROR: system io error" << std::endl;
    return -1;
  }

  // read ground truth and result poses
  Matrix4fVector poses_gt     = loadPoses(file_trajectory_ground_truth_);
  Matrix4fVector poses_result = loadPoses(file_trajectory_test_);

  // ds parse sequence number
  const std::string sequence_number_literal = file_sequence_.substr(0, 2);
  const uint32_t sequence_number            = std::stoi(sequence_number_literal);

  // plot status
  printf("Processing: %s (sequence: %i), poses: %lu/%lu\n",
         file_sequence_.c_str(),
         sequence_number,
         poses_result.size(),
         poses_gt.size());

  // check for errors
  if (poses_gt.size() == 0 || poses_result.size() != poses_gt.size()) {
    std::cerr << "eval|ERROR: Couldn't read (all) poses of: " << file_sequence_ << std::endl;
    return -1;
  }

  // compute sequence errors
  std::vector<errors> seq_err = calcSequenceErrors(poses_gt, poses_result);
  saveSequenceErrors(seq_err, error_dir + "/" + file_sequence_);

  // for first half => plot trajectory and compute individual stats
  // save + plot bird's eye view trajectories
  savePathPlot(poses_gt, poses_result, plot_path_dir + "/" + file_sequence_);
  std::vector<int32_t> roi = computeRoi(poses_gt, poses_result);
  result                   = plotPathPlot(plot_path_dir, roi, sequence_number);
  if (result != 0) {
    std::cerr << "eval|ERROR: unable to plot path" << std::endl;
    return -1;
  }

  // save + plot individual errors
  saveErrorPlots(seq_err, plot_error_dir, sequence_number_literal.c_str());
  result = plotErrorPlots(plot_error_dir, sequence_number_literal.c_str());
  if (result != 0) {
    std::cerr << "eval|ERROR: unable to plot error statistics" << std::endl;
    return -1;
  }

  return result;
}

const std::string banner =
  "\n\nUsage: ./srrg_kitti_evaluate_odometry_app -gt <string> -odom <string> -seq <string>\n"
  "Example: ./srrg_kitti_evaluate_odometry_app -gt 00.txt -odom tracker_output.txt -seq 00.txt\n"
  "Options:\n"
  "------------------------------------------\n"
  "-gt <string>                path to ground_truth file\n"
  "-odom <string>              path to tracker odometry path\n"
  "-seq <string>               sequence number, e.g. '00.txt' if not set, then seq=gt\n"
  "-h                          this help\n";

// ds parameters
std::string file_tracked_odometry = "";
std::string file_ground_truth     = "";
std::string file_sequence         = "";

// ds parse parameters
int c = 1;

int32_t main(int32_t argc, char** argv) {
  // ds always expects 6 arguments
  if (argc != 7) {
    std::cerr << banner << std::endl;
    return -1;
  }

  // ds parameters
  std::string file_tracked_odometry = "";
  std::string file_ground_truth     = "";
  std::string file_sequence         = "";

  // ds parse parameters
  int c = 1;
  while (c < argc) {
    if (!strcmp(argv[c], "-h")) {
      std::cerr << banner << std::endl;
      return -1;
    } else if (!strcmp(argv[c], "-gt")) {
      c++;
      file_ground_truth = argv[c];
    } else if (!strcmp(argv[c], "-odom")) {
      c++;
      file_tracked_odometry = argv[c];
    } else if (!strcmp(argv[c], "-seq")) {
      c++;
      file_sequence = argv[c];
    }
    c++;
  }

  // ds input validation
  if (file_sequence.empty())
    file_sequence = file_ground_truth;

  if (file_sequence.find(".txt") == std::string::npos) {
    std::cerr << "ERROR: missing file ending (e.g. txt) in provided sequence file: "
              << file_sequence << std::endl;
    return -1;
  }

  // ds configuration
  std::cerr << "file_ground_truth: " << file_ground_truth << std::endl;
  std::cerr << "file_tracked_odometry: " << file_tracked_odometry << std::endl;
  std::cerr << "file_sequence: " << file_sequence << std::endl;

  // run evaluation
  return eval(file_tracked_odometry, file_ground_truth, file_sequence);
}


#include <cstring>
#include <numeric>
#include <vector>
#include <array>
#include <random>

#include <boost/test/unit_test.hpp>

#include <cereal/cereal.hpp>
#include <cereal/types/bitset.hpp>
#include <cereal/archives/xml.hpp>
#include <cereal/archives/binary.hpp>
#include <cereal/archives/portable_binary.hpp>

#include <sstream>

#include "rfr/data_containers/default_data_container.hpp"
#include "rfr/splits/binary_split_one_feature_rss_loss.hpp"

typedef double num_t;
typedef unsigned int index_t;
typedef std::default_random_engine rng_type;

typedef rfr::data_containers::default_container<num_t, num_t, index_t> data_container_type;
typedef rfr::splits::binary_split_one_feature_rss_loss<num_t, num_t, index_t,rng_type,128> split_type;
typedef rfr::splits::data_info_t<num_t, num_t, index_t> info_t;

template <class T>
void print_vector (T v){
	for (auto e : v)
		std::cout<<e<<", ";
	std::cout<<"\b\b\n";
}

void print_pcs (std::vector<std::vector<num_t> > pcs){
	for (auto i: pcs){
		print_vector(i);
	}
}

data_container_type load_toy_data(){
	data_container_type data(2);

    std::string feature_file, response_file;

    feature_file  = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "toy_data_set_features.csv";
    response_file = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "toy_data_set_responses.csv";

    data.import_csv_files(feature_file, response_file);

	data.set_type_of_feature(1,10);

	BOOST_REQUIRE_EQUAL(data.get_type_of_feature(1), 10);

    return(data);
}

BOOST_AUTO_TEST_CASE(binary_split_one_feature_rss_loss_continuous_split_test){

	auto data = load_toy_data();

    std::vector<info_t > data_info(data.num_data_points());

	for (auto i=0u; i<data.num_data_points(); ++i){
		data_info[i].index=i;
		data_info[i].response = data.response(i);
		data_info[i].weight = 1;

	}

	std::array<std::vector<info_t>::iterator, 3> infos_split_it;
	std::vector<index_t> features_to_try(1,0);

	rng_type rng;

	split_type split1;
	num_t loss = split1.find_best_split(data, features_to_try,data_info.begin(), data_info.end(),infos_split_it,1, 1, rng);

	// actual loss independently computed in python
	BOOST_REQUIRE_CLOSE(loss, 23.33333333, 1e-4);

	// split criterion has to be in [59, 60) -> see python reference
	num_t split_val = split1.get_num_split_value();

	BOOST_REQUIRE(split_val >=59);
	BOOST_REQUIRE(split_val < 60);

	// test the () operator for the trainings data
	std::vector<index_t> operator_test = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};

	for (size_t i=0; i<operator_test.size(); i++){
		std::vector<num_t> tmp_feature_vector ({data.feature(0,i), data.feature(1,i)});
		BOOST_REQUIRE(split1(tmp_feature_vector) == operator_test[i]);
	}

	std::vector<std::vector<num_t> > pcs = { {-1000, 1000}, {0,1,2,3,4,5,6,7,8,9}};

	auto pcss = split1.compute_subspaces(pcs);

	BOOST_REQUIRE(std::find(pcss[0][1].begin(), pcss[0][1].end(), 0) != pcss[0][1].end());

	BOOST_CHECK_EQUAL(pcss[0][0][0], pcs[0][0]);
	BOOST_CHECK_EQUAL(pcss[0][0][1], split_val);
	BOOST_CHECK_EQUAL(pcss[1][0][0], split_val);
	BOOST_CHECK_EQUAL(pcss[1][0][1], pcs[0][1]);

	// make sure that x2 has not been altered
	BOOST_CHECK_EQUAL_COLLECTIONS( pcss[0][1].begin(), pcss[0][1].end(),
									pcs[1].begin(), pcs[1].end());

	BOOST_CHECK_EQUAL_COLLECTIONS( pcss[1][1].begin(), pcss[1][1].end(),
									pcs[1].begin(), pcs[1].end());

	split1.print_info();

}

BOOST_AUTO_TEST_CASE(binary_split_one_feature_rss_loss_categorical_split_test){

	auto data = load_toy_data();

    std::vector<info_t > data_info(data.num_data_points());

	for (auto i=0u; i<data.num_data_points(); ++i){
		data_info[i].index=i;
		data_info[i].response = data.response(i);
		data_info[i].weight = 1;

	}

	std::array<std::vector<info_t>::iterator, 3> infos_split_it;
	std::vector<index_t> features_to_try(1,1);

	rng_type rng;

	split_type split2;
	num_t loss = split2.find_best_split(data, features_to_try,data_info.begin(), data_info.end(),infos_split_it, 1, 1, rng);

    num_t total_weight = 0;
    for (auto it = infos_split_it[0]; it!=infos_split_it[1]; ++it)
        total_weight += (*it).weight;
    for (auto it = infos_split_it[1]; it!=infos_split_it[2]; ++it)
        total_weight += (*it).weight;
    BOOST_REQUIRE_CLOSE(total_weight, data.num_data_points(), 1e-4);

	// actual best split and loss independently computed in python
	BOOST_REQUIRE_CLOSE(loss, 88.57142857, 1e-6);
	auto split_set = split2.get_cat_split_set();
	std::cout<< split2.get_num_split_value()<<std::endl;
	std::cout<< split_set<<std::endl;
	BOOST_REQUIRE(split_set[0]);
	BOOST_REQUIRE(split_set[1]);
	BOOST_REQUIRE(!split_set[2]);

	// test the () operator for the trainings data
	std::vector<index_t> operator_test = {0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};

	// test the pcs splitting
	for (size_t i=0; i<operator_test.size(); i++){
		std::vector<num_t> tmp_feature_vector ({data.feature(0,i), data.feature(1,i)});
		BOOST_CHECK_MESSAGE(split2(tmp_feature_vector) == operator_test[i],split2(tmp_feature_vector) << "!=" <<  operator_test[i]<<" (index "<<i<<")\n");
	}

	std::vector<std::vector<num_t> > pcs = { {-1000, 1000}, {0,1,2,3,4,5,6,7,8,9}};

	auto pcss = split2.compute_subspaces(pcs);

	BOOST_REQUIRE( pcss[0][0][0] == pcs[0][0]);BOOST_REQUIRE( pcss[0][0][1] == pcs[0][1]);
	BOOST_REQUIRE( pcss[1][0][0] == pcs[0][0]);BOOST_REQUIRE( pcss[0][0][1] == pcs[0][1]);

	BOOST_REQUIRE(std::find(pcss[0][1].begin(), pcss[0][1].end(), 0) != pcss[0][1].end());
	BOOST_REQUIRE(std::find(pcss[0][1].begin(), pcss[0][1].end(), 1) != pcss[0][1].end());
	BOOST_REQUIRE(std::find(pcss[0][1].begin(), pcss[0][1].end(), 2) == pcss[0][1].end());

	BOOST_REQUIRE(std::find(pcss[1][1].begin(), pcss[1][1].end(), 0) == pcss[1][1].end());
	BOOST_REQUIRE(std::find(pcss[1][1].begin(), pcss[1][1].end(), 1) == pcss[1][1].end());
	BOOST_REQUIRE(std::find(pcss[1][1].begin(), pcss[1][1].end(), 2) != pcss[1][1].end());

	// test the can_be_split_function

	std::vector<num_t> eevee (data.num_features(), 0);

	BOOST_REQUIRE(split2.can_be_split(eevee));

	eevee[split2.get_feature_index()] = NAN;

	BOOST_REQUIRE(!split2.can_be_split(eevee));

	split2.print_info();

}

// check if it finds the best split out of the two above
BOOST_AUTO_TEST_CASE(binary_split_one_feature_rss_loss_find_best_split_test){

	auto data = load_toy_data();

    std::vector<info_t > data_info(data.num_data_points());
	for (auto i=0u; i<data.num_data_points(); ++i){
		data_info[i].index=i;
		data_info[i].response = data.response(i);
		data_info[i].weight = 1;
	}

	std::array<std::vector<info_t>::iterator, 3> infos_split_it;
	std::vector<index_t> features_to_try({0,1});

	rng_type rng;

	split_type split3;
	num_t loss = split3.find_best_split(data, features_to_try,data_info.begin(), data_info.end(),infos_split_it, 1, 1, rng);
	BOOST_REQUIRE_CLOSE(loss, 23.33333333, 1e-4);

    num_t total_weight = 0;
    for (auto it = infos_split_it[0]; it!=infos_split_it[1]; ++it)
        total_weight += (*it).weight;
    for (auto it = infos_split_it[1]; it!=infos_split_it[2]; ++it)
        total_weight += (*it).weight;
    BOOST_REQUIRE_CLOSE(total_weight, data.num_data_points(), 1e-4);

	num_t split_val = split3.get_num_split_value();

	BOOST_REQUIRE(split_val >=59);
	BOOST_REQUIRE(split_val < 60);
}

// test serialization
BOOST_AUTO_TEST_CASE(binary_split_one_feature_rss_loss_serialization){

    auto data = load_toy_data();

	std::vector<info_t > data_info(data.num_data_points());

	for (auto i=0u; i<data.num_data_points(); ++i){
		data_info[i].index=i;
		data_info[i].response = data.response(i);
		data_info[i].weight = 1;

	}

	std::array<std::vector<info_t>::iterator, 3> infos_split_it;
	std::vector<index_t> features_to_try({0,1});

	rng_type rng;

	split_type split4;
	num_t loss = split4.find_best_split(data, features_to_try,data_info.begin(), data_info.end(),infos_split_it, 1, 1, rng);

    num_t total_weight = 0;
    for (auto it = infos_split_it[0]; it!=infos_split_it[1]; ++it)
        total_weight += (*it).weight;
    for (auto it = infos_split_it[1]; it!=infos_split_it[2]; ++it)
        total_weight += (*it).weight;
    BOOST_REQUIRE_CLOSE(total_weight, data.num_data_points(), 1e-4);

	index_t index4 = split4.get_feature_index();
	auto split_val = split4.get_num_split_value();
	auto split_bits= split4.get_cat_split_set();
	std::ostringstream oss;
	{
		cereal::XMLOutputArchive oarchive(oss);
		oarchive(split4);
	}

	split_type split5;
	{
		std::istringstream iss(oss.str());
		cereal::XMLInputArchive iarchive(iss);
		iarchive(split5);
	}

	BOOST_REQUIRE(index4     == split5.get_feature_index());
	BOOST_REQUIRE(split_val  == split5.get_num_split_value());
	BOOST_REQUIRE(split_bits == split5.get_cat_split_set());

}

// test binary serialization
BOOST_AUTO_TEST_CASE(binary_split_one_feature_rss_loss_binary_serialization){

	auto data = load_toy_data();

	std::vector<info_t > data_info(data.num_data_points());

	for (auto i=0u; i<data.num_data_points(); ++i){
		data_info[i].index=i;
		data_info[i].response = data.response(i);
		data_info[i].weight = 1;

	}

	std::array<std::vector<info_t>::iterator, 3> infos_split_it;
	std::vector<index_t> features_to_try({0,1});

	rng_type rng;

	split_type split4;
	num_t loss = split4.find_best_split(data, features_to_try,data_info.begin(), data_info.end(),infos_split_it, 1, 1, rng);

    num_t total_weight = 0;
    for (auto it = infos_split_it[0]; it!=infos_split_it[1]; ++it)
        total_weight += (*it).weight;
    for (auto it = infos_split_it[1]; it!=infos_split_it[2]; ++it)
        total_weight += (*it).weight;
    BOOST_REQUIRE_CLOSE(total_weight, data.num_data_points(), 1e-4);

	index_t index4 = split4.get_feature_index();
	auto split_val = split4.get_num_split_value();
	auto split_bits= split4.get_cat_split_set();

	std::ostringstream oss;
	{
		cereal::PortableBinaryOutputArchive oarchive(oss);
		oarchive(split4);
	}

	split_type split5;
	{
		std::istringstream iss(oss.str());
		cereal::PortableBinaryInputArchive iarchive(iss);
		iarchive(split5);
	}

	BOOST_REQUIRE_EQUAL(index4,    split5.get_feature_index());
	BOOST_REQUIRE_EQUAL(split_val, split5.get_num_split_value());
	BOOST_REQUIRE_EQUAL(split_bits,split5.get_cat_split_set());

}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/inverse_gamma.hpp>

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

#include <dpMM/basemeasure.hpp>
#include <dpMM/distribution.hpp>
#include <dpMM/normalSphere.hpp>
#include <dpMM/iw.hpp>
#include <dpMM/sphere.hpp>
#include <dpMM/karcherMean.hpp>

using namespace Eigen;
using std::endl;
using std::cout;

/* Actually just places an IW prior on covariances in the tangent
 * plane.
 *
 * The point of tangentcy has to be set "manually". This makes the
 * clase usefull for models that externaly update the point of tangency
 * such as the Manhattan Frame
 */
template<typename T>
class IwTangent : public BaseMeasure<T>
{
public:

  IW<T> iw0_; // IW prior on the covariance of the normal in T_\muS^D
  Sphere<T> S_;
  NormalSphere<T> normalS_; // normal on sphere

  IwTangent(const IW<T>& iw, boost::mt19937* pRndGen);
  ~IwTangent();

  virtual BaseMeasure<T>* copy();
  virtual IwTangent<T>* copyNative();

  virtual baseMeasureType getBaseMeasureType() const {return(NIW_TANGENT); }

  /* for any point on the sphere; maps into T_muS and rotates north first */
  virtual T logLikelihood(const Matrix<T,Dynamic,1>& x) const ;
  virtual T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  // right now this does not support actual scatter!  it supports
  // weighted directional data though.  count is the weight
  // [counts,karcherMean,scatter around KarcherMean]
  virtual T logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const;
  /* assumes x is already in T_northS */
  virtual T logLikelihoodNorth(const Matrix<T,Dynamic,1>& x) const ;

  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  void posterior(const shared_ptr<ClGMMData<T> >& cldp, uint32_t k);
  /* assumes the x are already in T_northS correctly */
  void posteriorFromPtsInTpS(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k);
  // right now this does not support actual scatter!  it supports
  // weighted directional data though.  count is the weight
  // [counts,karcherMean,scatter around KarcherMean]
  void posteriorFromSS(const vector<Matrix<T,Dynamic,1> >&x, const
      VectorXu& z, uint32_t k);
  void posteriorFromSS(const Matrix<T,Dynamic,1> &x);

  void sample();

  T logPdfUnderPrior() const;
  T logPdfUnderPriorMarginalized() const;
//  T logPdfUnderPriorMarginalizedMerged(const shared_ptr<IwTangent<T> >& other) const;

  void print() const;
  virtual uint32_t getDim() const {return(uint32_t(normalS_.D_));};

//  virtual IwTangent<T>* merge(const IwTangent<T>& other);
//  void fromMerge(const IwTangent<T>& niwA, const IwTangent<T>& niwB);

  const Matrix<T,Dynamic,Dynamic>& scatter() const {return iw0_.scatter();};
  Matrix<T,Dynamic,Dynamic>& scatter() {return iw0_.scatter();};
  T count() const {return iw0_.count();};
  T& count() {return iw0_.count();};

  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return normalS_.Sigma();};

  const Matrix<T,Dynamic,1>& getMean() const {return normalS_.getMean();};
  void setMean(const Matrix<T,Dynamic,1>& mean) {return normalS_.setMean(mean);};

private:

//  void computeMergedSS( const IwTangent<T>& niwA,
//    const IwTangent<T>& niwB, Matrix<T,Dynamic,Dynamic>& scatterM,
//    Matrix<T,Dynamic,1>& muM, T& countM) const;

};

typedef IwTangent<double> IwTangentd;
typedef IwTangent<float> IwTangentf;


//Copyright (C) 2011 Pierre Moulon
//
//This program is free software: you can redistribute it and/or modify
//it under the terms of the GNU General Public License as published by
//the Free Software Foundation, either version 3 of the License, or
//(at your option) any later version.
//
//This program is distributed in the hope that it will be useful,
//but WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//GNU General Public License for more details.
//
//You should have received a copy of the GNU General Public License
//along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef LIBS_SVD_H_
#define LIBS_SVD_H_

#include <Eigen/Core>
#include <Eigen/SVD>
#include "extras/libNumerics/matrix.h"
#include "third_party/svd/matConversion.hpp"
#include "third_party/svd/eigenWrapper.hpp"

namespace SVDWrapper {

  typedef libNumerics::matrix<double> Mat;
  typedef libNumerics::vector<double> Vec;
  typedef Eigen::MatrixXd MatEigen;

  /// Solve the linear system Ax = 0 with ||x|| = 1.0 via SVD.
  /// Return true if the ratio of singular values SV(0)/SV(N-2) < dRatio.
  /// The return value indicates whether the solution is unique.
  static bool Nullspace(const Mat & A, Vec *nullspace, double dRatio=1e-5)
  {
    typedef Eigen::MatrixXd MatEigen;
    MatEigen AEigen, null;
    EigenWrapper::MatToEigen(A, AEigen);
    bool bOk = EigenWrapper::Nullspace(&AEigen, &null, dRatio);
    (*nullspace).read( null.data() );
    return bOk;
  }

  /// Inverse of norm-2 condition value (ratio of extreme singular values)
  inline double InvCond(const Mat& A) {
    Eigen::MatrixXd AEigen, sing;
    EigenWrapper::MatToEigen(A, AEigen);
    EigenWrapper::SingularValues(&AEigen, &sing);
    return sing(A.nrow()-1)/sing(0);
  }

  /// Make rank<=2.
  inline void EnforceRank2_3x3(const Mat& A, Mat *ARank)
  {
    Eigen::MatrixXd MEigen, MRank2Eigen;
    EigenWrapper::MatToEigen(A, MEigen);
    EigenWrapper::EnforceRank2_3x3(MEigen, &MRank2Eigen);
    EigenWrapper::EigenToMat(MRank2Eigen, *ARank);
  }

  /// Save the two last nullspace vector as 3x3 matrices.
  /// It uses Eigen to compute the SVD decomposition.
  inline void Nullspace2_Remap33(const Mat &A, Mat& f1, Mat& f2) {
      using Eigen::Map;
      using namespace EigenWrapper;

      MatEigen f1E, f2E;
      MatEigen AEigen;
      MatToEigen(A, AEigen);
      Nullspace2(&AEigen, &f1E, &f2E);

      typedef Eigen::Matrix<double, 3, 3> Mat3;
      typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> RMat3;
      // Update f1 and f2
      EigenToMat(Map<RMat3>(f1E.data()), f1);
      EigenToMat(Map<RMat3>(f2E.data()), f2);
  }

} // namespace SVDWrapper

#endif // LIBS_SVD_H_


// Loba2d library

// Copyright (c) 2015 Andrii Sydorchuk

// Use, modification and distribution is subject to the MIT License (MIT).

#include <ctime>
#include <iostream>
#include <random>
#include <vector>

#include <boost/polygon/voronoi_builder.hpp>

#include "loba2d/delaunay_graph.hpp"

const int RANDOM_SEED = 27;
const int NUM_POINTS = 1000000;

int main() {
  std::mt19937 gen(RANDOM_SEED);
  boost::polygon::voronoi_builder<int> vb;
  loba2d::delaunay_graph dg;
  std::vector< std::pair<int, int> > input;

  for (int i = 0; i < NUM_POINTS; ++i) {
    int x = gen() & ((1 << 30) - 1);
    int y = gen() & ((1 << 30) - 1);
    input.push_back(std::make_pair(x, y));
    vb.insert_point(x, y);
  }

  double t = clock();
  vb.construct(&dg);
  double elapsed = (clock() - t) / 1E6;

  std::cout << "Delaunay index constructed in: " << elapsed << "(secs)" << std::endl;
  std::cout << "Num. vertices: " << dg.vertices().size() << std::endl;
  std::cout << "Num. triangles: " << dg.triangles().size() << std::endl;
  std::cout << "Num. half-edges: " << dg.edges().size() << std::endl;

  if (dg.num_triangles() <= 10) {
    std::cout << "Triangles coordinates:" << std::endl;
    for (loba2d::delaunay_graph::const_triangle_iterator it = dg.triangles().begin();
         it != dg.triangles().end(); ++it) {
      const loba2d::delaunay_graph::edge_type* e = it->incident_edge();
      std::cout << "[";
      for (int i = 0; i < 3; ++i) {
        std::size_t idx = e->vertex0()->source_index();
        std::cout << " (" << input[idx].first << ", " << input[idx].second << ")";
        e = e->next();
      }
      std::cout << " ]" << std::endl;
    }
  }
}


#include "ouster/lidar_scan.h"

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

namespace ouster {

constexpr int LidarScan::N_FIELDS;

XYZLut make_xyz_lut(size_t w, size_t h, double range_unit,
                    double lidar_origin_to_beam_origin_mm,
                    const mat4d& transform,
                    const std::vector<double>& azimuth_angles_deg,
                    const std::vector<double>& altitude_angles_deg) {
    if (w <= 0 || h <= 0)
        throw std::invalid_argument("lut dimensions must be greater than zero");
    if (azimuth_angles_deg.size() != h || altitude_angles_deg.size() != h)
        throw std::invalid_argument("unexpected scan dimensions");

    Eigen::ArrayXd encoder(w * h);   // theta_e
    Eigen::ArrayXd azimuth(w * h);   // theta_a
    Eigen::ArrayXd altitude(w * h);  // phi

    const double azimuth_radians = M_PI * 2.0 / w;

    // populate angles for each pixel
    for (size_t v = 0; v < w; v++) {
        for (size_t u = 0; u < h; u++) {
            size_t i = u * w + v;
            encoder(i) = 2.0 * M_PI - (v * azimuth_radians);
            azimuth(i) = -azimuth_angles_deg[u] * M_PI / 180.0;
            altitude(i) = altitude_angles_deg[u] * M_PI / 180.0;
        }
    }

    XYZLut lut;

    // unit vectors for each pixel
    lut.direction = LidarScan::Points{w * h, 3};
    lut.direction.col(0) = (encoder + azimuth).cos() * altitude.cos();
    lut.direction.col(1) = (encoder + azimuth).sin() * altitude.cos();
    lut.direction.col(2) = altitude.sin();

    // offsets due to beam origin
    lut.offset = LidarScan::Points{w * h, 3};
    lut.offset.col(0) = encoder.cos() - lut.direction.col(0);
    lut.offset.col(1) = encoder.sin() - lut.direction.col(1);
    lut.offset.col(2) = -lut.direction.col(2);
    lut.offset *= lidar_origin_to_beam_origin_mm;

    // apply the supplied transform
    auto rot = transform.topLeftCorner(3, 3).transpose();
    auto trans = transform.topRightCorner(3, 1).transpose();
    lut.direction.matrix() *= rot;
    lut.offset.matrix() *= rot;
    lut.offset.matrix() += trans.replicate(w * h, 1);

    // apply scaling factor
    lut.direction *= range_unit;
    lut.offset *= range_unit;

    return lut;
}

LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut) {
    if (scan.w * scan.h != lut.direction.rows())
        throw std::invalid_argument("unexpected scan dimensions");

    auto reshaped = Eigen::Map<const Eigen::Array<LidarScan::raw_t, -1, 1>>(
        scan.field(LidarScan::RANGE).data(), scan.h * scan.w);
    auto nooffset = lut.direction.colwise() * reshaped.cast<double>();
    return (nooffset.array() == 0.0).select(nooffset, nooffset + lut.offset);
}

ScanBatcher::ScanBatcher(size_t w, const sensor::packet_format& pf)
    : w(w), h(pf.pixels_per_column), next_m_id(0), ls_write(w, h), pf(pf) {}

bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) {
    using row_view_t =
        Eigen::Map<Eigen::Array<LidarScan::raw_t, Eigen::Dynamic,
                                Eigen::Dynamic, Eigen::RowMajor>>;

    if (ls.w != w || ls.h != h)
        throw std::invalid_argument("unexpected scan dimensions");

    bool swapped = false;

    for (int icol = 0; icol < pf.columns_per_packet; icol++) {
        const uint8_t* col_buf = pf.nth_col(icol, packet_buf);
        const uint16_t m_id = pf.col_measurement_id(col_buf);
        const uint16_t f_id = pf.col_frame_id(col_buf);
        const std::chrono::nanoseconds ts(pf.col_timestamp(col_buf));
        const uint32_t encoder = pf.col_encoder(col_buf);
        const uint32_t status = pf.col_status(col_buf);
        const bool valid = (status == 0xffffffff);

        // drop invalid / out-of-bounds data in case of misconfiguration
        if (!valid || m_id >= w || f_id + 1 == ls_write.frame_id) continue;

        if (ls_write.frame_id != f_id) {
            // if not initializing with first packet
            if (ls_write.frame_id != -1) {
                // zero out remaining missing columns
                auto rows = h * LidarScan::N_FIELDS;
                row_view_t{ls_write.data.data(), rows, w}
                    .block(0, next_m_id, rows, w - next_m_id)
                    .setZero();

                // finish the scan and notify callback
                std::swap(ls, ls_write);
                swapped = true;
            }

            // start new frame
            next_m_id = 0;
            ls_write.frame_id = f_id;
        }

        // zero out missing columns if we jumped forward
        if (m_id >= next_m_id) {
            auto rows = h * LidarScan::N_FIELDS;
            row_view_t{ls_write.data.data(), rows, w}
                .block(0, next_m_id, rows, m_id - next_m_id)
                .setZero();
            next_m_id = m_id + 1;
        }

        ls_write.header(m_id) = {ts, encoder, status};
        for (uint8_t ipx = 0; ipx < h; ipx++) {
            const uint8_t* px_buf = pf.nth_px(ipx, col_buf);

            ls_write.block(m_id).row(ipx)
                << static_cast<LidarScan::raw_t>(pf.px_range(px_buf)),
                static_cast<LidarScan::raw_t>(pf.px_signal(px_buf)),
                static_cast<LidarScan::raw_t>(pf.px_ambient(px_buf)),
                static_cast<LidarScan::raw_t>(pf.px_reflectivity(px_buf));
        }
    }
    return swapped;
}

}  // namespace ouster


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, Clearpath Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#include <fuse_constraints/normal_delta_pose_2d.h>
#include <fuse_core/util.h>

#include <Eigen/Core>
#include <glog/logging.h>

namespace fuse_constraints
{

NormalDeltaPose2D::NormalDeltaPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) :
  A_(A),
  b_(b)
{
  CHECK_GT(A_.rows(), 0);
  CHECK_EQ(A_.cols(), 3);
  set_num_residuals(A_.rows());
}

bool NormalDeltaPose2D::Evaluate(
  double const* const* parameters,
  double* residuals,
  double** jacobians) const
{
  const fuse_core::Matrix2d R1_transpose = fuse_core::rotationMatrix2D(parameters[1][0]).transpose();  // orientation1
  const fuse_core::Vector2d position_delta =
      R1_transpose * fuse_core::Vector2d(parameters[2][0] - parameters[0][0],   // position2.x - position1.x
                                         parameters[2][1] - parameters[0][1]);  // position2.y - position1.y

  const fuse_core::Vector3d full_residuals_vector(
      position_delta[0] - b_[0], position_delta[1] - b_[1],
      fuse_core::wrapAngle2D(parameters[3][0] - parameters[1][0] - b_[2]));  // orientation2 - orientation1

  // Scale the residuals by the square root information matrix to account for the measurement uncertainty.
  Eigen::Map<fuse_core::VectorXd> residuals_vector(residuals, num_residuals());
  residuals_vector = A_ * full_residuals_vector;

  if (jacobians != nullptr)
  {
    // Jacobian wrt position1
    if (jacobians[0] != nullptr)
    {
      Eigen::Map<fuse_core::MatrixXd>(jacobians[0], num_residuals(), 2) = -A_.leftCols<2>() * R1_transpose;
    }

    // Jacobian wrt orientation1
    if (jacobians[1] != nullptr)
    {
      Eigen::Map<fuse_core::VectorXd>(jacobians[1], num_residuals()) =
          A_ * fuse_core::Vector3d(position_delta[1], -position_delta[0], -1);
    }

    // Jacobian wrt position2
    if (jacobians[2] != nullptr)
    {
      Eigen::Map<fuse_core::MatrixXd>(jacobians[2], num_residuals(), 2) = A_.leftCols<2>() * R1_transpose;
    }

    // Jacobian wrt orientation2
    if (jacobians[3] != nullptr)
    {
      Eigen::Map<fuse_core::VectorXd>(jacobians[3], num_residuals()) = A_.col(2);
    }
  }
  return true;
}

}  // namespace fuse_constraints


#include <cassert>
#include <cmath>
#include <algorithm>
#include <boost/functional/hash.hpp>

#include "fin.hh"

using namespace std;

Fin::Fin( const Fin & other )
  : Action( other ),
    _lambda( other._lambda )
{
}

Fin::Fin( const RemyBuffers::Fin & dna )
  : Action ( dna.domain() ),
    _lambda( dna.lambda() )
{
}

RemyBuffers::Fin Fin::DNA( void ) const
{
  RemyBuffers::Fin ret;

  ret.set_lambda( _lambda );
  ret.mutable_domain()->CopyFrom( _domain.DNA() );

  return ret;
}

vector< Fin > Fin::next_generation( void ) const
{
  vector< Fin> ret;

  auto lambda_alternatives = get_optimizer().lambda.alternatives( _lambda, true );

  printf("Alternatives: lambda %f to %f\n",
         *(min_element(lambda_alternatives.begin(), lambda_alternatives.end())),
         *(max_element(lambda_alternatives.begin(), lambda_alternatives.end()))
  );

  for ( const auto & alt_lambda : lambda_alternatives ) {
        Fin new_fin{ *this };
        new_fin._generation++;

        new_fin._lambda = alt_lambda;
        new_fin.round();

        ret.push_back( new_fin );
  }

  return ret;
}

string Fin::str( const unsigned int total ) const
{
  char tmp[ 256 ];
  snprintf( tmp, 256, "{%s} gen=%u usage=%.4f => (lambda=%f)",
            _domain.str().c_str(), _generation, double( _domain.count() ) / double( total ), _lambda );
  return tmp;
}

void Fin::round( void )
{
  _lambda = (1.0/10000.0) * int( 10000 * _lambda );
}

size_t hash_value( const Fin & fin )
{
  size_t seed = 0;
  boost::hash_combine( seed, fin._lambda );
  boost::hash_combine( seed, fin._domain );

  return seed;
}


#include "geodb/trajectory.hpp"

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

namespace geodb {

bool trajectory_unit::intersects(const bounding_box& b) const {
    namespace bg = boost::geometry;

    // Using double for all coordinates.
    using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
    using line_t = bg::model::segment<point_t>;
    using box_t = bg::model::box<point_t>;

    // Convert point to boost geometry type.
    auto convert_point = [](const vector3& p) {
        return point_t(p.x(), p.y(), p.t());
    };

    box_t box(convert_point(b.min()), convert_point(b.max()));
    line_t line(convert_point(start), convert_point(end));
    return bg::intersects(box, line);
}

} // namespace geodb


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

using Weight = int;

// 辺
struct Edge {
	size_t from;
	size_t to;
	Weight cost;
	Edge(size_t t, Weight c) : to(t), cost(c) {}
	Edge(size_t f, size_t t, Weight c) : from(f), to(t), cost(c) {}
	bool operator<(const Edge& rhs) const { return this->cost > rhs.cost; }
};

// グラフ G=(V,E)
struct Graph {
	size_t node;
	std::vector<std::vector<Edge>> edges;

	Graph(size_t n) : node(n), edges(n) {}
};

// 最短経路探索(非負閉路)
std::vector<int> dijkstra(const Graph& graph, const size_t s) {
	size_t n = graph.node;
	std::vector<int> used(n, -1);
	std::vector<Weight> distances(n, INF);

	distances[s] = 0;
	std::priority_queue<Edge> pq;
	pq.push(Edge(0, s, 0));
	while(!pq.empty()) {
		Edge edge = pq.top();
		pq.pop();
		if(used[edge.to] != -1) {
			continue;
		}
		used[edge.to] = edge.from;
		for(auto&& e : graph.edges[edge.to]) {
			Weight alt = edge.cost + e.cost;
			if(alt < distances[e.to]) {
				distances[e.to] = alt;
				pq.push(Edge(edge.to, e.to, alt));
			}
		}
	}
	return used;
}

int main() {
	int n, m;
	cin >> n >> m;
	VI a(m);
	VI b(m);
	Graph g(n);
	REP(i, m) {
		cin >> a[i] >> b[i];
		g.edges[a[i] - 1].push_back(Edge(a[i] - 1, b[i] - 1, 1));
		g.edges[b[i] - 1].push_back(Edge(b[i] - 1, a[i] - 1, 1));
	}
	vector<int> result = dijkstra(g, 0);
	bool is_enable = true;
	REP(i, n) {
		if(result[i] == -1) {
			is_enable = false;
			break;
		}
	}
	if(is_enable) {
		cout << "Yes" << endl;
		RANGE(i, 1, n) { cout << result[i] + 1 << endl; }
	} else {
		cout << "No" << endl;
	}
	return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_SCALED_INV_CHI_SQUARE_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_SCALED_INV_CHI_SQUARE_RNG_HPP

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/gamma_q.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_gamma.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    scaled_inv_chi_square_rng(double nu,
                              double s,
                              RNG& rng) {
      using boost::variate_generator;
      using boost::random::chi_squared_distribution;

      static const char* function("scaled_inv_chi_square_rng");

      check_positive_finite(function, "Degrees of freedom parameter", nu);
      check_positive_finite(function, "Scale parameter", s);

      variate_generator<RNG&, chi_squared_distribution<> >
        chi_square_rng(rng, chi_squared_distribution<>(nu));
      return nu * s / chi_square_rng();
    }

  }
}
#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Domain/FunctionsOfTime/QuaternionHelpers.hpp"

#include <boost/math/quaternion.hpp>

#include "DataStructures/DataVector.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/Gsl.hpp"

DataVector quaternion_to_datavector(
    const boost::math::quaternion<double>& input) noexcept {
  return DataVector{input.R_component_1(), input.R_component_2(),
                    input.R_component_3(), input.R_component_4()};
}

boost::math::quaternion<double> datavector_to_quaternion(
    const DataVector& input) noexcept {
  ASSERT(input.size() == 3 or input.size() == 4,
         "To form a quaternion, a DataVector can either have 3 or 4 components "
         "only. This DataVector has "
             << input.size() << " components.");
  if (input.size() == 3) {
    return boost::math::quaternion<double>(0.0, input[0], input[1], input[2]);
  } else {
    return boost::math::quaternion<double>(input[0], input[1], input[2],
                                           input[3]);
  }
}

// Normalize a `boost::math::quaternion`
void normalize_quaternion(
    const gsl::not_null<boost::math::quaternion<double>*> input) noexcept {
  *input /= abs(*input);
}


#ifndef STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_DIAG_HPP
#define STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_DIAG_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_vector.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>

namespace stan {
namespace math {

template <typename T1, typename T2, int R, int C>
inline Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                     Eigen::Dynamic, Eigen::Dynamic>
quad_form_diag(const Eigen::Matrix<T1, Eigen::Dynamic, Eigen::Dynamic>& mat,
               const Eigen::Matrix<T2, R, C>& vec) {
  using boost::math::tools::promote_args;
  check_vector("quad_form_diag", "vec", vec);
  check_square("quad_form_diag", "mat", mat);
  int size = vec.size();
  check_size_match("quad_form_diag", "rows of mat", mat.rows(), "size of vec",
                   size);
  Eigen::Matrix<typename promote_args<T1, T2>::type, Eigen::Dynamic,
                Eigen::Dynamic>
      result(size, size);
  for (int i = 0; i < size; i++) {
    result(i, i) = vec(i) * vec(i) * mat(i, i);
    for (int j = i + 1; j < size; ++j) {
      typename promote_args<T1, T2>::type temp = vec(i) * vec(j);
      result(j, i) = temp * mat(j, i);
      result(i, j) = temp * mat(i, j);
    }
  }
  return result;
}

}  // namespace math
}  // namespace stan
#endif


#include <iostream>
#include <vector>
#include <utility>
#include <boost/random.hpp>

class ising
{
private:
  std::vector<bool> configuration;
  std::vector<std::vector<double>> connects;

  double cost(const std::vector<bool>& state) const;
  void incliment(std::vector<bool>& state) const;
public:
  // constructors
  ising(const unsigned int N, boost::mt19937& eng);
  ising(const unsigned int N, const std::vector<std::vector<double>> Q);
  ising(const std::vector<bool> spins, const std::vector<std::vector<double>> Q);
  ising(const std::vector<bool> spins, boost::mt19937& eng);

  // destructor
  ~ising();

  // extract inner state/configuration
  std::vector<bool> state() const;

  // objective/cost function
  double cost() const;
  double delta(const unsigned int site) const;

  // flip procedure
  void flip(const unsigned int site);

  // all search
  std::pair<double, std::vector<bool>> all_search() const;

  // output for debug
  friend std::ostream& operator<<(std::ostream& os, const ising& obj);
  void show_interaction(std::ostream& os) const;
};