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

// std_real_concept is an archetype for built-in Real types.

// The main purpose in providing this type is to verify
// that std lib functions are found via a using declaration
// bringing those functions into the current scope, and not
// just because they happen to be in global scope.
//
// If ::pow is found rather than std::pow say, then the code
// will silently compile, but truncation of long doubles to
// double will cause a significant loss of precision.
// A template instantiated with std_real_concept will *only*
// compile if it std::whatever is in scope.

#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/math/tools/real_cast.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/policies/policy.hpp>

#include <ostream>
#include <istream>
#include <cmath>
#include <math.h> // fmodl

#ifndef BOOST_MATH_STD_REAL_CONCEPT_HPP
#define BOOST_MATH_STD_REAL_CONCEPT_HPP

namespace boost{ namespace math{

namespace concepts
{

class std_real_concept
{
public:
   // Constructors:
   std_real_concept() : m_value(0){}
   std_real_concept(char c) : m_value(c){}
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   std_real_concept(wchar_t c) : m_value(c){}
#endif
   std_real_concept(unsigned char c) : m_value(c){}
   std_real_concept(signed char c) : m_value(c){}
   std_real_concept(unsigned short c) : m_value(c){}
   std_real_concept(short c) : m_value(c){}
   std_real_concept(unsigned int c) : m_value(c){}
   std_real_concept(int c) : m_value(c){}
   std_real_concept(unsigned long c) : m_value(c){}
   std_real_concept(long c) : m_value(c){}
#if defined(BOOST_HAS_LONG_LONG) || defined(__DECCXX) || defined(__SUNPRO_CC)
   std_real_concept(unsigned long long c) : m_value(static_cast<long double>(c)){}
   std_real_concept(long long c) : m_value(static_cast<long double>(c)){}
#endif
   std_real_concept(float c) : m_value(c){}
   std_real_concept(double c) : m_value(c){}
   std_real_concept(long double c) : m_value(c){}

   // Assignment:
   std_real_concept& operator=(char c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned char c) { m_value = c; return *this; }
   std_real_concept& operator=(signed char c) { m_value = c; return *this; }
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   std_real_concept& operator=(wchar_t c) { m_value = c; return *this; }
#endif
   std_real_concept& operator=(short c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned short c) { m_value = c; return *this; }
   std_real_concept& operator=(int c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned int c) { m_value = c; return *this; }
   std_real_concept& operator=(long c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned long c) { m_value = c; return *this; }
#if defined(BOOST_HAS_LONG_LONG) || defined(__DECCXX) || defined(__SUNPRO_CC)
   std_real_concept& operator=(long long c) { m_value = static_cast<long double>(c); return *this; }
   std_real_concept& operator=(unsigned long long c) { m_value = static_cast<long double>(c); return *this; }
#endif
   std_real_concept& operator=(float c) { m_value = c; return *this; }
   std_real_concept& operator=(double c) { m_value = c; return *this; }
   std_real_concept& operator=(long double c) { m_value = c; return *this; }

   // Access:
   long double value()const{ return m_value; }

   // Member arithmetic:
   std_real_concept& operator+=(const std_real_concept& other)
   { m_value += other.value(); return *this; }
   std_real_concept& operator-=(const std_real_concept& other)
   { m_value -= other.value(); return *this; }
   std_real_concept& operator*=(const std_real_concept& other)
   { m_value *= other.value(); return *this; }
   std_real_concept& operator/=(const std_real_concept& other)
   { m_value /= other.value(); return *this; }
   std_real_concept operator-()const
   { return -m_value; }
   std_real_concept const& operator+()const
   { return *this; }

private:
   long double m_value;
};

// Non-member arithmetic:
inline std_real_concept operator+(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result += b;
   return result;
}
inline std_real_concept operator-(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result -= b;
   return result;
}
inline std_real_concept operator*(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result *= b;
   return result;
}
inline std_real_concept operator/(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result /= b;
   return result;
}

// Comparison:
inline bool operator == (const std_real_concept& a, const std_real_concept& b)
{ return a.value() == b.value(); }
inline bool operator != (const std_real_concept& a, const std_real_concept& b)
{ return a.value() != b.value();}
inline bool operator < (const std_real_concept& a, const std_real_concept& b)
{ return a.value() < b.value(); }
inline bool operator <= (const std_real_concept& a, const std_real_concept& b)
{ return a.value() <= b.value(); }
inline bool operator > (const std_real_concept& a, const std_real_concept& b)
{ return a.value() > b.value(); }
inline bool operator >= (const std_real_concept& a, const std_real_concept& b)
{ return a.value() >= b.value(); }

#if 0
// Non-member mixed compare:
template <class T>
inline bool operator == (const T& a, const std_real_concept& b)
{
   return a == b.value();
}
template <class T>
inline bool operator != (const T& a, const std_real_concept& b)
{
   return a != b.value();
}
template <class T>
inline bool operator < (const T& a, const std_real_concept& b)
{
   return a < b.value();
}
template <class T>
inline bool operator > (const T& a, const std_real_concept& b)
{
   return a > b.value();
}
template <class T>
inline bool operator <= (const T& a, const std_real_concept& b)
{
   return a <= b.value();
}
template <class T>
inline bool operator >= (const T& a, const std_real_concept& b)
{
   return a >= b.value();
}
#endif  // Non-member mixed compare:

} // namespace concepts
} // namespace math
} // namespace boost

namespace std{

// Non-member functions:
inline boost::math::concepts::std_real_concept acos(boost::math::concepts::std_real_concept a)
{ return std::acos(a.value()); }
inline boost::math::concepts::std_real_concept cos(boost::math::concepts::std_real_concept a)
{ return std::cos(a.value()); }
inline boost::math::concepts::std_real_concept asin(boost::math::concepts::std_real_concept a)
{ return std::asin(a.value()); }
inline boost::math::concepts::std_real_concept atan(boost::math::concepts::std_real_concept a)
{ return std::atan(a.value()); }
inline boost::math::concepts::std_real_concept atan2(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return std::atan2(a.value(), b.value()); }
inline boost::math::concepts::std_real_concept ceil(boost::math::concepts::std_real_concept a)
{ return std::ceil(a.value()); }
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline boost::math::concepts::std_real_concept fmod(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return fmodl(a.value(), b.value()); }
#else
inline boost::math::concepts::std_real_concept fmod(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return std::fmod(a.value(), b.value()); }
#endif
inline boost::math::concepts::std_real_concept cosh(boost::math::concepts::std_real_concept a)
{ return std::cosh(a.value()); }
inline boost::math::concepts::std_real_concept exp(boost::math::concepts::std_real_concept a)
{ return std::exp(a.value()); }
inline boost::math::concepts::std_real_concept fabs(boost::math::concepts::std_real_concept a)
{ return std::fabs(a.value()); }
inline boost::math::concepts::std_real_concept abs(boost::math::concepts::std_real_concept a)
{ return std::abs(a.value()); }
inline boost::math::concepts::std_real_concept floor(boost::math::concepts::std_real_concept a)
{ return std::floor(a.value()); }
inline boost::math::concepts::std_real_concept modf(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept* ipart)
{
   long double ip;
   long double result = std::modf(a.value(), &ip);
   *ipart = ip;
   return result;
}
inline boost::math::concepts::std_real_concept frexp(boost::math::concepts::std_real_concept a, int* expon)
{ return std::frexp(a.value(), expon); }
inline boost::math::concepts::std_real_concept ldexp(boost::math::concepts::std_real_concept a, int expon)
{ return std::ldexp(a.value(), expon); }
inline boost::math::concepts::std_real_concept log(boost::math::concepts::std_real_concept a)
{ return std::log(a.value()); }
inline boost::math::concepts::std_real_concept log10(boost::math::concepts::std_real_concept a)
{ return std::log10(a.value()); }
inline boost::math::concepts::std_real_concept tan(boost::math::concepts::std_real_concept a)
{ return std::tan(a.value()); }
inline boost::math::concepts::std_real_concept pow(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return std::pow(a.value(), b.value()); }
#if !defined(__SUNPRO_CC)
inline boost::math::concepts::std_real_concept pow(boost::math::concepts::std_real_concept a, int b)
{ return std::pow(a.value(), b); }
#else
inline boost::math::concepts::std_real_concept pow(boost::math::concepts::std_real_concept a, int b)
{ return std::pow(a.value(), static_cast<long double>(b)); }
#endif
inline boost::math::concepts::std_real_concept sin(boost::math::concepts::std_real_concept a)
{ return std::sin(a.value()); }
inline boost::math::concepts::std_real_concept sinh(boost::math::concepts::std_real_concept a)
{ return std::sinh(a.value()); }
inline boost::math::concepts::std_real_concept sqrt(boost::math::concepts::std_real_concept a)
{ return std::sqrt(a.value()); }
inline boost::math::concepts::std_real_concept tanh(boost::math::concepts::std_real_concept a)
{ return std::tanh(a.value()); }

} // namespace std

namespace boost{ namespace math{ namespace concepts{

// Streaming:
template <class charT, class traits>
inline std::basic_ostream<charT, traits>& operator<<(std::basic_ostream<charT, traits>& os, const std_real_concept& a)
{
   return os << a.value();
}
template <class charT, class traits>
inline std::basic_istream<charT, traits>& operator>>(std::basic_istream<charT, traits>& is, std_real_concept& a)
{
   long double v;
   is >> v;
   a = v;
   return is;
}

} // namespace concepts

namespace tools
{
// real_cast converts from T to integer and narrower floating-point types.

// Convert from T to integer types.

template <>
inline unsigned int real_cast<unsigned int, concepts::std_real_concept>(concepts::std_real_concept r)
{
   return static_cast<unsigned int>(r.value());
}

template <>
inline int real_cast<int, concepts::std_real_concept>(concepts::std_real_concept r)
{
   return static_cast<int>(r.value());
}

template <>
inline long real_cast<long, concepts::std_real_concept>(concepts::std_real_concept r)
{
   return static_cast<long>(r.value());
}

// Converts from T to narrower floating-point types, float, double & long double.

template <>
inline float real_cast<float, concepts::std_real_concept>(concepts::std_real_concept r)
{
   return static_cast<float>(r.value());
}
template <>
inline double real_cast<double, concepts::std_real_concept>(concepts::std_real_concept r)
{
   return static_cast<double>(r.value());
}
template <>
inline long double real_cast<long double, concepts::std_real_concept>(concepts::std_real_concept r)
{
   return r.value();
}

template <>
inline concepts::std_real_concept max_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return max_value<long double>();
}

template <>
inline concepts::std_real_concept min_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return min_value<long double>();
}

template <>
inline concepts::std_real_concept log_max_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return log_max_value<long double>();
}

template <>
inline concepts::std_real_concept log_min_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return log_min_value<long double>();
}

template <>
inline concepts::std_real_concept epsilon<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return tools::epsilon<long double>();
}

template <>
inline int digits<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{ // Assume number of significand bits is same as long double,
  // unless std::numeric_limits<T>::is_specialized to provide digits.
   return digits<long double>();
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_STD_REAL_CONCEPT_HPP


/*
 * WAnalyser.cpp
 *
 *  Created on: 12 Jan 2015
 *      Author: ejclemen
 */

#include "../../interface/Analysers/WAnalyser.h"

#include <boost/scoped_ptr.hpp>

namespace BAT {

void WAnalyser::analyse(const EventPtr event) {
	// weight_ = event->weight();
}

void WAnalyser::analyseHadronicW(const EventPtr event, const JetCollection jets, const JetCollection bjets) {

	weight_ = event->weight();

	// Get cleaned jets that aren't b tagged
	JetCollection jetsWithoutBs;
	for ( unsigned int jetIndex=0; jetIndex < jets.size(); ++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 ) jetsWithoutBs.push_back( thisJet );
	}

	if ( jetsWithoutBs.size() < 2 ) return;

	// Need these for some gen level studies
	const JetCollection genJets( event->GenJets() );
	histMan_->setCurrentHistogramFolder(histogramFolder_);
	treeMan_->setCurrentFolder(histogramFolder_);
	treeMan_->Fill("EventWeight", event->weight());

	// Get each jet pair combination and form a W candidate
	for ( unsigned int jet1Index=0; jet1Index < jetsWithoutBs.size()-1; ++jet1Index ) {
		for ( unsigned int jet2Index=jet1Index+1; jet2Index < jetsWithoutBs.size(); ++jet2Index ) {
			JetPointer jet1 = jetsWithoutBs[jet1Index];
			JetPointer jet2 = jetsWithoutBs[jet2Index];

			// W mass from RAW jets
			// Note there will be some bias, as only jets with corrected pt > 30 are in the ntuple
			ParticlePointer jet1Raw = jet1->raw_jet();
			ParticlePointer jet2Raw = jet2->raw_jet();

			if ( jet1Raw->pt() > 30 && jet2Raw->pt() > 30 ) {
				Particle hadronicWRaw(*jet1Raw + *jet2Raw);

				histMan_->H1D("hadronicWMass_RAW")->Fill(hadronicWRaw.mass() , weight_);
				histMan_->H1D("jetPt_RAW")->Fill(jet1Raw->pt() , weight_);
				histMan_->H1D("jetPt_RAW")->Fill(jet2Raw->pt() , weight_);
				histMan_->H1D("jetEta_RAW")->Fill(jet1Raw->eta() , weight_);
				histMan_->H1D("jetEta_RAW")->Fill(jet2Raw->eta() , weight_);
			}

			if (jet1->pt()<=30 || jet2->pt()<=30 ) continue;

			Particle hadronicW(*jet1 + *jet2);
			histMan_->setCurrentHistogramFolder(histogramFolder_);
			histMan_->H1D("hadronicWMass")->Fill(hadronicW.mass() , weight_);
			histMan_->H1D("jetPt")->Fill(jet1->pt() , weight_);
			histMan_->H1D("jetPt")->Fill(jet2->pt() , weight_);
			histMan_->H1D("jetEta")->Fill(jet1->eta() , weight_);
			histMan_->H1D("jetEta")->Fill(jet2->eta() , weight_);

			treeMan_->setCurrentFolder(histogramFolder_);
			treeMan_->Fill("mjj",hadronicW.mass());
			treeMan_->Fill("jetPt",jet1->pt());
			treeMan_->Fill("jetPt",jet2->pt());
			treeMan_->Fill("jetEta",jet1->eta());
			treeMan_->Fill("jetEta",jet2->eta());
			treeMan_->Fill("NPU",event->Vertices().size());

			// Look at matched generator jets
			const ParticlePointer genJet1 = jet1->matched_generated_jet();
			const ParticlePointer genJet2 = jet2->matched_generated_jet();

			if ( genJet1 != 0 && genJet2 != 0 ) {
				Particle hadronicW_fromGenJets(*genJet1 + *genJet2);
				histMan_->H1D("hadronicWMass_genJets")->Fill(hadronicW_fromGenJets.mass() , weight_);
				histMan_->H1D("jetPt_genJet")->Fill(genJet1->pt() , weight_);
				histMan_->H1D("jetPt_genJet")->Fill(genJet2->pt() , weight_);
				histMan_->H1D("jetEta_genJet")->Fill(genJet1->eta() , weight_);
				histMan_->H1D("jetEta_genJet")->Fill(genJet2->eta() , weight_);

				treeMan_->Fill("mjj_genJet",hadronicW.mass());
				treeMan_->Fill("genJetPt",genJet1->pt());
				treeMan_->Fill("genJetPt",genJet2->pt());
				treeMan_->Fill("genJetEta",genJet1->eta());
				treeMan_->Fill("genJetEta",genJet2->eta());

				// Now check if these gen jets correspond to those matched to the W decay products
				const TTGenInfoPointer ttGen( event->TTGenInfo() );
				int quarkGenJetIndex = ttGen->getQuarkGenJetIndex();
				int quarkBarGenJetIndex = ttGen->getQuarkBarGenJetIndex();
				if ( quarkGenJetIndex >= 0 && quarkBarGenJetIndex >= 0 ) {
					JetPointer quarkGenJet( genJets[quarkGenJetIndex] );
					JetPointer quarkBarGenJet( genJets[quarkBarGenJetIndex] );

					if ( ( genJet1->getFourVector() == quarkGenJet->getFourVector() && genJet2->getFourVector() == quarkBarGenJet->getFourVector() ) ||
						( genJet2->getFourVector() == quarkGenJet->getFourVector() && genJet1->getFourVector() == quarkBarGenJet->getFourVector() ) ) {

						// These reco jets actually correspond to thos from a W
						histMan_->H1D("hadronicWMass_recoMatchedToPartons")->Fill(hadronicW.mass() , weight_);
						histMan_->H1D("jetPt_recoMatchedToPartons")->Fill(jet1->pt() , weight_);
						histMan_->H1D("jetPt_recoMatchedToPartons")->Fill(jet2->pt() , weight_);
						histMan_->H1D("jetEta_recoMatchedToPartons")->Fill(jet1->eta() , weight_);
						histMan_->H1D("jetEta_recoMatchedToPartons")->Fill(jet2->eta() , weight_);

					}
				}
			}
		}
	}

	//  Get jets that are fairly separated from other jets
	// These jets are a further subset of jetsWithoutBs
	JetCollection cleanedJets;
	for ( unsigned int jet1Index=0; jet1Index < jetsWithoutBs.size(); ++jet1Index ) {
		JetPointer jet1 = jetsWithoutBs[jet1Index];
		double minDeltaR = 999.;
		for ( unsigned int jet2Index=jet1Index+1; jet2Index < jetsWithoutBs.size(); ++jet2Index ) {

			JetPointer jet2 = jetsWithoutBs[jet2Index];

			double deltaRToOtherJet = jet1->deltaR( jet2 );

			if ( deltaRToOtherJet < minDeltaR )
				minDeltaR = deltaRToOtherJet;
		}
		if ( minDeltaR > 1 ) {
			histMan_->H1D("minDeltaR_cleanedReco")->Fill( minDeltaR, weight_ );
			cleanedJets.push_back( jet1 );
		}
	}

	if ( cleanedJets.size() < 2 ) return;

	// Get each jet pair combination and form a W candidate
	for ( unsigned int jet1Index=0; jet1Index < cleanedJets.size()-1; ++jet1Index ) {
		for ( unsigned int jet2Index=jet1Index+1; jet2Index < cleanedJets.size(); ++jet2Index ) {
			JetPointer jet1 = cleanedJets[jet1Index];
			JetPointer jet2 = cleanedJets[jet2Index];

			if (jet1->pt()<=30 || jet2->pt()<=30 ) continue;

			Particle hadronicW(*jet1 + *jet2);

			histMan_->setCurrentHistogramFolder(histogramFolder_);
			histMan_->H1D("hadronicWMass_cleanedReco")->Fill(hadronicW.mass() , weight_);
			histMan_->H1D("jetPt_cleanedReco")->Fill(jet1->pt() , weight_);
			histMan_->H1D("jetPt_cleanedReco")->Fill(jet2->pt() , weight_);
			histMan_->H1D("jetEta_cleanedReco")->Fill(jet1->eta() , weight_);
			histMan_->H1D("jetEta_cleanedReco")->Fill(jet2->eta() , weight_);
		}
	}

}

void WAnalyser::analyseHadronicW_partons(const EventPtr event) {
	weight_ = event->weight();

	// Get partons associated with W decay
	const TTGenInfoPointer ttGen( event->TTGenInfo() );
	const ParticlePointer quarkParton = ttGen->getQuark();
	const ParticlePointer quarkBarParton = ttGen->getQuarkBar();
	const Particle hadronicW_fromPartons(*quarkParton + *quarkBarParton);

	if ( quarkParton->pt() > 30 && quarkBarParton->pt() > 30 &&
			fabs(quarkParton->eta()) < 2.5 && fabs(quarkBarParton->eta()) < 2.5 ) {

		histMan_->setCurrentHistogramFolder(histogramFolder_);
		histMan_->H1D("hadronicWMass_partons")->Fill(hadronicW_fromPartons.mass() , weight_);
		histMan_->H1D("jetPt_partons")->Fill(quarkParton->pt() , weight_);
		histMan_->H1D("jetPt_partons")->Fill(quarkBarParton->pt() , weight_);
		histMan_->H1D("jetEta_partons")->Fill(quarkParton->eta() , weight_);
		histMan_->H1D("jetEta_partons")->Fill(quarkBarParton->eta() , weight_);

		treeMan_->setCurrentFolder(histogramFolder_);
		treeMan_->Fill("mjj_parton",hadronicW_fromPartons.mass());
		treeMan_->Fill("partonPt",quarkParton->pt());
		treeMan_->Fill("partonPt",quarkBarParton->pt());
		treeMan_->Fill("partonEta",quarkParton->eta());
		treeMan_->Fill("partonEta",quarkBarParton->eta());

		// Get gen jets associated with partons
		// Note these gen jets are not cleaned against leptons.  But as we are considering the ones matched to the quarks from W decay, this shouldn't be too much of a problem
		const JetCollection genJets( event->GenJets() );
		int quarkGenJetIndex = ttGen->getQuarkGenJetIndex();
		int quarkBarGenJetIndex = ttGen->getQuarkBarGenJetIndex();

		if ( quarkGenJetIndex >= 0 && quarkBarGenJetIndex >= 0 ) {
			JetPointer quarkGenJet( genJets[quarkGenJetIndex] );
			JetPointer quarkBarGenJet( genJets[quarkBarGenJetIndex] );

			if ( quarkGenJet->pt() > 30 && quarkBarGenJet->pt() > 30 &&
				fabs(quarkGenJet->eta()) < 2.5 && fabs(quarkBarGenJet->eta()) < 2.5 ) {

				const Particle hadronicW_fromGenJetsMatchedToPartons(*quarkGenJet + *quarkBarGenJet);

				histMan_->H1D("hadronicWMass_partonsGenJets")->Fill(hadronicW_fromGenJetsMatchedToPartons.mass() , weight_);
				histMan_->H1D("jetPt_partonsGenJets")->Fill(quarkGenJet->pt() , weight_);
				histMan_->H1D("jetPt_partonsGenJets")->Fill(quarkBarGenJet->pt() , weight_);
				histMan_->H1D("jetEta_partonsGenJets")->Fill(quarkGenJet->eta() , weight_);
				histMan_->H1D("jetEta_partonsGenJets")->Fill(quarkBarGenJet->eta() , weight_);
			}
		}
	}
}

void WAnalyser::fillHistograms(std::string subcollection, std::string suffix) {
	histMan_->setCurrentHistogramFolder(histogramFolder_);

	// unsigned short numberOfSolutions = allSolutions.size();
	// for (unsigned short index = 0; index < numberOfSolutions; ++index) {
	// 	TtbarHypothesisPointer currentSolution = allSolutions.at(index);
	// 	const ParticlePointer resonance = currentSolution->resonance;

	// 	double hadronicWMass = currentSolution->hadronicW->mass();

	// 	if (index == 0) {
	// 		histMan_->H1D_BJetBinned("hadronicWMass" + suffix)->Fill(hadronicWMass, weight_ * scale_);
	// 	}

	// 	double normalisedWeight = weight_ * scale_ / numberOfSolutions;
	// 	histMan_->H1D_BJetBinned("hadronicWMass_allSolutions" + suffix)->Fill(hadronicWMass, normalisedWeight);
	// }
}

void WAnalyser::createHistograms() {
	histMan_->setCurrentHistogramFolder(histogramFolder_);

	histMan_->addH1D("hadronicWMass", "hadronic W mass; m(W_{had}) [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetPt", "jet pt; p_{t} [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetEta", "jet eta; #eta; events/0.06", 100, -3, 3);

	histMan_->addH1D("hadronicWMass_RAW", "hadronic W mass with RAW jets; m(W_{had}) [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetPt_RAW", "jet pt with RAW jets; p_{t} [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetEta_RAW", "jet eta with RAW jets; #eta; events/0.06", 100, -3, 3);

	histMan_->addH1D("hadronicWMass_genJets", "hadronic W mass from gen jets; m(W_{had}) [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetPt_genJet", "gen jet pt; p_{t} [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetEta_genJet", "gen jet eta; #eta; events/0.06", 100, -3, 3);

	histMan_->addH1D("hadronicWMass_partons", "hadronic W mass from partons; m(W_{had}) [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetPt_partons", "parton pt; p_{t} [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetEta_partons", "parton eta; #eta; events/0.06", 100, -3, 3);

	histMan_->addH1D("hadronicWMass_partonsGenJets", "hadronic W mass from gen jets matched to partons; m(W_{had}) [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetPt_partonsGenJets", "gen jet matched to parton pt; p_{t} [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetEta_partonsGenJets", "gen jet matched to parton eta; #eta; events/0.06", 100, -3, 3);

	histMan_->addH1D("hadronicWMass_recoMatchedToPartons", "hadronic W mass from reco jets matched to partons; m(W_{had}) [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetPt_recoMatchedToPartons", "reco jet matched to parton pt; p_{t} [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetEta_recoMatchedToPartons", "reco jet matched to parton eta; #eta; events/0.06", 100, -3, 3);

	histMan_->addH1D("hadronicWMass_cleanedReco", "hadronic W mass; m(W_{had}) [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetPt_cleanedReco", "jet pt; p_{t} [GeV]; events/1 GeV", 500, 0, 500);
	histMan_->addH1D("jetEta_cleanedReco", "jet eta; #eta; events/0.06", 100, -3, 3);
	histMan_->addH1D("minDeltaR_cleanedReco", "deltaR to nearest jet; #Delta R; events/0.013", 100, 0, 1.3);

}

void WAnalyser::createTrees() {
	treeMan_->setCurrentFolder(histogramFolder_);

	treeMan_->addBranch("mjj", "F", "W Bosons" + Globals::treePrefix_);
	treeMan_->addBranch("mjj_genJet", "F", "W Bosons" + Globals::treePrefix_);
	treeMan_->addBranch("mjj_parton", "F", "W Bosons" + Globals::treePrefix_);
	treeMan_->addBranch("jetPt", "F", "W Bosons" + Globals::treePrefix_, false);
	treeMan_->addBranch("jetEta", "F", "W Bosons" + Globals::treePrefix_, false);
	treeMan_->addBranch("genJetPt", "F", "W Bosons" + Globals::treePrefix_, false);
	treeMan_->addBranch("genJetEta", "F", "W Bosons" + Globals::treePrefix_, false);
	treeMan_->addBranch("partonPt", "F", "W Bosons" + Globals::treePrefix_, false);
	treeMan_->addBranch("partonEta", "F", "W Bosons" + Globals::treePrefix_, false);

	treeMan_->addBranch("NPU", "F", "W Bosons" + Globals::treePrefix_);
}

WAnalyser::WAnalyser(boost::shared_ptr<HistogramManager> histMan, boost::shared_ptr<TreeManager> treeMan, std::string histogramFolder) :
		BasicAnalyser(histMan, treeMan, histogramFolder) {
}

WAnalyser::~WAnalyser() {
}

} /* namespace BAT */


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

/// @file
/// Definitions corresponding to collision_operator.hpp.

#include <collision_operator.hpp>
#include <constants.hpp>
#include <limits>
#include <iostream>
#include <fstream>
#include <unsupported/Eigen/MatrixFunctions>
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/vector.hpp>
#if BOOST_VERSION >= 106700
#include <boost/container_hash/hash.hpp>
#else
#include <boost/functional/hash.hpp>
#endif
/// Threading through Intel TBB
#include <tbb/concurrent_unordered_map.h>
#include <tbb/parallel_for.h>
#include <mutex>

namespace alma {

Eigen::MatrixXd get_collision_operator_dense(
    const alma::Gamma_grid& grid,
    const alma::Crystal_structure& cell,
    std::vector<alma::Thirdorder_ifcs>& anhIFC,
    double Treference,
    boost::mpi::communicator& world) {
    /// First we are getting sizes
    std::size_t nqpoints = grid.nqpoints;
    std::size_t nbands = grid.get_spectrum_at_q(0).omega.size();
    std::size_t nmodes = nbands * nqpoints;

    std::size_t my_id = world.rank();
    std::size_t nprocs = world.size();

    /// Fill B matrix with 0
    if ((nmodes - 3) * (nmodes - 3) > std::numeric_limits<int>::max()) {
        if (my_id == 0) {
            std::cerr << "FATAL ERROR the number of matrix elements will not "
                         "fit in MPI buffer"
                      << std::endl;
            std::cerr << "max_buffer_elements = "
                      << std::numeric_limits<int>::max() << std::endl;
            std::cerr << "matrix_elements     = " << (nmodes - 3) * (nmodes - 3)
                      << std::endl;
        }
        world.barrier();
        world.abort(1);
    }

    Eigen::MatrixXcd cB = Eigen::MatrixXcd::Zero(
        nmodes - 3,
        nmodes - 3); /// We are not counting the Gamma-point acustic bands.

    /// We first calculate the equilibrium distribution for reference
    /// temperature:
    std::vector<double> neq(nmodes, 0.);

    for (std::size_t iq = 0; iq < nqpoints; iq++) {
        auto s = grid.get_spectrum_at_q(iq);
        for (std::size_t ib = 0; ib < nbands; ib++) {
            double omega = s.omega(ib);
            if (omega >= 1.0e-6) {
                neq[iq * nbands + ib] = alma::bose_einstein(omega, Treference);
            }
        }
    }

    /// Create a unique set of Three_phonon processes from irreductible triplets
    /// including the symmetry:
    auto limits = alma::my_jobs(grid.get_nequivalences(), nprocs, my_id);
    std::array<int, 2> signs(
        {{static_cast<int>(alma::threeph_type::emission),
          static_cast<int>(alma::threeph_type::absorption)}});

    std::vector<alma::Threeph_process> myprocesses;

    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>
        emission_processes;
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>
        absorption_processes;

    tbb::concurrent_unordered_map<std::array<std::size_t, 3>,
                                  alma::Threeph_process,
                                  std::hash<std::array<std::size_t, 3>>>
        emission_processes_tbb;
    tbb::concurrent_unordered_map<std::array<std::size_t, 3>,
                                  alma::Threeph_process,
                                  std::hash<std::array<std::size_t, 3>>>
        absorption_processes_tbb;

    /// Calculating the three phonon processes for full BZ. The anhIFC are
    /// needed in case that due to symmetrization we are registering some
    /// transition that was not available in the already calculated elements. We
    /// are calculating them under the assumption that the matrix element is
    /// invariant to rotations, inversions and reciprocity but the smearing is
    /// not.
    for (auto ic = limits[0]; ic < limits[1]; ++ic) {
        if (my_id == 0)
            std::cout << "#Recalculating fullBZ triplets "
                      << static_cast<std::size_t>(ic - limits[0]) << " / "
                      << static_cast<std::size_t>(limits[1] - limits[0])
                      << std::endl;
        auto iq1 = grid.get_representative(ic);
        auto coords1 = grid.one_to_three(iq1);
        auto spectrum1 = grid.get_spectrum_at_q(iq1);

        tbb::parallel_for(
            tbb::blocked_range<std::size_t>(0, grid.nqpoints),
            [&](tbb::blocked_range<std::size_t> iq2range) {
                for (std::size_t iq2 = iq2range.begin(); iq2 < iq2range.end();
                     ++iq2) {
                    auto coords2 = grid.one_to_three(iq2);
                    auto spectrum2 = grid.get_spectrum_at_q(iq2);
                    decltype(coords2) coords3;

                    // Emission and absorption processes satisfy different
                    // conservation rules, with the second phonon in
                    // different sides of the equations.
                    for (auto s : signs) {
                        for (auto i = 0; i < 3; ++i)
                            coords3[i] = coords1[i] + s * coords2[i];
                        auto iq3 = grid.three_to_one(coords3);
                        auto spectrum3 = grid.get_spectrum_at_q(iq3);

                        auto eqqtrip =
                            grid.equivalent_qtriplets({iq1, iq2, iq3});

                        for (decltype(nbands) im1 = 0; im1 < nbands; ++im1) {
                            if (alma::almost_equal(spectrum1.omega(im1), 0.))
                                continue;

                            for (decltype(nbands) im2 = 0; im2 < nbands;
                                 ++im2) {
                                if (alma::almost_equal(spectrum2.omega(im2),
                                                       0.))
                                    continue;

                                for (decltype(nbands) im3 = 0; im3 < nbands;
                                     ++im3) {
                                    if (alma::almost_equal(spectrum3.omega(im3),
                                                           0.))
                                        continue;

                                    auto delta =
                                        std::fabs(spectrum1.omega(im1) +
                                                  s * spectrum2.omega(im2) -
                                                  spectrum3.omega(im3));

                                    std::vector<double> vpsXdelta; //, sigmas;
                                    std::vector<alma::Threeph_process> mythree;

                                    for (auto& qt : eqqtrip) {
                                        auto v1 = grid.get_spectrum_at_q(qt[0])
                                                      .vg.col(im1);
                                        auto v2 = grid.get_spectrum_at_q(qt[1])
                                                      .vg.col(im2);
                                        auto v3 = grid.get_spectrum_at_q(qt[2])
                                                      .vg.col(im3);

                                        if (static_cast<alma::threeph_type>(
                                                s) ==
                                            alma::threeph_type::absorption) {
                                            auto sigma =
                                                1.0 *
                                                std::sqrt(
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v1)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v2)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v3)));
                                            alma::Threeph_process
                                                Gamma_of_triplet1(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[0],
                                                          qt[1],
                                                          qt[2]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im1, im2, im3}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet1);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet1
                                                        .compute_gaussian());
                                            }
                                            alma::Threeph_process
                                                Gamma_of_triplet2(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[1],
                                                          qt[0],
                                                          qt[2]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im2, im1, im3}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet2);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet2
                                                        .compute_gaussian());
                                            }
                                            alma::Threeph_process
                                                Gamma_of_triplet3(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[2],
                                                          qt[0],
                                                          qt[1]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im3, im1, im2}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet3);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet3
                                                        .compute_gaussian());
                                            }
                                            alma::Threeph_process
                                                Gamma_of_triplet4(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[2],
                                                          qt[1],
                                                          qt[0]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im3, im2, im1}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet4);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet4
                                                        .compute_gaussian());
                                            }
                                        }
                                        else {
                                            auto sigma =
                                                1.0 *
                                                std::sqrt(
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v1)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v2)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v3)));
                                            alma::Threeph_process
                                                Gamma_of_triplet1(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[0],
                                                          qt[1],
                                                          qt[2]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im1, im2, im3}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet1);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet1
                                                        .compute_gaussian());
                                            }
                                            alma::Threeph_process
                                                Gamma_of_triplet2(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[0],
                                                          qt[2],
                                                          qt[1]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im1, im3, im2}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet2);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet2
                                                        .compute_gaussian());
                                            }
                                            alma::Threeph_process
                                                Gamma_of_triplet3(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[2],
                                                          qt[1],
                                                          qt[0]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im3, im2, im1}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet3);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet3
                                                        .compute_gaussian());
                                            }
                                            alma::Threeph_process
                                                Gamma_of_triplet4(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[1],
                                                          qt[2],
                                                          qt[0]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im2, im3, im1}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet4);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet4
                                                        .compute_gaussian());
                                            }
                                        }
                                    }

                                    if (vpsXdelta.empty())
                                        continue;
                                    std::sort(mythree.begin(), mythree.end());
                                    double VP2 = mythree[0].compute_vp2(
                                        cell, grid, anhIFC);

                                    double SymGamma = 0.;
                                    for (auto& vD : vpsXdelta) {
                                        SymGamma += VP2 * vD;
                                    }

                                    SymGamma /= 4 * eqqtrip.size();

                                    /// If not 0 all register in map, we account
                                    /// for a state decaying into two of same
                                    /// index
                                    if (!almost_equal(SymGamma,
                                        0.,1.0e-12,1.0e-9)) {
                                        for (auto& peq : mythree) {
                                            peq.set_vp2(SymGamma);
                                            std::array<std::size_t, 3>
                                                triplet_indexes;
                                            for (std::size_t idx_ = 0; idx_ < 3;
                                                 idx_++)
                                                triplet_indexes[idx_] =
                                                    peq.q[idx_] * nbands +
                                                    peq.alpha[idx_];
                                            if (peq.type == alma::threeph_type::
                                                                absorption) {
                                                if (absorption_processes.count(
                                                        triplet_indexes) == 0) {
                                                    absorption_processes_tbb
                                                        .emplace(std::make_pair(
                                                            triplet_indexes,
                                                            peq));
                                                }
                                            }
                                            else {
                                                // We account for those states
                                                // by multiplying by 2
                                                if (triplet_indexes[1] ==
                                                    triplet_indexes[2]) {
                                                    double effective_vp2 =
                                                        2 * peq.get_vp2();
                                                    peq.set_vp2(effective_vp2);
                                                }
                                                if (emission_processes_tbb.count(
                                                        triplet_indexes) == 0) {
                                                    emission_processes_tbb
                                                        .emplace(std::make_pair(
                                                            triplet_indexes,
                                                            peq));
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            });
    }

    /// Passing tbb map to stl map for more classic stuff like MPI:
    /// It can be avoided if serialization of tbb container is achieved
    absorption_processes.insert(absorption_processes_tbb.begin(),
                                absorption_processes_tbb.end());
    emission_processes.insert(emission_processes_tbb.begin(),
                              emission_processes_tbb.end());

    /// We are now working on isotopic/mass-disoreder scattering
    if (my_id == 0)
        std::cout << "#Working on 2ph processes" << std::endl;

    std::unordered_map<std::pair<std::size_t, std::size_t>, double>
        isotopic_processes;

    auto twoph_processes = alma::find_allowed_twoph(grid, world, 1.0);

    double scalebroad_iso = 1.0;

    /// We need sigma full list to symmetrize:
    Eigen::ArrayXXd broadening_sigmas(nbands, grid.nqpoints);
    for (std::size_t iq = 0; iq < grid.nqpoints; ++iq) {
        for (std::size_t im = 0; im < nbands; ++im) {
            auto spectrum = grid.get_spectrum_at_q(iq);
            broadening_sigmas(im, iq) =
                scalebroad_iso * grid.base_sigma(spectrum.vg.col(im));
        }
    }
    // And refine them by removing outliers.
    auto percent = calc_percentiles_log(broadening_sigmas);
    double lbound = std::exp(percent[0] - 1.5 * (percent[1] - percent[0]));
    broadening_sigmas =
        (broadening_sigmas < lbound).select(lbound, broadening_sigmas);

    for (auto& twoph : twoph_processes) {
        auto b0 = twoph.alpha[0];
        auto b1 = twoph.alpha[1];

        auto sp0 = grid.get_spectrum_at_q(twoph.q[0]);
        auto sp1 = grid.get_spectrum_at_q(twoph.q[1]);

        auto delta = std::abs(sp0.omega(b0) - sp1.omega(b1));

        /// If some of them is Gamma-point and acoustic, ignore
        if (twoph.q[0] * nbands + b0 < 3 or twoph.q[1] * nbands + b1 < 3)
            continue;

        /// The scattering to itselt has no effect in scattering operator
        if (twoph.q[0] * nbands + b0 == twoph.q[1] * nbands + b1)
            continue;

        auto qpairs = grid.equivalent_qpairs({twoph.q[0], twoph.q[1]});

        std::vector<alma::Twoph_process> mytwo;

        double gamma = 0.;

        for (auto& qi : qpairs) {
            auto sigma = std::hypot(broadening_sigmas(b0, qi[0]),
                                    broadening_sigmas(b1, qi[1]));
            alma::Twoph_process t1(0,
                                   std::array<std::size_t, 2>({{qi[0], qi[1]}}),
                                   std::array<std::size_t, 2>({{b0, b1}}),
                                   delta,
                                   sigma);
            alma::Twoph_process t2(0,
                                   std::array<std::size_t, 2>({{qi[1], qi[0]}}),
                                   std::array<std::size_t, 2>({{b1, b0}}),
                                   delta,
                                   sigma);
            mytwo.push_back(t1);
            mytwo.push_back(t2);
            if (delta <= alma::constants::nsigma * sigma) {
                gamma += t1.compute_gamma(cell, grid);
                gamma += t2.compute_gamma(cell, grid);
            }
        }
        gamma /= 2 * qpairs.size();
        /// If some equivalent process is not null
        if (!almost_equal(gamma,0.,1.0e-12,1.0e-9)) {
            for (auto& peq : mytwo) {
                std::pair<std::size_t, std::size_t> pair_mode_ids =
                    std::make_pair(peq.q[0] * nbands + peq.alpha[0],
                                   peq.q[1] * nbands + peq.alpha[1]);
                if (isotopic_processes.count(pair_mode_ids) == 0)
                    isotopic_processes.emplace(
                        std::make_pair(pair_mode_ids, gamma));
            }
        }
    }

    /// Sync section
    if (nprocs > 1) {
        if (my_id == 0)
            std::cout << "#Element distribution along processes" << std::endl;
        /// Chunking map
        std::vector<std::unordered_map<std::array<std::size_t, 3>,
                                       alma::Threeph_process>>
            chunked_map_emission(nprocs);
        std::vector<std::unordered_map<std::array<std::size_t, 3>,
                                       alma::Threeph_process>>
            chunked_map_absorption(nprocs);
        std::vector<
            std::unordered_map<std::pair<std::size_t, std::size_t>, double>>
            chunked_map_isotopic(nprocs);
        for (auto& process_block : emission_processes) {
            auto id1 = process_block.first[0];
            /// Check where we want this
            auto mode_proc_id = alma::index_proc(id1, nmodes, nprocs);
            chunked_map_emission[mode_proc_id].insert(process_block);
        }
        for (auto& process_block : absorption_processes) {
            auto id1 = process_block.first[0];
            /// Check where we want this
            auto mode_proc_id = alma::index_proc(id1, nmodes, nprocs);
            chunked_map_absorption[mode_proc_id].insert(process_block);
        }
        for (auto& process_block : isotopic_processes) {
            auto id1 = process_block.first.first;
            /// Check where we want this
            auto mode_proc_id = alma::index_proc(id1, nmodes, nprocs);
            chunked_map_isotopic[mode_proc_id].insert(process_block);
        }

        /// Here we distributing the chunks (there is no real balance, but for a
        /// big mesh it is expected to be quite even)
        for (std::size_t iproc = 0; iproc < nprocs; iproc++) {
            std::vector<std::unordered_map<std::array<std::size_t, 3>,
                                           alma::Threeph_process>>
                collector_abs, collector_emi;
            std::vector<
                std::unordered_map<std::pair<std::size_t, std::size_t>, double>>
                collector_iso;
            /// Gathering the chunk of process in collector
            gather(world, chunked_map_absorption[iproc], collector_abs, iproc);
            gather(world, chunked_map_emission[iproc], collector_emi, iproc);
            gather(world, chunked_map_isotopic[iproc], collector_iso, iproc);
            /// If a am the collector put in my map
            if (my_id == iproc) {
                absorption_processes.clear();
                for (auto& map2insert : collector_abs) {
                    absorption_processes.insert(map2insert.begin(),
                                                map2insert.end());
                }
                emission_processes.clear();
                for (auto& map2insert : collector_emi) {
                    emission_processes.insert(map2insert.begin(),
                                              map2insert.end());
                }
                isotopic_processes.clear();
                for (auto& map2insert : collector_iso) {
                    isotopic_processes.insert(map2insert.begin(),
                                              map2insert.end());
                }
            }
            world.barrier();
        }

        if (my_id == 0)
            std::cout << "#Element distribution along processes=>DONE"
                      << std::endl;
    }
    std::size_t proc_nemi = emission_processes.size();
    std::size_t proc_nabs = absorption_processes.size();
    std::size_t proc_niso = isotopic_processes.size();
    std::size_t tnemi = 0;
    std::size_t tnabs = 0;
    std::size_t tniso = 0;

    boost::mpi::all_reduce(
        world, &proc_nemi, 1, &tnemi, std::plus<std::size_t>());
    boost::mpi::all_reduce(
        world, &proc_nabs, 1, &tnabs, std::plus<std::size_t>());
    boost::mpi::all_reduce(
        world, &proc_niso, 1, &tniso, std::plus<std::size_t>());

    if (my_id == 0) {
        std::cout << "#Nemi " << tnemi << std::endl;
        std::cout << "#Nabs " << tnabs << std::endl;
        std::cout << "#Niso " << tniso << std::endl;
    }
    std::size_t ntot = proc_nemi + proc_nabs + proc_niso;

    std::size_t iel = 0;

    for (auto& process_block : emission_processes) {
        auto process(process_block.second);
        auto q0 = process.q[0];
        auto q1 = process.q[1];
        auto q2 = process.q[2];
        auto b0 = process.alpha[0];
        auto b1 = process.alpha[1];
        auto b2 = process.alpha[2];
        double omega0 = grid.get_spectrum_at_q(q0).omega(b0);
        double omega1 = grid.get_spectrum_at_q(q1).omega(b1);
        double omega2 = grid.get_spectrum_at_q(q2).omega(b2);

        double factor = 1.0e6 * alma::constants::hbar * alma::constants::pi /
                        (4.0 * omega0 * omega1 * omega2) / nqpoints;

        double vpdelta = factor * process.get_vp2();

        if (process.type == alma::threeph_type::emission)
            vpdelta /= 2;

        if (process.type == alma::threeph_type::emission) {
            auto I = q0 * nbands + b0;
            auto J = q1 * nbands + b1;
            auto K = q2 * nbands + b2;
            /// Check if normal

            cB(I - 3, I - 3) = NeumaierSum(cB(I - 3, I - 3),
                                           -vpdelta * (neq[K] + neq[J] + 1.0));
            cB(I - 3, J - 3) =
                NeumaierSum(cB(I - 3, J - 3),
                            (omega0 / omega1) * vpdelta * (neq[K] - neq[I]));
            cB(I - 3, K - 3) =
                NeumaierSum(cB(I - 3, K - 3),
                            (omega0 / omega2) * vpdelta * (neq[J] - neq[I]));
        }
        else {
            std::cerr << "#Error building collision operator this process "
                         "should not be in this list"
                      << std::endl;
            world.abort(1);
        }

        iel++;
        if (my_id == 0) {
            std::cout << "#iel => " << iel << " / " << ntot << std::endl;
        }
    }

    for (auto& process_block : absorption_processes) {
        auto process(process_block.second);
        auto q0 = process.q[0];
        auto q1 = process.q[1];
        auto q2 = process.q[2];
        auto b0 = process.alpha[0];
        auto b1 = process.alpha[1];
        auto b2 = process.alpha[2];
        double omega0 = grid.get_spectrum_at_q(q0).omega(b0);
        double omega1 = grid.get_spectrum_at_q(q1).omega(b1);
        double omega2 = grid.get_spectrum_at_q(q2).omega(b2);

        double factor = 1.0e6 * alma::constants::hbar * alma::constants::pi /
                        (4.0 * omega0 * omega1 * omega2) / nqpoints;

        double vpdelta = factor * process.get_vp2();

        if (process.type == alma::threeph_type::absorption) {
            auto I = q0 * nbands + b0;
            auto J = q1 * nbands + b1;
            auto K = q2 * nbands + b2;

            cB(I - 3, I - 3) =
                NeumaierSum(cB(I - 3, I - 3), vpdelta * (neq[K] - neq[J]));
            cB(I - 3, J - 3) =
                NeumaierSum(cB(I - 3, J - 3),
                            omega0 / omega1 * vpdelta * (neq[K] - neq[I]));
            cB(I - 3, K - 3) = NeumaierSum(cB(I - 3, K - 3),
                                           omega0 / omega2 * vpdelta *
                                               (neq[J] + neq[I] + 1.0));
        }
        else {
            std::cerr << "#Error building collision operator this process "
                         "should not be in this list"
                      << std::endl;
            world.abort(1);
        }

        iel++;
        if (my_id == 0) {
            std::cout << "#iel => " << iel << " / " << ntot << std::endl;
        }
    }

    for (auto& process_block : isotopic_processes) {
        auto I = process_block.first.first;
        auto J = process_block.first.second;
        auto q0 = I / nbands;
        auto q1 = J / nbands;
        auto b0 = I % nbands;
        auto b1 = J % nbands;
        double omega0 = grid.get_spectrum_at_q(q0).omega(b0);
        double omega1 = grid.get_spectrum_at_q(q1).omega(b1);
        /// It goes with Umklapp as they do not conserve the momentum
        cB(I - 3, I - 3) = NeumaierSum(cB(I - 3, I - 3), -process_block.second);
        cB(I - 3, J - 3) = NeumaierSum(
            cB(I - 3, J - 3), (omega0 / omega1) * process_block.second);

        iel++;
        if (my_id == 0) {
            std::cout << "#iel => " << iel << " / " << ntot << std::endl;
        }
    }

    /// Now we collapse the matrix in root and broadcast to other procs:
    /// This is not RAM friendly (from memory point of view it will be cheaper
    /// to reduce by element or chunk of elements)
    Eigen::MatrixXd B(nmodes-3,nmodes-3);
    ///We collapse from procs and we
    ///eval the Neumaier Summations
    if (nprocs == 1 ) {
        for (decltype(nmodes) J=0; J < nmodes-3; J++) {
            for (decltype(nmodes) I=0; I < nmodes-3; I++) {
                B(I,J) =
                    evalNeumaierSum(cB(I,J));
            }
        }
    }
    else {
        for (decltype(nmodes) J = 0; J < nmodes - 3; J++) {
            for (decltype(nmodes) I = 0; I < nmodes - 3; I++) {
                std::vector<std::complex<double>> v;

                gather(world, cB(I, J), v, 0);

                /// Eval Neumaier
                if (my_id == 0) {
                    B(I, J) = combineNeumaierSums(v);
                }

                broadcast(world, B(I, J), 0);
            }
        }
    }

    ///Enforcing energy conservation
    for (std::size_t J = 0; J < nmodes - 3; J++) {
        /// Getting the mean of energy that we need to add back to enforce
        /// energy conservation column-major ordering makes those column
        /// operations efficient
        Eigen::VectorXd Bcol = B.col(J);

        auto deltaj_mean = -(NeumaierSum(Bcol.data(),
                         Bcol.rows())) / (nmodes - 3);
        /// We are now adding deltaj_mean to counter the defficit in
        /// conservation
        for (std::size_t I = 0; I < nmodes - 3; I++)
            B(I, J) += deltaj_mean;
        // Making sanity check:

        // Making sanity check:
        if (!alma::almost_equal(B.col(J).sum(),0.)) {
            std::cerr << "Error in enforcing energy"
                      << std::endl;
            world.abort(1);
        }
    }
    /// We return B, the user should take care and use std::move as it is
    /// cheaper than copying.

    return B;
}

void Arnoldi_algorithm(const Eigen::MatrixXd& A,
                       const Eigen::VectorXd& b,
                       int n,
                       Eigen::MatrixXd& B,
                       Eigen::MatrixXd& H,
                       double t) {
    // Init some variables
    int m = A.rows();
    H.resize(n, n);
    H.setZero();
    B.resize(m, n);
    B.setZero();
    Eigen::VectorXd q = b / b.norm(); // Normalize the input vector
    B.col(0) =
        q; // Use the normalized input vector as first Krylov subspace basis

    for (int k = 0; k < n; k++) {
        Eigen::VectorXd z = t * A * q;
        for (int j = 0; j < k;
             j++) { // We are substracting the projections from previous vectors
            H(j, k) = B.col(j).transpose() * z;
            z -= H(j, k) * B.col(j);
        }

	/// Break Arnoldi's, it is complete
        if (almost_equal(z.norm(),
            0.,1.0e-12,1.0e-9)) {
            break;
        }

        if (k + 1 < n) {
            H(k + 1, k) = z.norm();
            q = z / H(k + 1, k); // Getting new vector
            B.col(k + 1) = q;
        }
    }
}

propagator_H::propagator_H() {
}

propagator_H::propagator_H(Eigen::VectorXd& b) {
    std::size_t data_size = b.size();
    (this->signed_cumm).resize(data_size);

    this->pcol_abssum = b.array().abs().sum();

    double accumulator = 0.;
    for (std::size_t i = 0; i < data_size; i++) {
        accumulator += std::abs(b(i));
        (this->signed_cumm)(i) = (b(i) < 0.)
                                     ? -accumulator / (this->pcol_abssum)
                                     : accumulator / (this->pcol_abssum);
    }
}

propagator_H::propagator_H(const propagator_H& B) :
        signed_cumm(B.signed_cumm) , pcol_abssum(B.pcol_abssum) {
}

propagator_H::propagator_H(propagator_H&& B) :
        signed_cumm(std::move(B.signed_cumm)) ,
        pcol_abssum(std::move(B.pcol_abssum)) {
}

propagator_H& propagator_H::operator=(const propagator_H& B) {
    this->signed_cumm = B.signed_cumm;
    this->pcol_abssum = B.pcol_abssum;
    return *this;
}

bool propagator_H::operator<(const propagator_H& B) const {
    return this->pcol_abssum < B.pcol_abssum;
};

std::pair<int, std::size_t> propagator_H::get(double R) const {
    auto im = this->lower_bound(R);

    if ((this->signed_cumm)(im) > 0.)
        return std::make_pair(1, im + 3);
    else
        return std::make_pair(-1, im + 3);

    std::cerr << "Error in selection" << std::endl;
    exit(EXIT_FAILURE);
    return std::make_pair(0, 0);
}

Eigen::VectorXd propagator_H::get_signed_cumm() const {
    return this->signed_cumm;
}

std::size_t propagator_H::byte_size() const {
    return sizeof(double) * (this->signed_cumm).size() +
           sizeof(Eigen::VectorXd) + sizeof(double);
}

std::size_t propagator_H::size() const {
    return (this->signed_cumm).size();
}

alma::propagator_H lirp(alma::propagator_H& H1,
                        alma::propagator_H& H2,
                        double T1,
                        double T2,
                        double Tx) {
    Eigen::VectorXd Pint =
        H1.signed_cumm +
        (Tx - T1) * (H2.signed_cumm - H1.signed_cumm) / (T2 - T1);
    double absint = H1.pcol_abssum +
                    (Tx - T1) * (H2.pcol_abssum - H1.pcol_abssum) / (T2 - T1);
    return alma::propagator_H(Pint, absint);
}

void save_P(std::string fname,
            Eigen::MatrixXd& data,
            boost::mpi::communicator& world) {
    std::size_t master = 0;
    std::size_t my_id = world.rank();
    std::size_t nprocs = world.size();
    if (nprocs > 1) {
        for (std::size_t iproc = 0; iproc < nprocs; iproc++) {
            if (my_id == iproc) {
                if (my_id == master) {
                    Eigen::MatrixXd::Index nmodes = data.rows();
                    auto flags =
                        std::ios::out | std::ios::binary | std::ios::trunc;
                    std::ofstream out((fname).c_str(), flags);
                    out.write((char*)(&nmodes), sizeof(Eigen::MatrixXd::Index));
                    out.write((char*)data.data(), data.size() * sizeof(double));
                    out.close();
                }
                else {
                    /// Other nodes open but not erase the info
                    auto flags =
                        std::ios::out | std::ios::binary | std::ios::app;
                    std::ofstream out((fname).c_str(), flags);
                    out.write((char*)data.data(), data.size() * sizeof(double));
                    out.close();
                }
            }
            world.barrier();
        }
    }
    else {
        Eigen::MatrixXd::Index nmodes = data.rows();
        auto flags = std::ios::out | std::ios::binary | std::ios::trunc;
        std::ofstream out((fname).c_str(), flags);
        out.write((char*)(&nmodes), sizeof(Eigen::MatrixXd::Index));
        out.write((char*)data.data(), data.size() * sizeof(double));
        out.close();
    }
}

void load_P(std::string fname,
            Eigen::MatrixXd& pmat,
            boost::mpi::communicator& world) {
    std::size_t master = 0;
    std::size_t my_id = world.rank();
    std::size_t nprocs = world.size();

    std::vector<Eigen::MatrixXd> pmat_procs(nprocs);

    /// Read binary to memory only in master
    Eigen::MatrixXd::Index nmodes = 0;

    /// It might be
    if (my_id == master) {
        auto flags = std::ios::in | std::ios::binary;
        std::ifstream in((fname).c_str(), flags);
        in.read((char*)(&nmodes), sizeof(Eigen::MatrixXd::Index));
        /// Scale propagator (not the most efficient way)
        if (nprocs > 1) {
            for (std::size_t iproc = 0; iproc < nprocs; iproc++) {
                auto limits = alma::my_jobs(nmodes, nprocs, iproc);
                // If node do not contain info simply ignore
                if (limits[1] - limits[0] == 0)
                    continue;
                auto ncols = limits[1] - limits[0];
                /// Master is special case
                pmat_procs[iproc].resize(nmodes, ncols);
                in.read((char*)pmat_procs[iproc].data(),
                        nmodes * ncols * sizeof(double));
            }
        }
        else {
            pmat.resize(nmodes, nmodes);
            in.read((char*)pmat.data(), nmodes * nmodes * sizeof(double));
        }
        in.close();
    }

    // If multiple procs scatter info
    if (nprocs > 1)
        scatter(world, pmat_procs, pmat, 0.);
}

void build_P(Eigen::MatrixXd& A,
             Eigen::MatrixXd& P,
             double t,
             boost::mpi::communicator& world,
             double eps) {
    /// Check for optimal Kryslov subspace
    int ksize = 2;

    while (true) {
        /// Check that the error per element is lower than the eps
        if (error_bound(A, ksize, t) / A.size() < eps) {
            break;
        }
        ksize++;
    }

    /// Now we will break the problem by MPI processes, taking each of them care
    /// of a column.
    auto limits = alma::my_jobs(A.cols(), world.size(), world.rank());

    auto proc_cols = limits[1] - limits[0];

    if (proc_cols != 0)
        P.resize(A.cols(), proc_cols);

    std::mutex coutmutex;
    std::size_t colcount = 0;
    /// TBB parallelism combined with boost mpi (use MPI for diferent nodes and
    /// TBB inside the same node)
    tbb::parallel_for(
        tbb::blocked_range<std::size_t>(limits[0], limits[1]),
        [&](tbb::blocked_range<std::size_t> colrange) {
            for (auto ii = colrange.begin(); ii < colrange.end(); ii++) {
                coutmutex.lock();
                std::cout << "#Exp " << colcount << "/" << A.cols()
                          << std::endl;
                colcount++;
                coutmutex.unlock();
                Eigen::MatrixXd B, H;
                Eigen::VectorXd b(A.cols());
                b.setZero();
                b(ii) = 1.0;
                alma::Arnoldi_algorithm(A, b, ksize, B, H, t);
                P.col(ii - limits[0]) =
                    (B * (H.exp())).col(0); /// This is the Krylov approach to
                                            /// the exponential
            }
        });
}

void build_histogram(Eigen::MatrixXd& A,
                     std::unordered_map<std::size_t, propagator_H>& modes_histo,
                     double t,
                     boost::mpi::communicator& world,
                     double eps,
                     bool print_info) {
    /// Check for optimal Kryslov subspace
    int ksize = 2;

    while (true) {
        /// Check that the error per element is lower than the eps
        if (error_bound(A, ksize, t) / A.size() < eps) {
            break;
        }
        ksize++;
    }

    /// Now we will break the problem by MPI processes, taking each of them care
    /// of a column.
    auto limits = alma::my_jobs(A.cols(), world.size(), world.rank());

    std::size_t ram_size = 0, nel = 0;

    for (auto i = limits[0]; i < limits[1]; i++) {
        Eigen::MatrixXd B, H;
        Eigen::VectorXd b(A.cols());
        b.setZero();
        b(i) = 1.0;
        alma::Arnoldi_algorithm(A, b, ksize, B, H, t);
        b = (B * (H.exp()))
                .col(0); /// This is the Krylov approach to the exponential
        modes_histo[i + 3] = propagator_H(b);
        ram_size += modes_histo[i + 3].byte_size();
        nel += modes_histo[i + 3].size();
    }

    if (print_info) {
        std::cout << "the size in RAM " << ram_size << " bytes" << std::endl;
        std::cout << "non null elements count " << nel << " / " << A.size()
                  << std::endl;
        std::cout << "density " << (double)nel / (double)A.size() * 100.0
                  << " %" << std::endl;
    }
}

} // namespace alma


#ifndef _LOOKUP_TABLE_H_
#define _LOOKUP_TABLE_H_

#include <boost/tuple/tuple_comparison.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <memory>
#include <algorithm>
#include <vector>
#include <cstdlib>
#include <cassert>
#include <limits>
#include <boost/functional/hash.hpp>
#include <boost/optional.hpp>

namespace sdo
{

using boost::optional;
/**
 * Class to store a lookup table as a piecwise linear function.
 */
class LookupTable
{
public:
   using iterator = boost::zip_iterator<
      boost::tuple <std::vector<double>::const_iterator, std::vector<double>::const_iterator>
   >;

   void addPoint( double, double );

   iterator getPoint( unsigned i ) const;
   iterator begin() const;
   iterator end() const;

   std::vector<double>& getXvals()
   {
      return x;
   }

   std::vector<double>& getYvals()
   {
      return y;
   }

   const std::vector<double>& getXvals() const
   {
      return x;
   }

   const std::vector<double>& getYvals() const
   {
      return y;
   }

   std::size_t size() const {
      return x.size();
   }

   double operator()( double ) const;

   template<class Archive>
   void serialize( Archive& ar, const unsigned int version )
   {
      ar& x;
      ar& y;
   }

private:
   std::vector<double> x;
   std::vector<double> y;
};

// =================== Inline implementation =====================

inline void LookupTable::addPoint( double px, double py )
{
   x.push_back( px );
   y.push_back( py );
}

inline LookupTable::iterator LookupTable::getPoint( unsigned i ) const
{
   return boost::make_zip_iterator(
             boost::make_tuple(
                x.begin() + i,
                y.begin() + i ) );
}

inline LookupTable::iterator LookupTable::begin() const
{
   return boost::make_zip_iterator(
             boost::make_tuple(
                x.begin(),
                y.begin() ) );
}

inline LookupTable::iterator LookupTable::end() const
{
   return boost::make_zip_iterator(
             boost::make_tuple(
                x.end(),
                y.end() ) );
}

inline double LookupTable::operator()( double v ) const
{
   if( v >= *( x.end() - 1 ) )
      return *( y.end() - 1 );

   if( v <= *x.begin() )
      return *y.begin();

   int j = std::upper_bound(
              x.begin(),
              x.end(), v ) - x.begin() - 1;
   return  y[j] + ( y[j + 1] - y[j] ) * ( v - x[j] ) / ( x[j + 1] - x[j] );
}

inline
bool operator==( const LookupTable &a, const LookupTable &b )
{
   LookupTable::iterator aiter, biter;
   aiter = a.begin();
   biter = b.begin();

   while( aiter != a.end() && biter != b.end() )
   {
      if( *( aiter++ ) != *( biter++ ) )
         return false;
   }

   return aiter == a.end() && biter == b.end();
}

inline
std::size_t hash_value( const LookupTable& lkp )
{
   std::size_t hash = boost::hash_range( lkp.getXvals().begin(), lkp.getXvals().end() );
   boost::hash_combine(
      hash,
      boost::hash_range( lkp.getYvals().begin(), lkp.getYvals().end() )
   );
   return hash;
}

}

#endif // _LOOKUP_TABLE_H_


#include <NTL/lzz_p.h>

NTL_START_IMPL

NTL_TLS_GLOBAL_DECL(SmartPtr<zz_pInfoT>, zz_pInfo_stg)

NTL_CHEAP_THREAD_LOCAL zz_pInfoT *zz_pInfo = 0;

SmartPtr<zz_pInfoT> Build_zz_pInfo(FFTPrimeInfo *info)
{
   return MakeSmart<zz_pInfoT>(INIT_FFT, info);
}

zz_pInfoT::zz_pInfoT(long NewP, long maxroot)
{
   if (maxroot < 0) LogicError("zz_pContext: maxroot may not be negative");

   if (NewP <= 1) LogicError("zz_pContext: p must be > 1");
   if (NumBits(NewP) > NTL_SP_NBITS) ResourceError("zz_pContext: modulus too big");

   ZZ P, B, M, M1, MinusM;
   long n, i;
   long q, t;
   mulmod_t qinv;

   p = NewP;
   pinv = PrepMulMod(p);
   red_struct = sp_PrepRem(p);
   ll_red_struct = make_sp_ll_reduce_struct(p);
   ZZ_red_struct.build(p);

   p_info = 0;

   conv(P, p);

   sqr(B, P);
   LeftShift(B, B, maxroot+NTL_FFTFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseFFTPrime(n);
      q = GetFFTPrime(n);
      n++;
      mul(M, M, q);
   }

   if (n > 4) LogicError("zz_pInit: too many primes");

   NumPrimes = n;
   PrimeCnt = n;
   MaxRoot = CalcMaxRoot(q);

   if (maxroot < MaxRoot)
      MaxRoot = maxroot;

   negate(MinusM, M);
   MinusMModP = rem(MinusM, p);
   MinusMModPpinv = PrepMulModPrecon(MinusMModP, p, pinv);

   CoeffModP.SetLength(n);
   CoeffModPpinv.SetLength(n);
   x.SetLength(n);
   u.SetLength(n);
   uqinv.SetLength(n);

   for (i = 0; i < n; i++) {
      q = GetFFTPrime(i);
      qinv = GetFFTPrimeInv(i);

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q);
      CoeffModP[i] = rem(M1, p);
      CoeffModPpinv[i] = PrepMulModPrecon(CoeffModP[i], p, pinv);
      x[i] = ((double) t)/((double) q);
      u[i] = t;
      uqinv[i] = PrepMulModPrecon(t, q, qinv);
   }
}

zz_pInfoT::zz_pInfoT(INIT_FFT_TYPE, FFTPrimeInfo *info)
{
   p = info->q;
   pinv = info->qinv;
   red_struct = sp_PrepRem(p);
   ll_red_struct = make_sp_ll_reduce_struct(p);
   ZZ_red_struct.build(p);

   p_info = info;

   NumPrimes = 1;
   PrimeCnt = 0;

   MaxRoot = CalcMaxRoot(p);
}

// FIXME: we could make bigtab an optional argument

zz_pInfoT::zz_pInfoT(INIT_USER_FFT_TYPE, long q)
{
   long w;
   if (!IsFFTPrime(q, w)) LogicError("invalid user supplied prime");

   p = q;
   pinv = PrepMulMod(p);
   red_struct = sp_PrepRem(p);
   ll_red_struct = make_sp_ll_reduce_struct(p);
   ZZ_red_struct.build(p);

   p_info_owner.make();
   p_info = p_info_owner.get();

   long bigtab_index = -1;
#ifdef NTL_FFT_BIGTAB
   bigtab_index = 0;
#endif
   InitFFTPrimeInfo(*p_info, q, w, bigtab_index);

   NumPrimes = 1;
   PrimeCnt = 0;

   MaxRoot = CalcMaxRoot(p);
}

void zz_p::init(long p, long maxroot)
{
   zz_pContext c(p, maxroot);
   c.restore();

}

void zz_p::FFTInit(long index)
{
   zz_pContext c(INIT_FFT, index);
   c.restore();
}

void zz_p::UserFFTInit(long q)
{
   zz_pContext c(INIT_USER_FFT, q);
   c.restore();
}

zz_pContext::zz_pContext(long p, long maxroot) :
   ptr(MakeSmart<zz_pInfoT>(p, maxroot))
{ }

zz_pContext::zz_pContext(INIT_FFT_TYPE, long index)
{
   if (index < 0)
      LogicError("bad FFT prime index");

   UseFFTPrime(index);

   ptr =  FFTTables[index]->zz_p_context;
}

zz_pContext::zz_pContext(INIT_USER_FFT_TYPE, long q) :
   ptr(MakeSmart<zz_pInfoT>(INIT_USER_FFT, q))
{ }

void zz_pContext::save()
{
   NTL_TLS_GLOBAL_ACCESS(zz_pInfo_stg);
   ptr = zz_pInfo_stg;
}

void zz_pContext::restore() const
{
   NTL_TLS_GLOBAL_ACCESS(zz_pInfo_stg);
   zz_pInfo_stg = ptr;
   zz_pInfo = zz_pInfo_stg.get();
}

zz_pBak::~zz_pBak()
{
   if (MustRestore) c.restore();
}

void zz_pBak::save()
{
   c.save();
   MustRestore = true;
}

void zz_pBak::restore()
{
   c.restore();
   MustRestore = false;
}

istream& operator>>(istream& s, zz_p& x)
{
   NTL_ZZRegister(y);
   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

ostream& operator<<(ostream& s, zz_p a)
{
   NTL_ZZRegister(y);
   y = rep(a);
   s << y;

   return s;
}

// ***********************************************************************

#ifdef NTL_HAVE_LL_TYPE

// NOTE: the following code sequence will generate imulq
// instructions on x86_64 machines, which empirically is faster
// than using the mulq instruction or even the mulxq instruction,
// (tested on a Haswell machine).

long
InnerProd_LL(const long *ap, const zz_p *bp, long n, long d,
          sp_ll_reduce_struct dinv)
{
   const long BLKSIZE = (1L << min(20, 2*(NTL_BITS_PER_LONG-NTL_SP_NBITS)));

   unsigned long acc0 = 0;
   ll_type acc21;
   ll_init(acc21, 0);

   long i;
   for (i = 0; i <= n-BLKSIZE; i += BLKSIZE, ap += BLKSIZE, bp += BLKSIZE) {
      // sum ap[j]*rep(bp[j]) for j in [0..BLKSIZE)

      ll_type sum;
      ll_init(sum, 0);
      for (long j = 0; j < BLKSIZE; j += 4) {
         ll_imul_add(sum, ap[j+0], rep(bp[j+0]));
         ll_imul_add(sum, ap[j+1], rep(bp[j+1]));
         ll_imul_add(sum, ap[j+2], rep(bp[j+2]));
         ll_imul_add(sum, ap[j+3], rep(bp[j+3]));
      }

      ll_add(sum, acc0);
      acc0 = ll_get_lo(sum);
      ll_add(acc21, ll_get_hi(sum));
   }

   if (i < n) {
      // sum ap[i]*rep(bp[j]) for j in [0..n-i)

      ll_type sum;
      ll_init(sum, 0);
      long j = 0;
      for (; j <= n-i-4; j += 4) {
         ll_imul_add(sum, ap[j+0], rep(bp[j+0]));
         ll_imul_add(sum, ap[j+1], rep(bp[j+1]));
         ll_imul_add(sum, ap[j+2], rep(bp[j+2]));
         ll_imul_add(sum, ap[j+3], rep(bp[j+3]));
      }

      for (; j < n-i; j++)
         ll_imul_add(sum, ap[j], rep(bp[j]));

      ll_add(sum, acc0);
      acc0 = ll_get_lo(sum);
      ll_add(acc21, ll_get_hi(sum));
   }

   if (dinv.nbits == NTL_SP_NBITS)
      return sp_ll_red_31_normalized(ll_get_hi(acc21), ll_get_lo(acc21), acc0, d, dinv);
   else
      return sp_ll_red_31(ll_get_hi(acc21), ll_get_lo(acc21), acc0, d, dinv);
}

long
InnerProd_LL(const zz_p *ap, const zz_p *bp, long n, long d,
          sp_ll_reduce_struct dinv)
{
   const long BLKSIZE = (1L << min(20, 2*(NTL_BITS_PER_LONG-NTL_SP_NBITS)));

   unsigned long acc0 = 0;
   ll_type acc21;
   ll_init(acc21, 0);

   long i;
   for (i = 0; i <= n-BLKSIZE; i += BLKSIZE, ap += BLKSIZE, bp += BLKSIZE) {
      // sum ap[j]*rep(bp[j]) for j in [0..BLKSIZE)

      ll_type sum;
      ll_init(sum, 0);
      for (long j = 0; j < BLKSIZE; j += 4) {
         ll_imul_add(sum, rep(ap[j+0]), rep(bp[j+0]));
         ll_imul_add(sum, rep(ap[j+1]), rep(bp[j+1]));
         ll_imul_add(sum, rep(ap[j+2]), rep(bp[j+2]));
         ll_imul_add(sum, rep(ap[j+3]), rep(bp[j+3]));
      }

      ll_add(sum, acc0);
      acc0 = ll_get_lo(sum);
      ll_add(acc21, ll_get_hi(sum));
   }

   if (i < n) {
      // sum ap[i]*rep(bp[j]) for j in [0..n-i)

      ll_type sum;
      ll_init(sum, 0);
      long j = 0;
      for (; j <= n-i-4; j += 4) {
         ll_imul_add(sum, rep(ap[j+0]), rep(bp[j+0]));
         ll_imul_add(sum, rep(ap[j+1]), rep(bp[j+1]));
         ll_imul_add(sum, rep(ap[j+2]), rep(bp[j+2]));
         ll_imul_add(sum, rep(ap[j+3]), rep(bp[j+3]));
      }

      for (; j < n-i; j++)
         ll_imul_add(sum, rep(ap[j]), rep(bp[j]));

      ll_add(sum, acc0);
      acc0 = ll_get_lo(sum);
      ll_add(acc21, ll_get_hi(sum));
   }

   if (dinv.nbits == NTL_SP_NBITS)
      return sp_ll_red_31_normalized(ll_get_hi(acc21), ll_get_lo(acc21), acc0, d, dinv);
   else
      return sp_ll_red_31(ll_get_hi(acc21), ll_get_lo(acc21), acc0, d, dinv);
}

long
InnerProd_L(const long *ap, const zz_p *bp, long n, long d,
          sp_reduce_struct dinv, long bound)
{
   unsigned long sum = 0;
   long j = 0;

   if (n <= bound) {
      for (; j <= n-4; j += 4) {
	 sum += (ap[j+0]) * rep(bp[j+0]);
	 sum += (ap[j+1]) * rep(bp[j+1]);
	 sum += (ap[j+2]) * rep(bp[j+2]);
	 sum += (ap[j+3]) * rep(bp[j+3]);
      }

      for (; j < n; j++)
	 sum += (ap[j]) * rep(bp[j]);

      return rem(sum, d, dinv);
   }

   for (; j <= n-bound; j += bound) {

      long i = 0;
      for (; i <= bound-4; i += 4) {
         sum += (ap[j+i+0]) * rep(bp[j+i+0]);
         sum += (ap[j+i+1]) * rep(bp[j+i+1]);
         sum += (ap[j+i+2]) * rep(bp[j+i+2]);
         sum += (ap[j+i+3]) * rep(bp[j+i+3]);
      }

      for (; i < bound; i++) {
         sum += (ap[j+i]) * rep(bp[j+i]);
      }

      sum = rem(sum, d, dinv);
   }

   if (j >= n) return sum;

   for (; j <= n-4; j += 4) {
      sum += (ap[j+0]) * rep(bp[j+0]);
      sum += (ap[j+1]) * rep(bp[j+1]);
      sum += (ap[j+2]) * rep(bp[j+2]);
      sum += (ap[j+3]) * rep(bp[j+3]);
   }

   for (; j < n; j++)
      sum += (ap[j]) * rep(bp[j]);

   return rem(sum, d, dinv);
}

long
InnerProd_L(const zz_p *ap, const zz_p *bp, long n, long d,
          sp_reduce_struct dinv, long bound)
{
   unsigned long sum = 0;
   long j = 0;

   if (n <= bound) {
      for (; j <= n-4; j += 4) {
	 sum += rep(ap[j+0]) * rep(bp[j+0]);
	 sum += rep(ap[j+1]) * rep(bp[j+1]);
	 sum += rep(ap[j+2]) * rep(bp[j+2]);
	 sum += rep(ap[j+3]) * rep(bp[j+3]);
      }

      for (; j < n; j++)
	 sum += rep(ap[j]) * rep(bp[j]);

      return rem(sum, d, dinv);
   }

   for (; j <= n-bound; j += bound) {

      long i = 0;
      for (; i <= bound-4; i += 4) {
         sum += rep(ap[j+i+0]) * rep(bp[j+i+0]);
         sum += rep(ap[j+i+1]) * rep(bp[j+i+1]);
         sum += rep(ap[j+i+2]) * rep(bp[j+i+2]);
         sum += rep(ap[j+i+3]) * rep(bp[j+i+3]);
      }

      for (; i < bound; i++) {
         sum += rep(ap[j+i]) * rep(bp[j+i]);
      }

      sum = rem(sum, d, dinv);
   }

   if (j >= n) return sum;

   for (; j <= n-4; j += 4) {
      sum += rep(ap[j+0]) * rep(bp[j+0]);
      sum += rep(ap[j+1]) * rep(bp[j+1]);
      sum += rep(ap[j+2]) * rep(bp[j+2]);
      sum += rep(ap[j+3]) * rep(bp[j+3]);
   }

   for (; j < n; j++)
      sum += rep(ap[j]) * rep(bp[j]);

   return rem(sum, d, dinv);
}

#endif

NTL_END_IMPL


/* Copyright (c) 2016, Jakob Engel
 * 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 "FOVUndistorter.h"

#include <sstream>
#include <fstream>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <locale.h>

UndistorterFOV::UndistorterFOV()
{
	remapX = nullptr;
	remapY = nullptr;
	valid=false;
}

UndistorterFOV::UndistorterFOV(const char* configFileName)
{

	setlocale(LC_NUMERIC, "C");
	remapX = nullptr;
	remapY = nullptr;
	valid=false;

	// read parameters
	std::ifstream infile(configFileName);
	if(!infile.good())
	{
		printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName);
		return;
	}

	std::string l1,l2,l3,l4;
	std::getline(infile,l1);
	std::getline(infile,l2);
	std::getline(infile,l3);
	std::getline(infile,l4);

	// l1 & l2
	if(std::sscanf(l1.c_str(), "%f %f %f %f %f", &inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4]) == 5 &&
			std::sscanf(l2.c_str(), "%d %d", &in_width, &in_height) == 2)
	{
		printf("Input resolution: %d %d\n",in_width, in_height);
		printf("Input Calibration (fx fy cx cy): %f %f %f %f %f\n",
				in_width*inputCalibration[0], in_height*inputCalibration[1], in_width*inputCalibration[2], in_height*inputCalibration[3], inputCalibration[4]);
	}
	else
	{
		printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName);
		return;
	}

	// l3
	if(l3 == "crop")
	{
		outputCalibration[0] = -1;
		printf("Out: Crop\n");
	}
	else if(l3 == "full")
	{
		outputCalibration[0] = -2;
		printf("Out: Full\n");
	}
	else if(l3 == "none")
	{
		printf("NO RECTIFICATION\n");
		return;
	}
	else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5)
	{
		printf("Out: %f %f %f %f %f\n",
				outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]);
	}
	else
	{
		printf("Out: Failed to Read Output pars... not rectifying.\n");
		return;
	}

	// l4
	if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2)
	{
		printf("Output resolution: %d %d\n",out_width, out_height);
	}
	else
	{
		printf("Out: Failed to Read Output resolution... not rectifying.\n");
		return;
	}

	valid=true;

	// =============================== find optimal new camera matrix ===============================
	// prep warp matrices
	float dist = inputCalibration[4];
	float d2t = 2.0f * tan(dist / 2.0f);

	// current camera parameters
	float fx = inputCalibration[0] * in_width;
	float fy = inputCalibration[1] * in_height;
	float cx = inputCalibration[2] * in_width - 0.5;
	float cy = inputCalibration[3] * in_height - 0.5;

	// output camera parameters
	float ofx, ofy, ocx, ocy;

	// find new camera matrix for "crop" and "full"
	if (inputCalibration[4] == 0)
	{
		ofx = inputCalibration[0] * out_width;
		ofy = inputCalibration[1] * out_height;
		ocx = (inputCalibration[2] * out_width) - 0.5;
		ocy = (inputCalibration[3] * out_height) - 0.5;
	}
	else if(outputCalibration[0] == -1)	// "crop"
	{
		// find left-most and right-most radius
		float left_radius = (cx)/fx;
		float right_radius = (in_width-1 - cx)/fx;
		float top_radius = (cy)/fy;
		float bottom_radius = (in_height-1 - cy)/fy;

		float trans_left_radius = tan(left_radius * dist)/d2t;
		float trans_right_radius = tan(right_radius * dist)/d2t;
		float trans_top_radius = tan(top_radius * dist)/d2t;
		float trans_bottom_radius = tan(bottom_radius * dist)/d2t;

		ofy = fy * ((top_radius + bottom_radius) / (trans_top_radius + trans_bottom_radius)) * ((float)out_height / (float)in_height);
		ocy = (trans_top_radius/top_radius) * ofy*cy/fy;

		ofx = fx * ((left_radius + right_radius) / (trans_left_radius + trans_right_radius)) * ((float)out_width / (float)in_width);
		ocx = (trans_left_radius/left_radius) * ofx*cx/fx;

		printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy);
		printf("old K: %f %f %f %f\n",fx,fy,cx,cy);
	}
	else if(outputCalibration[0] == -2)	 // "full"
	{
		float left_radius = cx/fx;
		float right_radius = (in_width-1 - cx)/fx;
		float top_radius = cy/fy;
		float bottom_radius = (in_height-1 - cy)/fy;

		// find left-most and right-most radius
		float tl_radius = sqrt(left_radius*left_radius + top_radius*top_radius);
		float tr_radius = sqrt(right_radius*right_radius + top_radius*top_radius);
		float bl_radius = sqrt(left_radius*left_radius + bottom_radius*bottom_radius);
		float br_radius = sqrt(right_radius*right_radius + bottom_radius*bottom_radius);

		float trans_tl_radius = tan(tl_radius * dist)/d2t;
		float trans_tr_radius = tan(tr_radius * dist)/d2t;
		float trans_bl_radius = tan(bl_radius * dist)/d2t;
		float trans_br_radius = tan(br_radius * dist)/d2t;

		float hor = std::max(br_radius,tr_radius) + std::max(bl_radius,tl_radius);
		float vert = std::max(tr_radius,tl_radius) + std::max(bl_radius,br_radius);

		float trans_hor = std::max(trans_br_radius,trans_tr_radius) + std::max(trans_bl_radius,trans_tl_radius);
		float trans_vert = std::max(trans_tr_radius,trans_tl_radius) + std::max(trans_bl_radius,trans_br_radius);

		ofy = fy * ((vert) / (trans_vert)) * ((float)out_height / (float)in_height);
		ocy = std::max(trans_tl_radius/tl_radius,trans_tr_radius/tr_radius) * ofy*cy/fy;

		ofx = fx * ((hor) / (trans_hor)) * ((float)out_width / (float)in_width);
		ocx = std::max(trans_bl_radius/bl_radius,trans_tl_radius/tl_radius) * ofx*cx/fx;

		printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy);
		printf("old K: %f %f %f %f\n",fx,fy,cx,cy);
	}
	else
	{
		ofx = outputCalibration[0] * out_width;
		ofy = outputCalibration[1] * out_height;
		ocx = outputCalibration[2] * out_width-0.5;
		ocy = outputCalibration[3] * out_height-0.5;
	}

	outputCalibration[0] = ofx / out_width;
	outputCalibration[1] = ofy / out_height;
	outputCalibration[2] = (ocx+0.5) / out_width;
	outputCalibration[3] = (ocy+0.5) / out_height;
	outputCalibration[4] = 0;

	// =============================== build rectification map ===============================
	remapX = new float[out_width * out_height];
	remapY = new float[out_width * out_height];
	for(int y=0;y<out_height;y++)
		for(int x=0;x<out_width;x++)
		{
			remapX[x+y*out_width] = x;
			remapY[x+y*out_width] = y;
		}
	distortCoordinates(remapX, remapY, out_height*out_width);

	bool hasBlackPoints = false;
	for(int i=0;i<out_width * out_height;i++)
	{
		if(remapX[i] == 0) remapX[i] = 0.01;
		if(remapY[i] == 0) remapY[i] = 0.01;
		if(remapX[i] == in_width-1) remapX[i] = in_width-1.01;
		if(remapY[i] == in_height-1) remapY[i] = in_height-1.01;

		if(!(remapX[i] > 0 && remapY[i] > 0 && remapX[i] < in_width-1 &&  remapY[i] < in_height-1))
		{
			//printf("black pixel at %d %d %f %f!\n", i, i, remapX[i], remapY[i]);
			hasBlackPoints=true;
			remapX[i]=-1;
			remapY[i]=-1;

		}
	}

	if(hasBlackPoints)
		printf("\n\nFOV Undistorter: Warning! Image has black pixels.\n\n\n");

	// =============================== set Krect ===============================
	Krect.setIdentity();
	Krect(0,0) = outputCalibration[0] * out_width;
	Krect(1,1) = outputCalibration[1] * out_height;
	Krect(0,2) = outputCalibration[2] * out_width-0.5;
	Krect(1,2) = outputCalibration[3] * out_height-0.5;

	Korg.setIdentity();
	Korg(0,0) = inputCalibration[0] * in_width;
	Korg(1,1) = inputCalibration[1] * in_height;
	Korg(0,2) = inputCalibration[2] * in_width-0.5;
	Korg(1,2) = inputCalibration[3] * in_height-0.5;
}

UndistorterFOV::~UndistorterFOV()
{
	if(remapX != 0) delete[] remapX;
	if(remapY != 0) delete[] remapY;
}

void UndistorterFOV::distortCoordinates(float* in_x, float* in_y, int n)
{
	if(!valid)
	{
		printf("ERROR: invalid UndistorterFOV!\n");
		return;
	}

	float dist = inputCalibration[4];
	float d2t = 2.0f * tan(dist / 2.0f);

	// current camera parameters
	float fx = inputCalibration[0] * in_width;
	float fy = inputCalibration[1] * in_height;
	float cx = inputCalibration[2] * in_width - 0.5;
	float cy = inputCalibration[3] * in_height - 0.5;

	float ofx = outputCalibration[0]*out_width;
	float ofy = outputCalibration[1]*out_height;
	float ocx = outputCalibration[2]*out_width-0.5f;
	float ocy = outputCalibration[3]*out_height-0.5f;

	for(int i=0;i<n;i++)
	{
		float x = in_x[i];
		float y = in_y[i];
		float ix = (x - ocx) / ofx;
		float iy = (y - ocy) / ofy;

		float r = sqrtf(ix*ix + iy*iy);
		float fac = (r==0 || dist==0) ? 1 : atanf(r * d2t)/(dist*r);

		ix = fx*fac*ix+cx;
		iy = fy*fac*iy+cy;

		in_x[i] = ix;
		in_y[i] = iy;
	}
}

template<typename T>
void UndistorterFOV::undistort(const T* input, float* output, int nPixIn, int nPixOut) const
{
	if(!valid) return;

	if(nPixIn != in_width*in_height)
	{
		printf("ERROR: undistort called with wrong input image dismesions (expected %d pixel, got %d pixel)\n",
				in_width*in_height, nPixIn);
		return;
	}
	if(nPixOut != out_width*out_height)
	{
		printf("ERROR: undistort called with wrong output image dismesions (expected %d pixel, got %d pixel)\n",
				out_width*out_height, nPixOut);
		return;
	}

	for(int idx = 0; idx < out_width*out_height;idx++)
	{
		// get interp. values
		float xx = remapX[idx];
		float yy = remapY[idx];

		if(xx<0)
			output[idx] = 0;
		else
		{
			// get integer and rational parts
			int xxi = xx;
			int yyi = yy;
			xx -= xxi;
			yy -= yyi;
			float xxyy = xx*yy;

			// get array base pointer
			const T* src = input + xxi + yyi * in_width;

			// interpolate (bilinear)
			output[idx] =  xxyy * src[1+in_width]
								+ (yy-xxyy) * src[in_width]
								+ (xx-xxyy) * src[1]
								+ (1-xx-yy+xxyy) * src[0];
		}
	}
}
template void UndistorterFOV::undistort<float>(const float* input, float* output, int nPixIn, int nPixOut) const;
template void UndistorterFOV::undistort<unsigned char>(const unsigned char* input, float* output, int nPixIn, int nPixOut) const;