text:
#include "RutgersIAF/AnalysisPresenter/interface/Assembler.h"
#include "RutgersIAF/AnalysisPresenter/interface/AssemblerProjection.h"
#include "RutgersIAF/AnalysisPresenter/interface/BaseBundleProjection.h"
#include "RutgersIAF/AnalysisPresenter/interface/Bundle.h"
#include "RutgersIAF/AnalysisPresenter/interface/PhysicsContribution.h"

#include "Math/QuantFuncMathCore.h"
#include "TH2.h"
#include "TLegend.h"
#include "TLine.h"
#include "TPad.h"
#include "TPaveStats.h"
#include "TStyle.h"

#include <assert.h>
#include <boost/algorithm/string/join.hpp>
#include <boost/bind.hpp>
#include <boost/range/join.hpp>
#include <cmath>
#include <exception>

#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <unistd.h>
#include <iomanip>
#include <stdlib.h>

#include "RutgersIAF/debug.h"

using namespace std;

ClassImp(AssemblerProjection)

AssemblerProjection::AssemblerProjection(Assembler* assembler, std::vector<std::string> varNames, bool binForOverflow) : m_assembler(assembler), m_binForOverflow(binForOverflow), m_ranges(assembler->getRanges()), m_varNames(varNames) {
	m_name = boost::algorithm::join(m_varNames, ":");

	// Somehow putting all of this inline in the for loop doesn't work
	auto vData = m_assembler->getPhysicsContributions("data");
	auto vBackground = m_assembler->getPhysicsContributions("background");
	auto vSignal = m_assembler->getPhysicsContributions("signal");
	auto contributionsModel = boost::join(vBackground, vSignal);

	// Project event counts and uncertainty histograms
	for(auto &contribution : boost::join(vData, contributionsModel)) {
		if(m_typeProjections.find(contribution->getType()) == m_typeProjections.end()) {
			m_typeProjections.insert(make_pair(contribution->getType(), std::vector<BaseBundleProjection*>()));
		}

		m_typeProjections[contribution->getType()].push_back(contribution->project(m_varNames, binForOverflow));
	}

	prepareStacks();
}

AssemblerProjection::AssemblerProjection(const AssemblerProjection* parent, Bundle* bundle, TString missingName) : m_assembler(parent->m_assembler), m_binForOverflow(parent->m_binForOverflow), m_bundle(bundle), m_parent(parent), m_ranges(parent->m_ranges), m_name(parent->m_name), m_varNames(parent->m_varNames) {
	auto ranges = m_assembler->getRanges();
	m_assembler->setRanges(m_ranges);

	for(auto &parentTypeProjection : m_parent->m_typeProjections) {
		TString type = parentTypeProjection.first;

		if(bundle->getType() != type) {
			m_typeProjections[type] = m_parent->m_typeProjections.at(type);
			continue;

		}
		m_typeProjections.insert(make_pair(type, std::vector<BaseBundleProjection*>()));

		std::set<const PhysicsContribution*> contributions;
		for(auto &component : bundle->getComponents()) {
			BaseBundleProjection* projection = component->project(m_varNames, m_binForOverflow);
			m_typeProjections[type].push_back(projection);

			size_t oldSize = contributions.size();
			// Doing the next two lines in one line (without extra projectionContributions) gives infinite loop
			std::set<const PhysicsContribution*> projectionContributions = projection->getPhysicsContributions();
			contributions.insert(projectionContributions.begin(), projectionContributions.end());
			assert(contributions.size() == oldSize + projectionContributions.size());
		}

		Bundle* missing = new Bundle(type, missingName);
		for(auto &contribution : m_assembler->getPhysicsContributions(type)) {
			if(contributions.find(contribution) == contributions.end()) {
				if(missingName == "") {
					auto projection = contribution->project(m_varNames, m_binForOverflow);
					m_typeProjections[type].push_back(projection);

					size_t oldSize = contributions.size();
					// Doing the next two lines in one line (without extra projectionContributions) gives infinite loop
					std::set<const PhysicsContribution*> projectionContributions = projection->getPhysicsContributions();
					contributions.insert(projectionContributions.begin(), projectionContributions.end());
					assert(contributions.size() == oldSize + projectionContributions.size());
				} else {
					missing->addComponent(contribution);
				}
			}
		}
		if(missing->getComponents().size() > 0) {
			auto projection = missing->project(m_varNames, m_binForOverflow);
			m_typeProjections[type].push_back(projection);

			size_t oldSize = contributions.size();
			// Doing the next two lines in one line (without extra projectionContributions) gives infinite loop
			std::set<const PhysicsContribution*> projectionContributions = projection->getPhysicsContributions();
			contributions.insert(projectionContributions.begin(), projectionContributions.end());
			assert(contributions.size() == oldSize + projectionContributions.size());
		}
	}

	m_assembler->setRanges(ranges);

	prepareStacks();
}

AssemblerProjection::AssemblerProjection() {
	/* no-op */
}

AssemblerProjection::~AssemblerProjection() {
	//delete m_canvas; // TODO doing this causes a segfault -- probably something to do with http://root.cern.ch/phpBB3/viewtopic.php?f=14&t=11472
	for(auto &component : m_stacks) {
		delete component.second.first;
		delete component.second.second;
	}

	for(auto &typeProjection : m_typeProjections) {
		for(auto &projection : typeProjection.second) {
			delete projection;
		}
	}
}

double AssemblerProjection::addStackBinInQuadrature(THStack* stack, int i) const {
	TList* hists = stack->GetHists();
	double val2 = 0;
	TIterator* iter = new TListIter(hists);
	// If hists was 0, Next() returns 0, so we're fine. This happens if the stack is empty (common use case: stack for systematic uncertainties).
	while(TH1* obj = (TH1*)iter->Next()) {
		val2 += pow(obj->GetBinContent(i), 2);
	}
	return sqrt(val2);
}

AssemblerProjection* AssemblerProjection::bundle(Bundle* bundle, TString missingName) {
	return bundle ? new AssemblerProjection(this, bundle, missingName) : this;
}

double AssemblerProjection::extractStackBin(THStack* stack, int i, TString name) const {
	if(!stack->GetHists()) {
		return 0;
	}
	TH1* hUncertainty = (TH1*)stack->GetHists()->FindObject(name);
	return hUncertainty ? hUncertainty->GetBinContent(i) : 0;
}

double AssemblerProjection::getBin(TString type, int i) const {
	assert(has(type));
	TObjArray* stack = m_stacks.find(type)->second.first->GetStack();
	return stack ? ((TH1*)stack->Last())->GetBinContent(i) : 0;
}

double AssemblerProjection::getBin(TString type, int i, TString bundleName) const {
	assert(has(type, bundleName));
	TObjArray* stack = m_bundles.find(type)->second.find(bundleName)->second.first->GetStack();
	return stack ? ((TH1*)stack->Last())->GetBinContent(i) : 0;
}

double AssemblerProjection::getBinStat(TString type, int i) const {
	assert(has(type));
	TObjArray* stack = m_stacks.find(type)->second.first->GetStack();
	return stack ? ((TH1*)stack->Last())->GetBinError(i) : 0;
}

double AssemblerProjection::getBinStat(TString type, int i, TString bundleName) const {
	assert(has(type, bundleName));
	TObjArray* stack = m_bundles.find(type)->second.find(bundleName)->second.first->GetStack();
	return stack ? ((TH1*)stack->Last())->GetBinError(i) : 0;
}

double AssemblerProjection::getBinSyst(TString type, int i) const {
	assert(has(type));
	return addStackBinInQuadrature(m_stacks.find(type)->second.second, i);
}

double AssemblerProjection::getBinSyst(TString type, int i, TString name) const {
	assert(has(type));
	return extractStackBin(m_stacks.find(type)->second.second, i, name);
}

double AssemblerProjection::getBinSyst(TString type, int i, TString name, TString bundleName) const {
	assert(has(type, bundleName));
	return extractStackBin(m_bundles.find(type)->second.find(bundleName)->second.second, i, name);
}

std::vector<TString> AssemblerProjection::getBundleNames(TString type) const {
	assert(has(type));
	std::vector<TString> bundleNames;
	for(auto &bundle : m_bundles.find(type)->second) {
		bundleNames.push_back(bundle.first);
	}
	return bundleNames;
}

int AssemblerProjection::getDimensionality() const {
	return m_varNames.size();
}

TH1* AssemblerProjection::getHistogram(TString type) const {
	assert(has(type));
	TObjArray* stack = m_stacks.find(type)->second.first->GetStack();
	return stack ? (TH1*)((TH1*)stack->Last())->Clone() : 0;
}

std::vector<TH1*> AssemblerProjection::getHistograms(TString type) const {
	std::vector<TH1*> v;
	auto it = m_bundles.find(type);
	if(it != m_bundles.end()) {
		for(auto const &bundle : it->second) {
			TH1* h = (TH1*)bundle.second.first->GetStack()->Last()->Clone();
			for(int i = 0; i < h->GetNcells(); ++i) {
				double error2 = pow(h->GetBinError(i), 2);
				// Add up systematics for this bundle only
				for(auto const &uncertaintyName : getUncertaintyNames()) {
					error2 += pow(getBinSyst(type, i, uncertaintyName, bundle.first), 2);
				}
				h->SetBinError(i, sqrt(error2));
			}
			v.push_back(h);
		}
	}
	return v;
}

std::set<PhysicsContribution::metadata_t> AssemblerProjection::getMeta(TString type) const {
	if(type != "data") {
		throw std::runtime_error("meta information currently only supported for data");
	}

	std::set<PhysicsContribution::metadata_t> s;
	unsigned int nDuplicates = 0;

	auto ranges = m_assembler->getRanges();
	m_assembler->setRanges(m_ranges);

	for(auto &typeProjection : m_typeProjections.at(type)) {
		for(auto &contribution : typeProjection->getPhysicsContributions()) {
			for(auto &metadata : contribution->getMeta()) {
				auto ins = s.insert(metadata);
				if(!ins.second) {
					if(nDuplicates < 10) {
						cout << "Notice: duplicate entry in " << contribution->getName() << ": " << metadata.event << " " << metadata.run << " " << metadata.lumi << " " << metadata.fakeIncarnation << endl;
					}
					++nDuplicates;
				}
			}
		}
	}

	m_assembler->setRanges(ranges);

	if(nDuplicates > 0) {
		cout << "Notice: " << nDuplicates << " duplicate entries in (showing no more than 10)" << endl;
	}

	return s;
}

double AssemblerProjection::getMoment(TH1* h, int k, bool center) const {
	assert(getDimensionality() == 1);
	double moment = 0;
	for(int i = 1; i <= h->GetNbinsX(); ++i) {
		double idx = center
			? ((h->GetXaxis()->GetBinLowEdge(i) + h->GetXaxis()->GetBinUpEdge(i)) / 2)
			: h->GetXaxis()->GetBinLowEdge(i);
		moment += pow(idx, k) * h->GetBinContent(i);
	}
	return moment;
}

double AssemblerProjection::getMoment(TString type, int k, bool center) const {
	assert(getDimensionality() == 1);
	assert(has(type));
	TObjArray* stack = m_stacks.find(type)->second.first->GetStack();
	if(!stack) {
		return 0;
	}
	TH1* h = (TH1*)stack->Last();
	if(hasOverflowIncluded() && h->GetBinContent(h->GetNbinsX() + 1) != 0) {
		throw std::runtime_error("can't compute moment when overflow bin is not empty");
	}
	return getMoment(h, k, center);
}

std::vector<std::pair<int, int>> AssemblerProjection::getRanges() const {
	return m_ranges;
}

std::set<TString> AssemblerProjection::getUncertaintyNames() const {
	std::set<TString> uncertaintyNames;
	for(auto &typeProjection : m_typeProjections) {
		for(auto &projection : typeProjection.second) {
			for(auto &projectionUncertainties : projection->getUncertainties()) {
				uncertaintyNames.insert(projectionUncertainties.first);
			}
		}
	}
	return uncertaintyNames;
}

std::vector<string> AssemblerProjection::getVarNames() const {
	return m_varNames;
}

bool AssemblerProjection::has(TString type) const {
	return (m_stacks.find(type) != m_stacks.end() && m_stacks.find(type)->second.first->GetHists());
}

bool AssemblerProjection::has(TString type, TString bundleName) const {
	return (
		m_bundles.find(type) != m_bundles.end()
		&& m_bundles.find(type)->second.find(bundleName) != m_bundles.find(type)->second.end()
	);
}

bool AssemblerProjection::hasOverflowIncluded() const {
	return m_binForOverflow;
}

bool AssemblerProjection::isDistribution() const {
	return (getDimensionality() > 0);
}

TCanvas* AssemblerProjection::plot(bool log, TF1* f1, double xminFit, double xmaxFit) {
	assert(getDimensionality() <= 2);

	if(getDimensionality() == 2) {
		return plot2D(log);
	}

	bool hasBackground = has("background");
	bool doRatio  = (getDimensionality() <= 1) && hasBackground && !m_assembler->getMode("noRatioPlot");
	bool doZscore = m_assembler->getMode("ZscorePlot");
	bool debug = m_assembler->getMode("debug");
	m_canvas = new TCanvas("c1", "c1", 1200, doRatio ? 750 : 490);

	TPad *pad1 = new TPad("pad1", "pad1", 0, doRatio ? 0.3 : 0, 1, 1);
	if(doRatio) {
		pad1->SetBottomMargin(0.025);
		pad1->SetRightMargin(0.3);
	}
	pad1->Draw();
	pad1->cd();
	pad1->SetTicks(1, 1);

	TString title = m_name;
	if(getDimensionality() == 1) {
		TString name = m_assembler->getVarName(m_varNames.at(0));
		if(title != name) {
			title = TString::Format("%s (%s)", name.Data(), m_varNames.at(0).c_str());
		}
	}

	// TODO Not cloning causes segfault ...
	TH1* hData = (TH1*)m_stacks.find("data")->second.first->GetStack()->Last()->Clone();
	if(hasOverflowIncluded()) {
		// Set overflow bin content to make it show up in statistics box
		auto nEntries = hData->GetEntries();
		hData->SetBinContent(hData->GetNbinsX() + 1, hData->GetBinContent(hData->GetNbinsX()));
		hData->SetEntries(nEntries);
	}
	hData->SetLineWidth(1);
	hData->SetMarkerStyle(9);

	std::vector<TH1*> hSignals = getHistograms("signal");

	TH1* hBackground = 0;
	TH1* hBackgroundErr = 0;
	TH1* hRatio = 0;
	TH1* hRatioBkg = 0;
	if(hasBackground) {
		hBackground = (TH1*)m_stacks.find("background")->second.first->GetStack()->Last()->Clone("background");
		for(int i = 0; i < hBackground->GetNcells(); ++i) {
			// Add up stat. error in the background stack (ideally 0 with infinite MC) and the systematic uncertainties
			// Those are together the "comprehensive systematic uncertainty"
			double error2 = pow(hBackground->GetBinError(i), 2) + pow(getBinSyst("background", i), 2);
			hBackground->SetBinError(i, sqrt(error2));
		}

		hBackgroundErr = (TH1*)hBackground->Clone("backgroundErr");
		hBackgroundErr->Reset();
		const double alpha = 1 - 0.6827;
		for(int i = 0; i < hBackgroundErr->GetNcells(); ++i) {
			double n = hBackground->GetBinContent(i);
			double lo = (n == 0) ? 0 : ROOT::Math::gamma_quantile(alpha/2, n, 1.);
			double hi = ROOT::Math::gamma_quantile_c(alpha/2, n+1, 1);
			// Now, combine Poisson fluctuation range with "comprehensive systematic uncertainty" from above
			lo = sqrt(pow(n - lo, 2) + pow(hBackground->GetBinError(i), 2));
			hi = sqrt(pow(hi - n, 2) + pow(hBackground->GetBinError(i), 2));
			lo = n - lo;
			hi = n + hi;
			n = (lo + hi) / 2;
			hBackgroundErr->SetBinContent(i, n);
			hBackgroundErr->SetBinError(i, (hi - lo) / 2);
		}

		hRatio = (TH1*)hData->Clone("hRatio");
		hRatioBkg = debug ? (TH1*)hBackgroundErr->Clone("hRatioBkg") : (TH1*)hBackground->Clone("hRatioBkg");

		double signalMax = 0;
		for(auto const &hSignal : hSignals) {
			signalMax = max(signalMax, hSignal->GetMaximum());
		}
		hData->SetMaximum(max(1., max(hData->GetMaximum(), max(hBackground->GetMaximum(), signalMax))));
	}

	hData->SetMaximum(max(log ? 50. : 5., (log ? 5. : 1.5) * hData->GetMaximum()));
	if(log) {
		hData->SetMinimum(hasBackground ? min(0.1, 0.5 * hBackground->GetMinimum(0)) : 0.1);
		// Make sure the range spans at least one order of magnitude
		if(hData->GetMinimum() > hData->GetMaximum() / 15) {
			hData->SetMinimum(hData->GetMaximum() / 15);
		}
	} else {
		hData->SetMinimum(0);
	}
	hData->GetXaxis()->SetTitle(title);
	hData->SetLineColor(kBlack);

	if(doRatio) {
		hData->GetXaxis()->SetLabelFont(43);
		hData->GetXaxis()->SetLabelSize(0);
	}
	hData->GetYaxis()->SetLabelFont(43);
	hData->GetYaxis()->SetLabelSize(16);

	hData->Clone("dummy")->Draw("EP"); // use dummy name to allow unique name in TCanvas for the actual data histogram

	if(hasBackground) {
		((THStack*)m_stacks.find("background")->second.first->Clone())->Draw("HIST SAME"); // TODO crashes when not cloning
		hBackground->SetFillColor(kBlack);
		hBackground->SetFillStyle(3002);
		hBackground->Draw("E2 SAME");
		if(debug) {
			hBackgroundErr->SetFillColor(kBlack);
			hBackgroundErr->SetFillStyle(3013);
			hBackgroundErr->SetMarkerStyle(20);
			hBackgroundErr->SetMarkerSize(0);
			hBackgroundErr->Draw("SAME E2");
		}
		for(auto const &hSignal : hSignals) {
			hSignal->SetMarkerColor(kWhite);
			hSignal->SetMarkerStyle(21);
			hSignal->SetFillStyle(3008);
			hSignal->Draw("SAME E2 P");
		}
	}

	hData->Clone("dummy")->Draw("AXIS SAME"); // Documentation says that statistics box is not drawn with SAME. However, it is for this specific Draw(), and it has the title "dummy". We don't want that. So, ...
	hData->Draw("EP SAMES"); // ... let's request a statistics box for the actual data with SAMES. (Here, SAME doesn't draw the box. I propose a CERN experiment to study and develop a theory of the mysteries of ROOT.)
	gStyle->SetOptStat(111111);
	pad1->SetLogy(log);

	m_canvas->cd();
	TPad *pad2 = new TPad("pad2", "pad2", 0, 0, 1, doRatio ? 0.3 : 0);
	pad2->SetTopMargin(0.025);
	pad2->SetRightMargin(0.3);
	pad2->SetBottomMargin(0.2);
	pad2->SetTickx(1);
	pad2->SetTicky(1);
	pad2->SetGridy(1);
	pad2->Draw();
	pad2->cd();

	double ratio = 0;
	if(doRatio) {
		ratio = hData->Integral() / hBackground->Integral();
		TLine* line1 = new TLine(hData->GetBinLowEdge(1), 1, hData->GetBinLowEdge(hData->GetNbinsX()+1), 1);
		TLine* line2 = new TLine(hData->GetBinLowEdge(1), ratio, hData->GetBinLowEdge(hData->GetNbinsX()+1), ratio);
		line2->SetLineColor(kBlack);
		line2->SetLineWidth(2);
		hRatio->GetXaxis()->SetTitle(title);
		hRatio->SetTitle("");
		for(int i = 0; i < hRatio->GetXaxis()->GetNbins() + 1; ++i) {
			double yield = hBackground->GetBinContent(i);
			hRatio->SetBinContent(i, (yield == 0) ? 0 : hRatio->GetBinContent(i) / yield);
			hRatio->SetBinError(i, (yield == 0) ? 0 : hRatio->GetBinError(i) / yield);
			if(doZscore){
			  cout<<"Calculate ZScore: "<<endl;
			}
		}
		if(isDistribution()) {
			hRatio->GetXaxis()->SetLabelFont(43);
			hRatio->GetXaxis()->SetLabelSize(16);
		} else {
			hRatio->GetXaxis()->SetLabelFont(43);
			hRatio->GetXaxis()->SetLabelSize(0);
		}
		hRatio->GetYaxis()->SetLabelFont(43);
		hRatio->GetYaxis()->SetLabelSize(16);
		hRatio->GetYaxis()->SetTitle("Obs/Exp");
		hRatio->GetYaxis()->CenterTitle();
		hRatio->GetYaxis()->SetTitleSize(0.1);
		hRatio->GetXaxis()->SetTitleSize(0.1);
		hRatio->GetYaxis()->SetTitleOffset(0.25);
		hRatio->GetXaxis()->SetTitleOffset(0.8);
		hRatio->GetXaxis()->SetTickLength(0.1);
		hRatio->SetFillColor(kBlack);
		hRatio->SetFillStyle(3001);
		hRatio->SetMinimum(0);
		//if(hRatio->GetMaximum() < 0.5) {
		//	hRatio->SetMaximum(2 * hRatio->GetMaximum());
		//} else if(hRatio->GetMaximum() < 2) {
		//	hRatio->SetMaximum(2);
		//}
		hRatio->GetYaxis()->SetRangeUser(0,2.5);
		hRatio->GetYaxis()->SetRangeUser(hRatio->GetMinimum(0), hRatio->GetMaximum());
		hRatio->Draw("AXIS");
		double hRatioBkgErrorSum = 0;
		for(int i = 0; i < hRatioBkg->GetXaxis()->GetNbins() + 1; ++i) {
			double yield = hBackground->GetBinContent(i);
			hRatioBkg->SetBinContent(i, (yield == 0) ? 0 : hRatioBkg->GetBinContent(i) / yield);
			hRatioBkg->SetBinError(i, (yield == 0) ? 0 : hRatioBkg->GetBinError(i) / yield);

			if(hRatioBkg->GetBinContent(i) > hRatio->GetMaximum()) {
				double lo = hRatioBkg->GetBinContent(i) - hRatioBkg->GetBinError(i);
				hRatioBkg->SetBinContent(i, hRatio->GetMaximum());
				hRatioBkg->SetBinError(i, max(hRatioBkg->GetBinContent(i) - lo, 0.0));
			}
			hRatioBkgErrorSum += hRatioBkg->GetBinError(i);
		}
		// If all the uncertainties in hRatioBkg are 0 (this happens when the error bars are all out of y-axis range), ROOT would draw error bars filling all of the histogram frame. We don't want to draw anything in this case.
		if(hRatioBkgErrorSum > 0) {
			hRatioBkg->SetFillColor(kBlack);
			hRatioBkg->SetFillStyle(3002);
			hRatioBkg->SetMarkerStyle(20);
			hRatioBkg->SetMarkerSize(0);
			line1->Draw();
			if(debug) {
				line2->Draw();
			}
			hRatioBkg->Draw("E2 SAME");
		}
		gStyle->SetOptFit(1111);
		((TPaveStats*)hRatio->GetListOfFunctions()->FindObject("stats"))->SetOptStat(0);
		if(f1) {
			hRatio->Fit(f1, "", "SAME AXIS", xminFit, xmaxFit);
		}
		hRatio->Draw("SAME");
	}

	if(hasBackground) {
		ratio = hData->Integral() / hBackground->Integral();
		TLegend* legend = new TLegend(0.73,0.02,0.98,0.68);
		//TLegend* legend = new TLegend(0.73,0.1,0.98,0.92);
		legend->SetHeader(TString::Format("%.1f (r = %.3f)", hBackground->Integral(), ratio).Data());
		TList* hists = m_stacks.find("background")->second.first->GetHists();
		TIterator* iter = new TListIter(hists, false);
		while(TH1* obj = (TH1*)iter->Next()) {
			legend->AddEntry(obj, TString::Format("%s [%.1f]", obj->GetTitle(), obj->Integral()).Data());
		}
		delete iter;
		for(auto const &hSignal : hSignals) {
			legend->AddEntry((TObject*)0, "", "");
			hSignal->SetLineColor(kWhite);
			TString legendTitle = hSignal->GetTitle();
			legendTitle.ReplaceAll("[X]", TString::Format("[%.1f]", hSignal->Integral()));
			legend->AddEntry(hSignal, legendTitle.Data(), "FP");
		}
		m_canvas->cd();
		legend->Draw();
	}

	return m_canvas;
}

TCanvas* AssemblerProjection::plot2D(bool log) {
	assert(getDimensionality() == 2);

	bool hasBackground = has("background");
	bool doRatio = hasBackground && !m_assembler->getMode("noRatioPlot");
	//bool debug = m_assembler->getMode("debug");
	m_canvas = new TCanvas("c1", "c1", 700, doRatio ? 700 : 490);

	TPad *pad1 = new TPad("pad1", "pad1", 0, doRatio ? 0.3 : 0, 1, 1);
	pad1->Draw();
	pad1->cd();
	pad1->SetTicks(1, 1);

	// TODO Not cloning causes segfault ...
	TH2* hData = (TH2*)m_stacks.find("data")->second.first->GetStack()->Last()->Clone();
	if(hasOverflowIncluded()) {
		// Set overflow bin content to make it show up in statistics box
		auto nEntries = hData->GetEntries();
		hData->SetBinContent(hData->GetNbinsX() + 1, hData->GetBinContent(hData->GetNbinsX()));
		hData->SetEntries(nEntries);
	}
	hData->SetLineWidth(1);
	hData->SetMarkerStyle(9);

	auto hSignals = getHistograms("signal");

	TH2* hBackground = 0;
	TH2* hRatio = 0;
	if(hasBackground) {
		hBackground = (TH2*)m_stacks.find("background")->second.first->GetStack()->Last()->Clone("background");
		for(int i = 0; i < hBackground->GetNcells(); ++i) {
			// Add up stat. error in the background stack (ideally 0 with infinite MC) and the systematic uncertainties
			// Those are together the "comprehensive systematic uncertainty"
			double error2 = pow(hBackground->GetBinError(i), 2) + pow(getBinSyst("background", i), 2);
			hBackground->SetBinError(i, sqrt(error2));
		}

		hRatio = (TH2*)hData->Clone("hRatio");

		double signalMax = 0;
		for(auto const &hSignal : hSignals) {
			signalMax = max(signalMax, hSignal->GetMaximum());
		}
		hData->SetMaximum(max(1., max(hData->GetMaximum(), max(hBackground->GetMaximum(), signalMax))));
	}

	hData->SetMaximum(max(log ? 50. : 5., (log ? 5. : 1.5) * hData->GetMaximum()));
	if(log) {
		hData->SetMinimum(hasBackground ? min(0.1, 0.5 * hBackground->GetMinimum(0)) : 0.1);
		// Make sure the range spans at least one order of magnitude
		if(hData->GetMinimum() > hData->GetMaximum() / 15) {
			hData->SetMinimum(hData->GetMaximum() / 15);
		}
	} else {
		hData->SetMinimum(0);
	}

	THStack* hStack = (THStack*)m_stacks.find("background")->second.first->Clone();
	hStack->SetMinimum(hData->GetMinimum());
	hStack->SetMaximum(hData->GetMaximum());
	hStack->Draw();

	TString titleX = m_varNames.at(0).c_str();
	TString nameX = m_assembler->getVarName(m_varNames.at(0));
	if(titleX != nameX) {
		titleX = TString::Format("%s (%s)", nameX.Data(), m_varNames.at(0).c_str());
	}
	//hData->GetXaxis()->SetTitle(titleX);
	hStack->GetXaxis()->SetTitle(titleX);
	hStack->GetXaxis()->SetTitleOffset(1.6);

	TString titleY = m_varNames.at(1).c_str();
	TString nameY = m_assembler->getVarName(m_varNames.at(1));
	if(titleY != nameY) {
		titleY = TString::Format("%s (%s)", nameY.Data(), m_varNames.at(1).c_str());
	}
	//hData->GetYaxis()->SetTitle(titleY);
	hStack->GetYaxis()->SetTitle(titleY);
	hStack->GetYaxis()->SetTitleOffset(1.6);

	if(hasBackground) {
		hStack->Draw("LEGO1");

		for(auto const &hSignal : hSignals) {
			hSignal->SetMarkerColor(kWhite);
			hSignal->SetMarkerStyle(21);
			hSignal->SetFillStyle(3008);
			hSignal->Draw("SAME E2 P");
		}
	}

	hData->SetLineColor(kBlack);
	hData->SetLineWidth(1);
	hData->SetMarkerColor(kBlack);
	hData->SetMarkerStyle(8);
	hData->SetMarkerSize(0.7);
	hData->Draw("E2 P SAME");

	gStyle->SetOptStat(111111);
	pad1->SetLogz(log);
	pad1->SetPhi(210);

	double ratio = 0;
	if(hasBackground) {
		ratio = hData->Integral() / hBackground->Integral();

		TLegend* legend = new TLegend(0.64,0.6,0.98,0.92);
		legend->SetHeader(TString::Format("%.1f (r = %.3f)", hBackground->Integral(), ratio).Data());
		TList* hists = m_stacks.find("background")->second.first->GetHists();
		TIterator* iter = new TListIter(hists, false);
		while(TH2* obj = (TH2*)iter->Next()) {
			legend->AddEntry(obj, TString::Format("%s [%.1f]", obj->GetTitle(), obj->Integral()).Data());
		}
		delete iter;
		for(auto const &hSignal : hSignals) {
			legend->AddEntry((TObject*)0, "", "");
			hSignal->SetLineColor(kWhite);
			TString legendTitle = hSignal->GetTitle();
			legendTitle.ReplaceAll("[X]", TString::Format("[%.1f]", hSignal->Integral()));
			legend->AddEntry(hSignal, legendTitle.Data(), "FP");
		}
		legend->Draw();
	}

	m_canvas->cd();
	TPad *pad2 = new TPad("pad2", "pad2", 0, 0, 1, doRatio ? 0.3 : 0);
	pad2->SetTopMargin(0.025);
	pad2->Draw();
	pad2->cd();

	if(doRatio) {
		for(int i = 0; i < hRatio->GetNcells(); ++i) {
			// Compute residual (t-statistic)
			double residual = hRatio->GetBinContent(i) - hBackground->GetBinContent(i);
			double error2 = hBackground->GetBinContent(i); // this is (sqrt(n))^2
			error2 += pow(hBackground->GetBinError(i), 2);

			hRatio->SetBinContent(i, residual / sqrt(error2));

			// Uncertainty on residual doesn't have much meaning
			hRatio->SetBinError(i, 0);
		}
		hRatio->SetTitle("");
		hRatio->GetZaxis()->SetTitle("(data #minus background) / uncertainty");
		hRatio->SetMinimum();
		hRatio->SetMaximum();
		hRatio->Draw("COLZ");
		gStyle->SetOptFit(1111);
		((TPaveStats*)hRatio->GetListOfFunctions()->FindObject("stats"))->SetOptStat(0);
		hRatio->Draw("COLZ");
	}

	//pad2->SetPhi(210);

	return m_canvas;
}

void AssemblerProjection::prepareStacks() {
	// Combine correlated uncertainties and assemble contributions / bundle components into sorted histogram stack
	for(const auto &typeProjection : m_typeProjections) {
		// Intermediate structure for main histogram, and stack for systematic uncertainties (statistical ones are taken care of in main histogram
		std::vector<std::pair<TH1*, double>> vh;
		THStack* hsSyst = new THStack("hsSyst", m_assembler->getVarExp() + TString(" {") + m_assembler->getSelection() + TString("}"));

		// Bundle-wise structure (for datacard correlation bundles and such)
		m_bundles.insert(make_pair(typeProjection.first, std::map<TString, std::pair<THStack*, THStack*>>()));

		// Loop over signal / background components
		for(const auto &baseBundleProjection : typeProjection.second) {
			TString bundleName = baseBundleProjection->getSource()->getName();

			// Prepare bundle-wise stacks
			THStack* hsBundle = new THStack((TString("hs_") + bundleName).Data(), m_assembler->getVarExp() + TString(" {") + m_assembler->getSelection() + TString("}"));
			THStack* hsSystBundle = new THStack((TString("hsSyst_") + bundleName).Data(), m_assembler->getVarExp() + TString(" {") + m_assembler->getSelection() + TString("}"));
			m_bundles[typeProjection.first].insert(make_pair(bundleName, make_pair(hsBundle, hsSystBundle)));

			// Add histogram contents
			vh.push_back(make_pair(baseBundleProjection->getHistogram(), baseBundleProjection->getHistogram()->Integral()));

			// Same in the bundle-wise structure
			hsBundle->Add(baseBundleProjection->getHistogram());

			for(const auto &uncertainty : baseBundleProjection->getUncertainties()) {
				// Combine uncertainties into main histograms
				TH1* hUncertainty = hsSyst->GetHists()
					? (TH1*)hsSyst->GetHists()->FindObject(uncertainty.first)
					: 0;
				if(hUncertainty) {
					for(int j = 0; j < hUncertainty->GetNcells(); ++j) {
						double value = hUncertainty->GetBinContent(j);
						value = sqrt(value*value + pow(uncertainty.second->GetBinContent(j), 2));
						hUncertainty->SetBinContent(j, value);
					}
				} else {
					// We need to add a clone because we will manipulate this later
					hsSyst->Add((TH1*)uncertainty.second->Clone());
				}

				// Same in the bundle-wise structure (but uncertainty names can occur only once)
				hsSystBundle->Add(uncertainty.second);
			}
		}

		// Sort by amount of contribution
		std::sort(vh.begin()
			, vh.end()
			, boost::bind(&std::pair<TH1*, double>::second, _1) < boost::bind(&std::pair<TH1*, double>::second, _2)
		);

		// Prepare content stack
		THStack* hs = new THStack("hs", m_assembler->getVarExp() + TString(" {") + m_assembler->getSelection() + TString("}"));
		for(const auto &contribution : vh) {
			contribution.first->SetLineWidth(0);
			hs->Add(contribution.first);
		}

		if(m_stacks.find(typeProjection.first) != m_stacks.end()) {
			throw std::runtime_error("overwriting projection components not supported");
		}
		m_stacks.insert(make_pair(typeProjection.first, make_pair(hs, hsSyst)));
	}
}

void AssemblerProjection::print() const {
	assert(getDimensionality() <= 1);

	double sumData = 0;
	double sumBackground = 0;
	double sumBackgroundStat2 = 0;
	double sumBackgroundSyst = 0;
	double sumSignal = 0;
	double sumSignalStat2 = 0;
	double sumSignalSyst = 0;

	TH1* hData = (TH1*)m_stacks.find("data")->second.first->GetStack()->Last()->Clone();
	//cout << "data entries: " << hData->GetEntries() << endl;
	//cout << "data integral: " << hData->Integral() << endl;
	//cout << "data integral w/ overflow: " << hData->Integral(0, hData->GetNbinsX() + 1) << endl;
	cout << ((TH1*)m_stacks.find("data")->second.first->GetStack()->Last())->GetTitle() << endl;
	for(int i = 1; i <= hData->GetNbinsX(); ++i) {
		double lo = hData->GetXaxis()->GetBinLowEdge(i);
		double hi = hData->GetXaxis()->GetBinUpEdge(i);

		if(isDistribution()) {
			if(i < hData->GetNbinsX() || !hasOverflowIncluded()) {
				cout << m_varNames.at(0) << " " << lo << "-" << hi;
			} else {
				cout << m_varNames.at(0) << " " << lo << "-" << "inf";
			}
		}

		double contentData = getBin("data", i);
		sumData += contentData;

		if(isDistribution()) {
			printf("	%.0f", contentData);
		}

		if(has("background")) {
			double contentBackground = getBin("background", i);
			double contentBackgroundStat = getBinStat("background", i);
			double contentBackgroundSyst = getBinSyst("background", i);
			sumBackground += contentBackground;
			sumBackgroundStat2 += contentBackgroundStat*contentBackgroundStat;
			sumBackgroundSyst += contentBackgroundSyst;

			// TODO Replace by Poisson error
			if(isDistribution()) {
				printf(" : %.2f ± %.2f ± %.2f ± %.2f", contentBackground, sqrt(contentBackground), contentBackgroundStat, contentBackgroundSyst);
			}
		}

		if(has("signal")) {
			double contentSignal = getBin("signal", i);
			double contentSignalStat = getBinStat("signal", i);
			double contentSignalSyst = getBinSyst("signal", i);
			sumSignal += contentSignal;
			sumSignalStat2 += contentSignalStat*contentSignalStat;
			sumSignalSyst += contentSignalSyst;

			if(isDistribution()) {
				printf(" : %.2f ± %.2f ± %.2f", contentSignal, contentSignalStat, contentSignalSyst);
			}
		}

		if(isDistribution()) {
			cout << endl;
		}
	}
	if(isDistribution()) {
		printf("Sum:		%.0f", sumData);
	} else {
		printf("%s:	%.0f", m_name.Data(), sumData);
	}
	if(has("background")) {
		// TODO Replace by Poisson error
		printf(" : %.2f ± %.2f ± %.2f ± %.2f", sumBackground, sqrt(sumBackground), sqrt(sumBackgroundStat2), sumBackgroundSyst);
	}
	if(has("signal")) {
		printf(" : %.2f ± %.2f ± %.2f", sumSignal, sqrt(sumSignalStat2), sumSignalSyst);
	}
	cout << endl;
}

TString AssemblerProjection::printRA7table() const {
	assert(getDimensionality() <= 1);

	double sumData = 0;
	double sumBackground = 0;
	double sumBackgroundStat2 = 0;
	double sumBackgroundSyst = 0;
	double sumSignal = 0;
	double sumSignalStat2 = 0;
	double sumSignalSyst = 0;
	TString SRvalue = "";

	TH1* hData = (TH1*)m_stacks.find("data")->second.first->GetStack()->Last()->Clone();
	for(int i = 1; i <= hData->GetNbinsX(); ++i) {
		double contentData = getBin("data", i);
		sumData += contentData;

		if(!has("signal")) {
			double contentBackground = getBin("background", i);
			double contentBackgroundStat = getBinStat("background", i);
			double contentBackgroundSyst = getBinSyst("background", i);
			sumBackground += contentBackground;
			sumBackgroundStat2 += contentBackgroundStat*contentBackgroundStat;
			sumBackgroundSyst += contentBackgroundSyst;
		}

		if(has("signal")) {
			double contentSignal = getBin("signal", i);
			double contentSignalStat = getBinStat("signal", i);
			double contentSignalSyst = getBinSyst("signal", i);
			sumSignal += contentSignal;
			sumSignalStat2 += contentSignalStat*contentSignalStat;
			sumSignalSyst += contentSignalSyst;
		}
	}
	if(!has("signal")) {
		// TODO Replace by Poisson error
		SRvalue = TString::Format("%.0f : %.2f $\\pm$ %.2f $\\pm$ %.2f", sumData, sumBackground, sqrt(sumBackgroundStat2), sumBackgroundSyst);
	}
	if(has("signal")) {
		SRvalue = TString::Format("%.0f : %.2f $\\pm$ %.2f $\\pm$ %.2f", sumData, sumSignal, sqrt(sumSignalStat2), sumSignalSyst);
	}
	return SRvalue;
}

void AssemblerProjection::printMeta(TString type) const {
	for(auto &meta : getMeta(type)) {
		cout << meta.event << " " << meta.run << " " << meta.lumi << " " << meta.fakeIncarnation << endl;
	}
}

void AssemblerProjection::datacard(TString datacardName, bool isData, double statFactor, double systFactor) {
	cerr << "Datacards on projections currently not implemented. Please create a ChannelCollection." << endl;
}

max_stars_repo_name: peterthomassen/RutgersIAF

max_issues_repo_name: peterthomassen/RutgersIAF

max_forks_repo_name: peterthomassen/RutgersIAF


text:
#include <ros/ros.h>
#include <iostream>
#include <fstream>
//#include <pcl/console/parse.h>
//#include <pcl/filters/extract_indices.h>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <boost/thread/thread.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/sample_consensus/ransac.h>
//#include <pcl/sample_consensus/sac_model_plane.h>
//#include <pcl/sample_consensus/sac_model_sphere.h>
#include <algorithm>
#include <vector>
#include <string>
#include <numeric>
#include <cmath>
#include <time.h>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <dirent.h>

using namespace cv;
using namespace std;

clock_t t[100];

//read 3D points from pcd file
pcl::PointCloud<pcl::PointXYZ>::Ptr ReadPCD(char* filePath)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ> (filePath, *cloud) == -1)
	{
		PCL_ERROR("Couldn't read file box_cloud.pcd \n");

	}
	return cloud;
}

//read 3D points from ply file
pcl::PointCloud<pcl::PointXYZ>::Ptr ReadPLY(char* filePath)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PLYReader Reader;
	Reader.read(filePath, *cloud);
	return cloud;
}

//k nearest neighbor search
void PointsKNN(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int pointNum, int maxK, int** indexKNN, double** distKNN)
{
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud (cloud);
	pcl::PointXYZ searchPoint;
	std::vector<int> pointIdxNKNSearch(maxK);
  	std::vector<float> pointNKNSquaredDistance(maxK);
	for(int i=0; i<pointNum; i++)
	{
		searchPoint.x=cloud->points[i].x;
		searchPoint.y=cloud->points[i].y;
		searchPoint.z=cloud->points[i].z;
		if(kdtree.nearestKSearch (searchPoint, maxK, pointIdxNKNSearch, pointNKNSquaredDistance)>0)
		{
			for(int j=0; j<maxK; j++)
			{
				indexKNN[i][j]=pointIdxNKNSearch[j];
				distKNN[i][j]=pointNKNSquaredDistance[j];
			}

		}
	}

	/*for(int i=0; i<pointNum; i++)
	{
		for(int j=0; j<k; j++)
		{
			cout<<" "<<indexKNN[i][j];
		}
		cout<<endl;
	}
	cout<<endl;
	for(int i=0; i<pointNum; i++)
	{
		for(int j=0; j<k; j++)
		{
			cout<<" "<<distKNN[i][j];
		}
		cout<<endl;
	}*/
}

//compute sigma and NWR matrix, then get the normals of the points
cv::Mat NormalEstimation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int pointNum, int maxK, int** indexKNN, double** distKNN)
{
	cv::Mat diffM(3, 1, CV_64FC1);
	cv::Mat referP(3, 1, CV_64FC1);
	cv::Mat neighP(3, 1, CV_64FC1);
	cv::Mat nwrSample(3, 3, CV_64FC1);
	cv::Mat nwrMat(3, 3, CV_64FC1);
	cv::Mat normals(pointNum, 3, CV_64FC1);
	double sigma=0.2;
	//compute sigma
	//int nn;    //number of nonzero distance
	/*for(int i=0; i<pointNum; i++)
	{
		nn=0;
		for(int j=0; j<maxK; j++)
		{
			if(distKNN[i][j]!=0)
			{
				nn++;
			}
			if(nn==k)
			{
				sigma=distKNN[i][j]+sigma;
				break;
			}
		}
	}
	sigma=sigma/pointNum;*/
	//compute NWR Matrix
	t[10]=clock();
	//randomly pick some points
	//std::vector<int> randPoints;
	/*for(int i=0; i<pointNum; i++)
	{
		randPoints.push_back(i);
	}
	std::random_shuffle(randPoints.begin(), randPoints.end());
	pointNum=pointNum*randRate;*/
	//cout<<"randRate: "<<randRate<<endl;
	//cout<<"pointNum: "<<pointNum<<endl;
	for(int i=0; i<pointNum; i++)
	{
		referP.at<double>(0,0)=cloud->points[i].x;
		referP.at<double>(1,0)=cloud->points[i].y;
		referP.at<double>(2,0)=cloud->points[i].z;
		nwrMat=cv::Mat::zeros(cv::Size(3,3),CV_64FC1);
		//nn=0;
		for(int j=0; j<maxK; j++)
		{
			if(distKNN[i][j]!=0)
			{
				neighP.at<double>(0,0)=cloud->points[indexKNN[i][j]].x;
				neighP.at<double>(1,0)=cloud->points[indexKNN[i][j]].y;
				neighP.at<double>(2,0)=cloud->points[indexKNN[i][j]].z;
				diffM=neighP-referP;
				nwrSample=diffM*diffM.t();
				nwrSample=nwrSample/(distKNN[i][j]*distKNN[i][j]);
				nwrSample=nwrSample*exp(-(distKNN[i][j]*distKNN[i][j])/(2*sigma*sigma));
				nwrMat=nwrMat+nwrSample;
				//nn++;
			}
			/*if(nn==k)
			{
				break;
			}*/

		}
		//compute eigen value
		cv::Mat eigenvalue, eigenvector;
		cv::eigen(nwrMat, eigenvalue, eigenvector);
		//cout<<endl;
		//cout<<nwrMat<<endl;
		//cout<<endl;
		//cout<<eigenvalue<<endl;
		//cout<<endl;
		//cout<<eigenvector<<endl;
		normals.at<double>(i,0)=eigenvector.at<double>(2,0);
		normals.at<double>(i,1)=eigenvector.at<double>(2,1);
		normals.at<double>(i,2)=eigenvector.at<double>(2,2);
	}
	t[11]=clock();
	//printf("NormalEstimation: %lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
	return normals;
}

/*//Open 3D viewer and add point cloud
boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
  //viewer->addCoordinateSystem (1.0, "global");
  viewer->initCameraParameters ();
  return (viewer);
}*/

//implement one point ransac
cv::Mat OnePointRANSAC(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hori, cv::Mat horiPoints, int maxIteration, double p, double disThreshold, int fitNum, std::vector<int> &best_inliers, std::vector<int> isExist)
{
	int iter=0;
	//plane which ransac find: planeModel is the index of the point on the plane and the plane normal
	cv::Mat planeModel = cv::Mat::zeros(2, 3, CV_64FC1); //The first row is the point on the plane, the second row is the normal of the plane
	int inNum=0;
	std::vector<int> inliers;
	int randInd;
	double e;
	while(iter<maxIteration)
	{
		//randomly select one point from horizontal points
		//t[12]=clock();
		randInd=rand()%cloud_hori->points.size();
		inliers.clear();
		for(int i=0; i<cloud_hori->points.size(); i++)
		{
			if(i==randInd || isExist[i]==0)
			{
				continue;
			}
			//compute the distance between every point to the plane
			double x=cloud_hori->points[i].x-cloud_hori->points[randInd].x;
			double y=cloud_hori->points[i].y-cloud_hori->points[randInd].y;
			double z=cloud_hori->points[i].z-cloud_hori->points[randInd].z;
			double xn=horiPoints.at<double>(randInd,0);
			double yn=horiPoints.at<double>(randInd,1);
			double zn=horiPoints.at<double>(randInd,2);
			double norm_n=sqrt(xn*xn+yn*yn+zn*zn);
			double dis=abs(x*xn+y*yn+z*zn)/norm_n;
			if (dis<disThreshold)
			{
				inliers.push_back(i);
			}
		}
		if (inliers.size()>fitNum)
		{
			inliers.push_back(randInd);
			//cout<<"inliers number: "<<inliers.size()<<endl;
			//e=1-(double(inliers.size())/cloud->points.size());
			//cout<<"e: "<<e<<endl;
			//maxIteration=log(1-p)/log(1-(1-e));
			if (inliers.size()>inNum)
			{
				inNum=inliers.size();
				best_inliers=inliers;
				cout<<"inliers number: "<<inliers.size()<<endl;
				e=1-(double(inliers.size())/cloud_hori->points.size());
				cout<<"e: "<<e<<endl;
				maxIteration=log(1-p)/log(1-(1-e));
			}
		}
		//t[13]=clock();
		//printf("OnePointRANSAC one iteration time: %lf s\n",(double)(t[13]-t[12])/CLOCKS_PER_SEC);
		iter++;
	}
	cout<<"iteration times: "<<iter<<endl;
	cout<<"maxIteration: "<<maxIteration<<endl;
	//if do not find any plane
	if (inNum==0)
	{
		return planeModel;
	}
	//re-estimate the plane using all the inliers points
	cv::Mat pointsInlier(best_inliers.size(), 3,  CV_64FC1);
	double colSum[3]={0,0,0};
	for(int i=0; i<best_inliers.size(); i++)
	{
		pointsInlier.at<double>(i,0)=cloud_hori->points[best_inliers[i]].x;
		pointsInlier.at<double>(i,1)=cloud_hori->points[best_inliers[i]].y;
		pointsInlier.at<double>(i,2)=cloud_hori->points[best_inliers[i]].z;
		colSum[0]+=cloud_hori->points[best_inliers[i]].x;
		colSum[1]+=cloud_hori->points[best_inliers[i]].y;
		colSum[2]+=cloud_hori->points[best_inliers[i]].z;
	}
	cv::Mat u,w,v;
	SVD::compute(pointsInlier, w, u, v);
	//cout<<"w: "<<endl<<w<<endl;
	//cout<<"u: "<<endl<<u<<endl;
	//cout<<"v: "<<endl<<v.row(2)<<endl;
	//cout<<"mean of v: "<<endl<<mean(v)[0]<<endl;
	planeModel.at<double>(1,0)=v.at<double>(2,0);
	planeModel.at<double>(1,1)=v.at<double>(2,1);
	planeModel.at<double>(1,2)=v.at<double>(2,2);

	for(int i=0; i<3; i++)
	{
		planeModel.at<double>(0,i)=colSum[i]/best_inliers.size();
	}
	return planeModel;
}
//read point cloud from inputFile and find horizontal planes, then write results to outputFile
void FindHorizontalPlanes(char* inputFile, char* outputFile)
{
	//srand((unsigned)time(NULL));
	t[1]=clock();
	//ros::init(argc, argv, "read_points");
	//ros::NodeHandle n;
	//read 3D points from pcd file
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
	//cloud1 = ReadPCD("/home/victor/project_ws/src/ground_plane/data/home_data_ascii/scene1_ascii.pcd");
    cloud1 = ReadPCD(inputFile);
	//cloud1 = ReadPLY(inputFile);
	t[2]=clock();
	printf("%lf s\n",(double)(t[2]-t[1])/CLOCKS_PER_SEC);
	/*//write all the points in cloud to ply file
	ofstream myfile2("/home/victor/project_ws/src/ground_plane/src/allPoints_origin.ply", ios::out);
	if(!myfile2)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile2<<"ply"<<endl;
		myfile2<<"format ascii 1.0"<<endl;
		myfile2<<"element vertex "<<cloud1->points.size()<<endl;
		myfile2<<"property double x"<<endl;
		myfile2<<"property double y"<<endl;
		myfile2<<"property double z"<<endl;
		myfile2<<"property uchar red"<<endl;
		myfile2<<"property uchar green"<<endl;
		myfile2<<"property uchar blue"<<endl;
		myfile2<<"element face 0"<<endl;
		myfile2<<"end_header"<<endl;
		for(int i=0; i<cloud1->points.size(); i++)
		{
			myfile2<<cloud1->points[i].x<<" "<<cloud1->points[i].y<<" "<<cloud1->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile2.close();
	}*/

	//delete the nan points
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud = DeleteNAN(cloud1);
	t[3]=clock();
	printf("%lf s\n",(double)(t[3]-t[2])/CLOCKS_PER_SEC);
	int pointNum=cloud->points.size();
	//int pointNum=30;
	//cout the 3d points
	/*cout << "loaded " << cloud->width * cloud->height << " data points from box_cloud.pcd with the following fields: " << endl;
	for(int i=0; i<pointNum; i++)
	{
		cout<<" "<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<endl;
	}*/

	//write all the points in cloud to ply file
    char mkFile[100];
    sprintf(mkFile, "/home/victor/project_ws/src/ground_plane/src/%s/", outputFile);
    cout<<mkFile<<endl;
    int isCreate=mkdir(mkFile, S_IRWXU);
    if(!isCreate)
    {
        printf("Create Path:%s\n",mkFile);
    }
    else
    {
        printf("Create Path Failed!\n");
        return;
    }
    char outFile1[100];
    sprintf(outFile1, "/home/victor/project_ws/src/ground_plane/src/%s/allPoints.ply", outputFile);
	ofstream myfile1(outFile1, ios::out);
	if(!myfile1)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile1<<"ply"<<endl;
		myfile1<<"format ascii 1.0"<<endl;
		myfile1<<"element vertex "<<pointNum<<endl;
		myfile1<<"property double x"<<endl;
		myfile1<<"property double y"<<endl;
		myfile1<<"property double z"<<endl;
		myfile1<<"property uchar red"<<endl;
		myfile1<<"property uchar green"<<endl;
		myfile1<<"property uchar blue"<<endl;
		myfile1<<"element face 0"<<endl;
		myfile1<<"end_header"<<endl;
		for(int i=0; i<pointNum; i++)
		{
			myfile1<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile1.close();
	}

	t[4]=clock();
	printf("%lf s\n",(double)(t[4]-t[3])/CLOCKS_PER_SEC);
	//randomly pick some points from all the points
	/*std::vector<int> randPoints;
	double randRate=0.1;
	for(int i=0; i<pointNum; i++)
	{
		randPoints.push_back(i);
	}
	std::random_shuffle(randPoints.begin(), randPoints.end());
	pointNum=pointNum*randRate;*/
	//k nearest neighbor search
	int maxK=40;
  	int** indexKNN;
  	indexKNN=new int*[pointNum];
  	for(int i=0; i<pointNum; i++)
  	{
  		indexKNN[i]=new int[maxK];
	}
	double** distKNN;
	distKNN=new double*[pointNum];
	for(int i=0; i<pointNum; i++)
  	{
  		distKNN[i]=new double[maxK];
	}
	PointsKNN(cloud, pointNum, maxK, indexKNN, distKNN);

	t[5]=clock();
	printf("%lf s\n",(double)(t[5]-t[4])/CLOCKS_PER_SEC);

	/*cout<<endl;
	cout<<" "<<cloud->points[0].x<<" "<<cloud->points[0].y<<" "<<cloud->points[0].z<<endl;
	cout<<" "<<cloud->points[30000].x<<" "<<cloud->points[30000].y<<" "<<cloud->points[30000].z<<endl;
	cout<<" "<<cloud->points[10000].x<<" "<<cloud->points[10000].y<<" "<<cloud->points[10000].z<<endl;*/

	//normal estimation
	//cv::Mat normals(pointNum, 3, CV_64FC1);
	cv::Mat normals;
	normals=NormalEstimation(cloud, pointNum, maxK, indexKNN, distKNN);

	cout<<endl;
	cout<<"Number of normals: "<<normals.rows<<endl;
	cout<<"Number of points: "<<cloud->points.size()<<endl;

	t[6]=clock();
	printf("%lf s\n",(double)(t[6]-t[5])/CLOCKS_PER_SEC);

	//cout<<normals<<endl;

	//use kmeans to find the planes
	/*cv::Mat labels, centers;
	cv::Mat normals_32;
	normals.convertTo(normals_32, CV_32FC1);
	kmeans(normals_32, 3, labels, TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 1000, 0.1), 3, KMEANS_PP_CENTERS, centers);
	cout<<endl;
	cout<<labels<<endl;
	cout<<endl;
	cout<<centers<<endl;*/

	//find the points in horizontal plane
	//int normals_size=1;
	double ab, an, bn, cosr, interAngle;
	cv::Mat horiPoints;  //(1, 3, CV_64FC1); //the normals of the horizontal plane points
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hori (new pcl::PointCloud<pcl::PointXYZ>);
	//std::vector<int> indn;  //the index in normals
	std::vector<int> hori_ind; //the index in cloud for the horizontal points
	for(int i=0; i<normals.rows; i++)
	{
		//compute the angle between normal and yAxis direction (0,1,0)/zAxis direction (0,0,1)
		ab=normals.at<double>(i,2);
		an=1;
		bn=sqrt(normals.at<double>(i,0)*normals.at<double>(i,0)+normals.at<double>(i,1)*normals.at<double>(i,1)+normals.at<double>(i,2)*normals.at<double>(i,2));
		cosr=ab/(an*bn);
		//cout<<M_PI<<endl;
		//cout<<acos(cosr)*180/M_PI<<endl;
		interAngle=acos(cosr)*180/M_PI;
		/*if(isnan(interAngle))
		{
			cout<<"i="<<i<<endl;
			cout<<normals.at<double>(i,0)<<" "<<normals.at<double>(i,1)<<" "<<normals.at<double>(i,2)<<endl;
		}*/
		if(interAngle<=7 || interAngle>=173)
		{
			cv::Mat hpRow(1, 3, CV_64FC1);
			hpRow.at<double>(0,0)=normals.at<double>(i,0);
			hpRow.at<double>(0,1)=normals.at<double>(i,1);
			hpRow.at<double>(0,2)=normals.at<double>(i,2);
			cloud_hori->push_back(cloud->points[i]);
			hori_ind.push_back(i);
			//indn.push_back(i);
			//cout<<endl;
			//cout<<hpRow<<endl;
			horiPoints.push_back(hpRow);
		}
	}
	cout<<endl;
	cout<<horiPoints.rows<<endl;
	//cout<<horiPoints.row(0)<<endl;
	//cout<<horiPoints.row(1)<<endl;
	cout<<cloud_hori->points.size()<<endl;
	//cout<<indn.size()<<endl;
	//cout<<horiPoints<<endl;

	t[7]=clock();
	printf("%lf s\n",(double)(t[7]-t[6])/CLOCKS_PER_SEC);

	/*//write the normals into txt file
	char outFile3[100];
    sprintf(outFile3, "/home/victor/project_ws/src/ground_plane/src/%s/normals.txt", outputFile);
	ofstream myfile3(outFile3, ios::out);
	if(!myfile3)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		for(int i=0; i<horiPoints.rows; i++)
		{
			myfile3<<horiPoints.at<double>(i,0)<<" "<<horiPoints.at<double>(i,1)<<" "<<horiPoints.at<double>(i,2)<<endl;
		}
		myfile3.close();
	}*/

	//write the horizontal plane into ply file
    char outFile2[100];
    sprintf(outFile2, "/home/victor/project_ws/src/ground_plane/src/%s/horiPoints.ply", outputFile);
	ofstream myfile(outFile2, ios::out);
	if(!myfile)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile<<"ply"<<endl;
		myfile<<"format ascii 1.0"<<endl;
		myfile<<"element vertex "<<horiPoints.rows<<endl;
		myfile<<"property double x"<<endl;
		myfile<<"property double y"<<endl;
		myfile<<"property double z"<<endl;
		myfile<<"property uchar red"<<endl;
		myfile<<"property uchar green"<<endl;
		myfile<<"property uchar blue"<<endl;
		myfile<<"element face 0"<<endl;
		myfile<<"end_header"<<endl;
		for(int i=0; i<cloud_hori->points.size(); i++)
		{
			myfile<<cloud_hori->points[i].x<<" "<<cloud_hori->points[i].y<<" "<<cloud_hori->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile.close();
	}

	t[8]=clock();
	printf("%lf s\n",(double)(t[8]-t[7])/CLOCKS_PER_SEC);

	//use one point ransac to find horizontal plane
	std::vector<std::vector<int> > inliers;
	std::vector<cv::Mat> planeModel;
	int minNum=20;  //the minimum number of points in the plane
	double dis_thresh=0.01;  //threshold of the distance between point and plane or plane and plane
	std::vector<int> isExist(cloud_hori->points.size(), 1);
	//std::vector<int> isExist(cloud->points.size(), 1);

	while(accumulate(isExist.begin(), isExist.end(), 0)>minNum)
	{
		cv::Mat plane;  //first row is the point on the plane, second row is the normal
		std::vector<int> inl;
		//t[10]=clock();
		cout<<"isExist number: "<<endl<<accumulate(isExist.begin(), isExist.end(), 0)<<endl;
		//t[11]=clock();
		//printf("%lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
		//t[10]=clock();
		plane=OnePointRANSAC(cloud_hori, horiPoints, horiPoints.rows, 0.99, dis_thresh, minNum, inl, isExist);
		//t[11]=clock();
		//printf("OnePointRANSAC time: %lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
		if (countNonZero(plane)!=0)
		{
			planeModel.push_back(plane);
			inliers.push_back(inl);
			for(int i=0; i<inl.size(); i++)
			{
				isExist[inl[i]]=0;
			}
		}
		else
		{
			break;
		}
	}
	cout<<"planeModel size: "<<endl<<planeModel.size()<<endl;
	//aggregate the planes which should be the same one
	/*std::vector<std::vector<int> > inliers_fi;
	std::vector<cv::Mat> planeModel_fi;
	std::vector<int> indPlane;
	double disPlane;
	for(int i=0; i<planeModel.size(); i++)
	{
		indPlane.push_back(i);
	}
	for(int i=0; i<planeModel.size(); i++)
	{
		if (indPlane[i]!=i)
			continue;
		for(int j=i+1; j<planeModel.size(); j++)
		{
			if (indPlane[j]!=j)
				continue;
			//compute the offset of plane i
			double xn_i=planeModel[i].at<double>(1,0);
			double yn_i=planeModel[i].at<double>(1,1);
			double zn_i=planeModel[i].at<double>(1,2);
			double x_i=planeModel[i].at<double>(0,0);
			double y_i=planeModel[i].at<double>(0,1);
			double z_i=planeModel[i].at<double>(0,2);
			double os_i=-(xn_i*x_i+yn_i*y_i+zn_i*z_i);

			//compute the offset of plane j
			double xn_j=planeModel[j].at<double>(1,0);
			double yn_j=planeModel[j].at<double>(1,1);
			double zn_j=planeModel[j].at<double>(1,2);
			double x_j=planeModel[j].at<double>(0,0);
			double y_j=planeModel[j].at<double>(0,1);
			double z_j=planeModel[j].at<double>(0,2);
			double os_j=-(xn_j*x_j+yn_j*y_j+zn_j*z_j);

			disPlane=abs(planeModel[i].at<double>(0,2)-planeModel[j].at<double>(0,2));
			cout<<"i="<<i<<" j="<<j<<" distance: "<<disPlane<<endl;
			if (disPlane<dis_thresh)
			{
				indPlane[j]=indPlane[i];
			}
		}
	}
	for(int i=0; i<indPlane.size(); i++)
	{
		cout<<"indPlane["<<i<<"]= "<<indPlane[i]<<endl;
	}
	for(int i=0; i<indPlane.size(); i++)
	{
		if(indPlane[i]==-1)
		{
			continue;
		}
		std::vector<int> vecfi;
		vecfi.insert(vecfi.end(), inliers[i].begin(), inliers[i].end());
		cv::Mat pm(2, 3, CV_64FC1);
		pm.at<double>(0,0)=planeModel[i].at<double>(0,0)*inliers[i].size();
		pm.at<double>(0,1)=planeModel[i].at<double>(0,1)*inliers[i].size();
		pm.at<double>(0,2)=planeModel[i].at<double>(0,2)*inliers[i].size();
		int n=inliers[i].size();
		//pm.at<double>(1,0)=planeModel[i].at<double>(1,0);
		//pm.at<double>(1,1)=planeModel[i].at<double>(1,1);
		//pm.at<double>(1,2)=planeModel[i].at<double>(1,2);
		for(int j=i+1; j<indPlane.size(); j++)
		{
			if (indPlane[j]==indPlane[i])
			{
				pm.at<double>(0,0)=pm.at<double>(0,0)+planeModel[j].at<double>(0,0)*inliers[j].size();
				pm.at<double>(0,1)=pm.at<double>(0,1)+planeModel[j].at<double>(0,1)*inliers[j].size();
				pm.at<double>(0,2)=pm.at<double>(0,2)+planeModel[j].at<double>(0,2)*inliers[j].size();
				n+=inliers[j].size();
				vecfi.insert(vecfi.end(), inliers[j].begin(), inliers[j].end());
				indPlane[j]=-1;
			}
		}
		pm.at<double>(0,0)=pm.at<double>(0,0)/n;
		pm.at<double>(0,1)=pm.at<double>(0,1)/n;
		pm.at<double>(0,2)=pm.at<double>(0,2)/n;
		cv::Mat pointsIn(vecfi.size(), 3, CV_64FC1);
		for(int j=0; j<vecfi.size(); j++)
		{
			pointsIn.at<double>(j,0)=cloud->points[vecfi[j]].x;
			pointsIn.at<double>(j,1)=cloud->points[vecfi[j]].y;
			pointsIn.at<double>(j,2)=cloud->points[vecfi[j]].z;
		}
		cv::Mat u0,w0,v0;
		SVD::compute(pointsIn, w0, u0, v0);
		pm.at<double>(1,0)=v0.at<double>(2,0);
		pm.at<double>(1,1)=v0.at<double>(2,1);
		pm.at<double>(1,2)=v0.at<double>(2,2);
		planeModel_fi.push_back(pm);
		inliers_fi.push_back(vecfi);
		indPlane[i]=-1;
	}
	cout<<"After aggregate planeModel size: "<<endl<<planeModel_fi.size()<<endl;*/
	t[9]=clock();
	printf("%lf s\n", (double)(t[9]-t[8])/CLOCKS_PER_SEC);
	printf("Finding planes time: %lf s\n", (double)(t[9]-t[4])/CLOCKS_PER_SEC);

	//assign all the points in the point cloud to the nearest plane(distance less than threshold) and get final inliers
	//std::vector<std::vector<int> > inliers_final;
	//std::vector<cv::Mat> planeModel_final;
	std::vector<int> isUsed(cloud->points.size(), 0);
	for(int i=0; i<inliers.size(); i++)
	{
		for(int j=0; j<inliers[i].size(); j++)
		{
			inliers[i][j]=hori_ind[inliers[i][j]];//transfer the cloud_hori index to cloud index
			isUsed[inliers[i][j]]=1;
		}
	}

	for(int i=0; i<cloud->points.size(); i++)
	{
		if(isUsed[i]==1)
		{
			continue;
		}
		std::vector<double> dis_in(inliers.size(), 0);
		double min_dis;
		int min_ind;
		for(int j=0; j<inliers.size(); j++)
		{
			//compute the distance between every point to the inliers_fi planes
			//double x=cloud->points[i].x-planeModel_fi[j].at<double>(0,0);
			//double y=cloud->points[i].y-planeModel_fi[j].at<double>(0,1);
			//double z=cloud->points[i].z-planeModel_fi[j].at<double>(0,2);
			//double xn=planeModel_fi[j].at<double>(1,0);
			//double yn=planeModel_fi[j].at<double>(1,1);
			//double zn=planeModel_fi[j].at<double>(1,2);
			//double norm_n=sqrt(xn*xn+yn*yn+zn*zn);
			//dis_in[j]=abs(x*xn+y*yn+z*zn)/norm_n;
			dis_in[j]=abs(cloud->points[i].z-planeModel[j].at<double>(0,2));
			//cout<<"dis_in["<<j<<"]= "<<dis_in[j]<<endl;
			if (j==0)
			{
				min_dis=dis_in[j];
				min_ind=j;
			}
			else
			{
				if (dis_in[j]<min_dis)
				{
					min_dis=dis_in[j];
					min_ind=j;
				}
			}
		}
		if (min_dis<dis_thresh)
		{
			inliers[min_ind].push_back(i);
		}
		isUsed[i]=1;
	}
	//re-compute the normals and center point of the points in inliers_fi
	for(int i=0; i<inliers.size(); i++)
	{
		//cout<<"i= "<<i<<endl;
		cv::Mat pointsInlier(inliers[i].size(), 3,  CV_64FC1);
		double colSum[3]={0,0,0};
		for(int j=0; j<inliers[i].size(); j++)
		{
			//cout<<"j= "<<j<<"/"<<inliers_fi[i].size()<<endl;
			pointsInlier.at<double>(j,0)=cloud->points[inliers[i][j]].x;
			pointsInlier.at<double>(j,1)=cloud->points[inliers[i][j]].y;
			pointsInlier.at<double>(j,2)=cloud->points[inliers[i][j]].z;
			colSum[0]+=pointsInlier.at<double>(j,0);
			colSum[1]+=pointsInlier.at<double>(j,1);
			colSum[2]+=pointsInlier.at<double>(j,2);
		}
		cv::Mat u,w,v;
		SVD::compute(pointsInlier, w, u, v);
		planeModel[i].at<double>(1,0)=v.at<double>(2,0);
		planeModel[i].at<double>(1,1)=v.at<double>(2,1);
		planeModel[i].at<double>(1,2)=v.at<double>(2,2);

		for(int j=0; j<3; j++)
		{
			planeModel[i].at<double>(0,j)=colSum[j]/inliers[i].size();
		}
	}
	t[10]=clock();

	//write the horizontal plane into ply file and store the normals of each plane
	int num_allpoints=0;
	for(int i=0; i<inliers.size(); i++)
	{
		num_allpoints+=inliers[i].size();
		cout<<"inliers size: "<<endl<<inliers[i].size()<<endl;
		//write the horizontal plane into ply file
		char outPath[100];
		sprintf(outPath, "/home/victor/project_ws/src/ground_plane/src/%s/ransacPlane%d.ply", outputFile, i);
		ofstream myfile0(outPath, ios::out);
		if(!myfile0)
		{
			cout<<"error!"<<endl;
		}
		else
		{
			myfile0<<"ply"<<endl;
			myfile0<<"format ascii 1.0"<<endl;
			myfile0<<"element vertex "<<inliers[i].size()<<endl;
			myfile0<<"property double x"<<endl;
			myfile0<<"property double y"<<endl;
			myfile0<<"property double z"<<endl;
			myfile0<<"property uchar red"<<endl;
			myfile0<<"property uchar green"<<endl;
			myfile0<<"property uchar blue"<<endl;
			myfile0<<"element face 0"<<endl;
			myfile0<<"end_header"<<endl;
			for(int j=0; j<inliers[i].size(); j++)
			{
				myfile0<<cloud->points[inliers[i][j]].x<<" "<<cloud->points[inliers[i][j]].y<<" "<<cloud->points[inliers[i][j]].z<<" "<<"255 0 0"<<endl;
			}
			myfile0.close();

		}
		//store the normals and center point of each plane
		char outPath1[100];
		sprintf(outPath1, "/home/victor/project_ws/src/ground_plane/src/%s/normal%d.txt", outputFile, i);
		ofstream myfile2(outPath1, ios::out);
		if(!myfile2)
		{
			cout<<"error!"<<endl;
		}
		else
		{
			myfile2<<planeModel[i].at<double>(1,0)<<" "<<planeModel[i].at<double>(1,1)<<" "<<planeModel[i].at<double>(1,2)<<endl;
			myfile2<<planeModel[i].at<double>(0,0)<<" "<<planeModel[i].at<double>(0,1)<<" "<<planeModel[i].at<double>(0,2)<<endl;
		}
		myfile2.close();
	}

	//write all the horizontal planes into one ply file with different color
	/*int colorValue[3]={50, 150, 255};
	int RGBValue[27][3];
	int rgbn=0;
	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			for (int k=0; k<3; k++)
			{
				RGBValue[rgbn][0]=colorValue[i];
				RGBValue[rgbn][1]=colorValue[j];
				RGBValue[rgbn][2]=colorValue[k];
				rgbn++;
			}
		}
	}*/

	//read color from "rgb.txt" which is generated by jet in matlab
	int RGBValue[200][3];
	int rgbn=0;
    char inPath[100]="/home/victor/project_ws/src/ground_plane/src/rgb.txt";
	ifstream infile(inPath, ios::in);
	if(!infile)
	{
		cout<<"error reading rgb.txt!"<<endl;
	}
	else
	{
		for(int i=0; i<200; i++)
		{
			infile>>RGBValue[i][0]>>RGBValue[i][1]>>RGBValue[i][2];
		}
		infile.close();
	}

	//write the horizontal plane into ply file
	char outPath[100];
	sprintf(outPath, "/home/victor/project_ws/src/ground_plane/src/%s/ransacPlane(num:%d).ply", outputFile, int(inliers.size()));
	ofstream myfile0(outPath, ios::out);
	if(!myfile0)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile0<<"ply"<<endl;
		myfile0<<"format ascii 1.0"<<endl;
		myfile0<<"element vertex "<<num_allpoints<<endl;
		myfile0<<"property double x"<<endl;
		myfile0<<"property double y"<<endl;
		myfile0<<"property double z"<<endl;
		myfile0<<"property uchar red"<<endl;
		myfile0<<"property uchar green"<<endl;
		myfile0<<"property uchar blue"<<endl;
		myfile0<<"element face 0"<<endl;
		myfile0<<"end_header"<<endl;
		for (int i=0; i<inliers.size(); i++)
		{
			for(int j=0; j<inliers[i].size(); j++)
			{
				myfile0<<cloud->points[inliers[i][j]].x<<" "<<cloud->points[inliers[i][j]].y<<" "<<cloud->points[inliers[i][j]].z<<" "<<RGBValue[i][0]<<" "<<RGBValue[i][1]<<" "<<RGBValue[i][2]<<endl;
			}
		}
		myfile0.close();

	}

	//write the running time into txt file
	char outPath2[100];
	sprintf(outPath2, "/home/victor/project_ws/src/ground_plane/src/%s/time.txt", outputFile);
	ofstream myfile3(outPath2, ios::out);
	if(!myfile3)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile3<<(double)(t[9]-t[2])/CLOCKS_PER_SEC<<" "<<"s"<<endl;
		myfile3.close();

	}

	t[11]=clock();
	//printf("%lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
	printf("Find Horizontal Planes Time: %lf s\n", (double)(t[9]-t[2])/CLOCKS_PER_SEC);
	printf("Total Time: %lf s\n",(double)(t[11]-t[1])/CLOCKS_PER_SEC);

	/*//use ransac in pcl to find horizontal plane
	std::vector<int> inliers;
	pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud_hori));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
	ransac.setDistanceThreshold (.01);
	ransac.computeModel();
	ransac.getInliers(inliers);
	//copies all inliers of the model computed to another PointCloud
  	pcl::copyPointCloud<pcl::PointXYZ>(*cloud_hori, inliers, *final);
	//visualize the inliers
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = simpleVis(final);
	while (!viewer->wasStopped ())
  	{
    	viewer->spinOnce (100);
    	boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  	}*/
	//return 0;
}
//get all the files in path
void getFiles(char* dir_name, vector<string> &files)
{
    // check the parameter
    if(dir_name == NULL)
    {
        cout<<"dir_name is null !"<<endl;
        return;
    }

    // check if dir_name is a valid dir
    struct stat s;
    lstat(dir_name, &s);
    if(!S_ISDIR(s.st_mode))
    {
        cout<<"dir_name is not a valid directory!"<<endl;
        return;
    }

    struct dirent* filename;    // return value for readdir()
    DIR* dir;                   // return value for opendir()
    dir = opendir(dir_name);
    if(dir == NULL)
    {
        cout<<"Can not open dir "<<dir_name<<endl;
        return;
    }
    cout<<"Successfully opened the dir!"<<endl;

    //read all the files in the dir
    while((filename = readdir(dir)) != NULL)
    {
        // get rid of "." and ".."
        if( strcmp(filename->d_name, ".") == 0 ||
            strcmp(filename->d_name, "..") == 0 )
            continue;
        //cout<<filename->d_name<<endl;
        files.push_back(filename->d_name);
    }
}
int main(int argc, char **argv)
{
    srand((unsigned)time(NULL));
    ros::init(argc, argv, "read_points");
	ros::NodeHandle n;
    string folderName="home_data_ascii";
    //string folderName="office_data_ascii";
	//string folderName="rgbd-scenes-v2";
    char mkFile[100];
    sprintf(mkFile, "/home/victor/project_ws/src/ground_plane/src/%s/", folderName.c_str());
    cout<<mkFile<<endl;
    int isCreate=mkdir(mkFile, S_IRWXU);
    if(!isCreate)
    {
        printf("Create Path:%s\n",mkFile);
    }
    else
    {
        printf("Create Path Failed!\n");
        return 0;
    }
    char filePath[100];
    sprintf(filePath, "/home/victor/project_ws/src/ground_plane/data/%s", folderName.c_str());
    vector<string> files;
    getFiles(filePath, files);
    for(int i=0; i<files.size(); i++)
    {
        cout<<files[i].c_str()<<endl;
        char inPath[100];
        sprintf(inPath, "/home/victor/project_ws/src/ground_plane/data/%s/%s", folderName.c_str(), files[i].c_str());
        char outPath[100];
        string fname;
        fname.assign(files[i].begin(), files[i].end()-4);
        sprintf(outPath, "%s/%s", folderName.c_str(), fname.c_str());
        FindHorizontalPlanes(inPath, outPath);
    }
    return 0;
}

ext: cpp

max_stars_repo_name: JoeyYilinWang/Oriented-Point-Sampling

max_issues_repo_name: JoeyYilinWang/Oriented-Point-Sampling

max_forks_repo_name: JoeyYilinWang/Oriented-Point-Sampling


text:
#include <boost/asio.hpp>
#include <boost/asio/ssl.hpp>
#include <boost/connector/entity/entity_cache.hpp>
#include <boost/connector/order_book.hpp>
#include <boost/connector/property_value.hpp>
#include <boost/connector/vendor/ftx/interface/ftx_websocket_connector_concept.hpp>
#include <boost/connector/vendor/ftx/order_book_impl.hpp>
#include <boost/core/demangle.hpp>

#include <iostream>
#include <unordered_set>

namespace asio = boost::asio;
using namespace std::literals;
namespace connector = boost::connector;

namespace boost::connector
{
struct any_set_layer
{
    any_set_layer(std::shared_ptr< any_set_layer const > parent = nullptr)
    : parent_(std::move(parent))
    , map_()
    {
    }

    template < class T >
    T const *
    query() const
    {
        auto i = map_.find(typeid(T));
        if (i == map_.end())
            if (parent_)
                return parent_->query< T >();
            else
                return nullptr;
        else
            return std::any_cast< T >(std::addressof(i->second));
    }

    template < class T >
    T const &
    require() const
    try
    {
        return std::any_cast< T >(map_.at(typeid(T)));
    }
    catch (std::exception &)
    {
        std::ostringstream ss;
        ss << "any_set: " << boost::core::demangled_name(typeid(T))
           << " not found";
        std::throw_with_nested(std::invalid_argument(std::move(ss).str()));
    }

    template < class T >
    void
    set(T &&x)
    {
        using type = std::decay_t< T >;
        auto i     = map_.find(typeid(type));
        if (i == map_.end())
            map_.template emplace_hint(i, typeid(type), std::forward< T >(x));
        else
            i->second = std::forward< T >(x);
    }

    std::shared_ptr< any_set_layer const >          parent_;
    std::unordered_map< std::type_index, std::any > map_;
};

struct mutable_any_set
{
    mutable_any_set()
    : impl_(std::make_shared< any_set_layer >())
    {
    }

    mutable_any_set(any_set_layer &&my_layer)
    : impl_(std::make_shared< any_set_layer >(std::move(my_layer)))
    {
    }

    template < class T >
    mutable_any_set &
    set(T &&x)
    {
        impl_->set(std::forward< T >(x));
        return *this;
    }

    std::shared_ptr< any_set_layer >
    impl_ptr() &&
    {
        return std::move(impl_);
    }

    template < class T >
    T const *
    query() const
    {
        return impl_->template query< T >();
    }

    template < class T >
    T const &
    require() const
    {
        return impl_->template require< T >();
    }

  private:
    std::shared_ptr< any_set_layer > impl_;
};

struct any_set
{
    any_set(mutable_any_set &&mset)
    : impl_(std::move(mset).impl_ptr())
    {
    }

    mutable_any_set
    mutate()
    {
        return mutable_any_set(any_set_layer(impl_));
    }

    std::shared_ptr< any_set_layer const > impl_ = nullptr;
};

struct any_map_layer
{
    using map_type = std::unordered_map< std::string, std::any >;
    map_type map_;
};

struct mutable_any_map
{
};

struct any_map
{
    mutable_any_map
    mutate() const;
};

template < class Interface >
struct interface_tag
{
};

struct order_book_handle
{
    /// Construct an order_book from a context
    template < class T >
    order_book_handle(interface_tag< T > tag,
                      entity_context     context,
                      std::string        base,
                      std::string        term)
    : lifetime_(
          acquire(tag, std::move(context), std::move(base), std::move(term)))
    {
    }

  private:
    template < class T >
    std::shared_ptr< interface::order_book_concept >
    acquire(interface_tag< T > tag,
            entity_context    &context,
            std::string        base,
            std::string        term)
    {
        context.params(fix(mutate(context.params())
                               .set("base", std::move(base))
                               .set("term", std::move(base))));
    }

  private:
    std::shared_ptr< interface::order_book_concept > lifetime_;
};
}   // namespace boost::connector

int
main()
{
    namespace connector = boost::connector;

    auto m  = connector::property_map();
    auto m2 = fix(mutate(m).set("base", "USD"));
    auto a  = connector::property_value(std::string("USD"));

    std::cout << "before" << std::endl;
    std::cout << a << std::endl;
    std::cout << hash_value(a) << std::endl;
    a = std::move(a);
    std::cout << "after" << std::endl;
    std::cout << a << std::endl;
    std::cout << hash_value(a) << std::endl;

    std::unordered_set< connector::property_value,
                        boost::hash< connector::property_value > >
        my_s;
    my_s.emplace(std::move(a));

    std::exit(0);

    std::cout << "Hello, World!\n";

    auto ioc = boost::asio::io_context();
    auto sslctx =
        boost::asio::ssl::context(boost::asio::ssl::context::tls_client);

    auto s         = asio::ssl::stream< asio::ip::tcp::socket >(ioc, sslctx);
    s.next_layer() = asio::ip::tcp::socket(ioc);

    auto mdeps = boost::connector::mutable_dependency_map();
    mdeps.set("io_context", std::addressof(ioc));
    mdeps.set("client_ssl_context", std::addressof(sslctx));

    auto context = boost::connector::entity_context();
    context.set_dependencies(fix(std::move(mdeps)));

    asio::co_spawn(
        ioc.get_executor(),
        [&sslctx]() -> asio::awaitable< void >
        {
            using namespace connector;
            auto key =
                ftx_websocket_key { .url = "wss://ftx.com/ws/", .auth = {} };
            auto ftxconn = vendor::ftx::websocket_connector(
                make_lifetime_ptr<
                    vendor::ftx::ftx_websocket_connector_concept >(
                    co_await asio::this_coro::executor, sslctx, key));

            auto btcs = std::vector< order_book >();
            auto eths = std::vector< order_book >();
            for (int i = 0; i < 10; ++i)
            {
                btcs.emplace_back(
                    make_lifetime_ptr< vendor::ftx::order_book_impl >(
                        ftxconn, "BTC/USD"));
                eths.emplace_back(
                    make_lifetime_ptr< vendor::ftx::order_book_impl >(
                        ftxconn, "ETH/USD"));
            }

            auto t = asio::steady_timer(co_await asio::this_coro::executor);
            for (int i = 0; i < 10; ++i)
            {
                t.expires_after(5s);
                co_await t.async_wait(asio::use_awaitable);
                btcs[i].reset();
                eths[i].reset();
            }
        },
        asio::detached);

    ioc.run();
}


text:
// Copyright 2019-2022 Cambridge Quantum Computing
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#if !defined(_MSC_VER)
#pragma GCC diagnostic push

#if defined(__clang__)
#pragma GCC diagnostic ignored "-Wunknown-warning-option"
#if __has_warning("-Wdeprecated-copy")
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#else
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif

#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#pragma GCC diagnostic ignored "-Wuninitialized"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/sequential_vertex_coloring.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>

#if !defined(_MSC_VER)
#pragma GCC diagnostic pop
#endif

ext: hpp

max_stars_repo_name: vtomole/tket

max_issues_repo_name: vtomole/tket

max_forks_repo_name: vtomole/tket


text:
// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the linear model predictive
    control tool from the dlib C++ Library.  To explain what it does, suppose
    you have some process you want to control and the process dynamics are
    described by the linear equation:
        x_{i+1} = A*x_i + B*u_i + C
    That is, the next state the system goes into is a linear function of its
    current state (x_i) and the current control (u_i) plus some constant bias or
    disturbance.

    A model predictive controller can find the control (u) you should apply to
    drive the state (x) to some reference value, which is what we show in this
    example.  In particular, we will simulate a simple vehicle moving around in
    a planet's gravity.  We will use MPC to get the vehicle to fly to and then
    hover at a certain point in the air.

*/

#include <dlib/gui_widgets.h>
#include <dlib/control.h>
#include <dlib/image_transforms.h>

using namespace std;
using namespace dlib;

//  ----------------------------------------------------------------------------

int main()
{
    const int STATES = 4;
    const int CONTROLS = 2;

    // The first thing we do is setup our vehicle dynamics model (A*x + B*u + C).
    // Our state space (the x) will have 4 dimensions, the 2D vehicle position
    // and also the 2D velocity.  The control space (u) will be just 2 variables
    // which encode the amount of force we apply to the vehicle along each axis.
    // Therefore, the A matrix defines a simple constant velocity model.
    matrix<double,STATES,STATES> A;
    A = 1, 0, 1, 0,  // next_pos = pos + velocity
        0, 1, 0, 1,  // next_pos = pos + velocity
        0, 0, 1, 0,  // next_velocity = velocity
        0, 0, 0, 1;  // next_velocity = velocity

    // Here we say that the control variables effect only the velocity. That is,
    // the control applies an acceleration to the vehicle.
    matrix<double,STATES,CONTROLS> B;
    B = 0, 0,
        0, 0,
        1, 0,
        0, 1;

    // Let's also say there is a small constant acceleration in one direction.
    // This is the force of gravity in our model.
    matrix<double,STATES,1> C;
    C = 0,
        0,
        0,
        0.1;

    const int HORIZON = 30;
    // Now we need to setup some MPC specific parameters.  To understand them,
    // let's first talk about how MPC works.  When the MPC tool finds the "best"
    // control to apply it does it by simulating the process for HORIZON time
    // steps and selecting the control that leads to the best performance over
    // the next HORIZON steps.
    //
    // To be precise, each time you ask it for a control, it solves the
    // following quadratic program:
    //
    //     min     sum_i trans(x_i-target_i)*Q*(x_i-target_i) + trans(u_i)*R*u_i
    //    x_i,u_i
    //
    //     such that: x_0     == current_state
    //                x_{i+1} == A*x_i + B*u_i + C
    //                lower <= u_i <= upper
    //                0 <= i < HORIZON
    //
    // and reports u_0 as the control you should take given that you are currently
    // in current_state.  Q and R are user supplied matrices that define how we
    // penalize variations away from the target state as well as how much we want
    // to avoid generating large control signals.  We also allow you to specify
    // upper and lower bound constraints on the controls.  The next few lines
    // define these parameters for our simple example.

    matrix<double,STATES,1> Q;
    // Setup Q so that the MPC only cares about matching the target position and
    // ignores the velocity.
    Q = 1, 1, 0, 0;

    matrix<double,CONTROLS,1> R, lower, upper;
    R = 1, 1;
    lower = -0.5, -0.5;
    upper =  0.5,  0.5;

    // Finally, create the MPC controller.
    mpc<STATES,CONTROLS,HORIZON> controller(A,B,C,Q,R,lower,upper);

    // Let's tell the controller to send our vehicle to a random location.  It
    // will try to find the controls that makes the vehicle just hover at this
    // target position.
    dlib::rand rnd;
    matrix<double,STATES,1> target;
    target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
    controller.set_target(target);

    // Now let's start simulating our vehicle.  Our vehicle moves around inside
    // a 400x400 unit sized world.
    matrix<rgb_pixel> world(400,400);
    image_window win;
    matrix<double,STATES,1> current_state;
    // And we start it at the center of the world with zero velocity.
    current_state = 200,200,0,0;

    int iter = 0;
    while(!win.is_closed())
    {
        // Find the best control action given our current state.
        matrix<double,CONTROLS,1> action = controller(current_state);
        cout << "best control: " << trans(action);

        // Now draw our vehicle on the world.  We will draw the vehicle as a
        // black circle and its target position as a green circle.
        assign_all_pixels(world, rgb_pixel(255,255,255));
        const dpoint pos = point(current_state(0),current_state(1));
        const dpoint goal = point(target(0),target(1));
        draw_solid_circle(world, goal, 9, rgb_pixel(100,255,100));
        draw_solid_circle(world, pos, 7, 0);
        // We will also draw the control as a line showing which direction the
        // vehicle's thruster is firing.
        draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
        win.set_image(world);

        // Take a step in the simulation
        current_state = A*current_state + B*action + C;
        dlib::sleep(100);

        // Every 100 iterations change the target to some other random location.
        ++iter;
        if (iter > 100)
        {
            iter = 0;
            target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
            controller.set_target(target);
        }
    }
}

//  ----------------------------------------------------------------------------

max_stars_repo_name: SyllogismRXS/misc

max_issues_repo_name: SyllogismRXS/misc

max_forks_repo_name: SyllogismRXS/misc


text:
/*! \file demo_1d_containers.cpp
  \brief An example to demonstrate simple 1D plotting using a range of different STL containers.
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A Bristow 2008, 2012, 2021

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_1d_containers_1

/*`First a few includes to use Boost.Plot and various STL containers:
*/
#include <boost/svg_plot/svg_1d_plot.hpp>
  // using namespace boost::svg;
#include <boost/array.hpp>
  using boost::array;
#include <vector>
  using std::vector;
#include <set>
  using std::set; // Automatically sorted - though this is not useful to the plot process.
  using std::multiset; // At present using std::multiset, allowing duplicates, plot does not indicate duplicates.
  // With 2_D data in std::multimap, duplicate values are usefully displayed.
#include <list>
  using std::list;
#include <deque>
  using std::deque;

//] [/demo_1d_containers_1]

int main()
{
  {
//[demo_1d_containers_2

/*`STL vector is used as the container,
and fictional values are inserted using push_back.
Since this is a 1-D plot the order of data values is not important.
*/
  std::vector<float> values;
  values.push_back(3.1f);
  values.push_back(-5.5f);
  values.push_back(8.7f);
  values.push_back(0.5f);

/*`The constructor initializes a new 1D plot, called `my_plot`, and also sets all the many default values.
*/

  using namespace boost::svg;
  boost::svg::svg_1d_plot my_plot;

/*`Setting (member) functions are fairly self-explanatory:
Title provides a title at the top for the whole plot,
and plot adds a (unnamed) data series (naming isn't very useful if there is only one data series).
*/
  my_plot.title("vector<float> example");
/*`
[note One must insert the XML character entity equivalents of < for < and > for >).]
*/
  my_plot.plot(values);

/*`Write the SVG to a file.
*/
  my_plot.write("./demo_1d_vector_float.svg");
//] [/demo_1d_containers_2]
  }

  {
//[demo_1d_containers_3

  static boost::array<const long double, 4> values {3.1L,-5.5L, 8.7L, 0.5L};
  //! \note Using @c long double provokes an expected warning about conversion from 'T' to 'const double', possible loss of data.
  boost::svg::svg_1d_plot my_plot;
  my_plot.title("array<long double> example");
  my_plot.plot(values);
  my_plot.write("./demo_1d_array_long_double.svg");
//] [/demo_1d_containers_3]
  }

  {
//[demo_1d_containers_4
/*`If the container type is a set, then it can be filled with insert:*/
  std::set<double> values;
  values.insert(-8.4);
  values.insert(-2.3);
  values.insert(0.1);
  values.insert(5.6);
  values.insert(7.8);

  boost::svg::svg_1d_plot my_plot;
  my_plot.title("set<double> example");
  my_plot.plot(values);
  my_plot.write("./demo_1d_set_double.svg");
//] [/demo_1d_containers_4]
  }

  {
//[demo_1d_containers_5
/*`If the container type is a list, then it can be filled with push_back or push_front:*/
  std::list<double> values;
  values.push_back(-8.4);
  values.push_back(-2.3);
  values.push_back(0.1);
  values.push_back(5.6);
  values.push_back(7.8);
  boost::svg::svg_1d_plot my_plot;
  my_plot.title("list<double> example");
  my_plot.plot(values);
  my_plot.write("./demo_1d_list_double.svg");
//] [/demo_1d_containers_5]
  }

  {
//[demo_1d_containers_6
/*`If the container type is a deque, then it can be filled with push_back or push_front:*/
  std::deque<double> values;
  values.push_front(-8.4);
  values.push_front(-2.3);
  values.push_front(0.1);
  values.push_front(5.6);
  values.push_front(7.8);

  boost::svg::svg_1d_plot my_plot;
  my_plot.title("deque<double> example");
  my_plot.plot(values);
  my_plot.x_label("X values as doubles");

  my_plot.write("./demo_1d_deque_double.svg");
//] [/demo_1d_containers_6]
  }
  return 0;
} // int main()

/*
Sample output:
//[demo_1d_containers_output

demo_1d_containers.cpp
demo_1d_containers.vcxproj -> I:\Cpp\SVG_plot\svg_plot\x64\Release\demo_1d_containers.exe
Autorun "I:\Cpp\SVG_plot\svg_plot\x64\Release\demo_1d_containers.exe"
Plot written to file ./demo_1d_vector_float.svg.
Plot written to file ./demo_1d_array_long_double.svg.
Plot written to file ./demo_1d_set_double.svg.
Plot written to file ./demo_1d_list_double.svg.
Plot written to file ./demo_1d_deque_double.svg.
//] [/demo_1d_containers_output]
*/


text:
#include "MyXAODTools/Helper.h"

#include <string>
#include <fstream>
#include <iostream>
#include <sstream>
#include <boost/algorithm/string.hpp>

#include <TString.h>
#include <TVector2.h>

using namespace MyXAODTools;
using namespace std;

TChain* Helper::loader(const char* inFile_name, const char* chain_name)
{
    TChain* chain = new TChain(chain_name);
    TString in_name(inFile_name);
    if(in_name.Contains("root")) {
        chain->Add(inFile_name);
        return chain;
    }
    fstream input(inFile_name, fstream::in);
    string file_name;
    int ncounter = 0;
    while (input >> file_name){
        // cout << "adding: " << file_name << endl;
        if (file_name.at(0) == '#') continue;
        chain->Add(file_name.c_str());
        ncounter ++;
    }
    cout << "total events: " << chain->GetEntries() << " in " << ncounter << " files." << endl;
    input.close();
    return chain;
}

float MyXAODTools::delta_r(float eta1, float phi1, float eta2, float phi2)
{
    double dphi = fabs(TVector2::Phi_mpi_pi( TVector2::Phi_mpi_pi(phi1) - TVector2::Phi_mpi_pi(phi2) ));
    double deta = eta1 - eta2;
    return sqrt(deta * deta + dphi * dphi);
}

void MyXAODTools::tokenizeString(const string& str, char delim, vector<string>& tokens)
{
    tokens.clear();
    istringstream iss(str);
    string token;
    while ( getline(iss, token, delim) ){
        boost::algorithm::trim(token);
        tokens.push_back(token);
    }
}

ext: cxx


text:
#ifndef NWNRULES_H
#define NWNRULES_H

#include <unordered_map>
#include <memory>

#include <Nwn/base.hpp>
#include <Nwn/constmaplooper.hpp>

#include <boost/serialization/nvp.hpp>
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/unique_ptr.hpp>

namespace Nwn {

class Race;
class Character;
class AblBlock;
class ChClass;
class Feat;

using RaceContainer = std::unordered_map< std::string, std::unique_ptr< Race > >;
using ChClassContainer = std::unordered_map< std::string, std::unique_ptr< ChClass > >;
using FeatContainer = std::unordered_map< int, std::unique_ptr< Feat > >;

class Rules
{
public:
    explicit Rules();
    ~Rules();

    // DESCRIPTION
    void setDescription( const std::string d ) { description = d; }
    const std::string& getDescription() const { return description; }

    // RACES
    void setRace( std::unique_ptr< Nwn::Race > r );
    ConstMapLooper< Race, RaceContainer > getRaces() const { return ConstMapLooper< Race, RaceContainer >( races ); }
    const Race* getRaceByName( const std::string& name ) const;
    bool isRaceValid( const std::string& name ) const;
    void removeRace( const std::string& name );

    // CHARACTER CLASSES
    void setChClass( std::unique_ptr< Nwn::ChClass > r );
    ConstMapLooper< ChClass, ChClassContainer > getChClasses() const {
        return ConstMapLooper< ChClass, ChClassContainer >( chclasses );
    }
    const ChClass* getChClassByName( const std::string& name ) const;
    bool isChClassValid( const std::string& name ) const;
    void removeChClass( const std::string& name );

    // FEATS
    const Nwn::Feat* getFeat( int id ) const;
    void setFeat( std::unique_ptr< Nwn::Feat > f );
    ConstMapLooper< Feat, FeatContainer > getFeats() const { return ConstMapLooper< Feat, FeatContainer >( feats ); }

    // CHARACTER METHODS
    int getAblAtLvl( const Character* chr, AblScore abl, int lvl, const std::set< int > *featsUptoLvl = nullptr ) const;
    int getHpAtLvl( const Character* chr, int lvl ) const;
    int getBabAtLvl( const Character* chr, int lvl ) const;
    SavingThrows getSavesAtLvl( const Character* chr, int lvl ) const;

    /// Only at that level, not including earlier ones
    std::set< int > getFeatsGainedAtLvl( const Character* chr, int lvl ) const;

    /// Only at that level, not including earlier ones
    int getNumNormalFeatChoicesAtLvl( const Character* chr, int lvl ) const;
    /// Only at that level, not including earlier ones
    int getNumBonusFeatChoicesAtLvl( const Character* chr, int lvl ) const;
    /// Only at that level, not including earlier ones
    int getNumTotalFeatChoicesAtLvl( const Character* chr, int lvl ) const;

    bool isFeatAvailAtLvl( const Character* chr, int lvl, int featid ) const;

    std::set< int > getFeatsUptoLvl( const Character* nwnChar, int lvl ) const;

    // SERIALIZATION
    void save( const char* fileName ) const;
    void restore( const char* fileName );

private:
    friend class boost::serialization::access;

    template<class Archive>
    void serialize( Archive & ar, const unsigned int /* file_version */ ) {
        ar & boost::serialization::make_nvp( "description", description )
           & boost::serialization::make_nvp( "races", races )
           & boost::serialization::make_nvp( "chclasses", chclasses )
           & boost::serialization::make_nvp( "feats", feats );
    }

    std::string description;
    RaceContainer races;
    ChClassContainer chclasses;
    FeatContainer feats;
};

} // namespace Nwn

#endif // NWNRULES_H

max_stars_repo_name: rkibria/NwnCharGen

max_issues_repo_name: rkibria/NwnCharGen

max_forks_repo_name: rkibria/NwnCharGen


text:
#pragma once

#include <cmath>
#include <vector>
#include <chrono>
#include <functional>
#include <algorithm>

#include <boost/version.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/copy.hpp>
#if BOOST_VERSION >= 107200
#include <boost/math/statistics/univariate_statistics.hpp>
#else
#include <boost/math/tools/univariate_statistics.hpp>
#define CLT_BENCHMARK2_OLD_BOOST
#endif
#include <boost/math/distributions/normal.hpp>

#include "util.hpp"

namespace clt::bench {

#ifdef CLT_BENCHMARK2_OLD_BOOST
CLT_message("Workaround patch for old boost.");

namespace workaround {

template <class T> inline auto mean_and_sample_variance(const T &v)
{
    return std::make_tuple(boost::math::tools::mean(v),
                           boost::math::tools::sample_variance(v));
};

template <class T> inline auto interquartile_range(T &v)
{
    using boost::math::tools::median;

    std::sort(v.begin(), v.end());
    const size_t n = v.size();
    const ssize_t n_half = n / 2;
    auto it_lb = v.begin();
    auto it_le = std::next(it_lb, n_half);
    auto it_ue = v.end();
    auto it_ub = std::next(it_ue, -n_half);
    const auto lm = median(it_lb, it_le);
    const auto um = median(it_ub, it_ue);
    return um - lm;
};

} // namespace workaround
#endif

template <class T> inline auto calc_stats(const T &data)
{
    using vec_fp_t = std::vector<double>;
    using duration_t = typename T::value_type;
    using ratio_t = typename duration_t::period;
    using boost::copy;
    using boost::adaptors::transformed;
#ifndef CLT_BENCHMARK2_OLD_BOOST
    using boost::math::statistics::mean_and_sample_variance;
    using boost::math::statistics::median;
    using boost::math::statistics::median_absolute_deviation;
    using boost::math::statistics::interquartile_range;
#else
    CLT_message("Workaround patch for old boost.");
    using workaround::mean_and_sample_variance;
    using boost::math::tools::median;
    using boost::math::tools::median_absolute_deviation;
    using workaround::interquartile_range;
#endif

    constexpr double nscale = ratio_t::num;
    constexpr double dscale = ratio_t::den;

    vec_fp_t fp_data(data.size());
    copy(data | transformed([nscale, dscale](auto &&x) {
             return x.count() * nscale / dscale;
         }),
         fp_data.begin());
    const auto [smean, uvar] = mean_and_sample_variance(fp_data);
    const auto smedian = median(fp_data);
    const auto smad = median_absolute_deviation(fp_data, smedian);
    const auto siqr = interquartile_range(fp_data);
    return std::make_tuple(smean, uvar, smedian, smad, siqr);
}

/**
 * Output alpha and bound.
 * alpha-percent CI is [smean - bound, smean + bound].
 */
template <class RealType>
inline auto confidence_interval_bound(const RealType uvar,
                                      const size_t nsamples,
                                      const double alpha = 0.95)
{
    using std::sqrt;
    using norm_t = boost::math::normal_distribution<>;
    using boost::math::quantile;

    assert(0 <= alpha);
    assert(alpha <= 1);
    const auto residue = (1 - alpha) / 2;
    const auto ustdev = std::sqrt(uvar / nsamples);
    const auto stdnorm = norm_t{};
    const auto tail = quantile(stdnorm, 1 - residue);
    const auto bound = tail * ustdev;
    return std::make_tuple(alpha, bound);
}

struct Experiment {
    using timer_t = std::chrono::high_resolution_clock;
    using duration_t = timer_t::duration;
    using vec_duration_t = std::vector<duration_t>;

    std::function<void()> setup_func_;
    std::function<void()> target_func_;
    std::function<bool()> check_func_;
    size_t num_setup_;
    size_t num_reptition_;
    std::chrono::seconds limit_time_;

    Experiment()
        : setup_func_([]() {}), target_func_([]() {}),
          check_func_([]() { return true; }), num_setup_(10000),
          num_reptition_(500), limit_time_(5)
    {
    }
    size_t max_total_number_of_execution() const noexcept
    {
        return num_setup_ * num_reptition_;
    }
    vec_duration_t run();
};
} // namespace clt::bench

max_stars_repo_name: tell/aes-ni

max_issues_repo_name: tell/aes-ni

max_forks_repo_name: tell/aes-ni