/*
 * HitFitAnalyser.cpp
 *
 *  Created on: Jul 5, 2011
 *      Author: senkin
 */
#include "../../interface/Analysers/HitFitAnalyser.h"
#include "../../interface/LumiReWeighting.h"
#include "../../interface/EventWeightProvider.h"
#include "../../interface/PoissonMeanShifter.h"
#include "../../interface/GlobalVariables.h"
#include "../../interface/ReconstructionModules/ChiSquaredBasedTopPairReconstruction.h"
#include <boost/scoped_ptr.hpp>

//using namespace reweight;
using namespace BAT;

void HitFitAnalyser::analyse(const EventPtr event) {
	weight_ = event->weight() * prescale_ * scale_;
	//fit only the events that pass full ttbar selection
	if (!topEplusJetsRefSelection_->passesFullSelectionExceptLastTwoSteps(event))
		;
	return;

	const JetCollection jets = topEplusJetsRefSelection_->cleanedJets(event);
	LeptonPointer selectedLepton = topEplusJetsRefSelection_->signalLepton(event);
	METPointer met = event->MET();

	histMan_->H1D("m3_diff")->Fill(fabs(truthMatchEvent.M3() - TtbarHypothesis::M3(jets)));

	boost::scoped_ptr<ChiSquaredBasedTopPairReconstruction> chi2Reco(
			new ChiSquaredBasedTopPairReconstruction(selectedLepton, met, jets));
	if (!chi2Reco->meetsInitialCriteria()) { //reports details on failure and skips event
		cout << chi2Reco->getDetailsOnFailure();
		return;
	}
	TtbarHypothesisPointer bestTopHypothesis = chi2Reco->getBestSolution();

	//set MC matching flag
	if (event->getDataType() == DataType::TTJets)
		do_MC_matching = true;
	else
		do_MC_matching = false;

	//initialise pile-up shifters
	PoissonMeanShifter PShiftUp(0.6); // PU-systematic
	PoissonMeanShifter PShiftDown(-0.6); // PU-systematic

	//temporary file for jet fit info (ugly but works)
	string tempFileName = "temp.txt";
	ofstream tempFile(tempFileName.c_str());

	histMan_->setCurrentHistogramFolder("hitfitStudy");

	if (Globals::produceFitterASCIIoutput) {
		//write the event info into ASCII file
		outFile << "------------------------------------------" << endl;
		outFile << "BeginEvent" << endl;
		outFile << "RunNr: " << event->runnumber() << endl;
		outFile << "LumiSection: " << event->lumiblock() << endl;
		outFile << "EventNr: " << event->eventnumber() << endl;
		outFile << "EventWeight: " << weight_ << "  ";
		double nVertices(0);
		if (!event->isRealData()) {
			float lumiWeight = event->PileUpWeight();

			if (Globals::pileUpReweightingMethod == PileUpReweightingMethod::averagePileUp
					|| Globals::pileUpReweightingMethod == PileUpReweightingMethod::threeDReweighting)
				nVertices = event->averageNumberOfVertices();
			if (Globals::pileUpReweightingMethod == PileUpReweightingMethod::inTimePileUpOnly)
				nVertices = event->inTimeOnlyNumberOfVertices();
			float lumiWeightUp = lumiWeight * PShiftUp.ShiftWeight(nVertices);
			float lumiWeightDown = lumiWeight * PShiftDown.ShiftWeight(nVertices);
			outFile << lumiWeight << "  " << lumiWeightUp << "  " << lumiWeightDown;

		} else
			outFile << "1  1  1";

		outFile << endl;

		outFile << jets.size() << "  " << event->Vertices().size() << "  ";
		if (!event->isRealData()) {
			outFile << nVertices;
		} else
			outFile << "9999";

		outFile << "  ";
		if ((event->getDataType() == DataType::TTJets) or (event->getDataType() == DataType::ttbar161)
				or (event->getDataType() == DataType::ttbar163) or (event->getDataType() == DataType::ttbar166)
				or (event->getDataType() == DataType::ttbar169) or (event->getDataType() == DataType::ttbar175)
				or (event->getDataType() == DataType::ttbar178) or (event->getDataType() == DataType::ttbar181)
				or (event->getDataType() == DataType::ttbar184)) {
			outFile << "9999  9999  9999" << endl; //needs to be fixed
		} else
			outFile << "9999  9999  9999" << endl;
	}

	//prepare the jets collection
	// Copy jets into an array
	JetCollection jetCopy;
	for (JetCollection::const_iterator j = jets.begin(); j != jets.end(); ++j) {
		jetCopy.push_back(*j);
	}

	std::sort(jetCopy.begin(), jetCopy.end(), jetPtComp);

	jetsForFitting.clear();
	unsigned numJetsToFit = jetCopy.size();

	if (jetCopy.size() >= 4) {
		if (numJetsToFit > 4)
			numJetsToFit = 4;
		jetsForFitting.insert(jetsForFitting.begin(), jetCopy.begin(), jetCopy.begin() + numJetsToFit);
	}

	BatHitFit hhFitter(electronTranslator_, muonTranslator_, jetTranslator_, metTranslator_, hitfitDefault_,
			hitfitLepWMass_, hitfitHadWMass_, hitfitTopMass_);

	// Clear the internal state
	hhFitter.clear();

	const ElectronPointer signalElectron(boost::static_pointer_cast<Electron>(selectedLepton));
	//TODO: fix the fitter to accept lepton class OR particle!!
	hhFitter.AddLepton(*signalElectron);

	// Add jets into HitFit
	for (size_t jet = 0; jet != jetsForFitting.size(); ++jet) {
		hhFitter.AddJet(*jetsForFitting.at(jet));
	}

	//calculate all jets pt
	double all_jets_pt = 0;
	for (unsigned int i = 0; i != jetsForFitting.size(); ++i) {
		all_jets_pt = all_jets_pt + jetsForFitting[i]->pt();
	}

	histMan_->H1D("AllJetsPt")->Fill(all_jets_pt, weight_);

	// Add missing transverse energy into HitFit
	hhFitter.SetMet(*met);

	// Results of the fit for all jet permutation of the event
	std::vector<hitfit::Lepjets_Event> hitfitEventsInput;

	// Results of the fit for all jet permutation of the event
	std::vector<hitfit::Fit_Result> hitfitResult;
	//
	// R U N   H I T F I T
	//
	// Run the kinematic fit and get how many permutations is possible
	// in the fit

	size_t nHitFit = hhFitter.FitAllPermutation();

//   // Get the number of jets
//   nHitFitJet      = hhFitter.GetEvent().njets();

	// Get the input events for all permutations
	hitfitEventsInput = hhFitter.GetUnfittedEvent();

	// Get the fit results for all permutations
	hitfitResult = hhFitter.GetFitAllPermutation();

	double bestChi2 = 999.;
	unsigned bestX2pos = nHitFit + 1;

//   // Loop over all permutations and extract the information
	for (size_t fit = 0; fit != nHitFit; ++fit) {

		// Get the event before the fit
		hitfit::Lepjets_Event unfittedEvent = hitfitEventsInput[fit];

		// Get the event after the fit
		hitfit::Fit_Result fitResult = hitfitResult[fit];

		if (hitfitResult[fit].chisq() > 0.0) {
			histMan_->H1D("FittedTopMassAllSolutions")->Fill(fitResult.mt(), weight_);
			histMan_->H1D("FitChiSquaredAllSolutions")->Fill(fitResult.chisq(), weight_);
			histMan_->H1D("FitLogChiSqdAllSolutions")->Fill(log(fitResult.chisq()), weight_);
			const hitfit::Column_Vector &px = fitResult.pullx();
			const hitfit::Column_Vector &py = fitResult.pully();
			double sumPx = 0.0;
			for (int i = 0; i < px.num_row(); ++i) {
				histMan_->H1D("PullDistAllVarsAllSolutions")->Fill(px[i]);
				histMan_->H2D("PullDistPerVarAllSolutions")->Fill(px[i], i);
				sumPx += px[i] * px[i];
			}
			histMan_->H1D("PullSumSquaredAllSolutions")->Fill(sumPx);
			for (int i = 0; i < py.num_row(); ++i) {
				histMan_->H1D("PullDistYVarsAllSolutions")->Fill(py[i]);
			}

			if (Globals::produceFitterASCIIoutput) {
				if (fitResult.chisq() < 20) {
					tempFile << fitResult.mt() << "  " << fitResult.sigmt() << "  " << fitResult.chisq() << "  ";
					for (unsigned int i = 0; i != jetsForFitting.size(); ++i) {
						if (fitResult.jet_types()[i] == 13)
							tempFile << i;
					}
					tempFile << "  ";
					for (unsigned int i = 0; i != jetsForFitting.size(); ++i) {
						if (fitResult.jet_types()[i] == 14)
							tempFile << i;
					}
					tempFile << "  ";
					for (unsigned int i = 0; i != jetsForFitting.size(); ++i) {
						if (fitResult.jet_types()[i] == 12)
							tempFile << i;
					}
					tempFile << endl;
				}
			}

		}
		if (fitResult.chisq() > 0.0 && fitResult.chisq() < bestChi2) {
			bestChi2 = fitResult.chisq();
			bestX2pos = fit;
		}

	}

	//
	// END PART WHICH EXTRACTS INFORMATION FROM HITFIT
	//

	if (bestX2pos < nHitFit + 1) {
		histMan_->H1D("FittedTopMassBestSolution")->Fill(hitfitResult[bestX2pos].mt(), weight_);
		histMan_->H1D("FitChiSquaredBestSolution")->Fill(hitfitResult[bestX2pos].chisq(), weight_);
		histMan_->H1D("FitLogChiSqdBestSolution")->Fill(log(hitfitResult[bestX2pos].chisq()), weight_);

		const hitfit::Column_Vector &px = hitfitResult[bestX2pos].pullx();
		const hitfit::Column_Vector &py = hitfitResult[bestX2pos].pully();
		double sumPx = 0.0;
		for (int i = 0; i < px.num_row(); ++i) {
			histMan_->H1D("PullDistAllVarsBestSolution")->Fill(px[i]);
			histMan_->H2D("PullDistPerVarBestSolution")->Fill(px[i], i);
			sumPx += px[i] * px[i];
		}
		histMan_->H1D("PullSumSquaredBestSolution")->Fill(sumPx);
		for (int i = 0; i < py.num_row(); ++i) {
			histMan_->H1D("PullDistYVarsBestSolution")->Fill(py[i]);
		}

		//pass hitfit event into BAT format
		lepton_charge = selectedLepton->charge();
		BAT::TtbarHypothesis newHyp = BatEvent(hitfitResult[bestX2pos].ev());

		if (Globals::produceFitterASCIIoutput) {
			//outFile << "goodCombi: " << hitfitResult[bestX2pos].mt() << "  ";
			outFile << "goodCombi: ";

			for (unsigned int i = 0; i != jetsForFitting.size(); ++i) {
				if (hitfitResult[bestX2pos].jet_types()[i] == 13)
					outFile << i;
			}
			outFile << "  ";
			for (unsigned int i = 0; i != jetsForFitting.size(); ++i) {
				if (hitfitResult[bestX2pos].jet_types()[i] == 14)
					outFile << i;
			}
			outFile << "  ";
			for (unsigned int i = 0; i != jetsForFitting.size(); ++i) {
				if (hitfitResult[bestX2pos].jet_types()[i] == 12)
					outFile << i;
			}
			outFile << "  index: " << bestX2pos << endl;

			//copy temp file to outfile
			ifstream tempCopy(tempFileName.c_str());
			if (tempCopy.is_open()) {
				char c;
				while (tempCopy.get(c).good())
					outFile << c;
				tempCopy.close();
			}

			outFile << "FinishedEvent" << endl;
			outFile << "------------------------------------------" << endl;
		}
		tempFile.close();

	} else {
		if (Globals::produceFitterASCIIoutput)
			outFile << "No solutions found by HitFit for this event" << std::endl;
	}

}

HitFitAnalyser::HitFitAnalyser(boost::shared_ptr<HistogramManager> histMan, std::string histogramFolder) :
		BasicAnalyser(histMan, histogramFolder), //
		outFileName("FitResults.txt"), //
		outFile(outFileName.c_str()), //
		// The following five initializers read the config parameters for the
		// ASCII text files which contains the physics object resolutions.
		FitterPath_(Globals::TQAFPath), //
		hitfitDefault_(FitterPath_ + "TopQuarkAnalysis/TopHitFit/data/setting/RunHitFitConfiguration.txt"), //
		hitfitElectronResolution_(
				FitterPath_ + "TopQuarkAnalysis/TopHitFit/data/resolution/tqafElectronResolution.txt"), //
		hitfitMuonResolution_(FitterPath_ + "TopQuarkAnalysis/TopHitFit/data/resolution/tqafMuonResolution.txt"), //
		hitfitUdscJetResolution_(FitterPath_ + "TopQuarkAnalysis/TopHitFit/data/resolution/tqafUdscJetResolution.txt"), //
		hitfitBJetResolution_(FitterPath_ + "TopQuarkAnalysis/TopHitFit/data/resolution/tqafBJetResolution.txt"), //
		hitfitMETResolution_(FitterPath_ + "TopQuarkAnalysis/TopHitFit/data/resolution/tqafKtResolution.txt"), //
		// The following three initializers read the config parameters for the
		// values to which the leptonic W, hadronic W, and top quark masses are to
		// be constrained to.
		hitfitLepWMass_(80.4), //
		hitfitHadWMass_(80.4), //
		hitfitTopMass_(0.0), //
		// The following three initializers instantiate the translator between PAT objects
		// and HitFit objects using the ASCII text files which contains the resolutions.
		electronTranslator_(hitfitElectronResolution_), //
		muonTranslator_(hitfitMuonResolution_), //
		jetTranslator_(hitfitUdscJetResolution_, hitfitBJetResolution_), //
		metTranslator_(hitfitMETResolution_), //
		topEplusJetsRefSelection_(new TopPairEPlusJetsReferenceSelection())

{

}

BAT::TtbarHypothesis HitFitAnalyser::BatEvent(const hitfit::Lepjets_Event& ev) {
	// Do the electron
	//BAT::ElectronPointer newEle(new BAT::Electron(*truthMatchEvent.electronFromW));
	BAT::ElectronPointer newEle(new BAT::Electron());
	if (ev.nleps() > 0) {
		newEle->setFourVector(fourVectorFromHitFit(ev.lep(0).p()));
	} else {
		std::cout << "No electron in HitFit event!" << std::endl;
	}

	// Do the neutrino
	//BAT::ParticlePointer newMet(new BAT::Particle(*truthMatchEvent.neutrinoFromW));
	BAT::ParticlePointer newMet(new BAT::Particle());
	newMet->setFourVector(fourVectorFromHitFit(ev.met()));

	// Do the jets
	BAT::JetPointer newLep(new BAT::Jet());
	BAT::JetPointer newHad(new BAT::Jet());
	BAT::JetPointer newWj1(new BAT::Jet());
	BAT::JetPointer newWj2(new BAT::Jet());

	// We need to match up the jets in the HitFit event against the input ones
	// Assume that HitFit has preserved the jet order
	std::vector<hitfit::Lepjets_Event_Jet>::size_type i = 0;
	for (BAT::JetCollection::const_iterator j = jetsForFitting.begin(); j != jetsForFitting.end(); ++i, ++j) {
		FourVector hfJet = fourVectorFromHitFit(ev.jet(i).p());
		int hfJType = ev.jet(i).type();
//    if ((*j)->getFourVector().DeltaR(hfJet) < 0.005) {
		if (hfJType != hitfit::unknown_label) {
			BAT::Jet newJet(**j);
			newJet.setFourVector(hfJet);

			if (hfJType == hitfit::lepb_label)
				*newLep = newJet;
			if (hfJType == hitfit::hadb_label)
				*newHad = newJet;
			if (hfJType == hitfit::hadw1_label)
				*newWj1 = newJet;
			if (hfJType == hitfit::hadw2_label)
				*newWj2 = newJet;
		}
	}

	BAT::TtbarHypothesis hyp(newEle, newMet, newLep, newHad, newWj1, newWj2);

	// do MC matching study
	if (do_MC_matching) {
		//Particle Pointers for best fitted hypothesis
		BAT::ParticlePointer hadronicTop, leptonicTop, leptonicW, hadronicW;
		leptonicW = ParticlePointer(new Particle(*newMet + *newEle));
		if (newWj1 != newWj2)
			hadronicW = ParticlePointer(new Particle(*newWj1 + *newWj2));
		else
			hadronicW = newWj1;
		leptonicTop = ParticlePointer(new Particle(*newLep + *leptonicW));
		hadronicTop = ParticlePointer(new Particle(*newHad + *hadronicW));

		double deltaRelectron, deltaRhadronicBjet, deltaRleptonicBjet, deltaRWjet1, deltaRWjet2, sumDeltaR;
		deltaRelectron = truthMatchEvent.leptonFromW->deltaR(hyp.leptonFromW);
		deltaRhadronicBjet = truthMatchEvent.hadronicBJet->deltaR(hyp.hadronicBJet);
		deltaRleptonicBjet = truthMatchEvent.leptonicBjet->deltaR(hyp.leptonicBjet);

		histMan_->H1D("deltaPtElectron")->Fill(
				(truthMatchEvent.leptonFromW->pt() - hyp.leptonFromW->pt()) / truthMatchEvent.leptonFromW->pt());
		histMan_->H1D("deltaRelectron")->Fill(truthMatchEvent.leptonFromW->deltaR(hyp.leptonFromW));
		histMan_->H1D("deltaPtHadronicBjet")->Fill(
				(truthMatchEvent.hadronicBJet->pt() - hyp.hadronicBJet->pt()) / truthMatchEvent.hadronicBJet->pt());
		histMan_->H1D("deltaRhadronicBjet")->Fill(truthMatchEvent.hadronicBJet->deltaR(hyp.hadronicBJet));
		histMan_->H1D("deltaPtLeptonicBjet")->Fill(
				(truthMatchEvent.leptonicBjet->pt() - hyp.leptonicBjet->pt()) / truthMatchEvent.leptonicBjet->pt());
		histMan_->H1D("deltaRleptonicBjet")->Fill(truthMatchEvent.leptonicBjet->deltaR(hyp.leptonicBjet));
		if (truthMatchEvent.jet1FromW->deltaR(hyp.jet1FromW) < truthMatchEvent.jet1FromW->deltaR(hyp.jet2FromW)) {
			deltaRWjet1 = truthMatchEvent.jet1FromW->deltaR(hyp.jet1FromW);
			deltaRWjet2 = truthMatchEvent.jet2FromW->deltaR(hyp.jet2FromW);
			histMan_->H1D("deltaPtWjet1")->Fill(
					(truthMatchEvent.jet1FromW->pt() - hyp.jet1FromW->pt()) / truthMatchEvent.jet1FromW->pt());
			histMan_->H1D("deltaRWjet1")->Fill(truthMatchEvent.jet1FromW->deltaR(hyp.jet1FromW));
			histMan_->H1D("deltaPtWjet2")->Fill(
					(truthMatchEvent.jet2FromW->pt() - hyp.jet2FromW->pt()) / truthMatchEvent.jet2FromW->pt());
			histMan_->H1D("deltaRWjet2")->Fill(truthMatchEvent.jet2FromW->deltaR(hyp.jet2FromW));
		} else {
			deltaRWjet1 = truthMatchEvent.jet1FromW->deltaR(hyp.jet2FromW);
			deltaRWjet2 = truthMatchEvent.jet2FromW->deltaR(hyp.jet1FromW);
			histMan_->H1D("deltaPtWjet1")->Fill(
					(truthMatchEvent.jet1FromW->pt() - hyp.jet2FromW->pt()) / truthMatchEvent.jet1FromW->pt());
			histMan_->H1D("deltaRWjet1")->Fill(truthMatchEvent.jet1FromW->deltaR(hyp.jet2FromW));
			histMan_->H1D("deltaPtWjet2")->Fill(
					(truthMatchEvent.jet2FromW->pt() - hyp.jet1FromW->pt()) / truthMatchEvent.jet2FromW->pt());
			histMan_->H1D("deltaRWjet2")->Fill(truthMatchEvent.jet2FromW->deltaR(hyp.jet1FromW));
		}

		sumDeltaR = deltaRelectron + deltaRhadronicBjet + deltaRleptonicBjet + deltaRWjet1 + deltaRWjet2;
		histMan_->H1D("SumDeltaR")->Fill(sumDeltaR);

		histMan_->H1D("deltaLeptonicTopMass")->Fill(
				(truthMatchEvent.leptonicTop->mass() - leptonicTop->mass()) / truthMatchEvent.leptonicTop->mass());
		histMan_->H1D("deltaHadronicTopMass")->Fill(
				(truthMatchEvent.hadronicTop->mass() - hadronicTop->mass()) / truthMatchEvent.hadronicTop->mass());

		if (sumDeltaR < 0.4) {
			if (truthMatchEvent.jet1FromW->deltaR(hyp.jet1FromW) < truthMatchEvent.jet1FromW->deltaR(hyp.jet2FromW)) {
				histMan_->H1D("deltaPtWjet1Best")->Fill(
						(truthMatchEvent.jet1FromW->pt() - hyp.jet1FromW->pt()) / truthMatchEvent.jet1FromW->pt());
				histMan_->H1D("deltaPtWjet2Best")->Fill(
						(truthMatchEvent.jet2FromW->pt() - hyp.jet2FromW->pt()) / truthMatchEvent.jet2FromW->pt());
			} else {
				histMan_->H1D("deltaPtWjet1")->Fill(
						(truthMatchEvent.jet1FromW->pt() - hyp.jet2FromW->pt()) / truthMatchEvent.jet1FromW->pt());
				histMan_->H1D("deltaPtWjet2")->Fill(
						(truthMatchEvent.jet2FromW->pt() - hyp.jet1FromW->pt()) / truthMatchEvent.jet2FromW->pt());
			}
			histMan_->H1D("deltaPtElectronBest")->Fill(
					(truthMatchEvent.leptonFromW->pt() - hyp.leptonFromW->pt()) / truthMatchEvent.leptonFromW->pt());
			histMan_->H1D("deltaPtHadronicBjetBest")->Fill(
					(truthMatchEvent.hadronicBJet->pt() - hyp.hadronicBJet->pt()) / truthMatchEvent.hadronicBJet->pt());
			histMan_->H1D("deltaPtLeptonicBjetBest")->Fill(
					(truthMatchEvent.leptonicBjet->pt() - hyp.leptonicBjet->pt()) / truthMatchEvent.leptonicBjet->pt());
			histMan_->H1D("deltaLeptonicTopMassBest")->Fill(
					(truthMatchEvent.leptonicTop->mass() - leptonicTop->mass()) / truthMatchEvent.leptonicTop->mass());
			histMan_->H1D("deltaHadronicTopMassBest")->Fill(
					(truthMatchEvent.hadronicTop->mass() - hadronicTop->mass()) / truthMatchEvent.hadronicTop->mass());
		}
	}

	if (Globals::produceFitterASCIIoutput) {
		outFile << newEle->phi() << "  " << newEle->eta() << "  " << newEle->pt() << "  " << lepton_charge << "  "
				<< newMet->px() << "  " << newMet->py() << endl;
		outFile << jetsForFitting[0]->pt() << "  " << jetsForFitting[0]->eta() << "  " << jetsForFitting[0]->phi()
				<< "  " << jetsForFitting[0]->btagSSVHE() << "  ";
		outFile << jetsForFitting[1]->pt() << "  " << jetsForFitting[1]->eta() << "  " << jetsForFitting[1]->phi()
				<< "  " << jetsForFitting[1]->btagSSVHE() << "  ";
		outFile << jetsForFitting[2]->pt() << "  " << jetsForFitting[2]->eta() << "  " << jetsForFitting[2]->phi()
				<< "  " << jetsForFitting[2]->btagSSVHE() << "  ";
		outFile << jetsForFitting[3]->pt() << "  " << jetsForFitting[3]->eta() << "  " << jetsForFitting[3]->phi()
				<< "  " << jetsForFitting[3]->btagSSVHE() << endl;
	}
	return hyp;
}

FourVector HitFitAnalyser::fourVectorFromHitFit(const hitfit::Fourvec& v) {
	FourVector result(v.x(), v.y(), v.z(), v.t());
	return result;
}

void HitFitAnalyser::setMCTTbarHypothesis(const TtbarHypothesis& mcEvent) {
	truthMatchEvent = mcEvent;
}

void HitFitAnalyser::printFile(const string filename) {
	ifstream fin; // Initialise filestream object.
	char c;

	fin.open(filename.c_str(), ios::in); // Open an input filestream.

	// Check if file opened.
	// fin.fail() returns 1 if there is a fail in the filestream.
	if (fin.fail()) {
		cout << "Error: Unable to open " << filename << ".\n";
		exit(1);
	}

	fin.get(c); // Get first character for kicks.

	// While the stream hasn't failed or reached the end of file, read and display.
	while (!fin.fail() && !fin.eof()) {
		cout << c; // Display character.
		fin.get(c); // Get the next character from the stream.
	}

	fin.close(); // Always close the stream once done.
}

HitFitAnalyser::~HitFitAnalyser() {
	outFile.close();
}

void HitFitAnalyser::createHistograms() {
	histMan_->setCurrentHistogramFolder("hitfitStudy");
	histMan_->addH1D("AllJetsPt", "All jets Pt", 100, 0., 600.);
	histMan_->addH1D("FittedTopMassAllSolutions", "Fitted top mass all solutions", 100, 0., 400.);
	histMan_->addH1D("FitChiSquaredAllSolutions", "Fit chi-squared all solutions", 100, 0., 20.);
	histMan_->addH1D("FitLogChiSqdAllSolutions", "Fit log(chi-sqd) all solutions", 100, -5., 5.);
	histMan_->addH1D("FittedTopMassBestSolution", "Fitted top mass best solution", 100, 0., 400.);
	histMan_->addH1D("FitChiSquaredBestSolution", "Fit chi-squared best solution", 100, 0., 20.);
	histMan_->addH1D("FitLogChiSqdBestSolution", "Fit log(chi-sqd) best solutions", 100, -5., 5.);

	histMan_->addH1D("PullDistAllVarsAllSolutions", "Pulls well measured all solutions", 100, -10., 10.);
	histMan_->addH1D("PullSumSquaredAllSolutions", "Sum-squared pulls all solutions", 100, 0, 500.);
	histMan_->addH2D("PullDistPerVarAllSolutions", "Pulls well measured vs varno all", 100, -10., 10., 25, 0., 25.);
	histMan_->addH1D("PullDistYVarsAllSolutions", "Pulls poorly measured all", 100, -10., 10.);
	histMan_->addH1D("PullDistAllVarsBestSolution", "Pulls well measured best solution", 100, -10., 10.);
	histMan_->addH1D("PullSumSquaredBestSolution", "Sum-squared pulls best solution", 100, 0, 500.);
	histMan_->addH2D("PullDistPerVarBestSolution", "Pulls well measured vs varno best", 100, -10., 10., 25, 0., 25.);
	histMan_->addH1D("PullDistYVarsBestSolution", "Pulls poorly measured best", 100, -10., 10.);

	// MC matching study histograms
	histMan_->addH1D("deltaPtElectron", "Pt difference between truth and fitted electrons", 100, -1.5, 1.5);
	histMan_->addH1D("deltaRelectron", "DeltaR between truth and fitted electrons", 100, 0., 2.);
	histMan_->addH1D("deltaPtHadronicBjet", "Pt difference between truth and fitted b-jets from hadronic top", 100,
			-1.5, 1.5);
	histMan_->addH1D("deltaRhadronicBjet", "DeltaR between truth and fitted b-jets from hadronic top", 100, 0., 5.);
	histMan_->addH1D("deltaPtLeptonicBjet", "Pt difference between truth and fitted b-jets from leptonic top", 100,
			-1.5, 1.5);
	histMan_->addH1D("deltaRleptonicBjet", "DeltaR between truth and fitted b-jets from leptonic top", 100, 0., 5.);
	histMan_->addH1D("deltaPtWjet1", "Pt difference between truth and fitted W jets 1", 100, -1.5, 1.5);
	histMan_->addH1D("deltaRWjet1", "DeltaR between truth and fitted W jets 1", 100, 0., 5.);
	histMan_->addH1D("deltaPtWjet2", "Pt difference between truth and fitted W jets 2", 100, -1.5, 1.5);
	histMan_->addH1D("deltaRWjet2", "DeltaR between truth and fitted W jets 2", 100, 0., 5.);
	histMan_->addH1D("SumDeltaR", "Summarised deltaR of all e+jets", 100, 0., 25.);
	histMan_->addH1D("deltaPtElectronBest", "Pt difference between truth and fitted electrons for matched solutions",
			100, -1.5, 1.5);
	histMan_->addH1D("deltaPtHadronicBjetBest",
			"Pt difference between truth and fitted b-jets from hadronic top for matched solutions", 100, -1.5, 1.5);
	histMan_->addH1D("deltaPtLeptonicBjetBest",
			"Pt difference between truth and fitted b-jets from leptonic top for matched solutions", 100, -1.5, 1.5);
	histMan_->addH1D("deltaPtWjet1Best", "Pt difference between truth and fitted W jets 1 for matched solutions", 100,
			-1.5, 1.5);
	histMan_->addH1D("deltaPtWjet2Best", "Pt difference between truth and fitted W jets 2 for matched solutions", 100,
			-1.5, 1.5);
	histMan_->addH1D("deltaMET", "Delta MET between truth and fitted solutions", 100, -50., 50.);
	histMan_->addH1D("deltaMETbest", "Delta MET between truth and fitted matched solutions", 100, -50., 50.);
	histMan_->addH1D("deltaLeptonicTopMass", "Difference between truth and fitted leptonic top masses", 100, -1.5, 1.5);
	histMan_->addH1D("deltaHadronicTopMass", "Difference between truth and fitted hadronic top masses", 100, -1.5, 1.5);
	histMan_->addH1D("deltaLeptonicTopMassBest",
			"Difference between truth and fitted leptonic top masses for matched solutions", 100, -1.5, 1.5);
	histMan_->addH1D("deltaHadronicTopMassBest",
			"Difference between truth and fitted hadronic top masses for matched solutions", 100, -1.5, 1.5);

}


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#ifdef USE_INTRINSICS
#include "vector_x86.hpp"
#endif
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class Plane {
    PS::F64vec nvec;    // Normal vector of the projection surface
    PS::F64    cnst;    // Distance of the projection surface from the origin
    PS::F64vec base;    // The nearest point on the projection surface from the origin
    PS::F64vec axis[2]; // Newly defined vectors of the projection surface
    PS::F64vec pdsp;    // Vector of pallaral displacement on the projection surface
    PS::F64    wdth;    // Width of the projection surface

    void normalize() {
        PS::F64 vabs = sqrt(this->nvec * this->nvec);
        this->nvec = this->nvec / vabs;
        this->cnst = this->cnst / vabs;
    }

    void defineAxis() {
        if(this->nvec[0] == 0. && this->nvec[1] == 0. && this->nvec[2] == 1.) {
            this->axis[0] = PS::F64vec(1., 0., 0.);
            this->axis[1] = PS::F64vec(0., 1., 0.);
        } else if (this->nvec[0] == 0. && this->nvec[1] == 1. && this->nvec[2] == 0.) {
            this->axis[0] = PS::F64vec(1., 0., 0.);
            this->axis[1] = PS::F64vec(0., 0., 1.);
        } else if (this->nvec[0] == 1. && this->nvec[1] == 0. && this->nvec[2] == 0.) {
            this->axis[0] = PS::F64vec(0., 1., 0.);
            this->axis[1] = PS::F64vec(0., 0., 1.);
        } else {
            assert(NULL);
        }
    }

public:

    Plane() {
        this->nvec    = 0.;
        this->cnst    = 0.;
        this->base    = 0.;
        this->axis[0] = 0.;
        this->axis[1] = 0.;
        this->pdsp    = 0.;
        this->wdth    = 0.;
    }

    PS::F64 readData(FILE *fp) {
        fscanf(fp, "%lf%lf%lf%lf",
               &this->nvec[0], &this->nvec[1], &this->nvec[2], &this->cnst);
        fscanf(fp, "%lf%lf", &this->pdsp[0], &this->pdsp[1]);
        fscanf(fp, "%lf", &this->wdth);
        this->normalize();
        this->base = this->cnst * this->nvec;
        this->defineAxis();
    }

    PS::F64 writeData(FILE *fp) {
        if(PS::Comm::getRank() == 0) {
            fprintf(fp, "nvec: %+e %+e %+e\n",
                    this->nvec[0], this->nvec[1], this->nvec[2]);
            fprintf(fp, "cnst: %+e\n", this->cnst);
            fprintf(fp, "base: %+e %+e %+e\n",
                    this->base[0], this->base[1], this->base[2]);
            fprintf(fp, "axis[0]: %+e %+e %+e\n",
                    this->axis[0][0], this->axis[0][1], this->axis[0][2]);
            fprintf(fp, "axis[1]: %+e %+e %+e\n",
                    this->axis[1][0], this->axis[1][1], this->axis[1][2]);
            fprintf(fp, "pdsp: %+e %+e %+e\n",
                    this->pdsp[0], this->pdsp[1], this->pdsp[2]);
            fprintf(fp, "wdth: %+e\n", this->wdth);
        }
    }

    inline PS::F64 calcDistance(PS::F64vec pos) {
        return fabs(this->nvec * (pos - this->base));
    }

    inline PS::F64vec calcCoordinateOnPlane(PS::F64vec pos) {
        PS::F64 px = this->axis[0] * (pos - this->base) + pdsp[0];
        PS::F64 py = this->axis[1] * (pos - this->base) + pdsp[1];
        return PS::F64vec(px, py, 0.);
    }

    PS::F64 getWidth() {
        return this->wdth;
    }

};

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

};

template <class Tsph>
void calcDensityCenter(Tsph  & sph,
                       PS::F64vec & xdens_g,
                       PS::F64vec & vdens_g,
                       PS::F64vec & adens_g) {
    PS::F64    tdens = 0.;
    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        if(sph[i].istar != 0) {
            continue;
        }
        tdens += sph[i].dens;
        xdens += sph[i].dens * sph[i].pos;
        vdens += sph[i].dens * sph[i].vel;
        adens += sph[i].dens * sph[i].acc;
    }

    PS::F64    tdens_g = PS::Comm::getSum(tdens);
    xdens_g = PS::Comm::getSum(xdens);
    vdens_g = PS::Comm::getSum(vdens);
    adens_g = PS::Comm::getSum(adens);

    xdens_g = xdens_g / tdens_g;
    vdens_g = vdens_g / tdens_g;
    adens_g = adens_g / tdens_g;

    return;
}

template <class Tsph>
void projectOnPlane(char * ofile,
                    Plane & plane,
                    Tsph  & sph) {
    const  PS::S64 nmax = 256;
    static PS::S64 ptcl[nmax][nmax];
    static PS::F64 dens[nmax][nmax];
    static PS::F64 temp[nmax][nmax];
    static PS::F64 omgz[nmax][nmax];
    static PS::F64 entr[nmax][nmax];
    static PS::F64 istr[nmax][nmax];

    PS::F64 wdth  = plane.getWidth();
    PS::F64 dx    = wdth / (PS::F64)nmax;
    PS::F64 dxinv = 1. / dx;

    for(PS::S64 i = 0; i < nmax; i++) {
        for(PS::S64 j = 0; j < nmax; j++) {
            ptcl[i][j] = 0;
            dens[i][j] = 0.;
            temp[i][j] = 0.;
            omgz[i][j] = 0.;
            entr[i][j] = 0.;
            istr[i][j] = 0.;
        }
    }

    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;
    calcDensityCenter(sph, xdens, vdens, adens);

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        sph[i].pos -= xdens;
        sph[i].vel -= vdens;
        sph[i].acc -= adens;
        PS::F64vec omg_i  = sph[i].pos ^ sph[i].vel;
        PS::F64    omgz_i = (1. / (sph[i].pos[0] * sph[i].pos[0] + sph[i].pos[1] * sph[i].pos[1])) * omg_i[2];
        if(plane.calcDistance(sph[i].pos) > sph[i].ksr) {
            continue;
        }
        PS::F64vec pvec  = plane.calcCoordinateOnPlane(sph[i].pos);
        PS::S64    ix    = (PS::S64)((pvec[0] + 0.5 * wdth) * dxinv);
        PS::S64    iy    = (PS::S64)((pvec[1] + 0.5 * wdth) * dxinv);
        if(ix < 0 || nmax <= ix) {
            continue;
        }
        if(iy < 0 || nmax <= iy) {
            continue;
        }
        ptcl[ix][iy] += 1;
        dens[ix][iy] += sph[i].dens;
        temp[ix][iy] += sph[i].temp;
        omgz[ix][iy] += omgz_i;
        entr[ix][iy] += sph[i].entr;
        istr[ix][iy] += (sph[i].istar + 1);
    }

    static PS::S64 ptcl_g[nmax][nmax];
    static PS::F64 dens_g[nmax][nmax];
    static PS::F64 temp_g[nmax][nmax];
    static PS::F64 omgz_g[nmax][nmax];
    static PS::F64 entr_g[nmax][nmax];
    static PS::F64 istr_g[nmax][nmax];

    PS::S64 ierr = 0;
    ierr = MPI_Allreduce(ptcl, ptcl_g, nmax*nmax, MPI_LONG_LONG, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(dens, dens_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(temp, temp_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(omgz, omgz_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(entr, entr_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(istr, istr_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

    for(PS::S64 i = 0; i < nmax; i++) {
        for(PS::S64 j = 0; j < nmax; j++) {
            PS::F64 pinv = ((ptcl_g[i][j] != 0) ? (1. / ptcl_g[i][j]) : 0.);
            dens_g[i][j] *= pinv;
            temp_g[i][j] *= pinv;
            omgz_g[i][j] *= pinv;
            entr_g[i][j] *= pinv;
            istr_g[i][j] *= pinv;
        }
    }

    if(PS::Comm::getRank() == 0) {
        FILE * fp = fopen(ofile, "w");
        for(PS::S64 i = 0; i < nmax; i++) {
            for(PS::S64 j = 0; j < nmax; j++) {
                PS::F64 px = dx * (PS::F64)i - 0.5 * wdth;
                PS::F64 pz = dx * (PS::F64)j - 0.5 * wdth;
                fprintf(fp, "%+e %+e %+e %+e %+e %+e %+e\n",
                        px, pz,
                        dens_g[i][j], temp_g[i][j], omgz_g[i][j],
                        entr_g[i][j], istr_g[i][j]);
            }
        }
        fclose(fp);
    }

}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::S64 ibgn, iend, dsnp;
    Plane plane;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lld%lld%lld", &ibgn, &iend, &dsnp);
    plane.readData(fp);
    fclose(fp);
    plane.writeData(stdout);

    for(PS::S64 itime = ibgn; itime <= iend; itime += dsnp) {
        char tfile[1024];
        FILE *fp = NULL;
        PS::S64 tdir = 0;
        for(PS::S64 iidir = 0; iidir < 100; iidir++) {
            sprintf(tfile, "%s/t%02d/sph_t%04d_p%06d_i%06d.dat", idir, iidir, itime,
                    PS::Comm::getNumberOfProc(), 0);
            fp = fopen(tfile, "r");
            if(fp != NULL) {
                tdir = iidir;
                break;
            }
        }
        if(fp == NULL) {
            if(PS::Comm::getRank() == 0) {
                fprintf(stderr, "Not found %s\n", tfile);
            }
            continue;
        }
        fclose(fp);

        char sfile[1024];
        sprintf(sfile, "%s/t%02d/sph_t%04d", idir, tdir, itime);
        sph.readParticleAscii(sfile, "%s_p%06d_i%06d.dat");

        char ofile[1024];
        sprintf(ofile, "%s/anim_%04d.dat", odir, itime);
        projectOnPlane(ofile, plane, sph);
    }

    PS::Finalize();

    return 0;
}


#include "Level.hpp"
#include "SynthNet.hpp"

#include <boost/foreach.hpp>

#include <iostream>

using metaSMT::logic::predicate;

template<typename Solver, typename VerifyerContext>
struct SortNetSynth
{
  typedef bool result_type;

  unsigned size;
  SynthNet<Solver> syn_net;

  typedef std::list< Level > Net;
  Net ver_net;

  double time_synth;
  double time_verify;

  SortNetSynth ( unsigned min_depth, std::vector<std::string> const & inits )
  : size(inits[0].size())
  , syn_net(size)
  {
    for (unsigned i = 0; i < min_depth; i++) {
      syn_net.add_level();
    }
    BOOST_FOREACH (std::string const & init, inits) {
      syn_net.add_counterexample(init);
    }
  }

  void print_solution(std::ostream& out) {
     out << "found solution\n";
    //syn_net.print_all(out);
    syn_net.print_one(out);
  }

  void print_counterexample(std::ostream& out) {
  }

  bool verify() {
    boost::timer timer;
    VerifyerContext ver;
    bool success = syn_net.verify_or_add_counterexample(ver);
    time_verify += timer.elapsed();
    return success;
  }

  bool synth()
  {
    boost::timer timer;
    bool success = syn_net.synth();
    time_synth += timer.elapsed();
    return success;
  }

  result_type operator() ()
  {
    while(true) {
      while( synth() ) {
        print_solution(std::cout);
        if( verify() ) {
          std::cout << "Found Solution." << std::endl;
          print_solution(std::cout);
          return result_type();
        } else {
          print_counterexample(std::cout);
        }
      }
      std::cout << "add another level." << std::endl;
      syn_net.add_level();
    }
    return result_type();
  }

};


#include <iostream>
#include <boost/asio.hpp>
#include <boost/array.hpp>

using boost::asio::ip::udp;

class WebServer{
    boost::asio::io_service io_service;
    udp::endpoint endpoint;
    udp::socket base_socket;

    void accept_request(){
        auto remote_endpoint = std::make_shared<udp::endpoint>();
        auto buf = std::make_shared<boost::array<unsigned char, 1>>();
        base_socket.async_receive_from(boost::asio::buffer(*buf), *remote_endpoint, 0, [=](boost::system::error_code ec, std::size_t s){
            accept_request();
            if(!ec && (*buf)[0] == 'h'){
                std::cout << remote_endpoint->address() << ':' << remote_endpoint->port() << std::endl;
                auto socket = std::make_shared<udp::socket>(io_service);
                socket->open(udp::v4());
                socket->send_to(boost::asio::buffer(*buf), *remote_endpoint, 0);
                handle_request(socket, remote_endpoint);
            }
        });
    }

    void handle_request(std::shared_ptr<udp::socket> const& socket, std::shared_ptr<udp::endpoint> const& remote_endpoint){
        auto buf = std::make_shared<boost::array<char, 256>>();
        socket->async_receive_from(boost::asio::buffer(*buf), *remote_endpoint, [=](const boost::system::error_code &ec, size_t s){
            if(!ec){
                std::string message_in(buf->begin(), buf->begin()+s);
                std::string failmessage;
                if(message_in == "exit") return;
                else handle_request(socket, remote_endpoint);
                std::cout << message_in << std::endl;
                double a,b;
                bool isPlus = true;
                auto sep = message_in.find('+');
                if(sep == std::string::npos) {
                    sep = message_in.find('-');
                    if(sep == std::string::npos){
                        failmessage = "Couldn't find a + or -";
                    };
                    isPlus = false;
                }
                std::string s;
                try {
                    s = message_in.substr(0, sep);
                    a = std::stod(s);
                } catch(std::invalid_argument const &e){
                    failmessage = "Invalid argument " + s;
                } catch(std::out_of_range) {
                    failmessage = "First number too big/small";
                } catch (std::exception const& e) {
                    std::cout << typeid(e).name() << " from " << e.what() << std::endl;
                    return;
                }
                try {
                    s = message_in.substr(sep + 1);
                    b = std::stod(s);
                } catch(std::invalid_argument const &e) {
                    failmessage = "Invalid argument " + s;
                } catch(std::out_of_range) {
                    failmessage = "Second number too big/small";
                } catch (std::exception const& e) {
                    std::cout << typeid(e).name() << " from " << e.what() << std::endl;
                    return;
                }

                std::string message_out;

                if(failmessage.length()){
                    message_out = failmessage;
                }
                else {
                    message_out = std::to_string(isPlus ? a+b : a-b);
                }
                message_out += "\r\n";
                message_out += '\0';

                // Write to client
                socket->async_send_to(boost::asio::buffer(message_out), *remote_endpoint, [=](const boost::system::error_code &ec, size_t) {
                    // If not error:
                    if (!ec) {
                        std::cout << "server: reply sent to client" << std::endl;
                        std::cout << message_out << std::endl;
                    }
                });

            }
        });
    }

public:
    WebServer() : endpoint(udp::v4(), 8081), base_socket(io_service, endpoint){}

    void start(){
        accept_request();
        io_service.run();
    }

};

int main() {

    WebServer server;
    server.start();

    return 0;
}


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

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

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

#ifndef BOOST_GEOMETRY_STRATEGY_PROJECT_TRANSFORMER_HPP
#define BOOST_GEOMETRY_STRATEGY_PROJECT_TRANSFORMER_HPP

#include <boost/shared_ptr.hpp>

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/extensions/gis/projections/factory.hpp>
#include <boost/geometry/extensions/gis/projections/parameters.hpp>

namespace boost { namespace geometry { namespace projections
{
/*!
    \brief Transformation strategy to do transform using a Map Projection
    \ingroup transform
    \tparam LatLong first point type
    \tparam Cartesian second point type

    See also \link p03_projmap_example.cpp the projmap example \endlink
    where this last one plus a transformation using a projection are used.

 */
template <typename LatLong, typename Cartesian>
struct project_transformer
{
    typedef boost::shared_ptr<projection<LatLong, Cartesian> > projection_ptr;

    projection_ptr m_prj;

    inline project_transformer(projection_ptr& prj)
        : m_prj(prj)
    {}

    inline project_transformer(std::string const& par)
    {
        factory<LatLong, Cartesian, parameters> fac;
        m_prj.reset(fac.create_new(init(par)));
    }

    inline bool apply(LatLong const& p1, Cartesian& p2) const
    {
        // Latlong (LatLong -> Cartesian) will be projected, rest will be copied.
        // So first copy third or higher dimensions
        geometry::detail::conversion::point_to_point<LatLong, Cartesian, 2,
                geometry::dimension<Cartesian>::value> ::apply(p1, p2);
        return m_prj->forward(p1, p2);
    }

};

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_STRATEGY_PROJECT_TRANSFORMER_HPP


#include <boost/hana/max.hpp>


#include <PerfEvent.hpp>
#include <array>
#include <boost/random/mersenne_twister.hpp>
#include <iostream>
#include <pxart/msws.hpp>
#include <pxart/mt19937.hpp>
#include <pxart/simd128/msws.hpp>
#include <pxart/simd128/mt19937.hpp>
#include <pxart/simd128/xoroshiro128plus.hpp>
#include <pxart/simd256/msws.hpp>
#include <pxart/simd256/mt19937.hpp>
#include <pxart/simd256/xoroshiro128plus.hpp>
#include <pxart/xoroshiro128plus.hpp>
#include <random>
#include <sstream>

using namespace std;

struct benchmark {
  explicit benchmark(size_t samples) : n{samples} {
    cout << "iteration = " << n << "\n"
         << "cache size = " << cache_size << " B\n"
         << "samples = " << n * cache_size / sizeof(uint32_t) << "\n"
         << "\n";
  }

  template <typename result_type, typename RNG>
  benchmark& run(const char* name, RNG&& rng) noexcept {
    constexpr auto cache_count = cache_size / sizeof(result_type);
    array<result_type, cache_count> cache;

    for (int i = 0; i < 100; ++i)
      for (auto& x : cache) x = rng();

    {
      params.setParam("npc",  // numbers per call
                      to_string(sizeof(result_type) / sizeof(uint32_t)));
      params.setParam("name", name);
      PerfEventBlock e(n * cache_count, params, header);
      for (int i = 0; i < n; ++i)
        for (auto& x : cache) x = rng();
    }

    // cout << reinterpret_cast<uint32_t&>(
    //             cache[reinterpret_cast<uint32_t&>(cache[0]) % cache_count])
    //      << "\n";
    header = false;
    return *this;
  }

  template <typename RNG>
  benchmark& run(const char* name, RNG&& rng) noexcept {
    return run<decltype(rng())>(name, forward<RNG>(rng));
  }

  template <typename result_type, typename RNG>
  benchmark& run(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    constexpr auto cache_count = cache_size / sizeof(result_type);
    array<result_type, cache_count> cache;

    for (int i = 0; i < 100; ++i) {
      for (int j = 0; j < cache_count; j += 2) {
        cache[j] = rng1();
        cache[j + 1] = rng2();
      }
    }

    {
      params.setParam("npc",  // numbers per call
                      to_string(sizeof(result_type) / sizeof(uint32_t)));
      params.setParam("name", name);
      PerfEventBlock e(n * cache_count, params, header);
      for (int i = 0; i < n; ++i) {
        for (int j = 0; j < cache_count; j += 2) {
          cache[j] = rng1();
          cache[j + 1] = rng2();
        }
      }
    }

    // cout << reinterpret_cast<uint32_t&>(
    //             cache[reinterpret_cast<uint32_t&>(cache[0]) % cache_count])
    //      << "\n";
    header = false;
    return *this;
  }

  template <typename RNG>
  benchmark& run(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    return run<decltype(rng1())>(name, forward<RNG>(rng1), forward<RNG>(rng2));
  }

  benchmark& separate() noexcept {
    header = true;
    cout << "\n";
    return *this;
  }

  static constexpr int cache_size = 1 << 14;
  size_t n{};
  BenchmarkParameters params{};
  bool header = true;
};

int main(int argc, char** argv) {
  if (2 != argc) {
    cout << "usage:\n" << argv[0] << " <number of elements>\n";
    return -1;
  }

  stringstream input{argv[1]};
  uint64_t n;
  input >> n;

  random_device rd{};

  benchmark{n}  //
      .run<uint32_t>("std   mt", std::mt19937{rd()})
      .run("boost mt", boost::random::mt19937{rd()})
      .run("pxart mt", pxart::mt19937{pxart::mt19937::default_seeder{rd()}})
      .run("px mt256",
           pxart::simd256::mt19937{pxart::mt19937::default_seeder{rd()}})
      .run("px mt128",
           pxart::simd128::mt19937{pxart::mt19937::default_seeder{rd()}})
      .separate()
      .run("pxt xrsr", pxart::xrsr128p{rd})
      .run("xrsr  x2", pxart::xrsr128p{rd}, pxart::xrsr128p{rd})
      .run("px xs256", pxart::simd256::xrsr128p{rd})
      .run("xs256 x2", pxart::simd256::xrsr128p{rd},
           pxart::simd256::xrsr128p{rd})
      .run("px xs128", pxart::simd128::xrsr128p{rd})
      .run("xs128 x2", pxart::simd128::xrsr128p{rd},
           pxart::simd128::xrsr128p{rd})
      .separate()
      .run("pxt msws", pxart::msws{rd})
      .run("msws  x2", pxart::msws{rd}, pxart::msws{rd})
      .run("px mw256", pxart::simd256::msws{rd})
      .run("mw256 x2", pxart::simd256::msws{rd}, pxart::simd256::msws{rd})
      .run("px mw128", pxart::simd128::msws{rd})
      .run("mw128 x2", pxart::simd128::msws{rd}, pxart::simd128::msws{rd});
}


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

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

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

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

 /*!

    @ingroup group-operator
    Function object implementing eq capabilities

    Returns True or False
    according x and y are equal or not. Infix notation can be used with operator '=='

    This is a convenient alias of @ref is_equal
  **/
  const boost::dispatch::functor<tag::eq_> eq = {};
} }
#endif

#include <boost/simd/function/scalar/is_equal.hpp>
#include <boost/simd/function/simd/eq.hpp>

#endif


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GEOMETRIES_QUANTITY_POINT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GEOMETRIES_QUANTITY_POINT_HPP

#include <cstddef>

#include <boost/mpl/int.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/point.hpp>

#include <boost/units/quantity.hpp>

namespace boost { namespace geometry
{

namespace cs
{

template <typename Unit>
struct units_cartesian {};

}

namespace traits
{

template<typename U>
struct cs_tag<cs::units_cartesian<U> >
{
    typedef cartesian_tag type;
};

}

namespace model
{

// Define a point type to interoperate with Boost.Units, having
// 1. a constructor taking quantities
// 2. defining a quantified coordinate system
// Note that all values are still stored in "normal" types as double
template
<
    typename Units,
    std::size_t DimensionCount = 2,
    typename CoordinateType = double,
    typename CoordinateSystem = cs::units_cartesian<Units>
>
class quantity_point
    : public model::point<CoordinateType, DimensionCount, CoordinateSystem>
{
    typedef boost::units::quantity<Units, CoordinateType> qtype;

public :

    // Templated constructor to allow constructing with other units then qtype,
    // e.g. to convert from centimeters to meters
    template <typename Quantity>
    inline quantity_point(Quantity const& x, Quantity const& y)
        : model::point<CoordinateType, DimensionCount, CoordinateSystem>(
            qtype(x).value(),
            qtype(y).value())
    {}
};

}

// Adapt quantity_point to the Point Concept
#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template <typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct tag<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef point_tag type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct coordinate_type<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef CoordinateType type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct coordinate_system<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef CoordinateSystem type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct dimension<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
    : boost::mpl::int_<DimensionCount>
{};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem, std::size_t Dimension>
struct access<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem>, Dimension >
{
    static inline CoordinateType get(
        model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> const& p)
    {
        return p.template get<Dimension>();
    }

    static inline void set(model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem>& p,
        CoordinateType const& value)
    {
        p.template set<Dimension>(value);
    }
};

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GEOMETRIES_QUANTITY_POINT_HPP


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../../example_func_xy_jacobian.h"
#include "../../constraints_jacobian.h"

#define RENDER_PSI 0
#define RENDER_OBJECTIVE_FUNC 1
#define RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT 2

int render_type = RENDER_PSI;

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -20.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

double x_result = 5;
double y_result = 5;

double psi_trg = 0.99;

std::vector<std::pair<double, double>> path_result;
double contraint_a = 1;
double contraint_x_trg = 0;
double contraint_y_trg = 0;
double weight_constraint = 1;
int constraint_type = 0;
double angle_heading = 0;
double update_scale = 1.0;
double w_longitudal_motion = 0.01;

void obs_eq_constraint(double &delta, double a, double x, double x_trg, int type)
{
	if(type == 0){
		observation_equation_constraint(delta, a, x, x_trg);
	}else{
		observation_equation_sq_constraint(delta, a, x, x_trg);
	}
}
void obs_eq_constraint_jacobian(Eigen::Matrix<double, 1, 1> &j, double a, double x, double x_trg, int type)
{
	if(type == 0){
		observation_equation_constraint_jacobian(j, a, x, x_trg);
	}else{
		observation_equation_sq_constraint_jacobian(j, a, x, x_trg);
	}
}

int main(int argc, char *argv[]){
	path_result.emplace_back(x_result, y_result);

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("simple_optimization_problem_func_xy_with_constraints");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glLineWidth(1);

	/*glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(100.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 100.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 100.0f);
	glEnd();*/

	float scale = 10;
	switch(render_type){
		case RENDER_PSI:{
			glColor3f(1,0,0);

			float step = 0.1;
			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double psi1;
					example_func_xy(psi1, x, y);

					double psi2;
					example_func_xy(psi2, x+step, y);

					double psi3;
					example_func_xy(psi3, x+step, y+step);

					double psi4;
					example_func_xy(psi4, x, y+step);

					glColor3f(psi1,1-psi1,0);
					glBegin(GL_LINE_STRIP);
						glVertex3f(x,y,psi1*scale);
						glVertex3f(x+step,y,psi2*scale);
						glVertex3f(x+step,y+step,psi3*scale);
						glVertex3f(x,y+step,psi4*scale);
						glVertex3f(x,y,psi1*scale);
					glEnd();
				}
			}

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double psi;
				example_func_xy(psi,x_result,y_result);
				glVertex3f(x_result, y_result, psi*scale);
			glEnd();
			glPointSize(1);

			//heading
			glLineWidth(3);
			Eigen::Affine2d m_rot = Eigen::Affine2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			glColor3f(0.1,0.5,0.6);
			glBegin(GL_LINES);
				Eigen::Vector2d v(1,0);
				Eigen::Vector2d vt = m_rot * v;
				example_func_xy(psi,x_result,y_result);
				glVertex3f(x_result, y_result, psi*scale);
				glVertex3f(x_result + vt.x(), y_result + vt.y(), psi*scale);
			glEnd();
			glLineWidth(1);

			glLineWidth(2);
			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(const auto &p:path_result){
				double psi;
				example_func_xy(psi,p.first,p.second);
				glVertex3f(p.first,p.second, psi*scale);
			}
			glEnd();
			glLineWidth(1);

			glColor3f(0,0,0);
			glBegin(GL_LINES);
			for(float x = -10; x <= 10; x+=1){
				glVertex3f(-10, x, psi_trg*scale);
				glVertex3f(10, x, psi_trg*scale);

				glVertex3f(x, -10, psi_trg*scale);
				glVertex3f(x, 10, psi_trg*scale);
			}
			glEnd();
		break;
		}
		case RENDER_OBJECTIVE_FUNC:{

			glColor3f(1,0,0);
			float step = 0.1;
			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double psi;
					example_func_xy(psi, x, y);
					glColor3f(psi,1-psi,0);

					glBegin(GL_LINE_STRIP);
					    double psi1;
						observation_equation_example_func_xy(psi1, x, y, psi_trg);

						double psi2;
						observation_equation_example_func_xy(psi2, x+step, y, psi_trg);

						double psi3;
						observation_equation_example_func_xy(psi3, x+step, y+step, psi_trg);

						double psi4;
						observation_equation_example_func_xy(psi4, x, y+step, psi_trg);

						glVertex3f(x,y,           psi1*psi1*scale);
						glVertex3f(x+step,y,      psi2*psi2*scale);
						glVertex3f(x+step,y+step, psi3*psi3*scale);
						glVertex3f(x,y+step,      psi4*psi4*scale);
						glVertex3f(x,y,           psi1*psi1*scale);
					glEnd();
				}
			}

			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double cx_delta1;
					obs_eq_constraint(cx_delta1, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta1;
					obs_eq_constraint(cy_delta1, contraint_a, y, contraint_y_trg, constraint_type);

					double cx_delta2;
					obs_eq_constraint(cx_delta2, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta2;
					obs_eq_constraint(cy_delta2, contraint_a, y, contraint_y_trg, constraint_type);

					double cx_delta3;
					obs_eq_constraint(cx_delta3, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta3;
					obs_eq_constraint(cy_delta3, contraint_a, y+step, contraint_y_trg, constraint_type);

					double cx_delta4;
					obs_eq_constraint(cx_delta4, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta4;
					obs_eq_constraint(cy_delta4, contraint_a, y+step, contraint_y_trg, constraint_type);

					double psi;
					example_func_xy(psi, x, y);
					glColor3f(psi,1-psi,0);
					glBegin(GL_LINE_STRIP);
						glVertex3f(x,y,           cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
						glVertex3f(x+step,y,      cx_delta2*cx_delta2 + cy_delta2 * cy_delta2);
						glVertex3f(x+step,y+step, cx_delta3*cx_delta3 + cy_delta3 * cy_delta3);
						glVertex3f(x,y+step,      cx_delta4*cx_delta4 + cy_delta4 * cy_delta4);
						glVertex3f(x,y,           cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
					glEnd();
				}
			}

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double psi;
				observation_equation_example_func_xy(psi, x_result, y_result, psi_trg);
				glVertex3f(x_result, y_result, psi*psi*scale);
			glEnd();
			glPointSize(1);

			//heading
			glLineWidth(3);
			Eigen::Affine2d m_rot = Eigen::Affine2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			glColor3f(0.1,0.5,0.6);
			glBegin(GL_LINES);
				Eigen::Vector2d v(1,0);
				Eigen::Vector2d vt = m_rot * v;
				observation_equation_example_func_xy(psi, x_result, y_result, psi_trg);
				glVertex3f(x_result, y_result, psi*psi*scale);
				glVertex3f(x_result + vt.x(), y_result + vt.y(), psi*psi*scale);
			glEnd();
			glLineWidth(1);

			glLineWidth(2);
			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(const auto &p:path_result){
				double psi;
				observation_equation_example_func_xy(psi, p.first, p.second, psi_trg);
				glVertex3f(p.first, p.second, psi*psi*scale);
			}
			glEnd();
			glLineWidth(1);
			break;
		}
		case RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT:{
			glColor3f(1,0,0);
			float step = 0.1;
			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double delta1;
					observation_equation_example_func_xy(delta1, x, y, psi_trg);

					double cx_delta1;
					obs_eq_constraint(cx_delta1, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta1;
					obs_eq_constraint(cy_delta1, contraint_a, y, contraint_y_trg, constraint_type);

					double delta2;
					observation_equation_example_func_xy(delta2, x+step, y, psi_trg);

					double cx_delta2;
					obs_eq_constraint(cx_delta2, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta2;
					obs_eq_constraint(cy_delta2, contraint_a, y, contraint_y_trg, constraint_type);

					double delta3;
					observation_equation_example_func_xy(delta3, x+step, y+step, psi_trg);

					double cx_delta3;
					obs_eq_constraint(cx_delta3, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta3;
					obs_eq_constraint(cy_delta3, contraint_a, y+step, contraint_y_trg, constraint_type);

					double delta4;
					observation_equation_example_func_xy(delta4, x, y+step, psi_trg);

					double cx_delta4;
					obs_eq_constraint(cx_delta4, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta4;
					obs_eq_constraint(cy_delta4, contraint_a, y+step, contraint_y_trg, constraint_type);

					double psi;
					example_func_xy(psi, x, y);
					glColor3f(psi,1-psi,0);
					glBegin(GL_LINE_STRIP);
						glVertex3f(x,y,           delta1*delta1 + cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
						glVertex3f(x+step,y,      delta2*delta2 + cx_delta2*cx_delta2 + cy_delta2 * cy_delta2);
						glVertex3f(x+step,y+step, delta3*delta3 + cx_delta3*cx_delta3 + cy_delta3 * cy_delta3);
						glVertex3f(x,y+step,      delta4*delta4 + cx_delta4*cx_delta4 + cy_delta4 * cy_delta4);
						glVertex3f(x,y,           delta1*delta1 + cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
					glEnd();
				}
			}

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double delta;
				observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

				double cx_delta;
				obs_eq_constraint(cx_delta, contraint_a, x_result, contraint_x_trg, constraint_type);

				double cy_delta;
				obs_eq_constraint(cy_delta, contraint_a, y_result, contraint_y_trg, constraint_type);

				glVertex3f(x_result, y_result, delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
			glEnd();
			glPointSize(1);

			//heading
			glLineWidth(3);
			Eigen::Affine2d m_rot = Eigen::Affine2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			glColor3f(0.1,0.5,0.6);
			glBegin(GL_LINES);
				observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);
				obs_eq_constraint(cx_delta, contraint_a, x_result, contraint_x_trg, constraint_type);
				obs_eq_constraint(cy_delta, contraint_a, y_result, contraint_y_trg, constraint_type);

				Eigen::Vector2d v(1,0);
				Eigen::Vector2d vt = m_rot * v;

				glVertex3f(x_result, y_result, delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
				glVertex3f(x_result+vt.x(), y_result+vt.y(), delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
			glEnd();
			glLineWidth(1);

			glLineWidth(2);
			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(const auto &p:path_result){
				observation_equation_example_func_xy(delta, p.first, p.second, psi_trg);
				obs_eq_constraint(cx_delta, contraint_a, p.first, contraint_x_trg, constraint_type);
				obs_eq_constraint(cy_delta, contraint_a, p.second, contraint_y_trg, constraint_type);

				glVertex3f(p.first, p.second, delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
			}
			glEnd();
			glLineWidth(1);
			break;
		}
	}
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'o':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			ic = 0;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  0.000000001);
			tripletListB.emplace_back(ir, 0,  0);

			ic = 1;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  0.000000001);
			tripletListB.emplace_back(ir, 0,  0);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'c':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			ic = 0;
			ir = tripletListB.size();
			double c_delta;
			Eigen::Matrix<double, 1, 1> c_jacobian;
			obs_eq_constraint(c_delta, contraint_a, x_result, contraint_x_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, x_result, contraint_x_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			ic = 1;
			ir = tripletListB.size();
			obs_eq_constraint(c_delta, contraint_a, y_result, contraint_y_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, y_result, contraint_y_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'z':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			//anizotropic motion weights
			Eigen::Matrix2d m_rot = Eigen::Matrix2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			Eigen::Matrix2d m_w = Eigen::Matrix2d::Identity();
			m_w(0,0) = w_longitudal_motion;
			m_w(1,1) = 1;

			Eigen::Matrix2d m_c = (m_rot*m_w)*m_rot.transpose();

			ic = 0;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  m_c(0,0));
			tripletListP.emplace_back(ir, ir+1,  m_c(0,1));
			tripletListB.emplace_back(ir, 0,  0);

			ic = 1;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir-1,  m_c(1,0));
			tripletListP.emplace_back(ir, ir,  m_c(1,1));
			tripletListB.emplace_back(ir, 0,  0);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'x':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			//anizotropic motion weights
			Eigen::Matrix2d m_rot = Eigen::Matrix2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			Eigen::Matrix2d m_w = Eigen::Matrix2d::Identity();
			m_w(0,0) = w_longitudal_motion;
			m_w(1,1) = 1;

			Eigen::Matrix2d m_c = (m_rot*m_w)*m_rot.transpose();

			ic = 0;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  m_c(0,0));
			tripletListP.emplace_back(ir, ir+1,  m_c(0,1));
			tripletListB.emplace_back(ir, 0,  0);

			ic = 1;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir-1,  m_c(1,0));
			tripletListP.emplace_back(ir, ir,  m_c(1,1));
			tripletListB.emplace_back(ir, 0,  0);

			ic = 0;
			ir = tripletListB.size();
			double c_delta;
			Eigen::Matrix<double, 1, 1> c_jacobian;
			obs_eq_constraint(c_delta, contraint_a, x_result, contraint_x_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, x_result, contraint_x_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			ic = 1;
			ir = tripletListB.size();
			obs_eq_constraint(c_delta, contraint_a, y_result, contraint_y_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, y_result, contraint_y_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			x_result = ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 10;
			y_result = ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 10;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case '-':{
			contraint_a -= 0.01;
			if(contraint_a < 0)contraint_a= 0.01;
			break;
		}
		case '=':{
			contraint_a += 0.01;
			break;
		}
		case 'd':{
			contraint_x_trg -= 0.1;
			break;
		}
		case 'i':{
			contraint_x_trg += 0.1;
			break;
		}
		case 'f':{
			contraint_y_trg -= 0.1;
			break;
		}
		case 'g':{
			contraint_y_trg += 0.1;
			break;
		}
		case '1':{
			render_type = RENDER_PSI;
			break;
		}
		case '2':{
			render_type = RENDER_OBJECTIVE_FUNC;
			break;
		}
		case '3':{
			render_type = RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT;
			break;
		}
		case 'y':{
			psi_trg += 0.01;
			std::cout << "psi_trg: " << psi_trg << std::endl;
			break;
		}
		case 't':{
			psi_trg -= 0.01;
			std::cout << "psi_trg: " << psi_trg << std::endl;
			break;
		}
		case 'q':{
			x_result-=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case 'w':{
			x_result+=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case 'a':{
			y_result-=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case 's':{
			y_result+=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case '4':{
			constraint_type = 0;
			break;
		}
		case '5':{
			constraint_type = 1;
			break;
		}
		case '6':{
			weight_constraint /=10;
			std::cout << "weight_constraint: " << weight_constraint << std::endl;
			break;
		}
		case '7':{
			weight_constraint *=10;
			std::cout << "weight_constraint: " << weight_constraint << std::endl;
			break;
		}
		case '8':{
			angle_heading += 1;
			break;
		}
		case '9':{
			angle_heading -= 1;
			break;
		}
		case '[':{
			update_scale -= 0.01;
			if(update_scale <= 0) update_scale = 0.01;
			std::cout << "update_scale: " << update_scale << std::endl;
			break;
		}
		case ']':{
			update_scale += 0.01;
			std::cout << "update_scale: " << update_scale << std::endl;
			break;
		}
		case 'k':{
			w_longitudal_motion *= 10;
			std::cout << "w_longitudal_motion: " << w_longitudal_motion << std::endl;
			break;
		}
		case 'l':{
			w_longitudal_motion /= 10;
			std::cout << "w_longitudal_motion: " << w_longitudal_motion << std::endl;
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "o: optimize" << std::endl;
	std::cout << "c: optimize with constaint" << std::endl;
	std::cout << "z: optimize with anizotropic motion constaint" << std::endl;
	std::cout << "x: optimize with constaint and anizotropic motion constaint" << std::endl;
	std::cout << "-: contraint_a -= 0.01" << std::endl;
	std::cout << "=: contraint_a += 0.01" << std::endl;
	std::cout << "d: contraint_x_trg -= 0.1" << std::endl;
	std::cout << "i: contraint_x_trg += 0.1" << std::endl;
	std::cout << "f: contraint_y_trg -= 0.1" << std::endl;
	std::cout << "g: contraint_y_trg += 0.1" << std::endl;
	std::cout << "1: RENDER_PSI" << std::endl;
	std::cout << "2: RENDER_OBJECTIVE_FUNC" << std::endl;
	std::cout << "3: RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT" << std::endl;
	std::cout << "y: psi_trg += 0.01" << std::endl;
	std::cout << "t: psi_trg -= 0.01" << std::endl;
	std::cout << "q: x_result-=0.1" << std::endl;
	std::cout << "w: x_result+=0.1" << std::endl;
	std::cout << "a: y_result-=0.1" << std::endl;
	std::cout << "s: y_result+=0.1" << std::endl;
	std::cout << "4: constraint_type linear" << std::endl;
	std::cout << "5: constraint_type squared" << std::endl;
	std::cout << "6: weight_constraint /=10" << std::endl;
	std::cout << "7: weight_constraint *=10" << std::endl;
	std::cout << "8: angle_heading += 1" << std::endl;
	std::cout << "9: angle_heading -= 1" << std::endl;
	std::cout << "[: pdate_scale -= 0.01" << std::endl;
	std::cout << "]: pdate_scale += 0.01" << std::endl;
	std::cout << "k: w_longitudal_motion *= 10" << std::endl;
	std::cout << "l: w_longitudal_motion /= 10" << std::endl;
	std::cout << "r: random initial guess" << std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// wxWidgets World Mapper example

// #define EXAMPLE_WX_USE_GRAPHICS_CONTEXT 1

#include <fstream>
#include <sstream>

#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_array.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/multi/geometries/multi_geometries.hpp>

#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>
#include <boost/geometry/extensions/algorithms/selected.hpp>

// wxWidgets, if these headers are NOT found, adapt include path (and lib path)

#include "wx/wx.h"
#include "wx/math.h"
#include "wx/stockitem.h"

#ifdef EXAMPLE_WX_USE_GRAPHICS_CONTEXT
#include "wx/graphics.h"
#include "wx/dcgraph.h"
#endif

typedef boost::geometry::model::d2::point_xy<double> point_2d;
typedef boost::geometry::model::multi_polygon
    <
        boost::geometry::model::polygon<point_2d>
    > country_type;

// Adapt wxWidgets points to Boost.Geometry points such that they can be used
// in e.g. transformations (see below)
BOOST_GEOMETRY_REGISTER_POINT_2D(wxPoint, int, cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_POINT_2D(wxRealPoint, double, cs::cartesian, x, y)

// wxWidgets draws using wxPoint*, so we HAVE to use that.
// Therefore have to make a wxPoint* array
// 1) compatible with Boost.Geometry
// 2) compatible with Boost.Range (required by Boost.Geometry)
// 3) compatible with std::back_inserter (required by Boost.Geometry)

// For compatible 2):
typedef std::pair<wxPoint*,wxPoint*> wxPointPointerPair;

// For compatible 1):
BOOST_GEOMETRY_REGISTER_RING(wxPointPointerPair);

// For compatible 3):
// Specialize back_insert_iterator for the wxPointPointerPair
// (has to be done within "namespace std")
namespace std
{

template <>
class back_insert_iterator<wxPointPointerPair>
    : public std::iterator<std::output_iterator_tag, void, void, void, void>
{
public:

    typedef wxPointPointerPair container_type;

    explicit back_insert_iterator(wxPointPointerPair& x)
        : current(boost::begin(x))
        , end(boost::end(x))
    {}

    inline back_insert_iterator<wxPointPointerPair>&
                operator=(wxPoint const& value)
    {
        // Check if not passed beyond
        if (current != end)
        {
            *current++ = value;
        }
        return *this;
    }

    // Boiler-plate
    inline back_insert_iterator<wxPointPointerPair>& operator*()     { return *this; }
    inline back_insert_iterator<wxPointPointerPair>& operator++()    { return *this; }
    inline back_insert_iterator<wxPointPointerPair>& operator++(int) { return *this; }

private:
    boost::range_iterator<wxPointPointerPair>::type current, end;
};

} // namespace std

// ----------------------------------------------------------------------------
// Read an ASCII file containing WKT's
// ----------------------------------------------------------------------------
template <typename Geometry, typename Box>
inline void read_wkt(std::string const& filename, std::vector<Geometry>& geometries, Box& box)
{
    std::ifstream cpp_file(filename.c_str());
    if (cpp_file.is_open())
    {
        while (! cpp_file.eof() )
        {
            std::string line;
            std::getline(cpp_file, line);
            if (! line.empty())
            {
                Geometry geometry;
                boost::geometry::read_wkt(line, geometry);
                geometries.push_back(geometry);
                boost::geometry::expand(box, boost::geometry::return_envelope<Box>(geometry));
            }
        }
    }
}

// ----------------------------------------------------------------------------
class HelloWorldFrame: public wxFrame
{
public:
    HelloWorldFrame(wxFrame *frame, wxString const& title, wxPoint const& pos, wxSize const& size);

    void OnCloseWindow(wxCloseEvent& );
    void OnExit(wxCommandEvent& );

    DECLARE_EVENT_TABLE()
};

// ----------------------------------------------------------------------------
class HelloWorldCanvas: public wxWindow
{
public:
    HelloWorldCanvas(wxFrame *frame);

private:
    void DrawCountries(wxDC& dc);
    void DrawCountry(wxDC& dc, country_type const& country);

    void OnPaint(wxPaintEvent& );
    void OnMouseMove(wxMouseEvent&);

    typedef boost::geometry::strategy::transform::map_transformer
        <
            point_2d, wxPoint,
            true, true
        > map_transformer_type;

    typedef boost::geometry::strategy::transform::inverse_transformer
        <
            wxPoint, point_2d
        > inverse_transformer_type;

    boost::shared_ptr<map_transformer_type> m_map_transformer;
    boost::shared_ptr<inverse_transformer_type> m_inverse_transformer;

    boost::geometry::model::box<point_2d> m_box;
    std::vector<country_type> m_countries;
    int m_focus;

    wxBrush m_orange;
    wxFrame* m_owner;

DECLARE_EVENT_TABLE()
};

// ----------------------------------------------------------------------------
class HelloWorldApp: public wxApp
{
public:
    bool OnInit()
    {
        // Create the main frame window
        HelloWorldFrame *frame = new HelloWorldFrame(NULL, _T("Boost.Geometry for wxWidgets - Hello World!"), wxDefaultPosition, wxSize(640, 480));

        wxMenu *file_menu = new wxMenu;
        file_menu->Append(wxID_EXIT, wxGetStockLabel(wxID_EXIT));
        wxMenuBar* menuBar = new wxMenuBar;
        menuBar->Append(file_menu, _T("&File"));
        frame->SetMenuBar(menuBar);

        int width, height;
        frame->GetClientSize(&width, &height);

        (void) new HelloWorldCanvas(frame);

        // Show the frame
        frame->Show(true);

        return true;
    }
};

// ----------------------------------------------------------------------------
HelloWorldFrame::HelloWorldFrame(wxFrame *frame, wxString const& title, wxPoint const& pos, wxSize const& size)
    : wxFrame(frame, wxID_ANY, title, pos, size, wxDEFAULT_FRAME_STYLE | wxFULL_REPAINT_ON_RESIZE )
{
    CreateStatusBar(2);
}

void HelloWorldFrame::OnExit(wxCommandEvent& )
{
    this->Destroy();
}

void HelloWorldFrame::OnCloseWindow(wxCloseEvent& )
{
    static bool destroyed = false;
    if (! destroyed)
    {
        this->Destroy();
        destroyed = true;
    }
}

// ----------------------------------------------------------------------------
HelloWorldCanvas::HelloWorldCanvas(wxFrame *frame)
    : wxWindow(frame, wxID_ANY)
    , m_owner(frame)
    , m_focus(-1)
{
    boost::geometry::assign_inverse(m_box);
    read_wkt("../data/world.wkt", m_countries, m_box);
    m_orange = wxBrush(wxColour(255, 128, 0), wxSOLID);
}

void HelloWorldCanvas::OnMouseMove(wxMouseEvent &event)
{
    namespace bg = boost::geometry;

    if (m_inverse_transformer)
    {
        // Boiler-plate wxWidgets code
        wxClientDC dc(this);
        PrepareDC(dc);
        m_owner->PrepareDC(dc);

        // Transform the point to Lon/Lat
        point_2d point;
        bg::transform(event.GetPosition(), point, *m_inverse_transformer);

        // Determine selected object
        int i = 0;
        int previous_focus = m_focus;
        m_focus = -1;
        BOOST_FOREACH(country_type const& country, m_countries)
        {
            if (bg::selected(country, point, 0))
            {
                m_focus = i;
            }
            i++;
        }

        // On change:
        if (m_focus != previous_focus)
        {
            // Undraw old focus
            if (previous_focus >= 0)
            {
                dc.SetBrush(*wxWHITE_BRUSH);
                DrawCountry(dc, m_countries[previous_focus]);
            }
            // Draw new focus
            if (m_focus >= 0)
            {
                dc.SetBrush(m_orange);
                DrawCountry(dc, m_countries[m_focus]);
            }
        }

        // Create a string and set it in the status text
        std::ostringstream out;
        out << "Position: " << point.x() << ", " << point.y();
        m_owner->SetStatusText(wxString(out.str().c_str(), wxConvUTF8));
    }
}

void HelloWorldCanvas::OnPaint(wxPaintEvent& )
{
#if defined(EXAMPLE_WX_USE_GRAPHICS_CONTEXT)
    wxPaintDC pdc(this);
    wxGCDC gdc(pdc);
    wxDC& dc = (wxDC&) gdc;
#else
    wxPaintDC dc(this);
#endif

    PrepareDC(dc);

    static bool running = false;
    if (! running)
    {
        running = true;

        // Update the transformers
        wxSize sz = dc.GetSize();
        m_map_transformer.reset(new map_transformer_type(m_box, sz.x, sz.y));
        m_inverse_transformer.reset(new inverse_transformer_type(*m_map_transformer));

        DrawCountries(dc);

        running = false;
    }
}

void HelloWorldCanvas::DrawCountries(wxDC& dc)
{
    namespace bg = boost::geometry;

    dc.SetBackground(*wxLIGHT_GREY_BRUSH);
    dc.Clear();

    BOOST_FOREACH(country_type const& country, m_countries)
    {
        DrawCountry(dc, country);
    }
    if (m_focus != -1)
    {
        dc.SetBrush(m_orange);
        DrawCountry(dc, m_countries[m_focus]);
    }
}

void HelloWorldCanvas::DrawCountry(wxDC& dc, country_type const& country)
{
    namespace bg = boost::geometry;

    BOOST_FOREACH(bg::model::polygon<point_2d> const& poly, country)
    {
        // Use only outer, holes are (for the moment) ignored. This would need
        // a holey-polygon compatible wx object

        std::size_t n = boost::size(poly.outer());

        boost::scoped_array<wxPoint> points(new wxPoint[n]);

        wxPointPointerPair pair = std::make_pair(points.get(), points.get() + n);
        bg::transform(poly.outer(), pair, *m_map_transformer);

        dc.DrawPolygon(n, points.get());
    }
}

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

BEGIN_EVENT_TABLE(HelloWorldFrame, wxFrame)
    EVT_CLOSE(HelloWorldFrame::OnCloseWindow)
    EVT_MENU(wxID_EXIT, HelloWorldFrame::OnExit)
END_EVENT_TABLE()

BEGIN_EVENT_TABLE(HelloWorldCanvas, wxWindow)
    EVT_PAINT(HelloWorldCanvas::OnPaint)
    EVT_MOTION(HelloWorldCanvas::OnMouseMove)
END_EVENT_TABLE()

IMPLEMENT_APP(HelloWorldApp)


#include <boost/polygon/polygon_set_data.hpp>


//          Copyright Carl Philipp Reh 2009 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/boost_units_value.hpp>
#include <fcppt/catch/begin.hpp>
#include <fcppt/catch/end.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <catch2/catch.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_CATCH_BEGIN

TEST_CASE("boost_units_value", "[boost_units_value]")
{
  fcppt::boost_units_value const op{};

  CHECK(op(42 * boost::units::si::meter) == 42);
}

FCPPT_CATCH_END