/*

RPBA - Robust Parallel Bundle Adjustment

File rpbacore.cpp

Description: Parallel bundle adjustment core

Copyright 2019 Helmut Mayer, Bundeswehr University Munich, Germany, Helmut.Mayer@unibw.de

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

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

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

*/

#include "rpba.h"
#include "system.h"
#include <boost/math/special_functions/round.hpp>

void CalcPLSA(Eigen::MatrixXf &XX,
		std::vector<Eigen::MatrixXf> &xg,
		const std::vector<std::vector<float> > &wx, const std::vector<std::vector<float> > &wy,
		const std::vector<std::vector<float> > &wxy,
		std::vector<std::vector<IMAPNR> > &pointXXvio,
		std::vector<PMat> &PMatrices,
		std::vector<Camera> &cameras,
		const std::vector<int> &iwidth, const std::vector<int> &iheight,
		const bool robustflag,
		const std::vector<bool> &addflagsin,
		std::ostream& os, const int mt,
		const bool extevalflag) {

	std::vector<bool> addflags,addflagsold;
	addflags = addflagsin;

	int nraddp = 0;
	for (int i = 0; i < (int) addflags.size(); ++i)
		if (addflags[i])
			++nraddp;

	addflagsold = addflags;

	Eigen::Matrix3f Ksi;

	std::vector<std::vector<IMAPNR> > pointXXv,pointXXvold;
	pointXXv = pointXXvio;

	const int pxs = (int) pointXXv.size();
	int nrimages = (int) xg.size();

	bool gcpflag = false;

	Eigen::MatrixXd XXd(4,pxs), XXdold(4,pxs), XXdt(4,pxs), XXt(4,pxs);
	std::vector<Eigen::MatrixXd> Zs; // XXs;

	std::vector<int> imgind, imgindbv(nrimages);
	std::vector<Camera> cameraps(nrimages);
	std::vector<int> gcpinfo;
	std::vector<std::vector<float> > W3iji(6);
	std::vector<PMatd> PMatricesd(nrimages),PMatricesdold(nrimages),PMatricesdt(nrimages);
	std::vector<std::vector<double> > gcpdata,gcpdatait;

	for (int i = 0; i < nrimages; ++i) {
		cameraps[i] = cameras[i];
		PMatricesd[i] = PMatrices[i].cast <double> ();
		PMatricesdold[i] = PMatrices[i].cast <double> ();
	}

	XXd = XX.cast <double> ();

	int minnrimages = 70;
	const int minnrthreads = boost::math::iround((float) nrimages / (float) minnrimages);

	int nrthreads = mt;

	if (minnrthreads < nrthreads)
		nrthreads = minnrthreads;

	float ihw2 = 0.f;
	for(int i = 0; i < nrimages; ++i)
		ihw2 += (float) ((iheight[i] + iwidth[i]) * (iheight[i] + iwidth[i])) * 0.25f;
	ihw2 /= (float) nrimages;

	bool parflag = nrthreads > 1 && nrimages > 5;

	if (extevalflag) {
		EvalLSA(nraddp, cameraps, XXd, xg, wx, wy, wxy, pointXXv, PMatricesd, ihw2);
		return;
	}

	if (parflag) {

		std::vector<float> xiout;
		Eigen::MatrixXf wap;
		std::vector<float> sig0vio;
		double sig0g = 0.;
		long redundancy = 0;

		gcpflag = true;

		std::cout << "#threads: " << nrthreads << " " << minnrthreads << '\n';

		std::vector<std::vector<bool> > pitflagsv(pxs);
		std::vector<LSA*> LSAv(nrthreads);
		std::vector<Eigen::MatrixXd> XXdd(nrthreads);
		std::vector<std::vector<PMatd> > PMatricesdd(nrthreads);
		std::vector<std::vector<Camera> > camerasoit(nrthreads);
		std::vector<std::vector<std::vector<IMAPNR> > > pointXXvv(nrthreads);
		std::vector<IMAPNR> pxvv;
		std::vector<std::vector<IMAPNR> > pxvvv(nrthreads);
		std::vector<int> nriv(nrthreads+1);
		std::vector<std::vector<int> > imgindv(nrthreads),gcpinfov(nrthreads),pindv(nrthreads);
		std::vector<std::vector<std::vector<std::vector<float> > > > w3dvivv(nrthreads);
		std::vector<std::vector<std::vector<float> > > sig0vvv(nrthreads);
		std::vector<std::vector<std::vector<float> > > W3ijiv(nrthreads);
		std::vector<std::vector<float> > sig0vvs(nrimages);
		std::vector<long> redundancyv(nrthreads);
		std::vector<float> sig0wv(nrimages);
		std::vector<double> sig0gv(nrthreads);
		std::vector<std::vector<std::vector<double> > > gcpdatav;
		std::vector<int> beginv(nrthreads),endv(nrthreads);
		std::vector<Camera> camerasnew(nrimages),camerasold(nrimages);

		std::vector<std::vector<int> > imagesperpart;
		std::vector<idx_t> part(nrimages);

		const System::Time startp = System::getTickCount();
		imagesperpart.resize(nrthreads);
		PartitionpointXXv(pointXXv, nrimages, nrthreads, part, imagesperpart);
		std::cout << "\n\nPartitioning runtime: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n\n";

		nriv[0] = 0;
		for (int it = 0; it < nrthreads; ++it) {
			nriv[it+1] = nriv[it] + (int) imagesperpart[it].size();
			w3dvivv[it].resize(6);
		}
		nriv[nrthreads] = nrimages;

		std::vector<int> imageind(nrimages);

		int itj = -1;
		for (int it = 0; it < nrthreads; ++it)
			for (int j = 0; j < (int) imagesperpart[it].size(); ++j)
				imageind[++itj] = imagesperpart[it][j];

		for (int it = 0; it < nrthreads; ++it) {
			camerasoit[it] = cameras;
			PMatricesdd[it] = std::vector<PMatd>(nrimages);
			for (int i = 0; i < nrimages; ++i) {
				PMatricesdd[it][i] = PMatrices[i].cast <double> ();
			} // for i
			imgindv[it].resize(nriv[it+1] - nriv[it]);
			std::cout << "Thread: " << it << " #images " << nriv[it+1] - nriv[it] << " indices " << nriv[it] << ' ' << nriv[it+1] << '\n';
			for (int i = 0; i < nriv[it+1] - nriv[it]; ++i) {
				imgindv[it][i] = imageind[i + nriv[it]];
				imgindbv[imageind[i + nriv[it]]] = it;
			}
			std::sort(imgindv[it].begin(),imgindv[it].end());
			sig0vvv[it].resize(nrimages);
		} // for it

		gcpinfo.resize(pxs);
		std::vector<int>::iterator pxi;
		for (pxi = gcpinfo.begin(); pxi != gcpinfo.end(); ++pxi)
			(*pxi) = -1;

		sig0g = 0.;
		redundancy = 0;
		int nrparimages = (nrimages - 2) * 6 + 5 + nrimages * nraddp;

		int nrobs;
		double sig0opt = 1.e20;

		int iflag = 1, iter = 1;
		if (robustflag)
			iflag = 2;
		bool robustflagit;
		bool iniflag = true;

		gcpflag = true;

		for (int i = 0; i < pxs; ++i) {
			if ((int) pointXXv[i].size() > 0) {
				pitflagsv[i].resize(nrthreads);
				for (int j = 0; j < nrthreads; ++j)
					pitflagsv[i][j] = false;
				for (int j = 0; j < (int) pointXXv[i].size(); ++j)
					pitflagsv[i][imgindbv[pointXXv[i][j].image]] = true;
			} // if pointXXv
		} // for i

		gcpdatav.resize(nrthreads);

		float sig0frac = 1.01f;

		while (iflag > 0 && iter < 51) {
			robustflagit = false;

			if (iter > 1) {
				if (iter < 3)
					addflags[0] = addflags[1] = addflags[2] = false;
				else
					addflags = addflagsold;

				std::vector<std::ostringstream> osv(nrthreads);

				#pragma omp parallel for num_threads(nrthreads)
				for (int it = 0; it < nrthreads; ++it) {
					if (iter == 2)
						LSAv[it] = new LSA( camerasoit[it], addflags,
								true, imgindv[it], PMatricesdd[it],
								pointXXvv[it], XXdd[it], (int)pindv[it].size(), xg,
								wx, wy, wxy, W3iji,
								true, gcpinfov[it], gcpdatait,
								ihw2, false);
					else
						LSAv[it]->InputLSAP(addflags, camerasoit[it], imgindv[it], pointXXvv[it], PMatricesdd[it], XXdd[it], (int) pointXXvv[it].size(),
								gcpinfov[it]);

					LSAv[it]->Adjust(false, osv[it], true, 0, W3ijiv[it], true, gcpdatav[it]);

					LSAv[it]->OutputLSAP(PMatricesdd[it], pointXXvv[it], XXdd[it], gcpinfov[it], camerasoit[it]);
				} // for it

				for (int it = 0; it < nrthreads; ++it) {
					os << osv[it].str();
					for (int i = 0; i < (int) imgindv[it].size(); ++i) {
						PMatricesd[imgindv[it][i]] = PMatricesdd[it][imgindv[it][i]];
						const int iii = imgindv[it][i];
						cameraps[iii] = camerasoit[it][imgindv[it][i]];
						camerasnew[iii] = camerasoit[it][imgindv[it][i]];
					} // for i
				}
			} // it iter > 1

			if (iflag == 1) {
				robustflagit = robustflag;
				if (robustflag)
					sig0frac = 1.02f;
			}

			const int nrppt = pxs / nrthreads;
			if (iniflag) {
				for (int itt = 0; itt < nrthreads; ++itt) {
					beginv[itt] = nrppt * itt;
					endv[itt] = nrppt * (itt + 1);
					if (itt == nrthreads - 1)
						endv[itt] = pxs;

					const int eb = endv[itt] - beginv[itt];
					for (int vv = 0; vv < 6; ++vv) {
						w3dvivv[itt][vv].resize(eb);
						for (int ebb = 0; ebb < eb; ++ebb)
							w3dvivv[itt][vv][ebb].resize(nrthreads);
					}
					W3ijiv[itt].resize(6);

				} // for itt
			} // if iniflag

			if (iter > 1) {
				for (int i = 0; i < pxs; ++i)
					pointXXv[i].resize(0);

				for (int it= 0; it < nrthreads; ++it)
					for (int i = 0; i < (int) pindv[it].size(); ++i)
						for (size_t j = 0; j < pointXXvv[it][i].size(); ++j)
							pointXXv[pindv[it][i]].push_back(pointXXvv[it][i][j]);

				for (int i = 0; i < pxs; ++i)
					if ((int) pointXXv[i].size() < 2)
						pointXXv[i].clear();
			}
			else {
				for (int it = 0; it < nrthreads; ++it) {
					for (int i = 0; i < (int) imgindv[it].size(); ++i) {
						const int iii = imgindv[it][i];
						cameraps[iii] = cameras[iii];
						camerasnew[iii] =  cameras[iii];
					} // for i
				} // for it
			} // else

			for (int itt = 0; itt < nrthreads; ++itt)
				for (int i = 0; i < 6; ++i)
					W3ijiv[itt][i].resize(0);
			for (int i = 0; i < pxs; ++i) {
				if ((int) pointXXv[i].size() > 0) {
					for (int it = 0; it < nrthreads; ++it)
						pitflagsv[i][it] = false;
					for (int j = 0; j < (int) pointXXv[i].size(); ++j)
						pitflagsv[i][imgindbv[pointXXv[i][j].image]] = true;
				} // if pointXXv
			} // for i

			if (robustflagit) {
				#pragma omp parallel for num_threads(nrthreads)
				for (int itt = 0; itt < nrthreads; ++itt) {
					for (int iii = 0; iii < nrimages; ++iii)
						sig0vvv[itt][iii].resize(0);
					sig0gv[itt] = 0.;
					redundancyv[itt] = 0;
					for (int i = beginv[itt], ii = 0; i < endv[itt]; ++i, ++ii) {
						if ((int) pointXXv[i].size() > 0) {
							CalcLSAXMulti(cameraps,i,XXd,xg,wx,wy,wxy,pointXXv,PMatricesd,
									imgindbv, nrthreads, pitflagsv[i],
									w3dvivv[itt][0][ii],w3dvivv[itt][1][ii],w3dvivv[itt][2][ii],
									w3dvivv[itt][3][ii],w3dvivv[itt][4][ii],w3dvivv[itt][5][ii],
									sig0gv[itt],redundancyv[itt],sig0vvv[itt],sig0wv,1,false,false);
						} // if pointXX
					} // for i
				} // for itt

				sig0g = 0.;
				redundancy = 0;
				for (int itt = 0; itt < nrthreads; ++itt) {
					sig0g += sig0gv[itt];
					redundancy += redundancyv[itt];
				}

				for (int i = 0; i < nrimages; ++i)
					sig0vvs[i].resize(0);

				for (int itt = 0; itt < nrthreads; ++itt)
					for (int i = 0; i < nrimages; ++i)
						sig0vvs[i].insert(sig0vvs[i].begin(),sig0vvv[itt][i].begin(),sig0vvv[itt][i].end());
				for (int i = 0; i < nrimages; ++i) {
					if (sig0vvs[i].size() == 0) {
						std::cout << "Image " << i << " without observations!!!!!!!!!!!!!!!!!!!!!\n\n";
						sig0wv[i] = 0.f;
					}
					else {
						std::sort(sig0vvs[i].begin(),sig0vvs[i].end());
						sig0wv[i] = 1.f / (sig0vvs[i][(int) sig0vvs[i].size() / 2] * 2.1981f); // 1.4826 // 1.706f - 1.306^2
					}
				} // for i
			} // if robustflagit

			int robustmode = 0;
			if (robustflagit)
				robustmode = 2;

			bool weightonlyflag = false;
			if (iter == 1)
				weightonlyflag = true;

			#pragma omp parallel for num_threads(nrthreads)
			for (int itt = 0; itt < nrthreads; ++itt) {
				sig0gv[itt] = 0.;
				redundancyv[itt] = 0;
				for (int i = beginv[itt], ii = 0; i < endv[itt]; ++i, ++ii) {
					if ((int) pointXXv[i].size() > 0) {
						CalcLSAXMulti(cameraps,i,XXd,xg,wx,wy,wxy,pointXXv,PMatricesd,
								imgindbv, nrthreads, pitflagsv[i],
								w3dvivv[itt][0][ii],w3dvivv[itt][1][ii],w3dvivv[itt][2][ii],
								w3dvivv[itt][3][ii],w3dvivv[itt][4][ii],w3dvivv[itt][5][ii],
								sig0gv[itt],redundancyv[itt],sig0vvv[itt],sig0wv,robustmode,weightonlyflag,true);
					} // if pointxxv
				} // for i
			} // for itt

			sig0g = 0.;
			redundancy = 0;
			for (int itt = 0; itt < nrthreads; ++itt) {
				sig0g += sig0gv[itt];
				redundancy += redundancyv[itt];
			}

			nrobs = 0;
			for (int i = 0; i < pxs; ++i)
				nrobs += (int) pointXXv[i].size();
			nrobs *= 2;

			sig0g /= (double) (redundancy - nrparimages);

			if (iter > 0) {
				std::cout << "IT: " << iter << " " << iflag << "  " << sqrt(sig0g * ihw2) << ' ' << sig0opt / sig0g << "  ";
				std::cout << sig0g << " " << redundancy << " " << nrobs << " " << nrparimages << " " << nraddp << "  " << redundancy - nrparimages << '\n';
			}

			if (sig0g <= sig0opt) {
				XXdold = XXd;
				for (int i = 0; i < nrimages; ++i)
					PMatricesdold[i] = PMatricesd[i];
				pointXXvold = pointXXv;
				camerasold = camerasnew;
			}

			for (int itt = 0; itt < nrthreads; ++itt) {
				camerasoit[itt] = camerasnew;
				for (int i = 0; i < (int) imgindv[itt].size(); ++i)
					PMatricesdd[itt][imgindv[itt][i]] = PMatricesd[imgindv[itt][i]];
				pointXXvv[itt].resize(0);
				pindv[itt].resize(0);
			} // for itt

			int nrgcpnew = 0;
			for (int itt = 0; itt < nrthreads; ++itt)
				gcpdatav[itt].resize(0);

			for (int itt = 0; itt < nrthreads; ++itt) {
				for (int i = beginv[itt], ii = 0; i < endv[itt]; ++i, ++ii) {
					if ((int) pointXXv[i].size() < 2)
						continue;
					int gnum = 0;
					for (int it = 0; it < nrthreads; ++it) {
						if (pitflagsv[i][it])
							++gnum;
						if (gnum > 1)
							break;
					}
					if (gnum > 1) {
						gcpinfo[i] = nrgcpnew;
						for (int ittt = 0; ittt < nrthreads; ++ittt) {
							gcpdatav[ittt].push_back(std::vector<double>(3));
							gcpdatav[ittt][nrgcpnew][0] = XXd(0,i);
							gcpdatav[ittt][nrgcpnew][1] = XXd(1,i);
							gcpdatav[ittt][nrgcpnew][2] = XXd(2,i);
						} // for ittt
						for (int ittt = 0; ittt < nrthreads; ++ittt)
							if (pitflagsv[i][ittt]) {
								for (int vv = 0; vv < 6; ++vv)
									W3ijiv[ittt][vv].push_back(w3dvivv[itt][vv][ii][ittt]);
							}
						++nrgcpnew;
					}
					else {
						gcpinfo[i] = -1;
					} // else
				} // for i
			} // for itt

			for (int i = 0; i < pxs; ++i) {
				for (int it = 0; it < nrthreads; ++it) {
					pxvvv[it].resize(0);
				}
				for (int j = 0; j < (int) pointXXv[i].size(); ++j) {
					const int iipi = imgindbv[pointXXv[i][j].image];
					pxvvv[iipi].push_back(pointXXv[i][j]);
				}
				for (int it = 0; it < nrthreads; ++it) {
					if (pxvvv[it].size() > 0) {
						pointXXvv[it].push_back(pxvvv[it]);
						pindv[it].push_back(i);
					} // if
				} // for
			} // for i

			for (int it = 0; it < nrthreads; ++it)
				gcpinfov[it].resize((int) pindv[it].size());

			#pragma omp parallel for num_threads(nrthreads)
			for (int it = 0; it < nrthreads; ++it) {
				XXdd[it].resize(4,(int) pindv[it].size());
				for (int j = 0; j < (int) pindv[it].size(); ++j) {
					XXdd[it].col(j) = XXd.col(pindv[it][j]);
					gcpinfov[it][j] = gcpinfo[pindv[it][j]];
				} // for i
			} // for it

			if (iter > 1) {
				if (sig0g <= sig0opt) {
					if (sig0opt < sig0g * sig0frac && iter > 3) { // 1.05
						--iflag;
					}
					else {
						sig0opt = sig0g;
					}
				}
				else {
					if (iter > 3)
						--iflag;
				}
			}
			else
				iniflag = false;

			++iter;

		} // while iflag

		pointXXvio = pointXXvold;

		for (int i = 0; i < nrimages; ++i)
			PMatrices[i] = PMatricesdold[i].cast <float> ();

		XX = XXdold.cast <float> ();

		cameras = camerasold;

		for (int it = 0; it < nrthreads; ++it)
			delete LSAv[it];

	} // if nrimages >
	else {

		gcpflag = false;

		LSA *LSA1 = new LSA(cameras, addflags,
				false, imgind, PMatricesd, pointXXv, XXd, pxs, xg,
				wx, wy, wxy, W3iji,
				gcpflag, gcpinfo, gcpdata,
				ihw2, robustflag);

		LSA1->Adjust(robustflag, os, false, -1, W3iji, false, gcpdata);

		LSA1->OutputLSAc(PMatrices, pointXXvio, XX, cameras);

		delete LSA1;

	} // else

	return;
} //void CalcPLSAc

// Bundle adjustment for points for multiple images
bool CalcLSAXMulti(const std::vector<Camera> &cameraps,
		const int pi, Eigen::MatrixXd &XX,
		const std::vector<Eigen::MatrixXf> &xg,
		const std::vector<std::vector<float> > &wx, const std::vector<std::vector<float> > &wy,
		const std::vector<std::vector<float> > &wxy,
		std::vector<std::vector<IMAPNR> > &pointXXvio,
		const std::vector<PMatd> &PMatrices,
		const std::vector<int> &imgindbv, const int nrthreads, const std::vector<bool> &pitflags,
		std::vector<float> &wXXv, std::vector<float> &wYYv, std::vector<float> &wZZv,
		std::vector<float> &wXYv, std::vector<float> &wXZv, std::vector<float> &wYZv,
		double &sig0io, long &redundancy, std::vector<std::vector<float> > &sig0vv,
		const std::vector<float> &sig0wv, const int robustmode, const bool weightonlyflag, const bool wxyflag) {

	int js;
	int iter = 1;
	int iflag = 1;
	if (robustmode > 0)
		iflag = 2;
	float sig0 = 1.e20f;
	float X0,X1,X2,Xold0,Xold1,Xold2,X0in,X1in,X2in;
	float sig0opt = 1.e20f;

	X0 = Xold0 = X0in = (float) XX(0,pi);
	X1 = Xold1 = X1in = (float) XX(1,pi);
	X2 = Xold2 = X2in = (float) XX(2,pi);

	bool convflag = false;

	const int pxs = (int) pointXXvio[pi].size();
	int pxsd = pxs;
	int pxs2 = pxs * 2;

	std::vector<float> p1v(pxs), p2v(pxs), p3v(pxs);
	std::vector<float> wxi(pxs),wyi(pxs),wxyi(pxs),wxio(pxs),wyio(pxs),wxyio(pxs),sig0v(pxs);

	Eigen::Matrix3d eNpp, eNppi;
	Eigen::Vector3d exip;
	Eigen::Vector2d ecorr2;
	Eigen::Matrix<double, 2, 3> eBpp2;
	Eigen::Matrix<double, 3, 2> eNpp12;
	Eigen::MatrixXd eBpp2wv(2 * pxs, 3);
	Eigen::MatrixXd eNppv;

	std::vector<IMAPNR>::iterator pxvi;
	std::vector<double> corrv(pxs2);
	std::vector<bool> delf(pxs,false);
	float wsum = 0.f;

	if (wxyflag)
		eNppv = Eigen::MatrixXd::Constant(nrthreads * 3, 3, -1.);

	for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js){
		const int j = pxvi->image;
		const int pxx = pxvi->pointnr;
		wxi[js] = wxio[js] = wx[j][pxx];
		wyi[js] = wyio[js] = wy[j][pxx];
		wxyi[js] = wxyio[js] = wxy[j][pxx];

		wsum += wxi[js] + wyi[js];
	} // for pxvi

	CalcCorrPointMulti(cameraps, X0, X1, X2, pointXXvio[pi],corrv,PMatrices,xg,
			wxi,wyi,wxyi,wxio,wyio,wxyio,sig0opt,sig0v,p1v,p2v,p3v);

	if (weightonlyflag) {
		wsum /= (float) pxs2;
		sig0io += sig0opt / wsum;
		redundancy += pxs2 - 3;
	}

	while((iflag > -1) && (iter < 101)){ // 101

		eNpp.setZero();
		if (wxyflag)
			eNppv.setZero();
		for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js) {
			const int j = pxvi->image;

			const double x = p1v[js];
			const double y = p2v[js];

			const double P3 = p3v[js];
			const double P1 = p1v[js] * p3v[js];
			const double P2 = p2v[js] * p3v[js];

			eBpp2(0,0) = PMatrices[j](0,0) * P3 - PMatrices[j](2,0) * P1;
			eBpp2(0,1) = PMatrices[j](0,1) * P3 - PMatrices[j](2,1) * P1;
			eBpp2(0,2) = PMatrices[j](0,2) * P3 - PMatrices[j](2,2) * P1;
			eBpp2(1,0) = PMatrices[j](1,0) * P3 - PMatrices[j](2,0) * P2;
			eBpp2(1,1) = PMatrices[j](1,1) * P3 - PMatrices[j](2,1) * P2;
			eBpp2(1,2) = PMatrices[j](1,2) * P3 - PMatrices[j](2,2) * P2;

			CalcCalibratedDerivatives(eBpp2(0,0), eBpp2(1,0), eBpp2(0,1), eBpp2(1,1), eBpp2(0,2), eBpp2(1,2),
					x, y, cameraps[j].distpara[0], cameraps[j].distpara[1],
					cameraps[j].K(0,0), cameraps[j].K(1,1), cameraps[j].K(0,1));

			const int js2 = 2 * js;
			const int js21 = js2 + 1;
			eBpp2wv(js2,0) = eBpp2(0,0) * wxi[js] + eBpp2(1,0) * wxyi[js];
			eBpp2wv(js21,0) = eBpp2(1,0) * wyi[js] + eBpp2(0,0) * wxyi[js];
			eBpp2wv(js2,1) = eBpp2(0,1) * wxi[js] + eBpp2(1,1) * wxyi[js];
			eBpp2wv(js21,1) = eBpp2(1,1) * wyi[js] + eBpp2(0,1) * wxyi[js];
			eBpp2wv(js2,2) = eBpp2(0,2) * wxi[js] + eBpp2(1,2) * wxyi[js];
			eBpp2wv(js21,2) = eBpp2(1,2) * wyi[js] + eBpp2(0,2) * wxyi[js];

			eNpp.noalias() += eBpp2.transpose() * eBpp2wv.block<2,3>(js2,0);

			if (wxyflag) {
				if (nrthreads == 1)
					continue;
				else { // if (nn0flag) {
					const int ij = imgindbv[j];
					for (int i = 0; i < nrthreads; ++i)
						if (pitflags[i] && i != ij) {
							eNppv.block<3,3>(3 * i,0).noalias() += eBpp2.transpose() * eBpp2wv.block<2,3>(js * 2,0);
						}
				} // else
			}

		}

		if (wxyflag) {
			if (nrthreads == 1) {
				eNppv = eNpp;
			}
		}

		const double m1 = 1.001;

		eNpp(0,0) *= m1;
		eNpp(1,1) *= m1;
		eNpp(2,2) *= m1;

		eNppi = eNpp.inverse();

		exip.setZero();
		for (int i = 0; i < pxs; ++i) {
			ecorr2(0) = corrv[i * 2];
			ecorr2(1) = corrv[i * 2 + 1];

			exip.noalias() += eNppi * eBpp2wv.block<2,3>(i * 2,0).transpose() * ecorr2;
		}

		X0 = Xold0 + (float) exip(0);
		X1 = Xold1 + (float) exip(1);
		X2 = Xold2 + (float) exip(2);

		CalcCorrPointMulti(cameraps, X0, X1, X2, pointXXvio[pi],corrv,PMatrices,xg,
				wxi,wyi,wxyi,wxio,wyio,wxyio,sig0,sig0v,p1v,p2v,p3v);

		if (iflag == 1 && robustmode == 2) {
			for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js) {
				if (sig0v[js] * sig0wv[pxvi->image] > 16.f) {
					wxi[js] *= 1.e-4f;
					wyi[js] *= 1.e-4f;
					wxyi[js] *= 1.e-4f;
					if (wxi[js] + wyi[js] < 2.e-8f) {
						wxi[js] = wyi[js] = 1.e-8f;
						wxyi[js] = 0.f;
					}
					--pxsd;
					delf[js] = true;
				} // if sig0v
			} // for pxvi
			if (pxsd < 2)
				break;
		} // if robustflag

		if (sig0 < sig0opt * 1.001) {
			if (sig0opt < sig0 * 1.05f) // 1.01f
				--iflag;
			if (!weightonlyflag) {
				XX(0,pi) = Xold0 = X0;
				XX(1,pi) = Xold1 = X1;
				XX(2,pi) = Xold2 = X2;
			}
			sig0opt = sig0;
			convflag = true;
			if (wxyflag) {
				for (int i = 0; i < nrthreads; ++i) {
					const int i3 = 3 * i;
					const int i31 = i3 + 1;
					wXXv[i] = (float) eNppv(i3,0);
					wYYv[i] = (float) eNppv(i31,1);
					wZZv[i] = (float) eNppv(i3 + 2,2);
					wXYv[i] = (float) eNppv(i3,1);
					wXZv[i] = (float) eNppv(i3,2);
					wYZv[i] = (float) eNppv(i31,2);
				}
			}
		} //if sig0
		else {
			--iflag;
		}

		if (weightonlyflag)
			break;

		++iter;
	} //while iflag

	if (robustmode == 1) {
		for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js)
			sig0vv[pxvi->image].push_back(sig0v[js]);
	} // if robustmode

	if (robustmode == 2) {
		std::vector<int> delv;
		for (int i = 0; i < pxs; ++i)
			if (delf[i])
				delv.push_back(i);
		if ((int) delv.size() > 0) {
			pxs2 -= (int) delv.size() * 2;
			for (int i = (int)(delv.size()) - 1; i > -1; --i) {
				pointXXvio[pi].erase(pointXXvio[pi].begin() + delv[i]);
			}
		}
	} // if robustmode

	if (pointXXvio[pi].size() < 2 || !convflag) {
		pointXXvio[pi].clear();
		convflag = false;
		XX(0,pi) = X0in;
		XX(1,pi) = X1in;
		XX(2,pi) = X2in;
	}
	else {
		if (!weightonlyflag) {
			wsum /= (float) pxs2;
			sig0io += sig0opt / wsum;
			redundancy += pxs2 - 3;
		}
	}

	return(convflag);
} // bool CalcLSAXMulti

// Computation of corrections for bundle adjustment for points only for multi image case
void CalcCorrPointMulti(const std::vector<Camera> &cameraps,
		const double X0, const double X1, const double X2,
		std::vector<IMAPNR> &pxv,
		std::vector<double> &corrv,
		const std::vector<PMatd> &PM,
		const std::vector<Eigen::MatrixXf> &xg,
		const std::vector<float> &wx, const std::vector<float> &wy, const std::vector<float> &wxy,
		const std::vector<float> &wxo, const std::vector<float> &wyo, const std::vector<float> &wxyo,
		float &sig0, std::vector<float> &sig0v,
		std::vector<float> &p1v, std::vector<float> &p2v, std::vector<float> &p3v){

	int js;

	sig0 = 0;
	std::vector<IMAPNR>::iterator pxvi;
	for(pxvi = pxv.begin(), js = 0; pxvi != pxv.end(); ++pxvi, ++js){
		const int j = pxvi->image;
		const int pxx = pxvi->pointnr;
		const int js2 = js * 2;

//		if (fabs(PM[j](0,3)) > 1.e5 || fabs(PM[j](1,3)) > 1.e5 || fabs(PM[j](2,3)) > 1.e5)
//			continue;

		double p1 = PM[j](0,0) * X0 + PM[j](0,1) * X1 + PM[j](0,2) * X2 + PM[j](0,3);
		double p2 = PM[j](1,0) * X0 + PM[j](1,1) * X1 + PM[j](1,2) * X2 + PM[j](1,3);
		double p3 = PM[j](2,0) * X0 + PM[j](2,1) * X1 + PM[j](2,2) * X2 + PM[j](2,3);

		if (fabs(p3) < 1.e-3) {
			if (p3 > 0.)
				p3 = 1.e3;
			else
				p3 = -1.e3;
		}
		else
			p3 = 1. / p3;

		p1 *= p3;
		p2 *= p3;

		p1v[js] = (float) p1;
		p2v[js] = (float) p2;
		p3v[js] = (float) p3;

		RadialReconstructionImage(p1, p2, cameraps[j]);

		p1 *= cameraps[j].K(0,0);
		p2 *= cameraps[j].K(0,0);

		const double dx = xg[j](0,pxx) - p1;
		const double dy = xg[j](1,pxx) - p2;

		corrv[js2] = dx;
		corrv[js2 + 1] = dy;
		const float sig0i = (float) (dx * dx * wx[js] + dy * dy * wy[js] + 2 * dx * dy * wxy[js]);
		sig0 += sig0i;
		sig0v[js] = (float) (dx * dx * wxo[js] + dy * dy * wyo[js] + 2 * dx * dy * wxyo[js]);
	}

	return;
} //void CalcCorrPointMulti

void CalcCalibratedDerivatives(double &xp0, double &yp0, double &xp1, double &yp1, double &xp2, double &yp2,
		const double x, const double y, const double k2, const double k4,
		const double fx, const double fy, const double s) {

	const double xpo0 = xp0;
	const double ypo0 = yp0;
	const double xpo1 = xp1;
	const double ypo1 = yp1;
	const double xpo2 = xp2;
	const double ypo2 = yp2;

	const double xy2 = x * x + y * y;
	const double xy4 = xy2 * xy2;

	const double xxpc = k4 * xy4 + x * (2 * k2 * x + 4 * k4 * x * xy2) + k2 * xy2 + 1.;
	const double xypc = x * (4 * k4 * xy2 * y + 2 * k2 * y);
	const double yxpc = y * (4 * k4 * x * xy2 + 2 * k2 * x);
	const double yypc = y * (2 * k2 * y + 4 * k4 * xy2 * y) + k4 * xy4 + k2 * xy2 + 1.;

	xp0 = (xxpc * xpo0 + xypc * ypo0);
	yp0 = (yypc * ypo0 + yxpc * xpo0);
	xp0 = xp0 * fx + yp0 * s;
	yp0 *= fy;

	xp1 = (xxpc * xpo1 + xypc * ypo1);
	yp1 = (yypc * ypo1 + yxpc * xpo1);
	xp1 = xp1 * fx + yp1 * s;
	yp1 *= fy;

	xp2 = (xxpc * xpo2 + xypc * ypo2);
	yp2 = (yypc * ypo2 + yxpc * xpo2);
	xp2 = xp2 * fx + yp2 * s;
	yp2 *= fy;

	return;
} // CalcCalibratedDerivatives

// Evaluation of bundle adjustment for calibrated images
void EvalLSA(const int nraddp, const std::vector<Camera> &cameraps,
		const Eigen::MatrixXd &XXd,
		const std::vector<Eigen::MatrixXf> &xg,
		const std::vector<std::vector<float> > &wxi, const std::vector<std::vector<float> > &wyi,
		const std::vector<std::vector<float> > &wxyi,
		std::vector<std::vector<IMAPNR> > &pointXXv,
		const std::vector<PMatd> &PMatricesd,
		const float ihw2) {

	long redundancy = 0;
	long nrobs = 0;
	float sig0g = 0.f;
	int ii,jj;
	const int nrimages =  (int) PMatricesd.size();
	std::vector<std::vector<IMAPNR> >::iterator pxv;
	std::vector<IMAPNR>::iterator pxvi;
	std::vector<float> p1v,p2v,p3v;

	for (pxv = pointXXv.begin(), ii = 0; pxv != pointXXv.end(); ++pxv, ++ii) {
		const int pxss = (int) pxv->size();
		const int pxss2 = pxss * 2;

		p1v.resize(pxss);
		p2v.resize(pxss);
		p3v.resize(pxss);

		if (pxss > 1) {
			std::vector<float> wxii(pxss),wyii(pxss),wxyii(pxss),sig0v(pxss);
			std::vector<double> corrv(pxss2);
			float wsum = 0.f;
			float sig0optf;
			int js,pxx;
			for(pxvi = pxv->begin(), js = 0; pxvi != pxv->end(); ++pxvi, ++js) {
				jj = pxvi->image;
				pxx = pxvi->pointnr;
				wxii[js] = wxi[jj][pxx];
				wyii[js] = wyi[jj][pxx];
				wxyii[js] = wxyi[jj][pxx];

				wsum += wxii[js] + wyii[js];
			} // for pxvi

			wsum = wsum / (float) pxss2;

			std::vector<bool> pitflags(1);
			std::vector<int> imgindbv;
			CalcCorrPointMulti(cameraps, XXd(0,ii), XXd(1,ii), XXd(2,ii), (*pxv),corrv,PMatricesd,xg,
					wxii,wyii,wxyii,wxii,wyii,wxyii,sig0optf,sig0v,p1v,p2v,p3v);
			sig0g += sig0optf / wsum;
			redundancy += pxss2 - 3;
			nrobs += pxss2;
		} // if pxss
	} // for pxv

	sig0g *= 1.f / (float) (redundancy - (nrimages - 2) * 6 - 5 - nrimages * nraddp);

	std::cout << "ITTTT: " << sqrtf(sig0g * ihw2) << ' ' << nrobs << " " << redundancy - (nrimages - 2) * 6 - 5 - nrimages * nraddp << '\n';

	return;
} // EvalLSA

void PartitionpointXXv(std::vector<std::vector<IMAPNR> > &pointXXv, const int nrimages,
		const int nrthreads, std::vector<idx_t> &part,
		std::vector<std::vector<int> > &imagesperpart)
{
  const int minpointnr = 0; // minimum number of points used for the graph

  const System::Time startp = System::getTickCount();

  std::vector<std::vector<int> > AM(nrimages);
  std::vector<int> BM(nrimages,0);
  for (int i = 0; i < nrimages; ++i) {
	  AM[i].resize(nrimages);
	  for (int j = 0; j < nrimages; ++j)
	  	  AM[i][j] = 0;
	  } // for i

  std::vector<std::vector<IMAPNR> >::iterator pxv;
  std::vector<IMAPNR>::iterator pxvi,pxvj;

  for (pxv = pointXXv.begin(); pxv != pointXXv.end(); ++pxv) {
	  if ((int) pxv->size() < 2)
		  continue;
	  for (pxvi = pxv->begin(); pxvi != pxv->end(); ++pxvi) {
		  const int fi = pxvi->image;
		  BM[fi] += (int) pxv->size();
		  for (pxvj = pxvi + 1; pxvj != pxv->end(); ++pxvj) {
			  const int si = pxvj->image;
			  ++AM[fi][si];
			  ++AM[si][fi];
		  }
	  }
  }

  int nredges = 0;
  for (int i = 0; i < nrimages; ++i) {
	  for (int j = 0; j < nrimages; ++j) {
		  if (AM[i][j] > minpointnr)
			  ++nredges;
	  }
  }

  std::vector<idx_t> xadj(nrimages + 1), adjncy(nredges), vwgt(nrimages);
  xadj[0] = 0;

  int indexadj = -1;
  for (int i = 0; i < nrimages; ++i) {

    int nrconn = 0;
    for (int j = 0; j < nrimages; ++j)
    	if (AM[i][j] > minpointnr) {
    		adjncy[++indexadj] = j;
    		++nrconn;
    	}

    xadj[i+1] = xadj[i] + nrconn;
    vwgt[i] = (idx_t) pow(BM[i],0.3333);
  } // for

  std::cout << "Preparation time: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n";

  idx_t nrparts = nrthreads;
  idx_t inrimages = nrimages;
  idx_t edgecut;

  idx_t options[METIS_NOPTIONS];
  METIS_SetDefaultOptions(options);
  options[METIS_OPTION_CONTIG] = 1;

  // One constraint
  idx_t ncon = 1;

  METIS_PartGraphRecursive(&inrimages, &ncon, &xadj[0], &adjncy[0],
		  &vwgt[0], nullptr, nullptr, &nrparts, nullptr,
		  nullptr, &options[0], &edgecut, &part[0]);

  std::cout << "After partitioning: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n\n";

  for (int i = 0; i < nrimages; ++i)
	  imagesperpart[part[i]].push_back(i);

  std::cout << "Images per partition: ";
  for (int it = 0; it < nrthreads; ++it)
	  std::cout << imagesperpart[it].size() << ' ';
  std::cout << '\n';

  std::cout << "After analysis of parts: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n";

  return;
} // PartitionpointXXv


/**
 * vi:ts=4:shiftwidth=4:expandtab
 * vim600:fdm=marker
 *
 * maxentmodel.hpp  -  A Conditional Maximun Entropy Model
 *
 * Copyright (C) 2003 by Zhang Le <ejoy@users.sourceforge.net>
 * Begin       : 01-Jan-2003
 * Last Change : 08-Feb-2012.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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 Lesser General Public
 * License along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#ifndef MAXENTMODEL_H
#define MAXENTMODEL_H

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <vector>
#include <utility>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/shared_array.hpp>
#include <ostream>
#include <iostream>

#include "itemmap.hpp"
#include "meevent.hpp"

namespace boost {
    class timer;
}

/**
 * All classes and functions are placed in the namespace maxent.
 */
namespace maxent {
using namespace std;
using boost::shared_ptr;
using boost::shared_array;

extern int verbose;  // set this to 0 if you do not want verbose output

struct maxent_pickle_suite;
/**
 * This class implements a conditional Maximun Entropy Model.
 *
 * A conditional Maximun Entropy Model (also called log-linear model)has the
 * form:
 * \f$p(y|x)=\frac{1}{Z(x)} \exp \left[\sum_{i=1}^k\lambda_if_i(x,y) \right]\f$
 * Where x is a context and y is the outcome tag and p(y|x) is the conditional
 * probability.
 *
 * Normally the context x is composed of a set of contextual predicates.
 */
class MaxentModel /*: TODO: we need copyable? boost::noncopyable*/  {
    friend struct maxent_pickle_suite;

    // private:
    // virtual ~MaxentModel();

    public:
//    typedef std::string feature_type;
//    typedef std::string outcome_type;
    typedef me::feature_type feature_type;
    typedef me::feature_type outcome_type;
    typedef std::vector<pair<feature_type, float> > context_type;

    MaxentModel();

    void load(const string& model);

    void save(const string& model, bool binary = false) const;

    double eval(const context_type& context, const outcome_type& outcome) const;

    void eval_all(const context_type& context,
            std::vector<pair<outcome_type, double> >& outcomes,
            bool sort_result = true) const;

    outcome_type predict(const context_type& context) const;

    void begin_add_event();

    void add_event(const context_type& context,
            const outcome_type& outcome,
            size_t count = 1);

    void add_heldout_event(const context_type& context,
            const outcome_type& outcome,
            size_t count = 1);

    // wrapper functions for binary feature cases, provided for conviences
    void add_event(const vector<string>& context,
            const outcome_type& outcome,
            size_t count = 1);

    void add_heldout_event(const vector<string>& context,
            const outcome_type& outcome,
            size_t count = 1);

    double eval(const vector<string>& context,
            const outcome_type& outcome) const;

    void eval_all(const vector<string>& context,
            std::vector<pair<outcome_type, double> >& outcomes,
            bool sort_result = true) const;

    outcome_type predict(const vector<string>& context) const;

    /**
     * Add a set of events indicated by range [begin, end).
     * the value type of Iterator must be pair<context_type, outcome_type>
     */
    template <typename Iterator>
        void add_events(Iterator begin, Iterator end) {
            for (Iterator it = begin; it != end; ++it)
                this->add_event(it->first, it->second);
        }

    void end_add_event(size_t cutoff = 1);

    void train(size_t iter = 15, const std::string& method = "lbfgs",
            double sigma2 = 0.0, // non-zero enables Gaussian prior smoothing (global variance sigma^2)
            double tol = 1E-05);

     void dump_events(const string& model, bool binary = false) const;

    const char* __str__() const; // python __str__()

    // Python binding related functions {{{
#if defined(PYTHON_MODULE)

    // return the whole probabistic distribution [(outcome1, prob1),
    // (outcome2, prob2), ...] for a given context
    std::vector<pair<outcome_type, double> > py_eval(const context_type& context) const {
        static std::vector<pair<outcome_type, double> > outcomes;
        eval_all(context, outcomes);
        return outcomes;
    }
#endif
    // end py binding }}}

    private:
    double build_params(shared_ptr<me::ParamsType>& params,
            size_t& n_theta) const;
    double build_params2(shared_ptr<me::ParamsType>& params,
            size_t& n_theta) const;

            struct featid_hasher {
                size_t operator()(const pair<size_t, size_t>& p) const {
                    return p.first + p.second;
                }
            };

    struct cutoffed_event {
        cutoffed_event(size_t cutoff):m_cutoff(cutoff) {}
        bool operator()(const me::Event& ev) const {
            return ev.m_count < m_cutoff;
        }
        size_t m_cutoff;
    };

    struct cmp_outcome {
        bool operator()(const pair<outcome_type, double>& lhs,
                const pair<outcome_type, double>& rhs) const {
            return lhs.second > rhs.second;
        }
    };

    size_t m_n_theta;
    shared_ptr<me::MEEventSpace> m_es;
    shared_ptr<me::MEEventSpace> m_heldout_es;
    shared_ptr<me::PredMapType> m_pred_map;
    shared_ptr<me::OutcomeMapType> m_outcome_map;
    shared_ptr<me::ParamsType> m_params;
    shared_array<double> m_theta; // feature weights

    shared_ptr<boost::timer> m_timer;

    struct param_hasher {
        size_t operator()(const pair<size_t,size_t>& v) const {
            return size_t(~(v.first<< 1) + v.second);
        }
    };
};

#if defined(OLD_PYTHON_MODULE) //{{{ old python pickle support through Boost.Python
struct maxent_pickle_suite : boost::python::pickle_suite {
    static boost::python::tuple getstate(const MaxentModel& m)
    {
        if (!m.m_params)
            throw runtime_error("can not get state from empty model");
        using namespace boost::python;
        boost::python::list state;
        size_t i;

        shared_ptr<me::PredMapType> pred_map = m.m_pred_map;
        shared_ptr<me::OutcomeMapType> outcome_map = m.m_outcome_map;
        shared_ptr<me::ParamsType> params = m.m_params;
        size_t n_theta = m.m_n_theta;
        shared_array<double> theta = m.m_theta;

        // save pred_map
        state.append(pred_map->size());
        for (i = 0;i < pred_map->size(); ++i)
            state.append((*pred_map)[i]);

        // save outcome_map
        state.append(outcome_map->size());
        for (i = 0;i < outcome_map->size(); ++i)
            state.append((*outcome_map)[i]);

        // save params
        state.append(n_theta);
        assert(params->size() == pred_map->size());
        for (i = 0;i < params->size(); ++i) {
            boost::python::list oids;
            boost::python::list t;
            const std::vector<pair<size_t, size_t> >& a = (*params)[i];
            for (size_t j = 0; j < a.size(); ++j) {
                oids.append(a[j].first);
                t.append(a[j].second);
            }
            state.append(make_tuple(oids, t));
        }
        // save theta
        for (i = 0;i < n_theta; ++i)
            state.append(theta[i]);
        return boost::python::tuple(state);
    }

    static void setstate(MaxentModel& m, boost::python::tuple state)
    {
        using namespace boost::python;
        assert (!m.m_pred_map);
        assert (!m.m_outcome_map);
        assert (!m.m_params);
        assert (len(state) > 0);

        shared_ptr<me::PredMapType> pred_map(new me::PredMapType);
        shared_ptr<me::OutcomeMapType> outcome_map(new me::OutcomeMapType);
        shared_ptr<me::ParamsType> params(new me::ParamsType);
        size_t n_theta;
        shared_array<double> theta;

        size_t count;
        size_t i;
        size_t index = 0;

        // load pred_map
        count = extract<size_t>(state[index++]);
        for (i = 0; i < count; ++i)
            pred_map->add(extract<std::string>(state[index++]));

        // load outcome_map
        count = extract<size_t>(state[index++]);
        for (i = 0; i < count; ++i)
            outcome_map->add(extract<std::string>(state[index++]));

        // load params
        n_theta = extract<size_t>(state[index++]);
        for (i = 0; i < pred_map->size(); ++i) {
            tuple tmp(state[index++]);
            boost::python::list oids(tmp[0]);
            boost::python::list t(tmp[1]);
            std::vector<pair<size_t, size_t> > a;

            size_t k = extract<size_t>(oids.attr("__len__")());
            assert (k == len(t));

            for (size_t j = 0; j < k; ++j) {
                size_t oid = extract<size_t>(oids[j]);
                size_t fid = extract<size_t>(t[j]);
                a.push_back(std::make_pair(oid, fid));
            }
            params->push_back(a);
        }
        // extract theta
        theta.reset(new double[n_theta]);
        for (i = 0;i < n_theta; ++i)
            theta[i] = extract<double>(state[index++]);
        m.m_pred_map = pred_map;
        m.m_outcome_map = outcome_map;
        m.m_params = params;
        m.m_n_theta = n_theta;
        m.m_theta = theta;
    }
};
#endif // PYTHON_MODULE }}}

} // namespace maxent
#endif /* ifndef MAXENTMODEL_H */


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_ALLBITS_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_ALLBITS_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/meta/int_c.hpp>
#include <boost/simd/meta/float.hpp>
#include <boost/simd/meta/double.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Allbits generic tag

     Represents the Allbits constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct Allbits : ext::pure_constant_<Allbits>
    {
      typedef double default_type;
      typedef ext::pure_constant_<Allbits> parent;

      /// INTERNAL ONLY
      template<class Target, class Dummy=void>
      struct  apply
            : meta::int_c<typename Target::type, -1>
      {};
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_Allbits( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Allbits::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_ < apply < boost::dispatch::meta::uint32_<uint32_t>
                                  , Dummy
                                  >::value
                          > {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Allbits::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_ < apply < boost::dispatch::meta::uint64_<uint64_t>
                                  , Dummy
                                  >::value
                          > {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Allbits::apply<boost::dispatch::meta::uint8_<T>,Dummy>
          : meta::int_c<T, 0xFF> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Allbits::apply<boost::dispatch::meta::uint16_<T>,Dummy>
          : meta::int_c<T, 0xFFFFU> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Allbits::apply<boost::dispatch::meta::uint32_<T>,Dummy>
          : meta::int_c<T, 0xFFFFFFFFUL> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Allbits::apply<boost::dispatch::meta::uint64_<T>,Dummy>
          : meta::int_c<T, 0xFFFFFFFFFFFFFFFFULL> {};

  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Allbits, Site> dispatching_Allbits(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Allbits, Site>();
   }
   template<class... Args>
   struct impl_Allbits;
  }
  /*!
    Generates a value in the chosen type all bits of which are set to 1.

    @par Semantic:

    @code
    T r = Allbits<T>();
    @endcode

    is similar to

    @code
    if T is floating point
      r =  Nan<T>();
    else if T is signed integral
      r = -1;
    else
      r = Valmax<T>();
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Allbits, Allbits)
} }

#include <boost/simd/constant/common.hpp>

#endif


#include "cryptography.hh"

#include <boost/type_index.hpp>

#include <elle/cryptography/random.hh>

// On some platforms "random" exists:
//
// /usr/include/stdlib.h:321:17: note: previous declaration 'long int random()'
// extern long int random (void) __THROW;
//                 ^
namespace icrand = elle::cryptography::random;

/*--------.
| Operate |
`--------*/

static
void
test_operate_boolean()
{
  icrand::generate<bool>();
}

static
void
test_operate_real()
{
  double value1 = icrand::generate<double>();
  double value2 = icrand::generate<double>();
  // With very high probability.
  BOOST_CHECK_NE(value1, value2);
}

template <typename T>
void
test_operate_x(T min, T max)
{
  BOOST_TEST_MESSAGE("Testing T = "
                     << boost::typeindex::type_id<T>().pretty_name());
  {
    auto min = std::numeric_limits<T>::min();
    auto max = std::numeric_limits<T>::max();
    auto val = icrand::generate<T>();
    BOOST_TEST_MESSAGE("min = " << +min);
    BOOST_TEST_MESSAGE("val = " << +val);
    BOOST_TEST_MESSAGE("max = " << +max);
    BOOST_CHECK_LE(std::numeric_limits<T>::min(), val);
    BOOST_CHECK_LE(val, std::numeric_limits<T>::max());

  }
  {
    T val = icrand::generate<T>(min, max);
    BOOST_TEST_MESSAGE("min = " << +min);
    BOOST_TEST_MESSAGE("val = " << +val);
    BOOST_TEST_MESSAGE("max = " << +max);
    BOOST_CHECK_LE(min, val);
    BOOST_CHECK_LE(val, max);
  }
}

static
void
test_operate_string()
{
  std::string value1 = icrand::generate<std::string>(262);
  std::string value2 = icrand::generate<std::string>(262);
  // With very high probability.
  std::cerr << std::hex;
  BOOST_CHECK_NE(elle::ConstWeakBuffer(value1), elle::ConstWeakBuffer(value2));
}

static
void
test_operate_buffer()
{
  elle::Buffer value1 = icrand::generate<elle::Buffer>(262);
  elle::Buffer value2 = icrand::generate<elle::Buffer>(262);
  // With very high probability.
  std::cerr << std::hex;
  BOOST_CHECK_NE(value1, value2);
}

static
void
test_operate()
{
  test_operate_boolean();
  test_operate_x<char>(15, 48);
  test_operate_x<int8_t>(-60, -58);
  test_operate_x<int16_t>(-21000, 21000);
  test_operate_x<int32_t>(-848, 73435);
  test_operate_x<int64_t>(-324923, 32212394);
  test_operate_x<uint8_t>(75, 126);
  test_operate_x<uint16_t>(1238, 53104);
  test_operate_x<uint32_t>(424242, 424242);
  test_operate_x<uint64_t>(23409, 1209242094821);
  test_operate_real();
  test_operate_string();
  test_operate_buffer();
}

/*-----.
| Main |
`-----*/

ELLE_TEST_SUITE()
{
  boost::unit_test::test_suite* suite = BOOST_TEST_SUITE("random");
  suite->add(BOOST_TEST_CASE(test_operate));
  boost::unit_test::framework::master_test_suite().add(suite);
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MP_GENERIC_INTERCONVERT_HPP
#define BOOST_MP_GENERIC_INTERCONVERT_HPP

#include <boost/multiprecision/detail/default_ops.hpp>

namespace boost{ namespace multiprecision{ namespace detail{

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_floating_point>& /*to_type*/, const mpl::int_<number_kind_integer>& /*from_type*/)
{
   using default_ops::eval_get_sign;
   using default_ops::eval_bitwise_and;
   using default_ops::eval_convert_to;
   using default_ops::eval_right_shift;
   using default_ops::eval_ldexp;
   using default_ops::eval_add;
   // smallest unsigned type handled natively by "From" is likely to be it's limb_type:
   typedef typename canonical<unsigned char, From>::type   limb_type;
   // get the corresponding type that we can assign to "To":
   typedef typename canonical<limb_type, To>::type         to_type;
   From t(from);
   bool is_neg = eval_get_sign(t) < 0;
   if(is_neg)
      t.negate();
   // Pick off the first limb:
   limb_type limb;
   limb_type mask = ~static_cast<limb_type>(0);
   From fl;
   eval_bitwise_and(fl, t, mask);
   eval_convert_to(&limb, fl);
   to = static_cast<to_type>(limb);
   eval_right_shift(t, std::numeric_limits<limb_type>::digits);
   //
   // Then keep picking off more limbs until "t" is zero:
   //
   To l;
   unsigned shift = std::numeric_limits<limb_type>::digits;
   while(!eval_is_zero(t))
   {
      eval_bitwise_and(fl, t, mask);
      eval_convert_to(&limb, fl);
      l = static_cast<to_type>(limb);
      eval_right_shift(t, std::numeric_limits<limb_type>::digits);
      eval_ldexp(l, l, shift);
      eval_add(to, l);
      shift += std::numeric_limits<limb_type>::digits;
   }
   //
   // Finish off by setting the sign:
   //
   if(is_neg)
      to.negate();
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_integer>& /*to_type*/, const mpl::int_<number_kind_integer>& /*from_type*/)
{
   using default_ops::eval_get_sign;
   using default_ops::eval_bitwise_and;
   using default_ops::eval_convert_to;
   using default_ops::eval_right_shift;
   using default_ops::eval_left_shift;
   using default_ops::eval_bitwise_or;
   using default_ops::eval_is_zero;
   // smallest unsigned type handled natively by "From" is likely to be it's limb_type:
   typedef typename canonical<unsigned char, From>::type   limb_type;
   // get the corresponding type that we can assign to "To":
   typedef typename canonical<limb_type, To>::type         to_type;
   From t(from);
   bool is_neg = eval_get_sign(t) < 0;
   if(is_neg)
      t.negate();
   // Pick off the first limb:
   limb_type limb;
   limb_type mask = static_cast<limb_type>(~static_cast<limb_type>(0));
   From fl;
   eval_bitwise_and(fl, t, mask);
   eval_convert_to(&limb, fl);
   to = static_cast<to_type>(limb);
   eval_right_shift(t, std::numeric_limits<limb_type>::digits);
   //
   // Then keep picking off more limbs until "t" is zero:
   //
   To l;
   unsigned shift = std::numeric_limits<limb_type>::digits;
   while(!eval_is_zero(t))
   {
      eval_bitwise_and(fl, t, mask);
      eval_convert_to(&limb, fl);
      l = static_cast<to_type>(limb);
      eval_right_shift(t, std::numeric_limits<limb_type>::digits);
      eval_left_shift(l, shift);
      eval_bitwise_or(to, l);
      shift += std::numeric_limits<limb_type>::digits;
   }
   //
   // Finish off by setting the sign:
   //
   if(is_neg)
      to.negate();
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_floating_point>& /*to_type*/, const mpl::int_<number_kind_floating_point>& /*from_type*/)
{
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif
   //
   // The code here only works when the radix of "From" is 2, we could try shifting by other
   // radixes but it would complicate things.... use a string conversion when the radix is other
   // than 2:
   //
   if(std::numeric_limits<number<From> >::radix != 2)
   {
      to = from.str(0, std::ios_base::fmtflags()).c_str();
      return;
   }

   typedef typename canonical<unsigned char, To>::type ui_type;

   using default_ops::eval_fpclassify;
   using default_ops::eval_add;
   using default_ops::eval_subtract;
   using default_ops::eval_convert_to;

   //
   // First classify the input, then handle the special cases:
   //
   int c = eval_fpclassify(from);

   if(c == FP_ZERO)
   {
      to = ui_type(0);
      return;
   }
   else if(c == FP_NAN)
   {
      to = "nan";
      return;
   }
   else if(c == FP_INFINITE)
   {
      to = "inf";
      if(eval_get_sign(from) < 0)
         to.negate();
      return;
   }

   typename From::exponent_type e;
   From f, term;
   to = ui_type(0);

   eval_frexp(f, from, &e);

   static const int shift = std::numeric_limits<boost::intmax_t>::digits - 1;

   while(!eval_is_zero(f))
   {
      // extract int sized bits from f:
      eval_ldexp(f, f, shift);
      eval_floor(term, f);
      e -= shift;
      eval_ldexp(to, to, shift);
      typename boost::multiprecision::detail::canonical<boost::intmax_t, To>::type ll;
      eval_convert_to(&ll, term);
      eval_add(to, ll);
      eval_subtract(f, term);
   }
   typedef typename To::exponent_type to_exponent;
   if((e > (std::numeric_limits<to_exponent>::max)()) || (e < (std::numeric_limits<to_exponent>::min)()))
   {
      to = "inf";
      if(eval_get_sign(from) < 0)
         to.negate();
      return;
   }
   eval_ldexp(to, to, static_cast<to_exponent>(e));
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_rational>& /*to_type*/, const mpl::int_<number_kind_rational>& /*from_type*/)
{
   typedef typename component_type<number<From> >::type   from_component_type;
   typedef typename component_type<number<To> >::type     to_component_type;

   number<From> t(from);
   to_component_type n(numerator(t)), d(denominator(t));
   using default_ops::assign_components;
   assign_components(to, n.backend(), d.backend());
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_rational>& /*to_type*/, const mpl::int_<number_kind_integer>& /*from_type*/)
{
   typedef typename component_type<number<To> >::type     to_component_type;

   number<From> t(from);
   to_component_type n(t), d(1);
   using default_ops::assign_components;
   assign_components(to, n.backend(), d.backend());
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_floating_point>& /*to_type*/, const mpl::int_<number_kind_rational>& /*from_type*/)
{
   typedef typename component_type<number<From> >::type   from_component_type;
   using default_ops::eval_divide;

   number<From> t(from);
   from_component_type n(numerator(t)), d(denominator(t));
   number<To> fn(n), fd(d);
   eval_divide(to, fn.backend(), fd.backend());
}

}}} // namespaces

#endif  // BOOST_MP_GENERIC_INTERCONVERT_HPP


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_INTERFACE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_INTERFACE_HPP

#include <boost/concept_check.hpp>

#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>
#include <boost/geometry/algorithms/detail/closest_points/utilities.hpp>

#include <boost/geometry/algorithms/dispatch/closest_points.hpp>

#include <boost/geometry/algorithms/detail/distance/interface.hpp>

#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/geometries/adapted/boost_variant.hpp> // For backward compatibility
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/default_strategy.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/closest_points/services.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// If reversal is needed, perform it

template
<
    typename Geometry1,
    typename Geometry2,
    typename Tag1,
    typename Tag2
>
struct closest_points
<
    Geometry1, Geometry2,
    Tag1, Tag2, true
>
    : closest_points<Geometry2, Geometry1, Tag2, Tag1, false>
{
    template <typename Segment, typename Strategy>
    static inline void apply(Geometry1 const& g1, Geometry2 const& g2,
                             Segment& shortest_seg, Strategy const& strategy)
    {
        closest_points
            <
                Geometry2, Geometry1, Tag2, Tag1, false
            >::apply(g2, g1, shortest_seg, strategy);

        detail::closest_points::swap_segment_points::apply(shortest_seg);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy
{

template<typename Strategy>
struct closest_points
{
    template <typename Geometry1, typename Geometry2, typename Segment>
    static inline void apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             Segment& shortest_seg,
                             Strategy const& strategy)
    {
        dispatch::closest_points
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, shortest_seg, strategy);
    }
};

template <>
struct closest_points<default_strategy>
{
    template <typename Geometry1, typename Geometry2, typename Segment>
    static inline void
    apply(Geometry1 const& geometry1,
          Geometry2 const& geometry2,
          Segment& shortest_seg,
          default_strategy)
    {
        using strategy_type = typename strategies::closest_points::services::default_strategy
            <
                Geometry1, Geometry2
            >::type;

        dispatch::closest_points
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, shortest_seg, strategy_type());
    }
};

} // namespace resolve_strategy

namespace resolve_variant
{

template <typename Geometry1, typename Geometry2>
struct closest_points
{
    template <typename Segment, typename Strategy>
    static inline void apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             Segment& shortest_seg,
                             Strategy const& strategy)
    {
        resolve_strategy::closest_points
            <
                Strategy
            >::apply(geometry1, geometry2, shortest_seg, strategy);
    }
};

//TODO: Add support for DG/GC

} // namespace resolve_variant

/*!
\brief Calculate the closest points between two geometries \brief_strategy
\ingroup closest_points
\details
\details The free function closest_points calculates the distance between two geometries \brief_strategy. \details_strategy_reasons
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Segment Any type fulfilling a Segment Concept
\tparam Strategy \tparam_strategy{Closest Points}
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param shortest_seg Output segment containing the closest points
\param strategy \param_strategy{closest_points}
\note The strategy can be a point-point strategy. In case of distance point-line/point-polygon
    it may also be a point-segment strategy.
\qbk{distinguish,with strategy}
\qbk{
[heading Example]
[closest_points_strategy]
[closest_points_strategy_output]
[heading See also]
\* [link geometry.reference.algorithms.distance distance]
}
*/

template <typename Geometry1, typename Geometry2, typename Segment, typename Strategy>
inline void closest_points(Geometry1 const& geometry1,
                           Geometry2 const& geometry2,
                           Segment& shortest_seg,
                           Strategy const& strategy)
{
    concepts::check<Geometry1 const>();
    concepts::check<Geometry2 const>();

    detail::throw_on_empty_input(geometry1);
    detail::throw_on_empty_input(geometry2);

    resolve_variant::closest_points
        <
            Geometry1,
            Geometry2
        >::apply(geometry1, geometry2, shortest_seg, strategy);
}

/*!
\brief Compute the closest points between two geometries.
\ingroup closest_points
\details The free function closest_points calculates the closest points between two geometries. \details_default_strategy
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Segment Any type fulfilling a Segment Concept
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param shortest_seg Output segment containing the closest points
\qbk{
[heading Example]
[closest_points]
[closest_points_output]
[heading See also]
\* [link geometry.reference.algorithms.distance distance]
}
*/

template <typename Geometry1, typename Geometry2, typename Segment>
inline void closest_points(Geometry1 const& geometry1,
                           Geometry2 const& geometry2,
                           Segment& shortest_seg)
{
    closest_points(geometry1, geometry2, shortest_seg, default_strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_INTERFACE_HPP


#include <vectorizer/vectorizer.h>
#include <vectorizer/configuration.h>

#include <logger/logger.h>

#include <executor/os_proxy.h>
#include <executor/exceptions.h>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <interpreter/svg/document.h>
#include <interpreter/svg/location.h>

#include <boost/lexical_cast.hpp>

#include <fstream>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <queue>
#include <iostream>

template<>
struct std::hash<cv::Point2i>
{
	size_t operator()(const cv::Point2i& p) const
	{
		std::hash<int> parent;
		return (parent(p.x) + 0x9e3779b9) ^ (parent(p.y) << 7);
	}
};

namespace
{
	void dump_points(cv::Size imgSize, cv::vector<cv::vector<cv::Point2i> >& points, std::string path)
	{
		cv::Mat matContour(imgSize, CV_8UC1);
		for (size_t k = 0; k < points.size(); ++k)
		{
			const cv::vector<cv::Point2i>& contour = points[k];
			for (size_t k2 = 0; k2 < contour.size(); ++k2)
			{
				const cv::Point2i& p = contour[k2];
				matContour.at<uint8_t>(p) = 255;
			}
		}

		imwrite(path, matContour);
	}

	double dist(cv::Point2i a, cv::Point2i b)
	{
		double x = a.x - b.x;
		double y = a.y - b.y;

		return std::sqrt(x*x + y*y);
	}

	void seed_neighborhood(cv::Mat index, cv::Point2i center, int value, int radius)
	{
		int start_x = center.x - radius;
		int end_x = center.x + radius;

		int start_y = center.y - radius;
		int end_y = center.y + radius;

		if (start_x < 0)
			start_x = 0;
		if (start_y < 0)
			start_y = 0;
		if (end_x >= index.cols)
			end_x = index.cols - 1;
		if (end_y >= index.rows)
			end_y = index.rows - 1;

		for (int i = start_x; i <= end_x; ++i)
		{
			for (int j = start_y; j <= end_y; ++j)
				index.at<int>(cv::Point(i, j)) = value;
		}
	}

	bool edge_already_exists(const std::vector<std::pair<std::size_t, std::size_t> >& u, std::pair<std::size_t, std::size_t> e)
	{
		for (auto a : u)
		{
			if ((a.first == e.first && a.second == e.second) || (a.second == e.first && a.first == e.second))
				return true;
		}

		return false;
	}
}

namespace vectorizer
{
	class vectorizer::impl
	{
	public:
		impl(std::shared_ptr<configuration> conf) : config(conf)
		{}

		std::unique_ptr<output> vectorize(std::unique_ptr<input> in)
		{
			cv::Mat img = cv::imread(in->wide_lines_filename, CV_LOAD_IMAGE_ANYDEPTH);

			cv::threshold(img, img, 127, 255, cv::THRESH_BINARY);
			cv::bitwise_not(img, img);

			cv::vector<cv::vector<cv::Point2i> > chained_points;
			cv::findContours(img, chained_points, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
			std::cout << "Found: " << chained_points.size() << " contours" << std::endl;

			dump_points(img.size(), chained_points, config->workspace_path() + "contours.pgm");

			cv::vector<cv::vector<cv::Point2i> > vertices;
			for (std::size_t i = 0; i < chained_points.size(); ++i)
			{
				cv::vector<cv::Point2i> vertex_points;
				cv::approxPolyDP(chained_points[i], vertex_points, 15, true);
				vertices.push_back(vertex_points);
				std::cout << "From: " << chained_points[i].size() << " points detected: " << vertex_points.size() << "needed for contour" << std::endl;
			}

			std::vector<cv::Point2i> points;
			std::vector<std::pair<std::size_t, std::size_t> > edges;

			int START_SEED = 255;
			cv::Mat minimal(img.size(), CV_32SC1, cv::Scalar(START_SEED));
			int SEED_RADIUS = 30;
			for (std::size_t i = 0; i < vertices.size(); ++i)
			{
				if (vertices[i].size() == 2)
					continue;

				int index = minimal.at<int>(vertices[i][0]);
				if (index == START_SEED)
				{
					index = points.size();
					points.push_back(vertices[i][0]);
				}
				seed_neighborhood(minimal, vertices[i][0], index, SEED_RADIUS);

				for (std::size_t j = 1; j < vertices[i].size(); ++j)
				{
					int index2 = minimal.at<int>(vertices[i][j]);
					if (index2 == START_SEED)
					{
						index2 = points.size();
						points.push_back(vertices[i][j]);
					}

					seed_neighborhood(minimal, vertices[i][j], index2, SEED_RADIUS);

					if (index == index2)
						continue;

					auto e = std::make_pair(index, index2);
					if (!edge_already_exists(edges, e))
						edges.push_back(e);

					index = index2;
				}
			}
			cv::imwrite(config->workspace_path() + "minimal_points.pgm", minimal);

			std::cout << "Total: " << points.size() << " vertices" << std::endl;
			for (std::size_t i = 0; i < points.size(); ++i)
				std::cout << i << ".) (" << points[i].x << "," << points[i].y << ")" << std::endl;

			std::cout << "Total: " << edges.size() << " edges" << std::endl;
			for (std::size_t i = 0; i < edges.size(); ++i)
				std::cout << i << ".) (" << edges[i].first << "," << edges[i].second << ")" << std::endl;

			cv::Mat vectorized = cv::Mat::zeros(minimal.size(), CV_8UC1);
			for (std::size_t i = 0; i < edges.size(); ++i)
				cv::line(vectorized, points[edges[i].first], points[edges[i].second], 255);

			cv::imwrite(config->workspace_path() + "vectorized.pgm", vectorized);

			cv::Mat sss = cv::imread(in->wide_lines_filename, CV_LOAD_IMAGE_ANYDEPTH);
			std::unique_ptr<output> out(std::make_unique<output>());
			out->skeleton_image = sss;
			out->vertices = points;
			out->edges = edges;

			return out;
		}

		std::shared_ptr<configuration> config;
	};

	vectorizer::vectorizer(std::shared_ptr<configuration> config) : pimpl(std::make_unique<impl>(config))
	{}

	vectorizer::~vectorizer()
	{}

	std::unique_ptr<output> vectorizer::vectorize(std::unique_ptr<input> in)
	{
		return pimpl->vectorize(std::move(in));
	}
}


#ifndef INCLUDED_PPXTF_HPP
#define INCLUDED_PPXTF_HPP

#include <utility>
#include <vector>
#include <map>
#include <tuple>
#include <string>

// eigen
#include <Eigen/Core>

using p_t = std::pair<int, int>;
using r_t = std::vector<std::map<p_t, double> >;
using mat_t = Eigen::MatrixXd;
using vec_mat_t = std::vector<mat_t>;

std::tuple<mat_t, mat_t> sample_Lam_X_mu_X(const mat_t& X, int N, int nu0, const mat_t& W0, double beta0, double mu0, int K);
std::tuple<mat_t, mat_t> calc_XYXY_XYR(int K, const mat_t& B, const mat_t& C, const std::map<p_t, double>& rt1);
mat_t multivariate_normal(const mat_t& mu, const mat_t& Sigma);
void save_parameters(int K, int ItrNum, const std::string& prefix, const std::string& name, const vec_mat_t& A, const mat_t& mu_A, const mat_t& Lam_A, const std::string& X = "");
void seeding(unsigned seed = 0);
std::tuple<r_t, r_t, r_t> ReadTrainTransTensor(int N, int M, int ZeroNum, const std::string& infile);
std::tuple<r_t, r_t, r_t> ReadTrainVisitTensor(int N, int M, int T, int ZeroNum, const std::string& infile);
mat_t random_mat(int m, int n);
void read_tensor(r_t& r1, r_t& r2, r_t& r3, const std::string& infile);
std::vector<int> random_index(int n);

#endif // INCLUDED_PPXTF_HPP


#include <cmath>
#include <vector>
#include <string>
#include "save_pcd/common.h"
#include "save_pcd/tic_toc.h"
#include <opencv/cv.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sstream>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include<mutex>
#include<math.h>
#include <eigen3/Eigen/Dense>
#define PI 3.1415926
using std::atan2;
using std::cos;
using std::sin;
std::mutex mBuf;

std::vector< pcl::PointCloud<pcl::PointXYZ>::Ptr> Vec16;
std::vector< pcl::PointCloud<pcl::PointXYZ>::Ptr> Vec32;

std::string calibrationpcddir_ = " ";

void savetotalpcd()
{

    int key=0;
    static int index = 0;
    cv::Mat img=cv::Mat::zeros(100,100,CV_8U);
    cv::imshow("waitkey",img);
    key = cv::waitKey(1);
    std::cout<<key<<std::endl;

        if (Vec16.size() == 0 || Vec32.size() == 0)
        {
            return;
        }

    if(key=='s')
    {
        std::string lidarID = "0";
        pcl::PointCloud<pcl::PointXYZ> filtercloud_;
        pcl::PointCloud<pcl::PointXYZ>::Ptr pointclouds = Vec16[Vec16.size()-1];
        for(int i=0;i<pointclouds->points.size();i++)
        {
            double range = std::sqrt(pointclouds->points[i].x * pointclouds->points[i].x + pointclouds->points[i].y * pointclouds->points[i].y + pointclouds->points[i].z * pointclouds->points[i].z);
            if(range>0.5)
                filtercloud_.push_back(pointclouds->points[i]);
        }
    index++;
    std::stringstream sstr;
    sstr<<index<<"_"<<lidarID<<".pcd";
    std::string path = calibrationpcddir_ + sstr.str();
    pcl::io::savePCDFile(path, filtercloud_,false);

    lidarID = "1";
    pcl::PointCloud<pcl::PointXYZ> filtercloud_32;
    pointclouds = Vec32[Vec32.size()-1];
    for(int i=0;i<pointclouds->points.size();i++)
    {
        double range = std::sqrt(pointclouds->points[i].x * pointclouds->points[i].x + pointclouds->points[i].y * pointclouds->points[i].y + pointclouds->points[i].z * pointclouds->points[i].z);
        if(range>0.5)
            filtercloud_32.push_back(pointclouds->points[i]);
    }
    std::stringstream sstr32;
    sstr32<<index<<"_"<<lidarID<<".pcd";
    path = calibrationpcddir_ + sstr32.str();
    pcl::io::savePCDFile(path, filtercloud_32,false);
    }
}

void laserCloudHandler16(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{

    // std::cout<<"激光回调"<<std::endl;

    TicToc t_whole;
    TicToc t_prepare;

    pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

    Vec16.push_back(laserCloudIn);
    // savetotalpcd(laserCloudIn);
}

void laserCloudHandler32(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

    Vec32.push_back(laserCloudIn);
    // savetotalpcd(laserCloudIn);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "sphere");
    ros::NodeHandle nh;
    ros::param::get("~shot_save", calibrationpcddir_);
    // **************************************************************************************************************
    ros::Subscriber subLaserCloud16 = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_points_16", 1, laserCloudHandler16);
    ros::Subscriber subLaserCloud32 = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_points_32", 1, laserCloudHandler32);

    while(ros::ok())
    {
        ros::spinOnce();
        savetotalpcd();
    }

    return 0;
}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Bielefeld University
 *  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 Bielefeld University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <geometric_shapes/check_isometry.h>
#include <gtest/gtest.h>

TEST(Utils, checkIsometry)
{
  Eigen::Isometry3d t(Eigen::AngleAxisd(0.42, Eigen::Vector3d(1, 1, 1).normalized()));
  ASSERT_TRUE(::checkIsometry(t));

  const Eigen::Vector3d oldDiagonal = t.linear().diagonal();
  t.linear() = t.linear() * Eigen::DiagonalMatrix<double, 3, 3>(1.0, 2.0, 3.0);
  ASSERT_FALSE(::checkIsometry(t));

  t.matrix().row(3) = Eigen::Vector4d(1e-6, 1e-6, 1e-6, 1 - 1e-6);
  ASSERT_FALSE(::checkIsometry(t, CHECK_ISOMETRY_PRECISION));

  t.linear().diagonal() = oldDiagonal;
  ASSERT_TRUE(::checkIsometry(t, 1e-0, false));
  ASSERT_FALSE(::checkIsometry(t, 1e-1));
  ASSERT_FALSE(::checkIsometry(t, 1e-2, false));

  std::cerr.flush();
}

TEST(Utils, assertIsometry)
{
  Eigen::Isometry3d t(Eigen::AngleAxisd(0.42, Eigen::Vector3d(1, 1, 1).normalized()));
  ASSERT_ISOMETRY(t);

  t.linear() = t.linear() * Eigen::DiagonalMatrix<double, 3, 3>(1.0, 2.0, 3.0);
#ifdef NDEBUG
  ASSERT_ISOMETRY(t)  // noop in release mode
#else
  ASSERT_DEATH(ASSERT_ISOMETRY(t), "Invalid isometry transform");
#endif

  t.matrix().row(3) = Eigen::Vector4d(1e-6, 1e-6, 1e-6, 1 - 1e-6);
#ifdef NDEBUG
  ASSERT_ISOMETRY(t)  // noop in release mode
#else
  ASSERT_DEATH(ASSERT_ISOMETRY(t), "Invalid isometry transform");
#endif

  std::cerr.flush();
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <boost/tokenizer.hpp>
#include <stdexcept>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <utility>
#include <vector>
#include <string>
#include <cstdio>
#include <array>
#include <cmath>
#include <map>
#include "SimpleBayesianConsensusCaller.hpp"
#include "Coverage.hpp"
#include "ConsensusCaller.hpp"

using ChanZuckerberg::shasta::Consensus;
using Separator = boost::char_separator<char>;
using Tokenizer = boost::tokenizer<Separator>;
using std::runtime_error;
using std::ifstream;
using std::vector;
using std::string;
using std::printf;
using std::array;
using std::pair;
using std::make_pair;
using std::cout;
using std::pow;
using std::map;
using std::max;

using namespace ChanZuckerberg;
using namespace shasta;

// Helper function
void SimpleBayesianConsensusCaller::split_as_double(string s, char separator_char, vector<double>& tokens){
    Separator separator(&separator_char);
    Tokenizer tok{s, separator};

    for (string token: tok) {
        tokens.push_back(stod(token));
    }
}

// Helper function
void SimpleBayesianConsensusCaller::split_as_string(string s, char separator_char, vector<string>& tokens){
    Separator separator(&separator_char);
    Tokenizer tok{s, separator};

    for (string token: tok) {
        tokens.push_back(token);
    }
}

SimpleBayesianConsensusCaller::SimpleBayesianConsensusCaller(){
    max_runlength = 50;
    ignore_non_consensus_base_repeats = true;
    predict_gap_runlengths = false;
    count_gaps_as_zeros = false;

    const string fileName = "SimpleBayesianConsensusCaller.csv";
    ifstream matrix_file(fileName);
    if (not matrix_file.good()) {
        const string error_message = "Error opening file: " + fileName;
        throw runtime_error(error_message);
    }

    load_configuration(matrix_file);

    cout << "Using SimpleBayesianConsensusCaller with '"<< configuration_name <<"' configuration\n";
}

void SimpleBayesianConsensusCaller::print_probability_matrices(char separator){
    const uint32_t length = uint(probability_matrices[0].size());
    uint32_t n_bases = 4;

    for (uint32_t b=0; b<n_bases; b++){
        cout << '>' << Base::fromInteger(b).character() << " " << probability_matrices[b].size() << '\n';

        for (uint32_t i=0; i<length; i++){
            for (uint32_t j=0; j<length; j++){
                // Print with exactly 9 decimal values
                printf("%.9f",probability_matrices[b][i][j]);
                if (j != length-1){
                    cout << separator;
                }
            }
            cout << '\n';
        }
        if (b != n_bases-1){
            cout << '\n';
        }
    }
}

void SimpleBayesianConsensusCaller::print_priors(char separator){
    const uint32_t length = uint(priors[0].size());
    uint32_t n_bases = 2;

    for (uint32_t b=0; b<n_bases; b++){
        cout << '>' << Base::fromInteger(b).character() << " " << priors[b].size() << '\n';

        for (uint32_t i=0; i<length; i++){
            printf("%d %.9f",int(i), priors[b][i]);
            if (i != length-1){
                cout << separator;
            }
        }
        if (b != n_bases-1){
            cout << '\n';
        }
    }
}

void SimpleBayesianConsensusCaller::parse_name(ifstream& matrix_file, string& line){
    // Expect only one line to follow
    getline(matrix_file, line);
    configuration_name = line;
}

void SimpleBayesianConsensusCaller::parse_prior(ifstream& matrix_file, string& line, vector<string>& tokens){
    // Expect only one line to follow
    getline(matrix_file, line);

    // Initialize empty vector to fill with tokens from csv
    vector<double> row;

    // Assume csv format
    split_as_double(line, ',', row);

    // Two prior distributions exist. One for AT and one for GC, since observed reads are bidirectional
    if (tokens[0] == "AT"){
        priors[0] = row;
    }
    else if (tokens[0] == "GC"){
        priors[1] = row;
    }
}

void SimpleBayesianConsensusCaller::parse_likelihood(ifstream& matrix_file, string& line, vector<string>& tokens){
    char base;
    uint32_t base_index = 0;

    // Expect many lines (usually 51)
    while (getline(matrix_file, line)){

        // Stop iterating lines when blank line is encountered
        if (line.empty()){
            break;
        }

        // Initialize empty vector to fill with tokens from csv
        vector<double> row;

        // Assume csv format
        split_as_double(line, ',', row);

        base = tokens[0][0];
        base_index = uint32_t(Base::fromCharacter(base).value);

        probability_matrices[base_index].push_back(row);
    }
}

void SimpleBayesianConsensusCaller::load_configuration(ifstream& matrix_file){
    string line;

    while (getline(matrix_file, line)){

        // Header line (labeled via fasta-like headers)
        if (line[0] == '>'){
            vector<string> tokens;

            // Store the header
            line = line.substr(1, line.size()-1);
            split_as_string(line, ' ', tokens);

            if (tokens[0] == "Name"){
                parse_name(matrix_file, line);

            }else if (tokens[1] == "prior"){
                parse_prior(matrix_file, line, tokens);

            }else if (tokens[1] == "likelihood"){
                parse_likelihood(matrix_file, line, tokens);
            }
        }
    }
}

void SimpleBayesianConsensusCaller::print_log_likelihood_vector(vector<double>& log_likelihoods){
    int i = 0;
    for (auto& item: log_likelihoods){
        cout << i << " " << pow(10, item) << '\n';
        i++;
    }
}

void SimpleBayesianConsensusCaller::normalize_likelihoods(vector<double>& x, double x_max) const{
    for (uint32_t i=0; i<x.size(); i++){
        x[i] = x[i]-x_max;
    }
}

void SimpleBayesianConsensusCaller::factor_repeats(array<map<uint16_t,uint16_t>,2>& factored_repeats, const Coverage& coverage) const{
    // Store counts for each unique observation
    for (auto& observation: coverage.getReadCoverageData() ){
        // If NOT a gap, always increment
        if (not observation.base.isGap()) {
            factored_repeats[uint16_t(observation.strand)][uint16_t(observation.repeatCount)]++;
        // If IS a gap only increment if "count_gaps_as_zeros" is true
        }else if (count_gaps_as_zeros){
            factored_repeats[uint16_t(observation.strand)][0]++;
        }
    }
}

void SimpleBayesianConsensusCaller::factor_repeats(array<map<uint16_t,uint16_t>,2>& factored_repeats, const Coverage& coverage, AlignedBase consensus_base) const{
    // Store counts for each unique observation
    for (auto& observation: coverage.getReadCoverageData() ){
        // Ignore non consensus repeat values
        if (observation.base.value == consensus_base.value){
            // If NOT a gap, always increment
            if (not observation.base.isGap()) {
                factored_repeats[uint16_t(observation.strand)][uint16_t(observation.repeatCount)]++;
            // If IS a gap only increment if "count_gaps_as_zeros" is true
            }else if (count_gaps_as_zeros){
                factored_repeats[uint16_t(observation.strand)][0]++;
            }
        }
    }
}

uint16_t SimpleBayesianConsensusCaller::predict_runlength(const Coverage &coverage, AlignedBase consensusBase, vector<double>& log_likelihood_y) const{
    array <map <uint16_t,uint16_t>, 2> factored_repeats;    // Repeats grouped by strand and length

    size_t prior_index = -1;    // Used to determine which prior probability vector to access (AT=0 or GC=1)
    uint16_t x_i;               // Element of X = {x_0, x_1, ..., x_i} observed repeats
    uint16_t c_i;               // Number of times x_i was observed
    uint16_t y_j;               // Element of Y = {y_0, y_1, ..., y_j} true repeat between 0 and j=max_runlength
    double log_sum;             // Product (in logspace) of P(x_i|y_j) for each i

    double y_max_likelihood = -INF;     // Probability of most probable true repeat length
    uint16_t y_max = 0;                 // Most probable repeat length

    // Determine which index to use for this->priors
    if (consensusBase.character() == 'A' || consensusBase.character() == 'T'){
        prior_index = 0;
    }
    else if (consensusBase.character() == 'G' || consensusBase.character() == 'C'){
        prior_index = 1;
    }

    // Count the number of times each unique repeat was observed, to reduce redundancy in calculating log likelihoods/
    // Depending on class boolean "ignore_non_consensus_base_repeats" filter out observations
    if (ignore_non_consensus_base_repeats) {
        factor_repeats(factored_repeats, coverage, consensusBase);
    }
    else {
        factor_repeats(factored_repeats, coverage);
    }

    // Iterate all possible Y from 0 to j to calculate p(Y_j|X) where X is all observations 0 to i,
    // assuming i and j are less than max_runlength
    for (y_j = 0; y_j <= max_runlength; y_j++){
        // Initialize log_sum for this Y value using empirically determined priors
        log_sum = priors[prior_index][y_j];

        for (uint16_t strand = 0; strand <= factored_repeats.size() - 1; strand++){
            for (auto& item: factored_repeats[strand]){
                x_i = item.first;
                c_i = item.second;

                // In the case that observed runlength is too large for the matrix, cap it at max_runlength
                if (x_i > max_runlength){
                    x_i = max_runlength;
                }

                // Increment log likelihood for this y_j
                log_sum += double(c_i)*probability_matrices[consensusBase.value][y_j][x_i];
            }
        }

        log_likelihood_y[y_j] = log_sum;

        // Update max Y value if log likelihood is greater than previous maximum... Should do this outside this loop?
        if (log_sum > y_max_likelihood){
            y_max_likelihood = log_sum;
            y_max = y_j;
        }
    }

    normalize_likelihoods(log_likelihood_y, y_max_likelihood);

    return max(uint16_t(1), y_max);   // Don't allow zeroes...
}

AlignedBase SimpleBayesianConsensusCaller::predict_consensus_base(const Coverage& coverage) const{
    const vector<CoverageData>& coverage_data_vector = coverage.getReadCoverageData();
    vector<uint32_t> base_counts(5,0);
    uint32_t max_base_count = 0;
    uint8_t max_base = 4;   // Default to gap in case coverage is empty (is this possible?)
    uint32_t key;

    // Count bases. If it's a gap increment placeholder 4 in base_count vector
    for(const CoverageData& observation: coverage_data_vector) {
        if (not observation.base.isGap()) {
            key = observation.base.value;
            base_counts[key]++;
        }
        else{
            key = 4;
            base_counts[key]++;
        }
    }

    // Determine most represented base (consensus)
    for (uint32_t i=0; i<5; i++){
        if (base_counts[i] > max_base_count){
            max_base_count = base_counts[i];
            max_base = uint8_t(i);
        }
    }

    return AlignedBase::fromInteger(max_base);
}

Consensus SimpleBayesianConsensusCaller::operator()(const Coverage& coverage) const{
    // TODO: test that coverage is not empty?
    AlignedBase consensus_base;
    uint16_t consensus_repeat;
    vector<double> log_likelihoods(u_long(max_runlength), -INF);    // initialize as zeros in log space

    consensus_base = predict_consensus_base(coverage);

    if (predict_gap_runlengths) {
        // Predict all run lengths regardless of whether consensus base is a gap
        consensus_repeat = predict_runlength(coverage, consensus_base, log_likelihoods);
    }
    else {
        if (not consensus_base.isGap()) {
            // Consensus is NOT a gap character, and the configuration forbids predicting gaps
            consensus_repeat = predict_runlength(coverage, consensus_base, log_likelihoods);
        } else {
            // Consensus IS a gap character, and the configuration forbids predicting gaps
            consensus_repeat = 0;
        }
    }

    return Consensus(AlignedBase::fromInteger(consensus_base.value), consensus_repeat);
}

void testSimpleBayesianConsensusCaller(){
    SimpleBayesianConsensusCaller classifier;
    Coverage coverage;

    coverage.addRead(AlignedBase::fromInteger((uint8_t)1), 1, 1);    // Arguments are base, strand, repeat count.
    coverage.addRead(AlignedBase::fromInteger((uint8_t)1), 0, 2);
    coverage.addRead(AlignedBase::fromInteger((uint8_t)2), 1, 3);
    coverage.addRead(AlignedBase::fromInteger((uint8_t)1), 0, 2);
    coverage.addRead(AlignedBase::fromInteger((uint8_t)1), 1, 2);
    coverage.addRead(AlignedBase::fromInteger((uint8_t)2), 0, 2);
    coverage.addRead(AlignedBase::fromInteger((uint8_t)4), 0, 0);

    AlignedBase consensus_base;

    consensus_base = coverage.mostFrequentBase();

    cout << "CONSENSUS BASE = " << consensus_base << "\n";

    const Consensus consensus = classifier(coverage);

    cout << consensus.base << " " << consensus.repeatCount << '\n';
}


// Author: Gianni Lunardi, University of Trento 2022

#ifndef CROCODDYL_MULTIBODY_RESIDUALS_OBSTACLE_AVOIDANCE_HPP_
#define CROCODDYL_MULTIBODY_RESIDUALS_OBSTACLE_AVOIDANCE_HPP_

#include <pinocchio/multibody/fwd.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/multibody/fcl.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/geometry.hpp>
#include <Eigen/src/Core/DenseBase.h>

#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/core/residual-base.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"
#include "crocoddyl/multibody/data/multibody.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

/**

BRIEF FOR DOXYGEN

*/

template <typename _Scalar>
class ResidualModelObstacleAvoidanceTpl: public ResidualModelAbstractTpl<_Scalar> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef _Scalar Scalar;
    typedef MathBaseTpl<Scalar> MathBase;
    typedef ResidualModelAbstractTpl<Scalar> Base;
    typedef ResidualDataObstacleAvoidanceTpl<Scalar> Data;
    typedef StateMultibodyTpl<Scalar> StateMultibody;
    typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
    typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;

    typedef pinocchio::GeometryModel GeometryModel;
    typedef typename MathBase::Vector3s Vector3s;
    typedef typename MathBase::VectorXs VectorXs;
    typedef typename MathBase::Matrix3s Matrix3s;
    typedef typename MathBase::Matrix3xs Matrix3xs;
    typedef typename MathBase::Matrix6xs Matrix6xs;

    ResidualModelObstacleAvoidanceTpl(boost::shared_ptr<StateMultibody> state,
                                      const std::size_t nu,
                                      boost::shared_ptr<GeometryModel> geom_model,
                                      const pinocchio::PairIndex pair_id,
                                      const pinocchio::FrameIndex frame_id,
                                      const pinocchio::ReferenceFrame type,
                                      const double beta);

    /*
    Brief
    */

    virtual ~ResidualModelObstacleAvoidanceTpl();

    virtual void calc(const boost::shared_ptr<ResidualDataAbstract> &data,
                      const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &u);

    virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data,
                          const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &u);

    virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data);

    const pinocchio::GeometryModel &get_geometry() const;

    pinocchio::PairIndex get_pair_id() const;

    pinocchio::FrameIndex get_frame_id() const;

    pinocchio::ReferenceFrame get_type() const;

    void set_type(const pinocchio::ReferenceFrame type);

  protected:
    using Base::nu_;
    using Base::nr_;
    using Base::state_;
    using Base::q_dependent_;
    using Base::v_dependent_;
    using Base::unone_;
    const std::size_t nq_ = state_->get_nq();
    const std::size_t nv_ = state_->get_nv();
    const std::size_t ndx_ = state_->get_ndx();

  private:
    typename StateMultibody::PinocchioModel pin_model_;
    boost::shared_ptr<pinocchio::GeometryModel> geom_model_;
    pinocchio::PairIndex pair_id_;
    pinocchio::FrameIndex frame_id_;
    pinocchio::ReferenceFrame type_;
    const double beta_;

};

template <typename _Scalar>
struct ResidualDataObstacleAvoidanceTpl : public ResidualDataAbstractTpl<_Scalar> {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef _Scalar Scalar;
    typedef MathBaseTpl<Scalar> MathBase;
    typedef ResidualDataAbstractTpl<Scalar> Base;
    typedef StateMultibodyTpl<Scalar> StateMultibody;
    typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;

    typedef typename MathBase::MatrixXs MatrixXs;
    typedef typename MathBase::Matrix6xs Matrix6xs;
    typedef typename MathBase::Vector3s Vector3s;
    typedef typename MathBase::Vector6s Vector6s;

    template <template <typename Scalar> class Model>
    ResidualDataObstacleAvoidanceTpl(Model <Scalar> *const model, DataCollectorAbstract *const data)
        :   Base(model, data),
            geometry(pinocchio::GeometryData(model->get_geometry())),
            J(Matrix6xs::Zero(6, model->get_state()->get_nv())),
            dv_dx(Matrix6xs::Zero(6, model->get_state()->get_ndx())) {
        // Check that proper shared data has been passed
        DataCollectorMultibodyTpl<Scalar> *d = dynamic_cast<DataCollectorMultibodyTpl<Scalar> *>(shared);
        if (d == NULL) {
            throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibodyTpl");
        }

        // Avoids data casting at runtime
        pinocchio = d->pinocchio;
    }

    pinocchio::GeometryData geometry;
    pinocchio::DataTpl<Scalar>* pinocchio;
    Scalar dist;
    Vector3s p_diff;
    Vector6s v;
    Matrix6xs J;
    Matrix6xs dv_dx;
    using Base::r;
    using Base::Ru;
    using Base::Rx;
    using Base::shared;
};

}   // namespace crocoddyl

#include "crocoddyl/multibody/residuals/obstacle-avoidance.hxx"

#endif   // CROCODDYL_MULTIBODY_RESIDUALS_OBSTACLE_AVOIDANCE_HPP_