#include <gtest/gtest.h>
#include "headers/speed_constraints.h"
#include "headers/field_functions.h"
#include <boost/filesystem.hpp>
#include <chrono>

TEST(straight_4_robots_1, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 0.1), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 0.1), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_4_robots_1");

  for (int i = 0; i < 1200; i++)
  {
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 100 == 0)
    {
      printPlotWithArrows("straight_4_robots_1/straight_4_robots_1_" + std::to_string(i) + ".png",
                          "straight_4_robots_1", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(straight_4_robots_2, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(10 + i, 16 + i), 0.1), *og);
    boost::add_vertex(Obstacle(Position_t(16 + i, 10 + i), 0.1), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_4_robots_2");

  for (int i = 0; i < 1200; i++)
  {
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 100 == 0)
    {
      printPlotWithArrows("straight_4_robots_2/straight_4_robots_2_" + std::to_string(i) + ".png",
                          "straight_4_robots_2", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(straight_4_robots_3, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 5.2), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 5.2), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_4_robots_3");

  for (int i = 0; i < 1200; i++)
  {
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 100 == 0)
    {
      printPlotWithArrows("straight_4_robots_3/straight_4_robots_3_" + std::to_string(i) + ".png",
                          "straight_4_robots_3", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(straight_4_robots_4, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 36; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 5.2), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 5.2), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_4_robots_4");

  for (int i = 0; i < 1200; i++)
  {
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 100 == 0)
    {
      printPlotWithArrows("straight_4_robots_4/straight_4_robots_4_" + std::to_string(i) + ".png",
                          "straight_4_robots_4", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

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


/*
 * 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) {
}

BAT::TtbarHypothesis HitFitAnalyser::analyseAndReturn(const EventPtr event, const JetCollection jets, const JetCollection bjets, const LeptonPointer selectedLepton ) {
	weight_ = event->weight() * prescale_ * scale_;
	histMan_->setCurrentHistogramFolder(histogramFolder_);
	treeMan_->setCurrentFolder(histogramFolder_);
	treeMan_->Fill("EventWeight", weight_ );

	// const JetCollection jets(event->getCleanedJets( SelectionCriteria::ElectronPlusJetsReference ));
	// const JetCollection bJets(event->getCleanedBJets( SelectionCriteria::ElectronPlusJetsReference ));
	// const LeptonPointer selectedLepton = event->getSignalLepton( SelectionCriteria::ElectronPlusJetsReference );

	string metPrefix = METAlgorithm::names.at(0);
	const METPointer met(event->MET((METAlgorithm::value) 0));

	// Get cleaned jets that aren't b tagged
	JetCollection leadingLightJets;
	JetCollection leadingBJets;
	unsigned int maxNJet = std::min(5, int(jets.size()));
	for ( unsigned int jetIndex=0; jetIndex < maxNJet; ++jetIndex ) {
		bool isBJet = false;
		JetPointer thisJet = jets[jetIndex];
		for ( unsigned int bJetIndex=0; bJetIndex < bjets.size(); ++bJetIndex ) {
			JetPointer thisBJet = bjets[bJetIndex];
			if ( thisJet == thisBJet ) {
				isBJet = true;
				break;
			}
		}
		if ( !isBJet ) leadingLightJets.push_back( thisJet );
		else leadingBJets.push_back( thisJet );
	}

	histMan_->setCurrentHistogramFolder(histogramFolder_);

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

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

	JetCollection bJetCopy;
	for (JetCollection::const_iterator j = leadingBJets.begin(); j != leadingBJets.end(); ++j) {
		bJetCopy.push_back(*j);
	}

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

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

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

	bJetsForFitting.clear();
	numJetsToFit = bJetCopy.size();
	if (bJetCopy.size() >= 2) {
		if (numJetsToFit > 5)
			numJetsToFit = 5;
		bJetsForFitting.insert(bJetsForFitting.begin(), bJetCopy.begin(), bJetCopy.begin() + numJetsToFit);
	}

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

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

	// Add lepton into hitfit
	if ( isElectronChannel_ ) {
		const ElectronPointer signalElectron(boost::static_pointer_cast<Electron>(selectedLepton));
		//TODO: fix the fitter to accept lepton class OR particle!!
		hhFitter.AddLepton(*signalElectron);
	}
	else {
		const MuonPointer signalMuon(boost::static_pointer_cast<Muon>(selectedLepton));
		//TODO: fix the fitter to accept lepton class OR particle!!
		hhFitter.AddLepton(*signalMuon);
	}

	// Add jets into HitFit
	// Also check if jets matched to ttbar partons are in the jets passed to the fit
	bool quarkInCollection = false, quarkBarInCollection = false, lebBInCollection = false, hadBInCollection = false;
	for (size_t jet = 0; jet != jetsForFitting.size(); ++jet) {
		hhFitter.AddJet(*jetsForFitting.at(jet));
		// cout << "Adding light jet with csv : " << jetsForFitting.at(jet)->getBTagDiscriminator(BAT::BtagAlgorithm::value::CombinedSecondaryVertexV2) << endl;
		if ( jetsForFitting.at(jet)->ttbar_decay_parton() ) {
			int partonPdg = jetsForFitting.at(jet)->ttbar_decay_parton();
			if ( partonPdg == 3 ) quarkInCollection = true;
			else if ( partonPdg == 4 ) quarkBarInCollection = true;
			else if ( partonPdg == 5 ) lebBInCollection = true;
			else if ( partonPdg == 6 ) hadBInCollection = true;
		}
	}
	for (size_t jet = 0; jet != bJetsForFitting.size(); ++jet) {
		hhFitter.AddBJet(*bJetsForFitting.at(jet));
		// cout << "Adding b jet with csv : " << bJetsForFitting.at(jet)->getBTagDiscriminator(BAT::BtagAlgorithm::value::CombinedSecondaryVertexV2) << endl;
		if ( bJetsForFitting.at(jet)->ttbar_decay_parton() ) {
			int partonPdg = bJetsForFitting.at(jet)->ttbar_decay_parton();
			if ( partonPdg == 3 ) quarkInCollection = true;
			else if ( partonPdg == 4 ) quarkBarInCollection = true;
			else if ( partonPdg == 5 ) lebBInCollection = true;
			else if ( partonPdg == 6 ) hadBInCollection = true;
		}

	}

	// Check if jets matched to ttbar partons are in the jets passed to the fit
	if (do_MC_matching) {
		if ( quarkInCollection && quarkBarInCollection && lebBInCollection && hadBInCollection  ) {
			allTTBarJetsPassedToFit_ = true;
		}
		int lastTTBarJetPosition = positionOfLastTTBarJet( jets );
		if ( lastTTBarJetPosition != -1 ) {
			allTTBarJetsPassSelection_ = true;
			treeMan_->Fill("PositionOfLastTTbarJet", lastTTBarJetPosition + 1 );
		}
		else treeMan_->Fill("PositionOfLastTTbarJet", lastTTBarJetPosition );
	}

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

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

	// Container for 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 after the fit
		hitfit::Fit_Result fitResult = hitfitResult[fit];

		if (hitfitResult[fit].chisq() > 0.0) {

			treeMan_->Fill("FitChiSquaredAllSolutions",fitResult.chisq());
		// 	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]);
		// 	}
		}
		// Is this the permutation with smallest chi2?
		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_);

		treeMan_->Fill("FitChiSquaredBestSolutions",hitfitResult[bestX2pos].chisq());
		treeMan_->Fill("FitChiSquaredProbabilityBestSolutions",TMath::Prob(hitfitResult[bestX2pos].chisq(),1));

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

		treeMan_->Fill("FittedLeptonicTopPtBestSolution", newHyp.leptonicTop->pt());
		treeMan_->Fill("FittedHadronicTopPtBestSolution", newHyp.hadronicTop->pt());
		treeMan_->Fill("FittedLeptonicTopRapidityBestSolution", newHyp.leptonicTop->rapidity());
		treeMan_->Fill("FittedHadronicTopRapidityBestSolution", newHyp.hadronicTop->rapidity());
		treeMan_->Fill("FittedTTbarMassBestSolution", newHyp.resonance->mass());
		treeMan_->Fill("FittedTTbarPtBestSolution", newHyp.resonance->pt());
		treeMan_->Fill("FittedTTbarRapidityBestSolution", newHyp.resonance->rapidity());

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

		// Now need to perform second kinematic fit
		hitfit::Fit_Result secondFitResult = performSecondKinematicFit( unfittedEvent, event);
		BAT::TtbarHypothesis newHyp_afterSecondFit = BatEvent(secondFitResult.ev(), event, "SolutionCategory_second");

		treeMan_->Fill("FitChiSquaredBestSolutions_second",secondFitResult.chisq());
		treeMan_->Fill("FitChiSquaredProbabilityBestSolutions_second",TMath::Prob(secondFitResult.chisq(),2));

		treeMan_->Fill("FittedLeptonicTopPtBestSolution_second", newHyp_afterSecondFit.leptonicTop->pt());
		treeMan_->Fill("FittedHadronicTopPtBestSolution_second", newHyp_afterSecondFit.hadronicTop->pt());
		treeMan_->Fill("FittedLeptonicTopRapidityBestSolution_second", newHyp_afterSecondFit.leptonicTop->rapidity());
		treeMan_->Fill("FittedHadronicTopRapidityBestSolution_second", newHyp_afterSecondFit.hadronicTop->rapidity());
		treeMan_->Fill("FittedTTbarMassBestSolution_second", newHyp_afterSecondFit.resonance->mass());
		treeMan_->Fill("FittedTTbarPtBestSolution_second", newHyp_afterSecondFit.resonance->pt());
		treeMan_->Fill("FittedTTbarRapidityBestSolution_second", newHyp_afterSecondFit.resonance->rapidity());

		return newHyp_afterSecondFit;

	} else {
		// cout << "No HitFit solution found for this event" << endl;
		treeMan_->Fill("SolutionCategory", 0 );
		treeMan_->Fill("SolutionCategory_second", 0);
	}

	return BAT::TtbarHypothesis();

}

HitFitAnalyser::HitFitAnalyser(HistogramManagerPtr histMan, TreeManagerPtr treeMan, const bool isElectronChannel, std::string histogramFolder ) :
		BasicAnalyser(histMan, treeMan, histogramFolder), //
		// 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_(172.5), //
		lepton_charge(0.0),
		do_MC_matching(false),
		allTTBarJetsPassedToFit_(false),
		allTTBarJetsPassSelection_(false),
		// Tells hit fit whether event contains a signal electron or a signal muon
		isElectronChannel_(isElectronChannel), //
		// 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_)
{

}

BAT::TtbarHypothesis HitFitAnalyser::BatEvent(const hitfit::Lepjets_Event& ev, const EventPtr event, const string SolutionCategoryHist ) {
	// Do the electron
	BAT::LeptonPointer newLepton(new BAT::Electron());
	if ( !isElectronChannel_ ) {
		newLepton = BAT::LeptonPointer(new BAT::Muon());
	}

	if (ev.nleps() > 0) {
		newLepton->setFourVector(fourVectorFromHitFit(ev.lep(0).p()));
	} else {
		std::cout << "No lepton 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 newLepB(new BAT::Jet());
	BAT::JetPointer newHadB(new BAT::Jet());
	BAT::JetPointer newWj1(new BAT::Jet());
	BAT::JetPointer newWj2(new BAT::Jet());

	// Loop over jets in event
	for ( size_t fit_i = 0; fit_i < ev.njets(); ++fit_i ) {
		FourVector jetFV = fourVectorFromHitFit(ev.jet(fit_i).p());
		int jetType = ev.jet(fit_i).type();

		if ( jetType == hitfit::lepb_label ) {
			newLepB->setFourVector( jetFV );
		}
		else if ( jetType == hitfit::hadb_label ) {
			newHadB->setFourVector( jetFV );
		}
		else if ( jetType == hitfit::hadw1_label ) {
			newWj1->setFourVector( jetFV );
		}
		else if ( jetType == hitfit::hadw2_label ) {
			newWj2->setFourVector( jetFV );
		}
	}

	BAT::TtbarHypothesis hyp(newLepton, newMet, newLepB, newHadB, newWj1, newWj2);
	hyp.combineReconstructedObjects();

	// do MC matching study
	if (do_MC_matching && SolutionCategoryHist != "") {

		// Check if jets are in correct position
		if ( ( newWj1->ttbar_decay_parton() == 3 || newWj1->ttbar_decay_parton() == 4 ) &&
			 ( newWj2->ttbar_decay_parton() == 3 || newWj2->ttbar_decay_parton() == 4 ) &&
			 ( newLepB->ttbar_decay_parton() == 5 ) &&
			 ( newHadB->ttbar_decay_parton() == 6 ) ) {
			// Correct
			treeMan_->Fill(SolutionCategoryHist, 1 );
		}
		else if ( !( event->isSemiLeptonicElectron() || event->isSemiLeptonicMuon() ) ) {
			// Not a genuine semi leptonic event
			treeMan_->Fill(SolutionCategoryHist, 2 );
		}
		else if ( !allTTBarJetsPassSelection_ ) {
			// Not all jets from ttbar passed jet selection
			treeMan_->Fill(SolutionCategoryHist, 3 );
		}
		else if ( !allTTBarJetsPassedToFit_ ) {
			// Not all jets from ttbar were passed to fit
			treeMan_->Fill(SolutionCategoryHist, 4 );
		}
		else if ( allTTBarJetsPassedToFit_ && ( newWj1->ttbar_decay_parton() == 0 || newWj2->ttbar_decay_parton() == 0 || newLepB->ttbar_decay_parton() == 0 || newHadB->ttbar_decay_parton() != 0 ) ) {
			// All ttbar jets were available, but at least one incorrect jet used
			treeMan_->Fill(SolutionCategoryHist, 5 );
		}
		else if ( ( newWj1->ttbar_decay_parton() == 3 || newWj1->ttbar_decay_parton() == 4 ) &&
			 ( newWj2->ttbar_decay_parton() == 3 || newWj2->ttbar_decay_parton() == 4 ) &&
			 ( newLepB->ttbar_decay_parton() == 6 ) &&
			 ( newHadB->ttbar_decay_parton() == 5 ) ) {
			// B jets swapped, but W's correct
			treeMan_->Fill(SolutionCategoryHist, 6 );
		}
		else if ( ( ( newWj1->ttbar_decay_parton() == 5 || newWj1->ttbar_decay_parton() == 6 ) ||
			 ( newWj2->ttbar_decay_parton() == 5 || newWj2->ttbar_decay_parton() == 6 ) ) ||
			 ( ( newLepB->ttbar_decay_parton() == 3 || newLepB->ttbar_decay_parton() == 4 ) ||
			 ( newHadB->ttbar_decay_parton() == 3 || newHadB->ttbar_decay_parton() == 4 ) ) ) {
			// Light jet from W assigned as one of b's
			// Or B jet from top assigend as light jet
			treeMan_->Fill(SolutionCategoryHist, 7 );
		}
		else {
			// Just plain wrong
			treeMan_->Fill(SolutionCategoryHist, 8 );
		}
	}
	return hyp;
}

hitfit::Fit_Result HitFitAnalyser::performSecondKinematicFit(const hitfit::Lepjets_Event& unfittedEvent, const EventPtr event ) {

	// Same as first, but without top mass constraint
	// However recosntructed tops still constrained to have same mass
	BatHitFit hhFitter(electronTranslator_, muonTranslator_, jetTranslator_, metTranslator_, hitfitDefault_,
				hitfitLepWMass_, hitfitHadWMass_, 0);

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

	BAT::TtbarHypothesis unfittedBatEvent = BatEvent(unfittedEvent, event, "");

	// Add the lepton to the fit
	if ( isElectronChannel_ ) {
		BAT::ElectronPointer newLepton(new BAT::Electron());
		newLepton->setFourVector(unfittedBatEvent.leptonFromW->getFourVector());
		hhFitter.AddLepton( *newLepton );
	}
	else {
		BAT::MuonPointer newLepton(new BAT::Muon());
		newLepton->setFourVector(unfittedBatEvent.leptonFromW->getFourVector());
		hhFitter.AddLepton( *newLepton );
	}

	// Add the MET to the fit
	// Add missing transverse energy into HitFit
	hhFitter.SetMet(*unfittedBatEvent.met);

	// Add jets to the fit
	hhFitter.AddJet( *unfittedBatEvent.jet1FromW );
	hhFitter.AddJet( *unfittedBatEvent.jet2FromW );

	// Add b jets to the fit
	hhFitter.AddBJet( *unfittedBatEvent.leptonicBjet );
	hhFitter.AddBJet( *unfittedBatEvent.hadronicBJet );

	// Perform the fit
	size_t nHitFit = hhFitter.FitAllPermutation();

	// Get fit results
	std::vector<hitfit::Fit_Result> hitfitResult = hhFitter.GetFitAllPermutation();

	unsigned int bestChi2 = 9999;
	unsigned int bestSolution = 0;
	// Loop over solutions, and get the permutation that matches the intended permutation
	for (size_t fit = 0; fit != nHitFit; ++fit) {
		// Get the event after the fit
		hitfit::Fit_Result fitResult = hitfitResult[fit];
		if ( fitResult.ev().jet_permutation() == "WWbB" ) {
			double chi2 = fitResult.chisq();
			if ( chi2 < bestChi2 ) {
				bestChi2 = chi2;
				bestSolution = fit;
			}
		}
	}

	hitfit::Fit_Result bestFitResult = hitfitResult[bestSolution];

	return bestFitResult;

}

int HitFitAnalyser::positionOfLastTTBarJet(const JetCollection jets) {
	// Loop over jets and find position of last jet that comes from ttbar decay
	bool foundHadB = false;
	bool foundLepB = false;
	bool foundQ = false;
	bool foundQBar = false;
	for ( unsigned int jetIndex=0; jetIndex < jets.size(); ++jetIndex ) {
		JetPointer jet = jets[jetIndex];

		if ( jet->ttbar_decay_parton() == 3 ) foundQ = true;
		else if ( jet->ttbar_decay_parton() == 4 ) foundQBar = true;
		else if ( jet->ttbar_decay_parton() == 5 ) foundLepB = true;
		else if ( jet->ttbar_decay_parton() == 6 ) foundHadB = true;

		if ( foundQ && foundQBar && foundLepB && foundHadB ) return jetIndex;
	}
	return -1;

}

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) {
	cout << "Setting truth event" << endl;
	truthMatchEvent = mcEvent;
}

HitFitAnalyser::~HitFitAnalyser() {
}

void HitFitAnalyser::createHistograms() {
	// histMan_->setCurrentHistogramFolder(histogramFolder_);
	// histMan_->addH1D("AllJetsPt", "All jets Pt", 100, 0., 600.);
	// histMan_->addH1D("FittedTopMassAllSolutions", "Fitted top mass all solutions", 100, 0., 400.);
	// histMan_->addH1D("FittedTopPtAllSolutions", "Fitted top pt all solutions", 100, 0., 400.);

	// histMan_->addH1D("FittedTTbarMassAllSolutions", "Fitted ttbar mass all solutions", 100, 0., 1600.);
	// histMan_->addH1D("FittedTTbarPtAllSolutions", "Fitted ttbar pt all solutions", 100, 0., 1600.);

	// 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("FittedTopPtBestSolution", "Fitted top pt best solution", 20, 0., 400.);
	// histMan_->addH1D("FittedTopRapidityBestSolution", "Fitted top pt best solution", 50, -2.5, 2.5);

	// histMan_->addH1D("FittedTTbarMassBestSolution", "Fitted ttbar mass best solution", 80, 0., 1600.);
	// histMan_->addH1D("FittedTTbarPtBestSolution", "Fitted ttbar pt best solution", 15, 0., 300.);
	// histMan_->addH1D("FittedTTbarRapidityBestSolution", "Fitted ttbar pt best solution", 50, -2.5, 2.5);

	// 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);

}

void HitFitAnalyser::createTrees() {
	treeMan_->setCurrentFolder(histogramFolder_);
	treeMan_->addBranch("FitChiSquaredAllSolutions", "F", "HitFit" + Globals::treePrefix_, false);
	treeMan_->addBranch("FitChiSquaredBestSolutions", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FitChiSquaredProbabilityBestSolutions", "F", "HitFit" + Globals::treePrefix_);

	treeMan_->addBranch("FitChiSquaredBestSolutions_second", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FitChiSquaredProbabilityBestSolutions_second", "F", "HitFit" + Globals::treePrefix_);

	treeMan_->addBranch("FittedLeptonicTopPtBestSolution", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedHadronicTopPtBestSolution", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedLeptonicTopRapidityBestSolution", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedHadronicTopRapidityBestSolution", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedTTbarMassBestSolution", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedTTbarPtBestSolution", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedTTbarRapidityBestSolution", "F", "HitFit" + Globals::treePrefix_);

	treeMan_->addBranch("FittedLeptonicTopPtBestSolution_second", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedHadronicTopPtBestSolution_second", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedLeptonicTopRapidityBestSolution_second", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedHadronicTopRapidityBestSolution_second", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedTTbarMassBestSolution_second", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedTTbarPtBestSolution_second", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("FittedTTbarRapidityBestSolution_second", "F", "HitFit" + Globals::treePrefix_);

	treeMan_->addBranch("SolutionCategory", "F", "HitFit" + Globals::treePrefix_);
	treeMan_->addBranch("SolutionCategory_second", "F", "HitFit" + Globals::treePrefix_);

	treeMan_->addBranch("PositionOfLastTTbarJet", "F", "HitFit" + Globals::treePrefix_);
}


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

//-----------------------------------------------------------------------------
/*!\file    SelectivelyDampedLeastSquaresSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2020/03/27
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h>

// Pluginlib
#include <pluginlib/class_list_macros.h>

// other
#include <boost/algorithm/clamp.hpp>

/**
 * \class cartesian_controller_base::SelectivelyDampedLeastSquaresSolver
 *
 * Users may explicitly specify this solver with \a "selectively_damped_least_squares" as \a
 * ik_solver in their controllers.yaml configuration file for each controller:
 *
 * \code{.yaml}
 * <name_of_your_controller>:
 *     type: "<type_of_your_controller>"
 *     ik_solver: "selectively_damped_least_squares"
 *     ...
 * \endcode
 *
 */
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::SelectivelyDampedLeastSquaresSolver, cartesian_controller_base::IKSolver)

namespace cartesian_controller_base{

  SelectivelyDampedLeastSquaresSolver::SelectivelyDampedLeastSquaresSolver()
  {
  }

  SelectivelyDampedLeastSquaresSolver::~SelectivelyDampedLeastSquaresSolver(){}

  trajectory_msgs::JointTrajectoryPoint SelectivelyDampedLeastSquaresSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {
    // Compute joint Jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    Eigen::JacobiSVD<Eigen::Matrix<double, 6, Eigen::Dynamic> > JSVD(
      m_jnt_jacobian.data, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix<double, 6, 6> U = JSVD.matrixU();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> V = JSVD.matrixV();
    Eigen::Matrix<double, Eigen::Dynamic, 1> s = JSVD.singularValues();

    // Default recommendation by Buss and Kim.
    const double gamma_max = 3.141592653 / 4;

    Eigen::Matrix<double, Eigen::Dynamic, 1> sum_phi =
      Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(m_number_joints);

    // Compute each joint velocity with the SDLS method.  This implements the
    // algorithm as described in the paper (but for only one end-effector).
    // Also see Buss' own implementation:
    // https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html

    for (int i = 0; i < m_number_joints; ++i)
    {
      double alpha = U.col(i).transpose() * net_force;

      double N = U.col(i).head(3).norm();
      double M = 0;
      for (int j = 0; j < m_number_joints; ++j)
      {
        double rho = m_jnt_jacobian.data.col(j).head(3).norm();
        M += std::abs(V.col(i)[j]) * rho;
      }
      M *= 1.0 / s[i];

      double gamma = std::min(1.0, N / M) * gamma_max;

      Eigen::Matrix<double, Eigen::Dynamic, 1> phi = clampMaxAbs(1.0 / s[i] * alpha * V.col(i), gamma);
      sum_phi += phi;
    }

    m_current_velocities.data = clampMaxAbs(sum_phi, gamma_max);

    // Integrate once, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    applyJointLimits();

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  bool SelectivelyDampedLeastSquaresSolver::init(ros::NodeHandle& nh,
                                      const KDL::Chain& chain,
                                      const KDL::JntArray& upper_pos_limits,
                                      const KDL::JntArray& lower_pos_limits)
  {
    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_jacobian.resize(m_number_joints);

    return true;
  }

  Eigen::Matrix<double, Eigen::Dynamic, 1> SelectivelyDampedLeastSquaresSolver::clampMaxAbs(
    const Eigen::Matrix<double, Eigen::Dynamic, 1>& w, double d)
  {
    if (w.cwiseAbs().maxCoeff() <= d)
    {
      return w;
    }
    else
    {
      return d * w / w.cwiseAbs().maxCoeff();
    }
  }

} // namespace


/* Visualise a fHOG detector.
*  This program takes a fHOG detector as input and displays it in a window.
*
*  Usage:
*    ./view_hog detector.svm
*/

#include <dlib/gui_widgets.h>
#include <dlib/image_processing.h>
#include <dlib/cmd_line_parser.h>

#include <iostream>
#include <fstream>
#include <cstdlib>

using namespace std;
using namespace dlib;

int main(int argc, char** argv) {
  try {
    command_line_parser parser;
    parser.add_option("h","Display this help message.");

    parser.parse(argc, argv);
    const char* one_time_opts[] = {"h"};
    parser.check_one_time_options(one_time_opts);

    // Display help message
    if (parser.option("h")) {
      cout << "Usage: " << argv[0] << " [options] <fHOG SVM file>" << endl;
      parser.print_options();

      return EXIT_SUCCESS;
    }

    if (parser.number_of_arguments() == 0) {
      cout << "You must give a fHOG SVM file as input." << endl;
      cout << "\nTry the -h option for more information." << endl;
      return EXIT_FAILURE;
    }

    typedef scan_fhog_pyramid<pyramid_down<6> > image_scanner_type;
    object_detector<image_scanner_type> detector;
    deserialize(argv[1]) >> detector;
    image_window hogwin(draw_fhog(detector), "Learned fHOG detector");
    cout << "Press any key to exit!" << endl;
    cin.get(); // Wait input to exit
  }
  catch (exception& e) {
    cout << "\nexception thrown!" << endl;
    cout << e.what() << endl;
  }
}


#include "BootstrapRunner.h"
#include <vector>
#include <boost/timer.hpp>
using namespace std;

BootstrapRunner::BootstrapRunner(BoostrapConfig* bootstrapConfig, CftConfig cftConfig)
{
    this->bootstrapConfig = bootstrapConfig;
    this->cftData = new CftData(cftConfig);
    this->correlatorSet = new CorrelatorSet(cftData, bootstrapConfig->NumberOfScalarsToBootstrap);
}

BootstrapRunner::~BootstrapRunner()
{
    if (this->cftData != 0) {
        delete cftData;
    }

    if (this->correlatorSet != 0) {
        delete this->correlatorSet;
    }
}

float_type BootstrapRunner::ConstraintCost(CftData* cftData)
{
    float_type ret = .0;
    float_type lastDim = .0, dim;
    int lastSpin = -1;

    for (uint i = 1; i <= cftData->MaxPrimaryId(); i++) {
        dim = cftData->GetPrimaryDim(i);
        if (cftData->GetPrimarySpin(i) != lastSpin) {
            lastSpin = cftData->GetPrimarySpin(i);
            if (lastSpin == 0) {
                if (dim < cftData->D / 2.0 - 1) ret += (cftData->D / 2.0 - 1 - dim);
            } else {
                if (dim < lastSpin + cftData->D - 2) ret += (lastSpin + cftData->D - 2 - dim);
            }
        } else if (dim < lastDim) {
            ret += (lastDim - dim);
        }

        lastDim = dim;
    }

    return ret * bootstrapConfig->ConstraintFactor;
}

void BootstrapRunner::ConstraintCostDerivative(CftData* cftData, vector<int>& ops, vector<float_type>& opsDerivatives)
{
    assert(ops.size() == opsDerivatives.size());

    float_type dim, lastDim, nextDim;

    for (uint i = 0; i < ops.size(); i++) {
        int l = cftData->GetPrimarySpin(ops[i]);
        dim = cftData->GetPrimaryDim(ops[i]);

        if (ops[i] > 1 && cftData->GetPrimarySpin(ops[i] - 1) == l) {
            lastDim = cftData->GetPrimaryDim(ops[i] - 1);
            if (dim < lastDim) {
                opsDerivatives[i] -= bootstrapConfig->ConstraintFactor;
            }
        }

        if (ops[i] < cftData->MaxPrimaryId() && cftData->GetPrimarySpin(ops[i] + 1) == l) {
            nextDim = cftData->GetPrimaryDim(ops[i] + 1);
            if (dim > nextDim) {
                opsDerivatives[i] += bootstrapConfig->ConstraintFactor;
            }
        }

        if (ops[i] == 1 || cftData->GetPrimarySpin(ops[i] - 1) != l) {
            if (l == 0 && dim <= cftData->D / 2.0 - 1) {
                opsDerivatives[i] -= bootstrapConfig->ConstraintFactor;
            }

            if (l > 0 && dim <= cftData->D + l - 2.0) {
                opsDerivatives[i] -= bootstrapConfig->ConstraintFactor;
            }
        }
    }
}

void BootstrapRunner::Run()
{
    vector<OpeCoefficientKey> coefs;
    vector<int> ops;

    for (uint op = 1; op <= cftData->MaxPrimaryId(); op++) {
        if (op != cftData->StressTensorId) {
            ops.push_back(op);
        }
    }

    for (uint op1 = 1; op1 <= bootstrapConfig->NumberOfScalarsToBootstrap; op1++) {
        for (uint op2 = op1; op2 <= bootstrapConfig->NumberOfScalarsToBootstrap; op2++) {
            for (uint op3 = op2; op3 <= cftData->MaxPrimaryId(); op3++) {
                if (op3 != cftData->StressTensorId) {
                    coefs.push_back(OpeCoefficientKey(op1, op2, op3));
                }
            }
        }
    }

    vector<cpx_t> zs;
    zs.reserve(bootstrapConfig->SamplesEachStep);
    for (uint i = 0; i < bootstrapConfig->SamplesEachStep; i++) {
        float_type dr = random(0.0, 0.2);
        float_type theta = random(0.0, 2 * acos(-1.0));
        zs.push_back(cpx_t(dr * cos(theta), dr * sin(theta)) + 0.5);
    }

    vector<float_type> opsDerivatives(ops.size(), .0);
    vector<float_type> opsDerivativeTot(ops.size(), .0);
    vector<float_type> coefsDerivatives(coefs.size(), .0);
    vector<float_type> coefsDerivativeTot(coefs.size(), .0);

    float_type lastCost = std::numeric_limits<float_type>::max();
    vector<int> scalarIds;
    for (uint i = 1; i <= cftData->MaxScalarId(); i++) scalarIds.push_back(i);

    boost::timer stopwatch;

    while (true) {
        std::cout << "Time elapsed: " << stopwatch.elapsed() << " seconds." << endl;
        cftData->Output(scalarIds);

        opsDerivativeTot.resize(ops.size(), .0);
        coefsDerivativeTot.resize(coefs.size(), .0);

        float_type cost = .0;

        for (uint i = 0; i < zs.size(); i++) {
            this->correlatorSet->CostDerivatives(zs[i], ops, coefs, opsDerivatives, coefsDerivatives);
            cost += this->correlatorSet->Cost(zs[i]);

            for (uint j = 0; j < opsDerivatives.size(); j++) {
                opsDerivativeTot[j] += opsDerivatives[j];
            }

            for (uint j = 0; j < coefsDerivatives.size(); j++) {
                coefsDerivativeTot[j] += coefsDerivatives[j];
            }
        }

        cost /= zs.size();
        std::cout << "pure cost = " << cost << "\t";
        cost += ConstraintCost(cftData);
        std::cout << "total cost = " << cost << std::endl;

        if (cost < bootstrapConfig->Accuracy) break;

        float_type gradNorm = .0;
        for (uint j = 0; j < coefsDerivativeTot.size(); j++) {
            coefsDerivativeTot[j] /= zs.size();
            gradNorm += coefsDerivativeTot[j] * coefsDerivativeTot[j];
        }

        std::cout << "Coefs Derivative: " << coefsDerivativeTot << std::endl;

        for (uint j = 0; j < opsDerivativeTot.size(); j++) {
            opsDerivativeTot[j] /= zs.size();
        }

        std::cout << "Ops Derivative: " << opsDerivativeTot << std::endl;

        ConstraintCostDerivative(cftData, ops, opsDerivativeTot);

        std::cout << "Ops Derivative with constraints: " << opsDerivativeTot << std::endl;

        for (uint j = 0; j < opsDerivativeTot.size(); j++) {
            gradNorm += opsDerivativeTot[j] * opsDerivativeTot[j];
        }

        float_type factor = gradNorm / ((ops.size() + coefs.size()));
        factor = sqrt(factor);

        for (uint j = 0; j < coefsDerivativeTot.size(); j++) {
            coefsDerivativeTot[j] /= factor;
        }

        for (uint j = 0; j < opsDerivativeTot.size(); j++) {
            opsDerivativeTot[j] /= factor;
        }

        std::cout << "Coefs Derivative normalized: " << coefsDerivativeTot << std::endl;
        std::cout << "Ops Derivative normalized: " << opsDerivativeTot << std::endl;

        for (uint j = 0; j < opsDerivativeTot.size(); j++) {
            float_type dim = cftData->GetPrimaryDim(ops[j]);
            cftData->SetPrimaryDim(ops[j], dim - opsDerivativeTot[j] * bootstrapConfig->Step);
        }

        for (uint j = 0; j < coefsDerivativeTot.size(); j++) {
            float_type coef = cftData->GetOpeCoefficient(coefs[j]);
            cftData->SetOpeCoefficient(coefs[j], coef - coefsDerivativeTot[j] * bootstrapConfig->Step);
        }

        this->correlatorSet->UpdateCftData(cftData);
        lastCost = cost;
    }
}


#ifdef STEEMIT_BUILD_TESTNET

#include <boost/test/unit_test.hpp>

#include <golos/chain/account_object.hpp>
#include <golos/chain/comment_object.hpp>
#include <golos/protocol/steem_operations.hpp>

#include <golos/plugins/market_history/market_history_plugin.hpp>

#include "database_fixture.hpp"

using namespace golos::chain;
using namespace golos::protocol;

BOOST_FIXTURE_TEST_SUITE(market_history, database_fixture)

    BOOST_AUTO_TEST_CASE(mh_test) {
        using namespace golos::plugins::market_history;
        using by_id = golos::plugins::market_history::by_id;

        try {
            initialize();

            auto &mh_plugin = appbase::app().register_plugin<market_history_plugin>();
            boost::program_options::variables_map options;
            mh_plugin.plugin_initialize(options);

            open_database();

            startup();
            mh_plugin.plugin_startup();

            ACTORS((alice)(bob)(sam));
            generate_block();

            fund("alice", ASSET("1000.000 GOLOS"));
            fund("alice", ASSET("1000.000 GBG"));
            fund("bob", ASSET("1000.000 GOLOS"));
            fund("sam", ASSET("1000.000 GOLOS"));

            set_price_feed(price(ASSET("0.500 GBG"), ASSET("1.000 GOLOS")));

            signed_transaction tx;

            const auto &bucket_idx = db->get_index<bucket_index>().indices().get<by_bucket>();
            const auto &order_hist_idx = db->get_index<order_history_index>().indices().get<by_id>();

            BOOST_REQUIRE(bucket_idx.begin() == bucket_idx.end());
            BOOST_REQUIRE(order_hist_idx.begin() == order_hist_idx.end());
            validate_database();

            auto fill_order_a_time = db->head_block_time();
            auto time_a = fc::time_point_sec((fill_order_a_time.sec_since_epoch() / 15) * 15);

            limit_order_create_operation op;
            op.owner = "alice";
            op.amount_to_sell = ASSET("1.000 GBG");
            op.min_to_receive = ASSET("2.000 GOLOS");
            tx.operations.push_back(op);
            tx.set_expiration(
                    db->head_block_time() + STEEMIT_MAX_TIME_UNTIL_EXPIRATION);
            tx.sign(alice_private_key, db->get_chain_id());
            db->push_transaction(tx, 0);

            tx.operations.clear();
            tx.signatures.clear();

            op.owner = "bob";
            op.amount_to_sell = ASSET("1.500 GOLOS");
            op.min_to_receive = ASSET("0.750 GBG");
            tx.operations.push_back(op);
            tx.sign(bob_private_key, db->get_chain_id());
            db->push_transaction(tx, 0);

            generate_blocks(db->head_block_time() + (60 * 90));

            auto fill_order_b_time = db->head_block_time();

            tx.operations.clear();
            tx.signatures.clear();

            op.owner = "sam";
            op.amount_to_sell = ASSET("1.000 GOLOS");
            op.min_to_receive = ASSET("0.500 GBG");
            tx.operations.push_back(op);
            tx.set_expiration(
                    db->head_block_time() + STEEMIT_MAX_TIME_UNTIL_EXPIRATION);
            tx.sign(sam_private_key, db->get_chain_id());
            db->push_transaction(tx, 0);

            generate_blocks(db->head_block_time() + 60);

            auto fill_order_c_time = db->head_block_time();

            tx.operations.clear();
            tx.signatures.clear();

            op.owner = "alice";
            op.amount_to_sell = ASSET("0.500 GBG");
            op.min_to_receive = ASSET("0.900 GOLOS");
            tx.operations.push_back(op);
            tx.set_expiration(
                    db->head_block_time() + STEEMIT_MAX_TIME_UNTIL_EXPIRATION);
            tx.sign(alice_private_key, db->get_chain_id());
            db->push_transaction(tx, 0);

            tx.operations.clear();
            tx.signatures.clear();

            op.owner = "bob";
            op.amount_to_sell = ASSET("0.450 GOLOS");
            op.min_to_receive = ASSET("0.250 GBG");
            tx.operations.push_back(op);
            tx.set_expiration(
                    db->head_block_time() + STEEMIT_MAX_TIME_UNTIL_EXPIRATION);
            tx.sign(bob_private_key, db->get_chain_id());
            db->push_transaction(tx, 0);
            validate_database();

            auto bucket = bucket_idx.begin();

            BOOST_REQUIRE(bucket->seconds == 15);
            BOOST_REQUIRE(bucket->open == time_a);
            BOOST_REQUIRE(bucket->high_steem == ASSET("1.500 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.750 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 15);
            BOOST_REQUIRE(bucket->open == time_a + (60 * 90));
            BOOST_REQUIRE(bucket->high_steem == ASSET("0.500 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.250 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 15);
            BOOST_REQUIRE(bucket->open == time_a + (60 * 90) + 60);
            BOOST_REQUIRE(bucket->high_steem == ASSET("0.450 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("0.450 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("0.950 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.500 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 60);
            BOOST_REQUIRE(bucket->open == time_a);
            BOOST_REQUIRE(bucket->high_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.750 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 60);
            BOOST_REQUIRE(bucket->open == time_a + (60 * 90));
            BOOST_REQUIRE(bucket->high_steem == ASSET("0.500 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.250 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 60);
            BOOST_REQUIRE(bucket->open == time_a + (60 * 90) + 60);
            BOOST_REQUIRE(bucket->high_steem == ASSET("0.450 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("0.450 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("0.950 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.500 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 300);
            BOOST_REQUIRE(bucket->open == time_a);
            BOOST_REQUIRE(bucket->high_steem == ASSET("1.500 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.750 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 300);
            BOOST_REQUIRE(bucket->open == time_a + (60 * 90));
            BOOST_REQUIRE(bucket->high_steem == ASSET("0.450 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("0.450 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("1.450 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.750 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 3600);
            BOOST_REQUIRE(bucket->open == time_a);
            BOOST_REQUIRE(bucket->high_steem == ASSET("1.500 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.750 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket->seconds == 3600);
            BOOST_REQUIRE(bucket->open == time_a + (60 * 60));
            BOOST_REQUIRE(bucket->high_steem == ASSET("0.450 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("0.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("0.450 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("1.450 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("0.750 GBG").amount);
            bucket++;

            // In Steem GENESIS_TIME is rounded to seconds per day, that is why STEEMIT_GENESIS_TIME is used for validatation
            // But in Golos GENESIS_TIME isn't rounded to seconds per day
            const auto round_genesis_time = fc::time_point_sec((STEEMIT_GENESIS_TIME.sec_since_epoch() / 86400) * 86400);
            BOOST_REQUIRE(bucket->seconds == 86400);
            BOOST_REQUIRE(bucket->open == round_genesis_time);
            BOOST_REQUIRE(bucket->high_steem == ASSET("0.450 GOLOS ").amount);
            BOOST_REQUIRE(bucket->high_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->low_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->low_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->open_steem == ASSET("1.500 GOLOS").amount);
            BOOST_REQUIRE(bucket->open_sbd == ASSET("0.750 GBG").amount);
            BOOST_REQUIRE(bucket->close_steem == ASSET("0.450 GOLOS").amount);
            BOOST_REQUIRE(bucket->close_sbd == ASSET("0.250 GBG").amount);
            BOOST_REQUIRE(bucket->steem_volume == ASSET("2.950 GOLOS").amount);
            BOOST_REQUIRE(bucket->sbd_volume == ASSET("1.500 GBG").amount);
            bucket++;

            BOOST_REQUIRE(bucket == bucket_idx.end());

            auto order = order_hist_idx.begin();

            BOOST_REQUIRE(order->time == fill_order_a_time);
            BOOST_REQUIRE(order->op.current_owner == "bob");
            BOOST_REQUIRE(order->op.current_orderid == 0);
            BOOST_REQUIRE(order->op.current_pays == ASSET("1.500 GOLOS"));
            BOOST_REQUIRE(order->op.open_owner == "alice");
            BOOST_REQUIRE(order->op.open_orderid == 0);
            BOOST_REQUIRE(order->op.open_pays == ASSET("0.750 GBG"));
            order++;

            BOOST_REQUIRE(order->time == fill_order_b_time);
            BOOST_REQUIRE(order->op.current_owner == "sam");
            BOOST_REQUIRE(order->op.current_orderid == 0);
            BOOST_REQUIRE(order->op.current_pays == ASSET("0.500 GOLOS"));
            BOOST_REQUIRE(order->op.open_owner == "alice");
            BOOST_REQUIRE(order->op.open_orderid == 0);
            BOOST_REQUIRE(order->op.open_pays == ASSET("0.250 GBG"));
            order++;

            BOOST_REQUIRE(order->time == fill_order_c_time);
            BOOST_REQUIRE(order->op.current_owner == "alice");
            BOOST_REQUIRE(order->op.current_orderid == 0);
            BOOST_REQUIRE(order->op.current_pays == ASSET("0.250 GBG"));
            BOOST_REQUIRE(order->op.open_owner == "sam");
            BOOST_REQUIRE(order->op.open_orderid == 0);
            BOOST_REQUIRE(order->op.open_pays == ASSET("0.500 GOLOS"));
            order++;

            BOOST_REQUIRE(order->time == fill_order_c_time);
            BOOST_REQUIRE(order->op.current_owner == "bob");
            BOOST_REQUIRE(order->op.current_orderid == 0);
            BOOST_REQUIRE(order->op.current_pays == ASSET("0.450 GOLOS"));
            BOOST_REQUIRE(order->op.open_owner == "alice");
            BOOST_REQUIRE(order->op.open_orderid == 0);
            BOOST_REQUIRE(order->op.open_pays == ASSET("0.250 GBG"));
            order++;

            BOOST_REQUIRE(order == order_hist_idx.end());
        }
        FC_LOG_AND_RETHROW()
    }

BOOST_AUTO_TEST_SUITE_END()
#endif


/*
The mapping algorithm is an advanced implementation of the following open source project:
  [blam](https://github.com/erik-nelson/blam).
Modifier: livox  dev@livoxtech.com
Modifier: enzan  liu@enzan-k.com

Copyright (c) 2015, The Regents of the University of California (Regents).
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

   1. Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.

   2. Redistributions in binary form must reproduce the above
      copyright notice, this list of conditions and the following
      disclaimer in the documentation and/or other materials provided
      with the distribution.

   3. Neither the name of the copyright holder nor the names of its
      contributors may be used to endorse or promote products derived
      from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/

#include <cstdlib>
#include <cstdio>
#include <ctime>
#include <sstream>
#include <fstream>
#include <string>
#include <iostream>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <sys/stat.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <geometry_utils/Transform3.h>
#include <point_cloud_mapper/PointCloudMapper.h>

using namespace std;
namespace gu = geometry_utils;

#define PI (3.1415926535897932346f)

#define PBSTR "||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||"
#define PBWIDTH 60

void printProgress(double percentage)
{
    int val = (int)(percentage * 100);
    int lpad = (int)(percentage * PBWIDTH);
    int rpad = PBWIDTH - lpad;
    printf("\r%3d%% [%.*s%*s]", val, lpad, PBSTR, rpad, "");
    fflush(stdout);
}

char framesDir[100] = "../data/Target-LiDAR-Frames";

std::string itos(int i)
{
    std::stringstream s;
    s << i;
    return s.str();
}

int main()
{
    std::cout << "Start calibration..." << std::endl;

    // 1 Reading H-LiDAR map data

    std::cout << "Reading H-LiDAR map data..." << std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr H_LiDAR_Map(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("../data/H-LiDAR-Map-data/H_LiDAR_Map.pcd", *H_LiDAR_Map) == -1)
    {
        PCL_ERROR("Couldn't read H_LiDAR_Map \n");
        return (-1);
    }
    std::cout << "Loaded " << H_LiDAR_Map->size() << " data points from H_LiDAR_Map.pcd" << std::endl;

    // put it into map
    PointCloudMapper maps;
    maps.Initialize();
    pcl::PointCloud<pcl::PointXYZ>::Ptr unused(new pcl::PointCloud<pcl::PointXYZ>);
    maps.InsertPoints(H_LiDAR_Map, unused.get());

    // 2 Reading H-LiDAR's Trajectory and init guess
    ifstream T_Mat_File("../data/T_Matrix.txt");
    Eigen::Matrix4f T_Matrix = Eigen::Matrix4f::Identity();

    ifstream initFile("../data/Init_Matrix.txt");

    Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f init_guess_0 = Eigen::Matrix4f::Identity();

    for (int mat_i = 0; mat_i != 4; mat_i++)
    {
        for (int mat_j = 0; mat_j != 4; mat_j++)
        {
            initFile >> init_guess(mat_i, mat_j);
        }
    }

    init_guess_0 = init_guess;
    // 3 Reading L-LiDAR frames

    struct dirent **namelist;
    int framenumbers = scandir(framesDir, &namelist, 0, alphasort) - 2;
    int frame_count = 100000;
    int cframe_count = 0;
    cout << "Loaded " << framenumbers << " frames from Target-LiDAR" << endl;

    // prepare ICP
    pcl::PointCloud<pcl::PointXYZ>::Ptr ICP_output_cloud(new pcl::PointCloud<pcl::PointXYZ>); //not use,but necessary
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setTransformationEpsilon(0.0000000001); //0.0000000001
    icp.setMaxCorrespondenceDistance(10);
    icp.setMaximumIterations(35);
    icp.setRANSACIterations(0);
    icp.setMaximumOptimizerIterations(50); // default 20

    // prepare display
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
        viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer_final->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> map_color(H_LiDAR_Map, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> match_color(H_LiDAR_Map, 0, 255, 0);

    viewer_final->addPointCloud<pcl::PointXYZ>(H_LiDAR_Map, map_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
    viewer_final->addPointCloud<pcl::PointXYZ>(H_LiDAR_Map, match_color, "match cloud"); //display the match cloud

    // prepare save matrix
    char filename[] = "../data/calib_data.txt";
    ofstream fout(filename);
    fout.setf(ios::fixed, ios::floatfield);
    fout.precision(7);

    // START
    while (!viewer_final->wasStopped())
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr frames(new pcl::PointCloud<pcl::PointXYZ>);
        if (pcl::io::loadPCDFile<pcl::PointXYZ>(string(framesDir) + "/" + itos(frame_count) + ".pcd", *frames) == -1)
        {
            PCL_ERROR("Couldn't read H_LiDAR_Map \n");
            return (-1);
        }
        //std::cout << "Loaded " << frames->size() << " data points from frames" << std::endl;

        //Load H-LiDAR's Trajectory
        for (int mat_i = 0; mat_i != 4; mat_i++)
        {
            for (int mat_j = 0; mat_j != 4; mat_j++)
            {
                T_Mat_File >> T_Matrix(mat_i, mat_j);
            }
        }

        // 4 Start calibration

        pcl::PointCloud<pcl::PointXYZ>::Ptr trans_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr final_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::transformPointCloud(*frames, *trans_output_cloud, init_guess); //Tiny_T * init_guess   ->update this matrix
        pcl::transformPointCloud(*trans_output_cloud, *final_output_cloud, T_Matrix);

        pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_L(new pcl::PointCloud<pcl::PointXYZ>); //201 neighbors points from nap202
        maps.ApproxNearestNeighbors(*final_output_cloud, neighbors_L.get());

        // INVERSE T_mat
        gu::Transform3 inverse_mat;
        inverse_mat.translation = gu::Vec3(T_Matrix(0, 3), T_Matrix(1, 3), T_Matrix(2, 3));
        inverse_mat.rotation = gu::Rot3(T_Matrix(0, 0), T_Matrix(0, 1), T_Matrix(0, 2),
                                        T_Matrix(1, 0), T_Matrix(1, 1), T_Matrix(1, 2),
                                        T_Matrix(2, 0), T_Matrix(2, 1), T_Matrix(2, 2));

        const gu::Transform3 estimate = gu::PoseInverse(inverse_mat); // integrated_estimate from config parameters
        const Eigen::Matrix<double, 3, 3> T_Matrix_Inverse_R = estimate.rotation.Eigen();
        const Eigen::Matrix<double, 3, 1> T_Matrix_Inverse_T = estimate.translation.Eigen();

        Eigen::Matrix4d T_Matrix_Inverse;
        T_Matrix_Inverse.block(0, 0, 3, 3) = T_Matrix_Inverse_R;
        T_Matrix_Inverse.block(0, 3, 3, 1) = T_Matrix_Inverse_T;

        // Core step
        pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_trans(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::transformPointCloud(*neighbors_L, *neighbors_trans, T_Matrix_Inverse);

        // Do ICP and get the tiny trans T
        icp.setInputSource(trans_output_cloud); // 201
        icp.setInputTarget(neighbors_trans); // 202 (201's neighbor's point cloud)
        icp.align(*ICP_output_cloud);
        const Eigen::Matrix4f Tiny_T = icp.getFinalTransformation();

        //std::cout << "Score: " << icp.getFitnessScore() << std::endl;
        if (icp.getFitnessScore() > 1)
        {
            //std::cout<<"not match, skip this"<<std::endl;
            init_guess = init_guess_0;
            //continue;
        }
        else
        {
            Eigen::Matrix4f Final_Calib_T = Eigen::Matrix4f::Identity();

            Final_Calib_T = Tiny_T * init_guess;
            //std::cout << Final_Calib_T.matrix() << std::endl;
            init_guess = Final_Calib_T;

            // Out put Euler angle
            gu::Vector3 EulerAngle;
            gu::Rot3 rot_mat(Final_Calib_T(0, 0), Final_Calib_T(0, 1), Final_Calib_T(0, 2),
                             Final_Calib_T(1, 0), Final_Calib_T(1, 1), Final_Calib_T(1, 2),
                             Final_Calib_T(2, 0), Final_Calib_T(2, 1), Final_Calib_T(2, 2));
            EulerAngle = rot_mat.GetEulerZYX();
            const Eigen::Matrix<double, 3, 1> EulerAngle_T = EulerAngle.Eigen();
            //std::cout<<"EulerAngle:  "<<EulerAngle_T(0,0)<<"  "<<EulerAngle_T(1,0)<<"  "<<EulerAngle_T(2,0)<<"  "<<std::endl;

            if (icp.getFitnessScore() < 0.1)
                fout << frame_count - 100000 << " " << icp.getFitnessScore() << " " << Final_Calib_T(0, 3) << " " << Final_Calib_T(1, 3) << " " << Final_Calib_T(2, 3) << " " << EulerAngle_T(0, 0) << " " << EulerAngle_T(1, 0) << " " << EulerAngle_T(2, 0) << endl; //x,y,z,roll,pitch,yaw
        }

        frame_count++;
        cframe_count++;

        printProgress((double)cframe_count / (double)framenumbers);
        viewer_final->updatePointCloud<pcl::PointXYZ>(final_output_cloud, match_color, "match cloud");
        viewer_final->spinOnce(10);

        if (cframe_count == framenumbers)
        {
            std::cout << "\n Matching complete." << std::endl;
            break;
        }
    }
    return 0;
}


#include <boost/units/systems/si/angular_acceleration.hpp>


#ifndef FSTCRITERIONSUBSETSIZE_H
#define FSTCRITERIONSUBSETSIZE_H

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

   \file    criterion_subsetsize.hpp
   \brief   Criterion_Subset_Size returns negative subset size - rates smaller subsets higher
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    October 2010
   \version 3.0.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include "error.hpp"
#include "global.hpp"
#include "criterion.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

/*! \brief Returns negative subset size - rates smaller subsets higher.

    This trivial criterion is intended as secondary criterion only to be used
    in conjunction with Result_Tracker_Regularizer, implementing feature
    selection regularization (improvement of feature selection generalization
    ability and result robustness) as described in paper "Somol, Grim, Pudil:
    The Problem of Fragile Feature Subset Preference in Feature Selection Methods
    and A Proposal of Algorithmic Workaround. In Proc. ICPR 2010.  IEEE Computer
    Society, 2010".
*/
template<class RETURNTYPE, class SUBSET>
class Criterion_Subset_Size : public Criterion<RETURNTYPE,SUBSET> { // adapter class
public:
	typedef boost::shared_ptr<SUBSET> PSubset;
	Criterion_Subset_Size() {notify("Criterion_Subset_Size constructor.");}
	virtual ~Criterion_Subset_Size() {notify("Criterion_Subset_Size destructor.");}

	virtual bool evaluate(RETURNTYPE &result, const PSubset sub) {assert(sub); result=RETURNTYPE(sub->get_d()); notify("CSS result=",result,", getd ",sub->get_d(),"."); return true;}

	Criterion_Subset_Size* clone() const;
	Criterion_Subset_Size* sharing_clone() const {throw fst_error("Criterion_Subset_Size::sharing_clone() not supported, use Criterion_Subset_Size::clone() instead.");}
	Criterion_Subset_Size* stateless_clone() const {throw fst_error("Criterion_Subset_Size::stateless_clone() not supported, use Criterion_Subset_Size::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const {os << "Criterion_Subset_Size()"; return os;}
private:
	Criterion_Subset_Size(const Criterion_Subset_Size& css); // copy-constructor
protected:
};

template<class RETURNTYPE, class SUBSET>
Criterion_Subset_Size<RETURNTYPE,SUBSET>::Criterion_Subset_Size(const Criterion_Subset_Size& css)
{
	notify("Criterion_Subset_Size copy-constructor.");
}

template<class RETURNTYPE, class SUBSET>
Criterion_Subset_Size<RETURNTYPE,SUBSET>* Criterion_Subset_Size<RETURNTYPE,SUBSET>::clone() const
{
	Criterion_Subset_Size<RETURNTYPE,SUBSET> *clone=new Criterion_Subset_Size<RETURNTYPE,SUBSET>(*this);
	clone->set_cloned();
	return clone;
}

} // namespace
#endif // FSTCRITERIONSUBSETSIZE_H ///:~


// $Id$
// vim:tabstop=2
/***********************************************************************
K-best Batch MIRA for Moses
Copyright (C) 2012, National Research Council Canada / Conseil national
de recherches du Canada
***********************************************************************/

/**
  * k-best Batch Mira, as described in:
  *
  * Colin Cherry and George Foster
  * Batch Tuning Strategies for Statistical Machine Translation
  * NAACL 2012
  *
  * Implemented by colin.cherry@nrc-cnrc.gc.ca
  *
  * To license implementations of any of the other tuners in that paper,
  * please get in touch with any member of NRC Canada's Portage project
  *
  * Input is a set of n-best lists, encoded as feature and score files.
  *
  * Output is a weight file that results from running MIRA on these
  * n-btest lists for J iterations. Will return the set that maximizes
  * training BLEU.
 **/

#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <ctime>
#include <cassert>
#include <iostream>
#include <string>
#include <vector>
#include <utility>
#include <algorithm>

#include <boost/program_options.hpp>
#include <boost/scoped_ptr.hpp>

#include "BleuScorer.h"
#include "HypPackEnumerator.h"
#include "MiraFeatureVector.h"
#include "MiraWeightVector.h"

using namespace std;
using namespace MosesTuning;

namespace po = boost::program_options;

ValType evaluate(HypPackEnumerator* train, const AvgWeightVector& wv) {
  vector<ValType> stats(kBleuNgramOrder*2+1,0);
  for(train->reset(); !train->finished(); train->next()) {
    // Find max model
    size_t max_index=0;
    ValType max_score=0;
    for(size_t i=0;i<train->cur_size();i++) {
      MiraFeatureVector vec(train->featuresAt(i));
      ValType score = wv.score(vec);
      if(i==0 || score > max_score) {
        max_index = i;
        max_score = score;
      }
    }
    // Update stats
    const vector<float>& sent = train->scoresAt(max_index);
    for(size_t i=0;i<sent.size();i++) {
      stats[i]+=sent[i];
    }
  }
  return unsmoothedBleu(stats);
}

int main(int argc, char** argv)
{
  const ValType BLEU_RATIO = 5;
  bool help;
  string denseInitFile;
  string sparseInitFile;
  vector<string> scoreFiles;
  vector<string> featureFiles;
  int seed;
  string outputFile;
  float c = 0.01;      // Step-size cap C
  float decay = 0.999; // Pseudo-corpus decay \gamma
  int n_iters = 60;    // Max epochs J
  bool streaming = false; // Stream all k-best lists?
  bool no_shuffle = false; // Don't shuffle, even for in memory version
  bool model_bg = false; // Use model for background corpus
  bool verbose = false; // Verbose updates
  bool safe_hope = false; // Model score cannot have more than BLEU_RATIO times more influence than BLEU

  // Command-line processing follows pro.cpp
  po::options_description desc("Allowed options");
  desc.add_options()
      ("help,h", po::value(&help)->zero_tokens()->default_value(false), "Print this help message and exit")
      ("scfile,S", po::value<vector<string> >(&scoreFiles), "Scorer data files")
      ("ffile,F", po::value<vector<string> > (&featureFiles), "Feature data files")
      ("random-seed,r", po::value<int>(&seed), "Seed for random number generation")
      ("output-file,o", po::value<string>(&outputFile), "Output file")
      ("cparam,C", po::value<float>(&c), "MIRA C-parameter, lower for more regularization (default 0.01)")
      ("decay,D", po::value<float>(&decay), "BLEU background corpus decay rate (default 0.999)")
      ("iters,J", po::value<int>(&n_iters), "Number of MIRA iterations to run (default 60)")
      ("dense-init,d", po::value<string>(&denseInitFile), "Weight file for dense features")
      ("sparse-init,s", po::value<string>(&sparseInitFile), "Weight file for sparse features")
      ("streaming", po::value(&streaming)->zero_tokens()->default_value(false), "Stream n-best lists to save memory, implies --no-shuffle")
      ("no-shuffle", po::value(&no_shuffle)->zero_tokens()->default_value(false), "Don't shuffle hypotheses before each epoch")
      ("model-bg", po::value(&model_bg)->zero_tokens()->default_value(false), "Use model instead of hope for BLEU background")
      ("verbose", po::value(&verbose)->zero_tokens()->default_value(false), "Verbose updates")
      ("safe-hope", po::value(&safe_hope)->zero_tokens()->default_value(false), "Mode score's influence on hope decoding is limited")
      ;

  po::options_description cmdline_options;
  cmdline_options.add(desc);
  po::variables_map vm;
  po::store(po::command_line_parser(argc,argv).
            options(cmdline_options).run(), vm);
  po::notify(vm);
  if (help) {
      cout << "Usage: " + string(argv[0]) +  " [options]" << endl;
      cout << desc << endl;
      exit(0);
  }

  cerr << "kbmira with c=" << c << " decay=" << decay << " no_shuffle=" << no_shuffle << endl;

  if (vm.count("random-seed")) {
    cerr << "Initialising random seed to " << seed << endl;
    srand(seed);
  } else {
    cerr << "Initialising random seed from system clock" << endl;
    srand(time(NULL));
  }

  // Initialize weights
  ///
  // Dense
  vector<parameter_t> initParams;
  if(!denseInitFile.empty()) {
    ifstream opt(denseInitFile.c_str());
    string buffer;
    if (opt.fail()) {
      cerr << "could not open dense initfile: " << denseInitFile << endl;
      exit(3);
    }
    parameter_t val;
    getline(opt,buffer);
    istringstream strstrm(buffer);
    while(strstrm >> val) {
      initParams.push_back(val);
    }
    opt.close();
  }
  size_t initDenseSize = initParams.size();
  // Sparse
  if(!sparseInitFile.empty()) {
    if(initDenseSize==0) {
      cerr << "sparse initialization requires dense initialization" << endl;
      exit(3);
    }
    ifstream opt(sparseInitFile.c_str());
    if(opt.fail()) {
      cerr << "could not open sparse initfile: " << sparseInitFile << endl;
      exit(3);
    }
    int sparseCount=0;
    parameter_t val; std::string name;
    while(opt >> name >> val) {
      size_t id = SparseVector::encode(name) + initDenseSize;
      while(initParams.size()<=id) initParams.push_back(0.0);
      initParams[id] = val;
      sparseCount++;
    }
    cerr << "Found " << sparseCount << " initial sparse features" << endl;
    opt.close();
  }

  MiraWeightVector wv(initParams);

  // Initialize background corpus
  vector<ValType> bg;
  for(int j=0;j<kBleuNgramOrder;j++){
    bg.push_back(kBleuNgramOrder-j);
    bg.push_back(kBleuNgramOrder-j);
  }
  bg.push_back(kBleuNgramOrder);

  // Training loop
  boost::scoped_ptr<HypPackEnumerator> train;
  if(streaming)
    train.reset(new StreamingHypPackEnumerator(featureFiles, scoreFiles));
  else
    train.reset(new RandomAccessHypPackEnumerator(featureFiles, scoreFiles, no_shuffle));
  cerr << "Initial BLEU = " << evaluate(train.get(), wv.avg()) << endl;
  ValType bestBleu = 0;
  for(int j=0;j<n_iters;j++)
  {
    // MIRA train for one epoch
    int iNumHyps = 0;
    int iNumExamples = 0;
    int iNumUpdates = 0;
    ValType totalLoss = 0.0;
    for(train->reset(); !train->finished(); train->next()) {
      // Hope / fear decode
      ValType hope_scale = 1.0;
      size_t hope_index=0, fear_index=0, model_index=0;
      ValType hope_score=0, fear_score=0, model_score=0;
      int iNumHypsBackup = iNumHyps;
      for(size_t safe_loop=0; safe_loop<2; safe_loop++) {
        iNumHyps = iNumHypsBackup;
        ValType hope_bleu, hope_model;
        for(size_t i=0; i< train->cur_size(); i++) {
          const MiraFeatureVector& vec=train->featuresAt(i);
          ValType score = wv.score(vec);
          ValType bleu = sentenceLevelBackgroundBleu(train->scoresAt(i),bg);
          // Hope
          if(i==0 || (hope_scale*score + bleu) > hope_score) {
            hope_score = hope_scale*score + bleu;
            hope_index = i;
            hope_bleu = bleu;
            hope_model = score;
          }
          // Fear
          if(i==0 || (score - bleu) > fear_score) {
            fear_score = score - bleu;
            fear_index = i;
          }
          // Model
          if(i==0 || score > model_score) {
            model_score = score;
            model_index = i;
          }
          iNumHyps++;
        }
        // Outer loop rescales the contribution of model score to 'hope' in antagonistic cases
        // where model score is having far more influence than BLEU
        hope_bleu *= BLEU_RATIO; // We only care about cases where model has MUCH more influence than BLEU
        if(safe_hope && safe_loop==0 && abs(hope_model)>1e-8 && abs(hope_bleu)/abs(hope_model)<hope_scale)
          hope_scale = abs(hope_bleu) / abs(hope_model);
        else break;
      }
      // Update weights
      if(hope_index!=fear_index) {
        // Vector difference
        const MiraFeatureVector& hope=train->featuresAt(hope_index);
        const MiraFeatureVector& fear=train->featuresAt(fear_index);
        MiraFeatureVector diff = hope - fear;
        // Bleu difference
        const vector<float>& hope_stats = train->scoresAt(hope_index);
        ValType hopeBleu = sentenceLevelBackgroundBleu(hope_stats, bg);
        const vector<float>& fear_stats = train->scoresAt(fear_index);
        ValType fearBleu = sentenceLevelBackgroundBleu(fear_stats, bg);
        assert(hopeBleu + 1e-8 >= fearBleu);
        ValType delta = hopeBleu - fearBleu;
        // Loss and update
        ValType diff_score = wv.score(diff);
        ValType loss = delta - diff_score;
        if(verbose) {
          cerr << "Updating sent " << train->cur_id() << endl;
          cerr << "Wght: " << wv << endl;
          cerr << "Hope: " << hope << " BLEU:" << hopeBleu << " Score:" << wv.score(hope) << endl;
          cerr << "Fear: " << fear << " BLEU:" << fearBleu << " Score:" << wv.score(fear) << endl;
          cerr << "Diff: " << diff << " BLEU:" << delta << " Score:" << diff_score << endl;
          cerr << "Loss: " << loss << " Scale: " << hope_scale << endl;
          cerr << endl;
        }
        if(loss > 0) {
          ValType eta = min(c, loss / diff.sqrNorm());
          wv.update(diff,eta);
          totalLoss+=loss;
          iNumUpdates++;
        }
        // Update BLEU statistics
        const vector<float>& model_stats = train->scoresAt(model_index);
        for(size_t k=0;k<bg.size();k++) {
          bg[k]*=decay;
          if(model_bg)
            bg[k]+=model_stats[k];
          else
            bg[k]+=hope_stats[k];
        }
      }
      iNumExamples++;
    }
    // Training Epoch summary
    cerr << iNumUpdates << "/" << iNumExamples << " updates"
         << ", avg loss = " << (totalLoss / iNumExamples);

    // Evaluate current average weights
    AvgWeightVector avg = wv.avg();
    ValType bleu = evaluate(train.get(), avg);
    cerr << ", BLEU = " << bleu << endl;
    if(bleu > bestBleu) {
      size_t num_dense = train->num_dense();
      if(initDenseSize>0 && initDenseSize!=num_dense) {
        cerr << "Error: Initial dense feature count and dense feature count from n-best do not match: "
             << initDenseSize << "!=" << num_dense << endl;
        exit(1);
      }
      // Write to a file
      ostream* out;
      ofstream outFile;
      if (!outputFile.empty() ) {
        outFile.open(outputFile.c_str());
        if (!(outFile)) {
          cerr << "Error: Failed to open " << outputFile << endl;
          exit(1);
        }
        out = &outFile;
      } else {
        out = &cout;
      }
      for(size_t i=0;i<avg.size();i++) {
        if(i<num_dense)
          *out << "F" << i << " " << avg.weight(i) << endl;
        else {
          if(abs(avg.weight(i))>1e-8)
            *out << SparseVector::decode(i-num_dense) << " " << avg.weight(i) << endl;
        }
      }
      outFile.close();
      bestBleu = bleu;
    }
  }
  cerr << "Best BLEU = " << bestBleu << endl;
}
// --Emacs trickery--
// Local Variables:
// mode:c++
// c-basic-offset:2
// End:


#include "../is_valid_char.hxx"
#include "../parse_vector.hxx"
#include "../parse_number.hxx"
#include "../../../../Damped_Rational.hxx"

#include <boost/algorithm/string.hpp>

#include <algorithm>
#include <iterator>
#include <string>

const char *parse_damped_rational(const char *begin, const char *end,
                                  Damped_Rational &damped_rational)
{
  const std::string damped_literal("DampedRational[");

  // Parse constant numbers
  for(auto current = begin; current != end; ++current)
    {
      if(*current == damped_literal.front())
        {
          break;
        }
      if(is_valid_char(*current))
        {
          damped_rational.base = 1;
          auto comma(std::find(current, end, ','));
          if(comma == end)
            {
              throw std::runtime_error(
                "Missing a comma when parsing a constant as a DampedRational. "
                " The parsed string is\n\t'"
                + std::string(current, end) + "'");
            }
          damped_rational.constant = Boost_Float(parse_number(current, comma));
          return comma;
        }
    }

  auto damped_start(
    std::search(begin, end, damped_literal.begin(), damped_literal.end()));
  if(damped_start == end)
    {
      throw std::runtime_error("Could not find '" + damped_literal + "'");
    }

  auto comma(std::find(damped_start, end, ','));
  if(comma == end)
    {
      throw std::runtime_error("Missing comma after DampedRational.constant");
    }
  auto constant_start(std::next(damped_start, damped_literal.size()));
  damped_rational.constant = Boost_Float(parse_number(constant_start, comma));

  auto start_poles(std::next(comma));
  auto end_poles(parse_vector(start_poles, end, damped_rational.poles));

  comma = std::find(end_poles, end, ',');
  if(comma == end)
    {
      throw std::runtime_error("Missing comma after DampedRational.poles");
    }

  auto start_base(std::next(comma));
  comma = std::find(start_base, end, ',');
  if(comma == end)
    {
      throw std::runtime_error("Missing comma after DampedRational.base");
    }
  damped_rational.base = Boost_Float(parse_number(start_base, comma));

  auto start_variable(std::next(comma));
  const auto close_bracket(std::find(start_variable, end, ']'));
  if(close_bracket == end)
    {
      throw std::runtime_error("Missing ']' at end of DampedRational");
    }

  return std::next(close_bracket);
}