/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Mitsuba is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/render/scene.h>
#include <mitsuba/render/volume.h>
#include <mitsuba/core/statistics.h>
#include <boost/algorithm/string.hpp>

MTS_NAMESPACE_BEGIN

/**
 * \brief When the following line is uncommented, the medium implementation
 * stops integrating density when it is determined that the segment has a
 * throughput of less than 'Epsilon' (see \c mitsuba/core/constants.h)
 */
#define HETVOL_EARLY_EXIT 1

/// Generate a few statistics related to the implementation?
// #define HETVOL_STATISTICS 1

#if defined(HETVOL_STATISTICS)
static StatsCounter avgNewtonIterations("Heterogeneous volume",
		"Avg. # of Newton-Bisection iterations", EAverage);
static StatsCounter avgRayMarchingStepsTransmittance("Heterogeneous volume",
		"Avg. # of ray marching steps (transmittance)", EAverage);
static StatsCounter avgRayMarchingStepsSampling("Heterogeneous volume",
		"Avg. # of ray marching steps (sampling)", EAverage);
static StatsCounter earlyExits("Heterogeneous volume",
		"Number of early exits", EPercentage);
#endif

/*!\plugin{heterogeneous}{Heterogeneous participating medium}
 * \order{2}
 * \parameters{
 *     \parameter{method}{\String}{
 *         Specifies the sampling method that is used to generate
 *         scattering events within the medium.
 *         \begin{enumerate}[(i)]
 *             \item \code{simpson}: Sampling is done by inverting a
 *             deterministic quadrature rule based on composite
 *             Simpson integration over small ray segments. Benefits
 *             from the use of good sample generators (e.g. \pluginref{ldsampler}).
 *             \item \code{woodcock}: Generate samples using
 *             Woodcock tracking. This is usually faster and
 *             always unbiased, but has the disadvantages of not benefiting
 *             from good sample generators and not providing
 *             information that is required by bidirectional
 *             rendering techniques.
 *         \end{enumerate}
 *         Default: \texttt{woodcock}
 *     }
 *     \parameter{density}{\Volume}{
 *         Volumetric data source that supplies the medium densities
 *         (in inverse scene units)
 *     }
 *     \parameter{albedo}{\Volume}{
 *         Volumetric data source that supplies the
 *         single-scattering albedo
 *     }
 *     \parameter{orientation}{\Volume}{
 *         Optional: volumetric data source that supplies the
 *         local particle orientations throughout the medium
 *     }
 *     \parameter{scale}{\Float}{
 *         Optional scale factor that will be applied to the \code{density} parameter.
 *         Provided for convenience when accomodating data based on different units,
 *         or to simply tweak the density of the medium. \default{1}
 *     }
 *     \parameter{\Unnamed}{\Phase}{
 *          A nested phase function that describes the directional
 *          scattering properties of the medium. When none is specified,
 *          the renderer will automatically use an instance of
 *          \pluginref{isotropic}.
 *     }
 * }
 *
 * \renderings{
 *     \medrendering{40}{medium_heterogeneous_density_40}
 *     \medrendering{200}{medium_heterogeneous_density_200}
 *     \medrendering{1000}{medium_heterogeneous_density_1000}
 *     \vspace{-2mm}
 *     \caption{Renderings of an index-matched medium using different scale factors (\lstref{hetvolume})}
 * }
 *
 * This plugin provides a flexible heterogeneous medium implementation, which
 * acquires its data from nested \code{volume} instances. These can be
 * constant, use a procedural function, or fetch data from disk, e.g. using a
 * memory-mapped density grid. See \secref{volumes} for details on volume data
 * sources.
 *
 * Instead of allowing separate volumes to be provided for the scattering
 * and absorption parameters \code{sigmaS} and \code{sigmaA} (as is done in
 * \pluginref{homogeneous}), this class instead takes the approach of
 * enforcing a spectrally uniform value of \code{sigmaT}, which must be
 * provided using a nested scalar-valued volume named \code{density}.
 *
 * Another nested spectrum-valued \code{albedo} volume must also be provided, which is
 * used to compute the scattering coefficient $\sigma_s$ using the expression
 * $\sigma_s = \code{scale} * \code{density} * \code{albedo}$ (i.e. 'albedo' contains the
 * single-scattering albedo of the medium.
 *
 * Optionally, one can also provide an vector-valued \code{orientation} volume,
 * which contains local particle orientation that will be passed to
 * scattering models that support this, such as a the Micro-flake or
 * Kajiya-Kay phase functions.
 *
 * \vspace{4mm}
 *
 * \begin{xml}[label=lst:hetvolume,caption=A simple heterogeneous medium backed by a grid volume]
 * <!-- Declare a heterogeneous participating medium named 'smoke' -->
 * <medium type="heterogeneous" id="smoke">
 *     <string name="method" value="simpson"/>
 *
 *     <!-- Acquire density values from an external data file -->
 *     <volume name="density" type="gridvolume">
 *         <string name="filename" value="frame_0150.vol"/>
 *     </volume>
 *
 *     <!-- The albedo is constant and set to 0.9 -->
 *     <volume name="albedo" type="constvolume">
 *         <spectrum name="value" value="0.9"/>
 *     </volume>
 *
 *     <!-- Use an isotropic phase function -->
 *     <phase type="isotropic"/>
 *
 *     <!-- Scale the density values as desired -->
 *     <float name="scale" value="200"/>
 *  </medium>
 *
 * <!-- Attach the index-matched medium to a shape in the scene -->
 * <shape type="obj">
 *     <!-- Load an OBJ file, which contains a mesh version
 *          of the axis-aligned box of the volume data file -->
 *     <string name="filename" value="bounds.obj"/>
 *
 *     <!-- Reference the medium by ID -->
 *     <ref name="interior" id="smoke"/>
 *
 *     <!-- If desired, this shape could also declare
 *          a BSDF to create an index-mismatched
 *          transition, e.g.
 *
 *     <bsdf type="dielectric"/>
 *     -->
 * </shape>
 * \end{xml}
 */
class HeterogeneousMedium : public Medium {
public:
	/// Possible integration modes
	enum EIntegrationMethod {
		/**
		 * \brief Use deterministic composite Simpson quadrature both
		 * to compute transmittances, and to sample scattering locations
		 */
		ESimpsonQuadrature = 0,

		/**
		 * \brief Use stochastic Woodcock tracking. This is potentially
		 * faster and more robust, but has the disadvantage of being
		 * incompatible with bidirectional rendering methods, which
		 * usually need to know the probability of a sample.
		 */
		EWoodcockTracking
	};

	HeterogeneousMedium(const Properties &props)
		: Medium(props) {
		m_stepSize = props.getFloat("stepSize", 0);
		m_scale = props.getFloat("scale", 1);
		if (props.hasProperty("sigmaS") || props.hasProperty("sigmaA"))
			Log(EError, "The 'sigmaS' and 'sigmaA' properties are only supported by "
				"homogeneous media. Please use nested volume instances to supply "
				"these parameters");

		if (props.hasProperty("densityMultiplier"))
			Log(EError, "The 'densityMultiplier' parameter has been deprecated and is now called 'scale'.");

		std::string method = boost::to_lower_copy(props.getString("method", "woodcock"));
		if (method == "woodcock")
			m_method = EWoodcockTracking;
		else if (method == "simpson")
			m_method = ESimpsonQuadrature;
		else
			Log(EError, "Unsupported integration method \"%s\"!", method.c_str());
	}

	/* Unserialize from a binary data stream */
	HeterogeneousMedium(Stream *stream, InstanceManager *manager)
		: Medium(stream, manager) {
		m_method = (EIntegrationMethod) stream->readInt();
		m_scale = stream->readFloat();
		m_density = static_cast<VolumeDataSource *>(manager->getInstance(stream));
		m_albedo = static_cast<VolumeDataSource *>(manager->getInstance(stream));
		m_orientation = static_cast<VolumeDataSource *>(manager->getInstance(stream));
		m_stepSize = stream->readFloat();
		configure();
	}

	/* Serialize the volume to a binary data stream */
	void serialize(Stream *stream, InstanceManager *manager) const {
		Medium::serialize(stream, manager);
		stream->writeInt(m_method);
		stream->writeFloat(m_scale);
		manager->serialize(stream, m_density.get());
		manager->serialize(stream, m_albedo.get());
		manager->serialize(stream, m_orientation.get());
		stream->writeFloat(m_stepSize);
	}

	void configure() {
		Medium::configure();
		if (m_density.get() == NULL)
			Log(EError, "No density specified!");
		if (m_albedo.get() == NULL)
			Log(EError, "No albedo specified!");
		m_densityAABB = m_density->getAABB();
		m_anisotropicMedium =
			m_phaseFunction->needsDirectionallyVaryingCoefficients();

		/* Assumes that the density medium does not
		   contain values greater than one! */
		m_maxDensity = m_scale * m_density->getMaximumFloatValue();
		if (m_anisotropicMedium)
			m_maxDensity *= m_phaseFunction->sigmaDirMax();
		m_invMaxDensity = 1.0f/m_maxDensity;

		if (m_stepSize == 0) {
			m_stepSize = std::min(
				m_density->getStepSize(), m_albedo->getStepSize());
			if (m_orientation != NULL)
				m_stepSize = std::min(m_stepSize,
					m_orientation->getStepSize());

			if (m_stepSize == std::numeric_limits<Float>::infinity())
				Log(EError, "Unable to infer a suitable step size for deterministic "
						"integration, please specify one manually using the 'stepSize' "
						"parameter.");
		}

		if (m_anisotropicMedium && m_orientation.get() == NULL)
			Log(EError, "Cannot use anisotropic phase function: "
				"did not specify a particle orientation field!");
	}

	void addChild(const std::string &name, ConfigurableObject *child) {
		if (child->getClass()->derivesFrom(MTS_CLASS(VolumeDataSource))) {
			VolumeDataSource *volume = static_cast<VolumeDataSource *>(child);

			if (name == "albedo") {
				Assert(volume->supportsSpectrumLookups());
				m_albedo = volume;
			} else if (name == "density") {
				Assert(volume->supportsFloatLookups());
				m_density = volume;
			} else if (name == "orientation") {
				Assert(volume->supportsVectorLookups());
				m_orientation = volume;
			} else {
				Medium::addChild(name, child);
			}
		} else {
			Medium::addChild(name, child);
		}
	}

	/*
	 * This function uses Simpson quadrature to compute following
	 * integral:
	 *
	 *    \int_{ray.mint}^{ray.maxt} density(ray(x)) dx
	 *
	 * The integration proceeds by splitting the function into
	 * approximately \c (ray.maxt-ray.mint)/m_stepSize segments,
	 * each of which are then approximated by a quadratic polynomial.
	 * The step size must be chosen so that this approximation is
	 * valid given the behavior of the integrand.
	 *
	 * \param ray
	 *    Ray segment to be used for the integration
	 *
	 * \return
	 *    The integrated density
	 */
	Float integrateDensity(const Ray &ray) const {
		/* Determine the ray segment, along which the
		   density integration should take place */
		Float mint, maxt;
		if (!m_densityAABB.rayIntersect(ray, mint, maxt))
			return 0.0f;

		mint = std::max(mint, ray.mint);
		maxt = std::min(maxt, ray.maxt);
		Float length = maxt-mint, maxComp = 0;

		Point p = ray(mint), pLast = ray(maxt);

		/* Ignore degenerate path segments */
		for (int i=0; i<3; ++i)
			maxComp = std::max(std::max(maxComp,
				std::abs(p[i])), std::abs(pLast[i]));
		if (length < 1e-6f * maxComp)
			return 0.0f;

		/* Compute a suitable step size */
		uint32_t nSteps = (uint32_t) std::ceil(length / m_stepSize);
		nSteps += nSteps % 2;
		const Float stepSize = length/nSteps;
		const Vector increment = ray.d * stepSize;

		#if defined(HETVOL_STATISTICS)
			avgRayMarchingStepsTransmittance.incrementBase();
			earlyExits.incrementBase();
		#endif

		/* Perform lookups at the first and last node */
		Float integratedDensity = lookupDensity(p, ray.d)
			+ lookupDensity(pLast, ray.d);

		#if defined(HETVOL_EARLY_EXIT)
			const Float stopAfterDensity = -math::fastlog(Epsilon);
			const Float stopValue = stopAfterDensity*3.0f/(stepSize
					* m_scale);
		#endif

		p += increment;

		Float m = 4;
		for (uint32_t i=1; i<nSteps; ++i) {
			integratedDensity += m * lookupDensity(p, ray.d);
			m = 6 - m;

			#if defined(HETVOL_STATISTICS)
				++avgRayMarchingStepsTransmittance;
			#endif

			#if defined(HETVOL_EARLY_EXIT)
				if (integratedDensity > stopValue) {
					// Reached the threshold -- stop early
					#if defined(HETVOL_STATISTICS)
						++earlyExits;
					#endif
					return std::numeric_limits<Float>::infinity();
				}
			#endif

			Point next = p + increment;
			if (p == next) {
				Log(EWarn, "integrateDensity(): unable to make forward progress -- "
						"round-off error issues? The step size was %e, mint=%f, "
						"maxt=%f, nSteps=%i, ray=%s", stepSize, mint, maxt, nSteps,
						ray.toString().c_str());
				break;
			}
			p = next;
		}

		return integratedDensity * m_scale
			* stepSize * (1.0f / 3.0f);
	}

	/**
	 * This function uses composite Simpson quadrature to solve the
	 * following integral equation for \a t:
	 *
	 *    \int_{ray.mint}^t density(ray(x)) dx == desiredDensity
	 *
	 * The integration proceeds by splitting the function into
	 * approximately \c (ray.maxt-ray.mint)/m_stepSize segments,
	 * each of which are then approximated by a quadratic polynomial.
	 * The step size must be chosen so that this approximation is
	 * valid given the behavior of the integrand.
	 *
	 * \param ray
	 *    Ray segment to be used for the integration
	 *
	 * \param desiredDensity
	 *    Right hand side of the above equation
	 *
	 * \param integratedDensity
	 *    Contains the final integrated density. Upon success, this value
	 *    should closely match \c desiredDensity. When the equation could
	 *    \a not be solved, the parameter contains the integrated density
	 *    from \c ray.mint to \c ray.maxt (which, in this case, must be
	 *    less than \c desiredDensity).
	 *
	 * \param t
	 *    After calling this function, \c t will store the solution of the above
	 *    equation. When there is no solution, it will be set to zero.
	 *
	 * \param densityAtMinT
	 *    After calling this function, \c densityAtMinT will store the
	 *    underlying density function evaluated at \c ray(ray.mint).
	 *
	 * \param densityAtT
	 *    After calling this function, \c densityAtT will store the
	 *    underlying density function evaluated at \c ray(t). When
	 *    there is no solution, it will be set to zero.
	 *
	 * \return
	 *    When no solution can be found in [ray.mint, ray.maxt] the
	 *    function returns \c false.
	 */
	bool invertDensityIntegral(const Ray &ray, Float desiredDensity,
			Float &integratedDensity, Float &t, Float &densityAtMinT,
			Float &densityAtT) const {
		integratedDensity = densityAtMinT = densityAtT = 0.0f;

		/* Determine the ray segment, along which the
		   density integration should take place */
		Float mint, maxt;
		if (!m_densityAABB.rayIntersect(ray, mint, maxt))
			return false;
		mint = std::max(mint, ray.mint);
		maxt = std::min(maxt, ray.maxt);
		Float length = maxt - mint, maxComp = 0;
		Point p = ray(mint), pLast = ray(maxt);

		/* Ignore degenerate path segments */
		for (int i=0; i<3; ++i)
			maxComp = std::max(std::max(maxComp,
				std::abs(p[i])), std::abs(pLast[i]));
		if (length < 1e-6f * maxComp)
			return 0.0f;

		/* Compute a suitable step size (this routine samples the integrand
		   between steps, hence the factor of 2) */
		uint32_t nSteps = (uint32_t) std::ceil(length / (2*m_stepSize));
		Float stepSize = length / nSteps,
			  multiplier = (1.0f / 6.0f) * stepSize
				  * m_scale;
		Vector fullStep = ray.d * stepSize,
			   halfStep = fullStep * .5f;

		Float node1 = lookupDensity(p, ray.d);

		if (ray.mint == mint)
			densityAtMinT = node1 * m_scale;
		else
			densityAtMinT = 0.0f;

		#if defined(HETVOL_STATISTICS)
			avgRayMarchingStepsSampling.incrementBase();
		#endif

		for (uint32_t i=0; i<nSteps; ++i) {
			Float node2 = lookupDensity(p + halfStep, ray.d),
				  node3 = lookupDensity(p + fullStep, ray.d),
				  newDensity = integratedDensity + multiplier *
						(node1+node2*4+node3);
			#if defined(HETVOL_STATISTICS)
				++avgRayMarchingStepsSampling;
			#endif
			if (newDensity >= desiredDensity) {
				/* The integrated density of the last segment exceeds the desired
				   amount -- now use the Simpson quadrature expression and
				   Newton-Bisection to find the precise location of the scattering
				   event. Note that no further density queries are performed after
				   this point; instead, the density are modeled based on a
				   quadratic polynomial that is fit to the last three lookups */

				Float a = 0, b = stepSize, x = a,
					  fx = integratedDensity - desiredDensity,
					  stepSizeSqr = stepSize * stepSize,
					  temp = m_scale / stepSizeSqr;
				int it = 1;

				#if defined(HETVOL_STATISTICS)
					avgNewtonIterations.incrementBase();
				#endif
				while (true) {
					#if defined(HETVOL_STATISTICS)
						++avgNewtonIterations;
					#endif
					/* Lagrange polynomial from the Simpson quadrature */
					Float dfx = temp * (node1 * stepSizeSqr
						- (3*node1 - 4*node2 + node3)*stepSize*x
						+ 2*(node1 - 2*node2 + node3)*x*x);
					#if 0
						cout << "Iteration " << it << ":  a=" << a << ", b=" << b
							<< ", x=" << x << ", fx=" << fx << ", dfx=" << dfx << endl;
					#endif

					x -= fx/dfx;

					if (EXPECT_NOT_TAKEN(x <= a || x >= b || dfx == 0))
						x = 0.5f * (b + a);

					/* Integrated version of the above Lagrange polynomial */
					Float intval = integratedDensity + temp * (1.0f / 6.0f) * (x *
						(6*node1*stepSizeSqr - 3*(3*node1 - 4*node2 + node3)*stepSize*x
						+ 4*(node1 - 2*node2 + node3)*x*x));
					fx = intval-desiredDensity;

					if (std::abs(fx) < 1e-6f) {
						t = mint + stepSize * i + x;
						integratedDensity = intval;
						densityAtT = temp * (node1 * stepSizeSqr
							- (3*node1 - 4*node2 + node3)*stepSize*x
							+ 2*(node1 - 2*node2 + node3)*x*x);
						return true;
					} else if (++it > 30) {
						Log(EWarn, "invertDensityIntegral(): stuck in Newton-Bisection -- "
							"round-off error issues? The step size was %e, fx=%f, dfx=%f, "
							"a=%f, b=%f", stepSize, fx, dfx, a, b);
						return false;
					}

					if (fx > 0)
						b = x;
					else
						a = x;
				}
			}

			Point next = p + fullStep;
			if (p == next) {
				Log(EWarn, "invertDensityIntegral(): unable to make forward progress -- "
						"round-off error issues? The step size was %e", stepSize);
				break;
			}
			integratedDensity = newDensity;
			node1 = node3;
			p = next;
		}

		return false;
	}

	Spectrum evalTransmittance(const Ray &ray, Sampler *sampler) const {
		if (m_method == ESimpsonQuadrature || sampler == NULL) {
			return Spectrum(math::fastexp(-integrateDensity(ray)));
		} else {
			/* When Woodcock tracking is selected as the sampling method,
			   we can use this method to get a noisy (but unbiased) estimate
			   of the transmittance */
			Float mint, maxt;
			if (!m_densityAABB.rayIntersect(ray, mint, maxt))
				return Spectrum(1.0f);
			mint = std::max(mint, ray.mint);
			maxt = std::min(maxt, ray.maxt);

			#if defined(HETVOL_STATISTICS)
				avgRayMarchingStepsTransmittance.incrementBase();
			#endif
			int nSamples = 2; /// XXX make configurable
			Float result = 0;

			for (int i=0; i<nSamples; ++i) {
				Float t = mint;
				while (true) {
					t -= math::fastlog(1-sampler->next1D()) * m_invMaxDensity;
					if (t >= maxt) {
						result += 1;
						break;
					}

					Point p = ray(t);
					Float density = lookupDensity(p, ray.d) * m_scale;

					#if defined(HETVOL_STATISTICS)
						++avgRayMarchingStepsTransmittance;
					#endif

					if (density * m_invMaxDensity > sampler->next1D())
						break;
				}
			}
			return Spectrum(result/nSamples);
		}
	}

	bool sampleDistance(const Ray &ray, MediumSamplingRecord &mRec,
			Sampler *sampler) const {
		Float integratedDensity, densityAtMinT, densityAtT;
		bool success = false;

		if (m_method == ESimpsonQuadrature) {
			Float desiredDensity = -math::fastlog(1-sampler->next1D());
			if (invertDensityIntegral(ray, desiredDensity, integratedDensity,
					mRec.t, densityAtMinT, densityAtT)) {
				mRec.p = ray(mRec.t);
				success = true;
				Spectrum albedo = m_albedo->lookupSpectrum(mRec.p);
				mRec.sigmaS = albedo * densityAtT;
				mRec.sigmaA = Spectrum(densityAtT) - mRec.sigmaS;
				mRec.orientation = m_orientation != NULL
					? m_orientation->lookupVector(mRec.p) : Vector(0.0f);
			}

			Float expVal = math::fastexp(-integratedDensity);
			mRec.pdfFailure = expVal;
			mRec.pdfSuccess = expVal * densityAtT;
			mRec.pdfSuccessRev = expVal * densityAtMinT;
			mRec.transmittance = Spectrum(expVal);
			mRec.time = ray.time;
		} else {
			/* The following information is invalid when
			   using Woodcock-tracking */
			mRec.pdfFailure = 1.0f;
			mRec.pdfSuccess = 1.0f;
			mRec.pdfSuccessRev = 1.0f;
			mRec.transmittance = Spectrum(1.0f);
			mRec.time = ray.time;

			#if defined(HETVOL_STATISTICS)
				avgRayMarchingStepsSampling.incrementBase();
			#endif

			Float mint, maxt;
			if (!m_densityAABB.rayIntersect(ray, mint, maxt))
				return false;
			mint = std::max(mint, ray.mint);
			maxt = std::min(maxt, ray.maxt);

			Float t = mint, densityAtT = 0;
			while (true) {
				t -= math::fastlog(1-sampler->next1D()) * m_invMaxDensity;
				if (t >= maxt)
					break;

				Point p = ray(t);
				densityAtT = lookupDensity(p, ray.d) * m_scale;
				#if defined(HETVOL_STATISTICS)
					++avgRayMarchingStepsSampling;
				#endif
				if (densityAtT * m_invMaxDensity > sampler->next1D()) {
					mRec.t = t;
					mRec.p = p;
					Spectrum albedo = m_albedo->lookupSpectrum(p);
					mRec.sigmaS = albedo * densityAtT;
					mRec.sigmaA = Spectrum(densityAtT) - mRec.sigmaS;
					mRec.transmittance = Spectrum(densityAtT != 0.0f ? 1.0f / densityAtT : 0);
					if (!std::isfinite(mRec.transmittance[0])) // prevent rare overflow warnings
						mRec.transmittance = Spectrum(0.0f);
					mRec.orientation = m_orientation != NULL
						? m_orientation->lookupVector(p) : Vector(0.0f);
					mRec.medium = this;
					success = true;
					break;
				}
			}
		}
		mRec.medium = this;

		return success && mRec.pdfSuccess > 0;
	}

	void eval(const Ray &ray, MediumSamplingRecord &mRec) const {
		if (m_method == ESimpsonQuadrature) {
			Float expVal = math::fastexp(-integrateDensity(ray));
			Float mintDensity = lookupDensity(ray(ray.mint), ray.d) * m_scale;
			Float maxtDensity = 0.0f;
			Spectrum maxtAlbedo(0.0f);
			if (ray.maxt < std::numeric_limits<Float>::infinity()) {
				Point p = ray(ray.maxt);
				maxtDensity = lookupDensity(p, ray.d) * m_scale;
				maxtAlbedo = m_albedo->lookupSpectrum(p);
			}
			mRec.transmittance = Spectrum(expVal);
			mRec.pdfFailure = expVal;
			mRec.pdfSuccess = expVal * maxtDensity;
			mRec.pdfSuccessRev = expVal * mintDensity;
			mRec.sigmaS = maxtAlbedo * maxtDensity;
			mRec.sigmaA = Spectrum(maxtDensity) - mRec.sigmaS;
			mRec.time = ray.time;
			mRec.medium = this;
		} else {
			Log(EError, "eval(): unsupported integration method!");
		}
	}

	bool isHomogeneous() const {
		return false;
	}

	std::string toString() const {
		std::ostringstream oss;
		oss << "HeterogeneousMedium[" << endl
			<< "  density = " << indent(m_density.toString()) << "," << endl
			<< "  albedo = " << indent(m_albedo.toString()) << "," << endl
			<< "  orientation = " << indent(m_orientation.toString()) << "," << endl
			<< "  stepSize = " << m_stepSize << "," << endl
			<< "  scale = " << m_scale << endl
			<< "]";
		return oss.str();
	}

	MTS_DECLARE_CLASS()
protected:
	inline Float lookupDensity(const Point &p, const Vector &d) const {
		Float density = m_density->lookupFloat(p);
		if (m_anisotropicMedium && density != 0) {
			Vector orientation = m_orientation->lookupVector(p);
			if (!orientation.isZero())
				density *= m_phaseFunction->sigmaDir(dot(d, orientation));
			else
				return 0;
		}
		return density;
	}
protected:
	EIntegrationMethod m_method;
	ref<VolumeDataSource> m_density;
	ref<VolumeDataSource> m_albedo;
	ref<VolumeDataSource> m_orientation;
	Float m_scale;
	bool m_anisotropicMedium;
	Float m_stepSize;
	AABB m_densityAABB;
	Float m_maxDensity;
	Float m_invMaxDensity;
};

MTS_IMPLEMENT_CLASS_S(HeterogeneousMedium, false, Medium)
MTS_EXPORT_PLUGIN(HeterogeneousMedium, "Heterogeneous medium");
MTS_NAMESPACE_END


#include <Eigen/Dense>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <eigen-checks/gtest.h>

#include "maplab-common/eigen-proto.h"
#include "maplab-common/pose_types.h"
#include "maplab-common/test/testing-entrypoint.h"
#include "maplab-common/test/testing-predicates.h"

namespace common {
namespace eigen_proto {

template <typename EigenType>
class EigenProtoSemiStaticTest : public ::testing::Test {
 protected:
  static EigenType referenceEigenObject();
  static proto::SemiStaticMatrixd referenceProto();
  static proto::SemiStaticMatrixd inconsistentProto() {
    proto::SemiStaticMatrixd result = referenceProto();
    result.mutable_data()->RemoveLast();
    return result;
  }
  // 1D dynamic vectors can't be death-checked against inconsistent protos.
  static bool needsTestWithInconsistentProto() {
    return true;
  }
  static proto::SemiStaticMatrixd generateProto(size_t n) {
    proto::SemiStaticMatrixd result;
    for (size_t i = 0; i < n; ++i) {
      result.add_data(i);
    }
    return result;
  }
  static proto::SemiStaticMatrixd overFilledProto() {
    proto::SemiStaticMatrixd result;
    for (int i = 0; i < 100; ++i) {
      result.add_data(0);
    }
    return result;
  }
  void checkProto() {
    proto::SemiStaticMatrixd proto = referenceProto();
    EXPECT_EQ(proto.data_size(), proto_.data_size());
    for (int i = 0; i < proto.data_size(); ++i) {
      EXPECT_EQ(proto.data(i), proto_.data(i));
    }
  }
  // Quaternion needs different specialization.
  void checkMatrix() {
    EXPECT_TRUE(EIGEN_MATRIX_EQUAL(eigen_object_, referenceEigenObject()));
  }
  static Eigen::Vector4d quaternionCoeffs() {
    Eigen::Vector4d result(0, 1, 2, 3);
    result.normalize();
    return result;
  }
  static Eigen::Vector3d positionCoords() {
    Eigen::Vector3d result(1, 2, 3);
    return result;
  }
  EigenType eigen_object_;
  proto::SemiStaticMatrixd proto_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <>
Eigen::Vector3d
EigenProtoSemiStaticTest<Eigen::Vector3d>::referenceEigenObject() {
  return Eigen::Vector3d(0, 1, 2);
}
template <>
proto::SemiStaticMatrixd
EigenProtoSemiStaticTest<Eigen::Vector3d>::referenceProto() {
  return generateProto(3);
}

template <>
Eigen::Matrix<double, 2, 3>
EigenProtoSemiStaticTest<Eigen::Matrix<double, 2, 3>>::referenceEigenObject() {
  Eigen::Matrix<double, 2, 3> result;
  // serialization is col-major, while initialization is row-major,
  // thus this reordering:
  result << 0, 2, 4, 1, 3, 5;
  return result;
}
template <>
proto::SemiStaticMatrixd
EigenProtoSemiStaticTest<Eigen::Matrix<double, 2, 3>>::referenceProto() {
  return generateProto(6);
}

template <>
Eigen::VectorXd
EigenProtoSemiStaticTest<Eigen::VectorXd>::referenceEigenObject() {
  Eigen::VectorXd result;
  result.resize(4);
  result << 0, 1, 2, 3;
  return result;
}
template <>
proto::SemiStaticMatrixd
EigenProtoSemiStaticTest<Eigen::VectorXd>::referenceProto() {
  return generateProto(4);
}
template <>
bool EigenProtoSemiStaticTest<
    Eigen::VectorXd>::needsTestWithInconsistentProto() {
  return false;
}

template <>
Eigen::Matrix2Xd
EigenProtoSemiStaticTest<Eigen::Matrix2Xd>::referenceEigenObject() {
  Eigen::Matrix2Xd result;
  result.resize(2, 3);
  // serialization is col-major, while initialization is row-major,
  // thus this reordering:
  result << 0, 2, 4, 1, 3, 5;
  return result;
}
template <>
proto::SemiStaticMatrixd
EigenProtoSemiStaticTest<Eigen::Matrix2Xd>::referenceProto() {
  return generateProto(6);
}

template <>
Eigen::Quaterniond
EigenProtoSemiStaticTest<Eigen::Quaterniond>::referenceEigenObject() {
  Eigen::Vector4d coeffs = quaternionCoeffs();
  return Eigen::Quaterniond(coeffs[0], coeffs[1], coeffs[2], coeffs[3]);
}
template <>
proto::SemiStaticMatrixd
EigenProtoSemiStaticTest<Eigen::Quaterniond>::referenceProto() {
  proto::SemiStaticMatrixd result;
  Eigen::Vector4d coeffs = quaternionCoeffs();
  // Eigen Quaternion constructor takes wxyz, but it serializes to xyzw,
  // thus this reordering
  result.add_data(coeffs[1]);
  result.add_data(coeffs[2]);
  result.add_data(coeffs[3]);
  result.add_data(coeffs[0]);
  return result;
}
template <>
void EigenProtoSemiStaticTest<Eigen::Quaterniond>::checkMatrix() {
  EXPECT_TRUE(
      EIGEN_MATRIX_EQUAL(
          eigen_object_.coeffs(), referenceEigenObject().coeffs()));
}

template <>
pose::Transformation
EigenProtoSemiStaticTest<pose::Transformation>::referenceEigenObject() {
  Eigen::Vector4d coeffs = quaternionCoeffs();
  Eigen::Quaterniond quaternion(coeffs[0], coeffs[1], coeffs[2], coeffs[3]);
  Eigen::Vector3d position = positionCoords();
  return pose::Transformation(quaternion, position);
}
template <>
proto::SemiStaticMatrixd
EigenProtoSemiStaticTest<pose::Transformation>::referenceProto() {
  proto::SemiStaticMatrixd result;
  Eigen::Vector4d coeffs = quaternionCoeffs();
  // Eigen Quaternion constructor takes wxyz, but it serializes to xyzw,
  // thus this reordering.
  result.add_data(coeffs[1]);
  result.add_data(coeffs[2]);
  result.add_data(coeffs[3]);
  result.add_data(coeffs[0]);

  Eigen::Vector3d position = positionCoords();
  result.add_data(position[0]);
  result.add_data(position[1]);
  result.add_data(position[2]);
  return result;
}
template <>
void EigenProtoSemiStaticTest<pose::Transformation>::checkMatrix() {
  EXPECT_TRUE(
      EIGEN_MATRIX_EQUAL(
          eigen_object_.getPosition(), referenceEigenObject().getPosition()));
  EXPECT_TRUE(
      EIGEN_MATRIX_EQUAL(
          eigen_object_.getRotation().toImplementation().coeffs(),
          referenceEigenObject().getRotation().toImplementation().coeffs()));
}

typedef ::testing::Types<Eigen::Vector3d, Eigen::Matrix<double, 2, 3>,
                         Eigen::VectorXd, Eigen::Matrix2Xd, Eigen::Quaterniond,
                         pose::Transformation>
    MyTypes;

TYPED_TEST_CASE(EigenProtoSemiStaticTest, MyTypes);

TYPED_TEST(EigenProtoSemiStaticTest, serialize) {
  serialize(this->referenceEigenObject(), this->proto_.mutable_data());
  this->checkProto();
}

TYPED_TEST(EigenProtoSemiStaticTest, serializeIntoUnderfilled) {
  this->proto_ = this->inconsistentProto();
  serialize(this->referenceEigenObject(), this->proto_.mutable_data());
  this->checkProto();
}

TYPED_TEST(EigenProtoSemiStaticTest, serializeIntoOverfilled) {
  this->proto_ = this->overFilledProto();
  serialize(this->referenceEigenObject(), this->proto_.mutable_data());
  this->checkProto();
}

TYPED_TEST(EigenProtoSemiStaticTest, deserialize) {
  deserialize(this->referenceProto().data(), &this->eigen_object_);
  this->checkMatrix();
}

TYPED_TEST(EigenProtoSemiStaticTest, deserializeInconsistent) {
  if (this->needsTestWithInconsistentProto()) {
    EXPECT_DEATH(
        deserialize(this->inconsistentProto().data(), &this->eigen_object_),
        "^");
  }
}

class EigenProtoDynamicTest : public EigenProtoSemiStaticTest<Eigen::MatrixXd> {
 protected:
  static proto::MatrixXd referenceProto() {
    proto::MatrixXd result;
    result.set_rows(2);
    result.set_cols(3);
    for (int i = 0; i < 6; ++i) {
      result.add_data(i);
    }
    return result;
  }
  static proto::MatrixXd inconsistentProto() {
    proto::MatrixXd result;
    result.set_rows(2);
    result.set_cols(3);
    for (int i = 0; i < 5; ++i) {
      result.add_data(i);
    }
    return result;
  }
  static proto::MatrixXd overFilledProto() {
    proto::MatrixXd result;
    result.set_rows(2);
    result.set_cols(3);
    for (int i = 0; i < 100; ++i) {
      result.add_data(i);
    }
    return result;
  }
  void checkProto() {
    proto::MatrixXd proto = referenceProto();
    EXPECT_EQ(proto.data_size(), proto_.data_size());
    for (int i = 0; i < proto.data_size(); ++i) {
      EXPECT_EQ(proto.data(i), proto_.data(i));
    }
  }
  proto::MatrixXd proto_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <>
Eigen::MatrixXd
EigenProtoSemiStaticTest<Eigen::MatrixXd>::referenceEigenObject() {
  Eigen::MatrixXd result;
  result.resize(2, 3);
  // serialization is col-major, while initialization is row-major,
  // thus this reordering:
  result << 0, 2, 4, 1, 3, 5;
  return result;
}

TEST_F(EigenProtoDynamicTest, serialize) {
  serialize(this->referenceEigenObject(), &this->proto_);
  this->checkProto();
}

TEST_F(EigenProtoDynamicTest, serializeIntoUnderfilled) {
  this->proto_ = this->inconsistentProto();
  serialize(this->referenceEigenObject(), this->proto_.mutable_data());
  this->checkProto();
}

TEST_F(EigenProtoDynamicTest, serializeIntoOverfilled) {
  this->proto_ = this->overFilledProto();
  serialize(this->referenceEigenObject(), this->proto_.mutable_data());
  this->checkProto();
}

TEST_F(EigenProtoDynamicTest, deserialize) {
  deserialize(this->referenceProto(), &this->eigen_object_);
  EXPECT_TRUE(
      EIGEN_MATRIX_EQUAL(this->eigen_object_, this->referenceEigenObject()));
}

TEST_F(EigenProtoDynamicTest, deserializeInsufficient) {
  EXPECT_DEATH(
      deserialize(this->inconsistentProto(), &this->eigen_object_), "^");
}

}  // namespace eigen_proto
}  // namespace common

MAPLAB_UNITTEST_ENTRYPOINT


// Boost.Geometry Index
//
// R-tree boxes validating visitor implementation
//
// Copyright (c) 2011-2015 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_RTREE_UTILITIES_ARE_BOXES_OK_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_UTILITIES_ARE_BOXES_OK_HPP

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/index/detail/rtree/node/node.hpp>

namespace boost { namespace geometry { namespace index { namespace detail { namespace rtree { namespace utilities {

namespace visitors {

template <typename Value, typename Options, typename Translator, typename Box, typename Allocators>
class are_boxes_ok
    : public rtree::visitor<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag, true>::type
{
    typedef typename rtree::internal_node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type internal_node;
    typedef typename rtree::leaf<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type leaf;

public:
    are_boxes_ok(Translator const& tr, bool exact_match)
        : result(false), m_tr(tr), m_is_root(true), m_exact_match(exact_match)
    {}

    void operator()(internal_node const& n)
    {
        typedef typename rtree::elements_type<internal_node>::type elements_type;
        elements_type const& elements = rtree::elements(n);

        if (elements.empty())
        {
            result = false;
            return;
        }

        Box box_bckup = m_box;
        bool is_root_bckup = m_is_root;

        m_is_root = false;

        for ( typename elements_type::const_iterator it = elements.begin();
              it != elements.end() ; ++it)
        {
            m_box = it->first;

            rtree::apply_visitor(*this, *it->second);

            if ( result == false )
                return;
        }

        m_box = box_bckup;
        m_is_root = is_root_bckup;

        Box box_exp = rtree::elements_box<Box>(elements.begin(), elements.end(), m_tr);

        if ( m_exact_match )
            result = m_is_root || geometry::equals(box_exp, m_box);
        else
            result = m_is_root || geometry::covered_by(box_exp, m_box);
    }

    void operator()(leaf const& n)
    {
        typedef typename rtree::elements_type<leaf>::type elements_type;
        elements_type const& elements = rtree::elements(n);

        // non-root node
        if (!m_is_root)
        {
            if ( elements.empty() )
            {
                result = false;
                return;
            }

            Box box_exp = rtree::values_box<Box>(elements.begin(), elements.end(), m_tr);

            if ( m_exact_match )
                result = geometry::equals(box_exp, m_box);
            else
                result = geometry::covered_by(box_exp, m_box);
        }
        else
            result = true;
    }

    bool result;

private:
    Translator const& m_tr;
    Box m_box;
    bool m_is_root;
    bool m_exact_match;
};

} // namespace visitors

template <typename Rtree> inline
bool are_boxes_ok(Rtree const& tree, bool exact_match = true)
{
    typedef utilities::view<Rtree> RTV;
    RTV rtv(tree);

    visitors::are_boxes_ok<
        typename RTV::value_type,
        typename RTV::options_type,
        typename RTV::translator_type,
        typename RTV::box_type,
        typename RTV::allocators_type
    > v(rtv.translator(), exact_match);

    rtv.apply_visitor(v);

    return v.result;
}

}}}}}} // namespace boost::geometry::index::detail::rtree::utilities

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_UTILITIES_ARE_BOXES_OK_HPP


/*ckwg +29
 * Copyright 2013-2014 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any 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 AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief local_geo_cs implementation
 */

#include "local_geo_cs.h"
#include <boost/math/constants/constants.hpp>
#include <boost/foreach.hpp>

namespace maptk
{

/// scale factor converting radians to degrees
const double rad2deg = 180.0 / boost::math::constants::pi<double>();
/// scale factor converting degrees to radians
const double deg2rad = boost::math::constants::pi<double>() / 180.0;
/// scale factor convertint feet into meters
const double foot2meter = 0.3048;

/// Constructor
local_geo_cs
::local_geo_cs(algo::geo_map_sptr alg)
: geo_map_algo_(alg),
  utm_origin_(0.0, 0.0, 0.0),
  utm_origin_zone_(-1)
{
}

/// Use the pose data provided by INS to update camera pose
void
local_geo_cs
::update_camera(const ins_data& ins, camera_d& cam,
                rotation_d const& rot_offset) const
{
  if( !geo_map_algo_ )
  {
    return;
  }

  // Apply offset rotation specifically on the lhs of the INS
  cam.set_rotation(rot_offset * rotation_d(ins.yaw * deg2rad,
                                           ins.pitch * deg2rad,
                                           ins.roll * deg2rad));

  // TODO: Possibly add a positional offset optional parameter, also.
  double x,y;
  int zone;
  bool is_north_hemi;
  geo_map_algo_->latlon_to_utm(ins.lat, ins.lon,
                               x, y, zone, is_north_hemi, utm_origin_zone_);
  // INS DATA altitude currently in feet. Converting to meters.
  cam.set_center(vector_3d(x, y, ins.alt * foot2meter) - utm_origin_);
}

/// Use the camera pose to update an INS data structure
void
local_geo_cs
::update_ins_data(const camera_d& cam, ins_data& ins) const
{
  if( !geo_map_algo_ )
  {
    return;
  }
  cam.rotation().get_yaw_pitch_roll(ins.yaw, ins.pitch, ins.roll);
  ins.yaw *= rad2deg;
  ins.pitch *= rad2deg;
  ins.roll *= rad2deg;
  vector_3d c = cam.get_center() + utm_origin_;
  geo_map_algo_->utm_to_latlon(c.x(), c.y(), utm_origin_zone_, true,
                               ins.lat, ins.lon);
  // camera Z in meters while INS data altitude represented in feet
  ins.alt = c.z() / foot2meter;
  ins.source_name = "MAPTK";
}

/// Use a sequence of ins_data objects to initialize a sequence of cameras
std::map<frame_id_t, camera_sptr>
initialize_cameras_with_ins(const std::map<frame_id_t, ins_data>& ins_map,
                            const camera_d& base_camera,
                            local_geo_cs& lgcs,
                            rotation_d const& rot_offset)
{
  std::map<frame_id_t, camera_sptr> cam_map;
  maptk::vector_3d mean(0,0,0);
  camera_d active_cam(base_camera);

  bool update_local_origin = false;
  if( lgcs.utm_origin_zone() < 0 && !ins_map.empty())
  {
    // if a local coordinate system has not been established,
    // use the coordinates of the first camera
    update_local_origin = true;
    const ins_data& ins = ins_map.begin()->second;
    double x,y;
    int zone;
    bool is_north_hemi;
    lgcs.geo_map_algo()->latlon_to_utm(ins.lat, ins.lon,
                                       x, y, zone, is_north_hemi);
    lgcs.set_utm_origin_zone(zone);
    lgcs.set_utm_origin(vector_3d(x, y, 0.0));
  }
  typedef std::map<frame_id_t, ins_data>::value_type ins_map_val_t;
  BOOST_FOREACH(ins_map_val_t const &p, ins_map)
  {
    const ins_data& ins = p.second;
    lgcs.update_camera(ins, active_cam, rot_offset);
    mean += active_cam.center();
    cam_map[p.first] = camera_sptr(new camera_d(active_cam));
  }

  if( update_local_origin )
  {
    mean /= static_cast<double>(cam_map.size());
    // only use the mean easting and northing
    mean[2] = 0.0;

    // shift the UTM origin to the mean of the cameras easting and northing
    lgcs.set_utm_origin(lgcs.utm_origin() + mean);

    // shift all cameras to the new coordinate system.
    typedef std::map<frame_id_t, camera_sptr>::value_type cam_map_val_t;
    BOOST_FOREACH(cam_map_val_t const &p, cam_map)
    {
      camera_d* cam = dynamic_cast<camera_d*>(p.second.get());
      cam->set_center(cam->get_center() - mean);
    }
  }

  return cam_map;
}

/// Update a sequence of ins_data from a sequence of cameras and local_geo_cs
void
update_ins_from_cameras(const std::map<frame_id_t, camera_sptr>& cam_map,
                        const local_geo_cs& lgcs,
                        std::map<frame_id_t, ins_data>& ins_map)
{
  if( lgcs.utm_origin_zone() < 0 )
  {
    // TODO throw an exception here
    std::cerr << "local geo coordinates do not have an origin" <<std::endl;
    return;
  }

  typedef std::map<frame_id_t, camera_sptr>::value_type cam_map_val_t;
  BOOST_FOREACH(cam_map_val_t const &p, cam_map)
  {
    ins_data& active_ins = ins_map[p.first];
    if( camera_d* cam = dynamic_cast<camera_d*>(p.second.get()) )
    {
      lgcs.update_ins_data(*cam, active_ins);
    }
    else if( camera_f* cam = dynamic_cast<camera_f*>(p.second.get()) )
    {
      lgcs.update_ins_data(camera_d(*cam), active_ins);
    }
  }
}

} // end namespace maptk


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef GREENSFUNCTION_HPP
#define GREENSFUNCTION_HPP

#include <iosfwd>

#include "Config.hpp"

#include <Eigen/Core>

#include "DerivativeTypes.hpp"
#include "IGreensFunction.hpp"
#include "utils/Stencils.hpp"

/*! \file GreensFunction.hpp
 *  \class GreensFunction
 *  \brief Templated interface for Green's functions
 *  \author Luca Frediani and Roberto Di Remigio
 *  \date 2012-2014
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \tparam IntegratorPolicy policy for the calculation of the matrix represenation of S and D
 *  \tparam ProfilePolicy    dielectric profile type
 */

template <typename DerivativeTraits,
          typename IntegratorPolicy,
          typename ProfilePolicy,
          typename Derived>
class GreensFunction: public IGreensFunction
{
public:
    GreensFunction() : delta_(1.0e-04), integrator_(IntegratorPolicy()) {}
    GreensFunction(double f) : delta_(1.0e-04), integrator_(IntegratorPolicy(f)) {}
    virtual ~GreensFunction() {}
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_1}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the source point.
     *  \param[in] normal_p1 the normal vector to p1
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeSource(const Eigen::Vector3d & normal_p1,
                            const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const
    {
        DerivativeTraits t1[3], t2[3];
        t1[0] = p1(0); t1[1] = p1(1); t1[2] = p1(2);
        t1[0][1] = normal_p1(0); t1[1][1] = normal_p1(1); t1[2][1] = normal_p1(2);
        t2[0] = p2(0); t2[1] = p2(1); t2[2] = p2(2);
        return this->operator()(t1, t2)[1];
    }
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point.
     *  \param[in] normal_p2 the normal vector to p2
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeProbe(const Eigen::Vector3d & normal_p2,
                                   const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final
    {
        DerivativeTraits t1[3], t2[3];
        t1[0] = p1(0); t1[1] = p1(1); t1[2] = p1(2);
        t2[0] = p2(0); t2[1] = p2(1); t2[2] = p2(2);
        t2[0][1] = normal_p2(0); t2[1][1] = normal_p2(1); t2[2][1] = normal_p2(2);
        return this->operator()(t1, t2)[1];
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the source point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientSource(const Eigen::Vector3d & p1,
                                           const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeSource(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the probe point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientProbe(const Eigen::Vector3d & p1,
                                          const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeProbe(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }

    /*! Whether the Green's function describes a uniform environment */
    virtual bool uniform() const __final __override { return profiles::uniform(this->profile_); }
    /*! Returns a dielectric permittivity profile */
    virtual Permittivity permittivity() const __final __override { return this->profile_; }

    friend std::ostream & operator<<(std::ostream & os, GreensFunction & gf) {
        return gf.printObject(os);
    }
protected:
    /*! Evaluates the Green's function given a pair of points
     *  \param[in] source the source point
     *  \param[in]  probe the probe point
     */
    virtual DerivativeTraits operator()(DerivativeTraits * source, DerivativeTraits * probe) const = 0;
    /*! Returns value of the kernel of the \f$\mathcal{S}\f$ integral operator, i.e. the value of the
     *  Greens's function for the pair of points p1, p2: \f$ G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     *  \note Relies on the implementation of operator() in the subclasses and that is all subclasses
     *  need to implement. Thus this method is marked __final.
     */
    virtual double kernelS_impl(const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final __override
    {
        DerivativeTraits sp[3], pp[3];
        sp[0] = p1(0); sp[1] = p1(1); sp[2] = p1(2);
        pp[0] = p2(0); pp[1] = p2(1); pp[2] = p2(2);
        return this->operator()(sp, pp)[0];
    }
    virtual std::ostream & printObject(std::ostream & os) __override
    {
        os << "Green's Function" << std::endl;
        return os;
    }
    double delta_;
    IntegratorPolicy integrator_;
    ProfilePolicy profile_;
};

template <typename IntegratorPolicy,
          typename ProfilePolicy,
          typename Derived>
class GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, Derived>: public IGreensFunction
{
public:
    GreensFunction() : delta_(1.0e-04), integrator_(IntegratorPolicy()) {}
    GreensFunction(double f) : delta_(1.0e-04), integrator_(IntegratorPolicy(f)) {}
    virtual ~GreensFunction() {}
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_1}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the source point.
     *  \param[in] normal_p1 the normal vector to p1
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeSource(const Eigen::Vector3d & normal_p1,
                            const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const
    {
        return threePointStencil(pcm::bind(&GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, Derived>::kernelS, this, pcm::_1, pcm::_2),
                                p1, p2, normal_p1, this->delta_);
    }
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point.
     *  \param[in] normal_p2 the normal vector to p2
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeProbe(const Eigen::Vector3d & normal_p2,
                                   const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final
    {
        return threePointStencil(pcm::bind(&GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, Derived>::kernelS, this, pcm::_1, pcm::_2),
                                p2, p1, normal_p2, this->delta_);
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the source point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientSource(const Eigen::Vector3d & p1,
                                           const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeSource(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the probe point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientProbe(const Eigen::Vector3d & p1,
                                          const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeProbe(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }

    /*! Whether the Green's function describes a uniform environment */
    virtual bool uniform() const __final __override { return profiles::uniform(this->profile_); }
    /*! Returns a dielectric permittivity profile */
    virtual Permittivity permittivity() const __final __override { return this->profile_; }

    friend std::ostream & operator<<(std::ostream & os, GreensFunction & gf) {
        return gf.printObject(os);
    }
protected:
    /*! Evaluates the Green's function given a pair of points
     *  \param[in] source the source point
     *  \param[in]  probe the probe point
     */
    virtual Numerical operator()(Numerical * source, Numerical * probe) const = 0;
    /*! Returns value of the kernel of the \f$\mathcal{S}\f$ integral operator, i.e. the value of the
     *  Greens's function for the pair of points p1, p2: \f$ G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     *  \note Relies on the implementation of operator() in the subclasses and that is all subclasses
     *  need to implement. Thus this method is marked __final.
     */
    virtual double kernelS_impl(const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final __override
    {
        return this->operator()(const_cast<Numerical *>(p1.data()), const_cast<Numerical *>(p2.data()));
    }
    virtual std::ostream & printObject(std::ostream & os) __override
    {
        os << "Green's Function" << std::endl;
        return os;
    }
    double delta_;
    IntegratorPolicy integrator_;
    ProfilePolicy profile_;
};

#endif // GREENSFUNCTION_HPP


#define EIGEN_FAST_MATH 0
#define EIGEN_USE_MKL_ALL 1
#include <ATen/ATen.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <benchmark/benchmark.h>
#include <iostream>
#include <omp.h>
#include <sleef.h>
#include <stdexcept>
#include <typeinfo>

// Mimic TH alignment
constexpr size_t _ALIGNMENT = 64;

void make_vector(float *data_, size_t size) {
  for (size_t i = 0; i < size; i++) {
    data_[i] = (float)(i % 1024);
  }
}

void make_float_data(float **data_, size_t size) {
  if (posix_memalign((void **)data_, _ALIGNMENT, size * sizeof(float)))
    throw std::invalid_argument("received negative value");
  memset(*data_, 0, size * sizeof(float));
}

float get_random_value() {
  std::random_device
      rd; // Will be used to obtain a seed for the random number engine
  std::mt19937 gen(rd()); // Standard mersenne_twister_engine seeded with rd()
  std::uniform_int_distribution<> dis(1, 6);
  return dis(gen);
}

float do_something(float a) {
  benchmark::DoNotOptimize(a = a * a);
  return a;
}

using InnerStride = Eigen::InnerStride<Eigen::Dynamic>;

template <typename T>
using EigenMatrixMap =
    Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>,
               Eigen::AlignmentType::Aligned64,
               Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>;

template <typename T>
using EigenVectorMap =
    Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>,
               Eigen::AlignmentType::Aligned64,
               Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>;

// General benchmark setup: Allocate fresh memory every 64 iterations
// Call the op once to warmup, once after outside timings to maintain lifespan.

#define BM_BenchATenReduceOp(name, op)                                         \
  static void BM_ATen##name(benchmark::State &state, int64_t stride,           \
                            int64_t size__, int64_t iter) {                    \
    for (auto _ : state) {                                                     \
      state.PauseTiming();                                                     \
      benchmark::ClobberMemory();                                              \
      double size_ = std::sqrt((double)(size__));                              \
      int size = (int)(size_);                                                 \
      state.counters["stride"] = stride;                                       \
      state.counters["size"] = size;                                           \
      state.counters["iter"] = iter;                                           \
      benchmark::ClobberMemory();                                              \
      at::Tensor a =                                                           \
          at::rand({size, size, stride}, at::CPU(at::kFloat)).select(2, 0);    \
      at::Tensor b =                                                           \
          at::rand({size, stride}, at::CPU(at::kFloat)).select(1, 0);          \
      op;                                                                      \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
      for (int j = 0; j < iter; ++j) {                                         \
        op;                                                                    \
      }                                                                        \
      state.PauseTiming();                                                     \
      op;                                                                      \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
    }                                                                          \
  }

#define BM_BenchATenOp(name, op)                                               \
  static void BM_ATen##name(benchmark::State &state, int64_t stride,           \
                            int64_t size, int64_t iter) {                      \
    for (auto _ : state) {                                                     \
      state.PauseTiming();                                                     \
      benchmark::ClobberMemory();                                              \
      state.counters["stride"] = stride;                                       \
      state.counters["size"] = size;                                           \
      state.counters["iter"] = iter;                                           \
      at::Tensor a =                                                           \
          at::rand({size, stride}, at::CPU(at::kFloat)).select(1, 0);          \
      at::Tensor b =                                                           \
          at::rand({size, stride}, at::CPU(at::kFloat)).select(1, 0);          \
      at::Tensor c;                                                            \
      op;                                                                      \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
      for (int j = 0; j < iter; ++j) {                                         \
        op;                                                                    \
      }                                                                        \
      state.PauseTiming();                                                     \
      op;                                                                      \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
    }                                                                          \
  }

#define BM_BenchEigenReduceOp(name, op, dim)                                   \
  static void BM_Eigen##name(benchmark::State &state, int64_t stride,          \
                             int64_t size__, int64_t iter) {                   \
    for (auto _ : state) {                                                     \
      state.PauseTiming();                                                     \
      double size_ = std::sqrt((double)(size__));                              \
      int size = (int)(size_);                                                 \
      state.counters["stride"] = stride;                                       \
      state.counters["size"] = size;                                           \
      state.counters["iter"] = iter;                                           \
      float *data_ = NULL;                                                     \
      make_float_data(&data_, size *size *stride);                             \
      make_vector(data_, size *size *stride);                                  \
      float *out_data_ = NULL;                                                 \
      make_float_data(&out_data_, size *stride);                               \
      make_vector(out_data_, size *stride);                                    \
      EigenMatrixMap<float> a(                                                 \
          data_, size, size,                                                   \
          Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(1, stride));           \
      EigenVectorMap<float> b(                                                 \
          out_data_, size,                                                     \
          Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(1, stride));           \
      op;                                                                      \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
      for (int j = 0; j < iter; ++j) {                                         \
        op;                                                                    \
      }                                                                        \
      state.PauseTiming();                                                     \
      op;                                                                      \
      free(data_);                                                             \
      free(out_data_);                                                         \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
    }                                                                          \
  }

#define BM_BenchEigenOp(name, op)                                              \
  static void BM_Eigen##name(benchmark::State &state, int64_t stride,          \
                             int64_t size, int64_t iter) {                     \
    for (auto _ : state) {                                                     \
      state.PauseTiming();                                                     \
      state.counters["stride"] = stride;                                       \
      state.counters["size"] = size;                                           \
      state.counters["iter"] = iter;                                           \
      float *data_ = NULL;                                                     \
      make_float_data(&data_, size *stride);                                   \
      make_vector(data_, size *stride);                                        \
      float *out_data_ = NULL;                                                 \
      make_float_data(&out_data_, size *stride);                               \
      make_vector(out_data_, size *stride);                                    \
      EigenVectorMap<float> a(                                                 \
          data_, size,                                                         \
          Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(1, stride));           \
      EigenVectorMap<float> b(                                                 \
          out_data_, size,                                                     \
          Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(1, stride));           \
      float c = get_random_value();                                            \
      op;                                                                      \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
      for (int j = 0; j < iter; ++j) {                                         \
        op;                                                                    \
      }                                                                        \
      state.PauseTiming();                                                     \
      c = do_something(c);                                                     \
      op;                                                                      \
      free(data_);                                                             \
      free(out_data_);                                                         \
      benchmark::ClobberMemory();                                              \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
    }                                                                          \
  }

#define BM_BenchReduceOp(op)                                                   \
  BM_BenchEigenOp(_reduce_##op, c += a.op());                                  \
  BM_BenchEigenReduceOp(_reduce_colwise_##op, b = a.colwise().op(), 0);        \
  BM_BenchEigenReduceOp(_reduce_rowwise_##op, b = a.rowwise().op(), 1);        \
  BM_BenchATenOp(_reduce_##op, c = a.op());                                    \
  BM_BenchATenReduceOp(_reduce_colwise_##op, b = a.op(0));                     \
  BM_BenchATenReduceOp(_reduce_rowwise_##op, b = a.op(1));

#define BM_BenchUnaryOp(op)                                                    \
  BM_BenchEigenOp(_unary_##op, b = a.array().op());                            \
  BM_BenchATenOp(_unary_##op, at::op##_out(b, a));

#define BM_BenchUnaryWithSleefOp(op)                                           \
  static void BM_Sleef_##op(benchmark::State &state, int64_t size,             \
                            int64_t iter) {                                    \
    for (auto _ : state) {                                                     \
      state.PauseTiming();                                                     \
      benchmark::ClobberMemory();                                              \
      state.counters["stride"] = 1;                                            \
      state.counters["size"] = size;                                           \
      state.counters["iter"] = iter;                                           \
      float *a_ptr;                                                            \
      float *b_ptr;                                                            \
      int ret =                                                                \
          posix_memalign((void **)&a_ptr, _ALIGNMENT, size * sizeof(float));   \
      ret |=                                                                   \
          posix_memalign((void **)&b_ptr, _ALIGNMENT, size * sizeof(float));   \
      if (ret)                                                                 \
        throw std::runtime_error("memory align failed");                       \
      int64_t vec_size = 8;                                                    \
      assert(size % _ALIGNMENT == 0);                                          \
      benchmark::ClobberMemory();                                              \
      state.ResumeTiming();                                                    \
      for (int j = 0; j < iter; ++j) {                                         \
        int64_t d = 0;                                                         \
        for (; d < size - (size % vec_size); d += vec_size) {                  \
          __m256 values = _mm256_load_ps(a_ptr + d);                           \
          values = Sleef_##op##f8_u10(values);                                 \
          _mm256_store_ps(b_ptr + d, values);                                  \
        }                                                                      \
      }                                                                        \
    }                                                                          \
  }                                                                            \
  BM_BenchUnaryOp(op);

// Commented out means not supported
// TODO: Add comparison between intrinsics and ATen
BM_BenchReduceOp(sum);
BM_BenchReduceOp(prod);
BM_BenchUnaryWithSleefOp(exp);
BM_BenchUnaryWithSleefOp(log);
BM_BenchUnaryOp(floor);
// BM_BenchUnaryWithSleefOp(acos);
// BM_BenchUnaryWithSleefOp(asin);
// BM_BenchUnaryWithSleefOp(atan);
// BM_BenchOp(erf);
// BM_BenchUnaryWithSleefOp(expm1);
// BM_BenchUnaryWithSleefOp(log10);
// BM_BenchUnaryWithSleefOp(log1p);
// BM_BenchOp(log2);
// BM_BenchUnaryOp(round);
// BM_BenchUnaryOp(sqrt);
// BM_BenchOp(trunc);

// BENCHMARK_MAIN();

int main(int argc, char **argv) {
  int64_t iter = 64;
  for (int64_t size = 32768; size < 64777264; size *= 2) {
    benchmark::RegisterBenchmark("BM_Sleef_log", &BM_Sleef_log, size, iter);
    benchmark::RegisterBenchmark("BM_Sleef_exp", &BM_Sleef_exp, size, iter);
    for (int64_t stride = 1; stride < 16; stride *= 2) {
      benchmark::RegisterBenchmark("BM_Eigen_unary_log", &BM_Eigen_unary_log,
                                   stride, size, iter);
      benchmark::RegisterBenchmark("BM_Eigen_unary_exp", &BM_Eigen_unary_exp,
                                   stride, size, iter);
      benchmark::RegisterBenchmark("BM_Eigen_unary_floor",
                                   &BM_Eigen_unary_floor, stride, size, iter);

      benchmark::RegisterBenchmark("BM_ATen_unary_log", &BM_ATen_unary_log,
                                   stride, size, iter);
      benchmark::RegisterBenchmark("BM_ATen_unary_exp", &BM_ATen_unary_exp,
                                   stride, size, iter);
      benchmark::RegisterBenchmark("BM_ATen_unary_floor", &BM_ATen_unary_floor,
                                   stride, size, iter);

      benchmark::RegisterBenchmark("BM_Eigen_reduce_sum", &BM_Eigen_reduce_sum,
                                   stride, size, iter);
      benchmark::RegisterBenchmark("BM_Eigen_reduce_colwise_sum",
                                   &BM_Eigen_reduce_colwise_sum, stride, size,
                                   iter);
      benchmark::RegisterBenchmark("BM_Eigen_reduce_rowwise_sum",
                                   &BM_Eigen_reduce_rowwise_sum, stride, size,
                                   iter);

      benchmark::RegisterBenchmark("BM_ATen_reduce_sum", &BM_ATen_reduce_sum,
                                   stride, size, iter);
      benchmark::RegisterBenchmark("BM_ATen_reduce_colwise_sum",
                                   &BM_ATen_reduce_colwise_sum, stride, size,
                                   iter);
      benchmark::RegisterBenchmark("BM_ATen_reduce_rowwise_sum",
                                   &BM_ATen_reduce_rowwise_sum, stride, size,
                                   iter);

      benchmark::RegisterBenchmark("BM_Eigen_reduce_prod",
                                   &BM_Eigen_reduce_prod, stride, size, iter);
      benchmark::RegisterBenchmark("BM_Eigen_reduce_colwise_prod",
                                   &BM_Eigen_reduce_colwise_prod, stride, size,
                                   iter);
      benchmark::RegisterBenchmark("BM_Eigen_reduce_rowwise_prod",
                                   &BM_Eigen_reduce_rowwise_prod, stride, size,
                                   iter);

      benchmark::RegisterBenchmark("BM_ATen_reduce_prod", &BM_ATen_reduce_prod,
                                   stride, size, iter);
      benchmark::RegisterBenchmark("BM_ATen_reduce_colwise_prod",
                                   &BM_ATen_reduce_colwise_prod, stride, size,
                                   iter);
      benchmark::RegisterBenchmark("BM_ATen_reduce_rowwise_prod",
                                   &BM_ATen_reduce_rowwise_prod, stride, size,
                                   iter);
    }
  }
  benchmark::Initialize(&argc, argv);
  benchmark::RunSpecifiedBenchmarks();
}

// int main() {
// // Create a tensor of 2 dimensions
// Eigen::Tensor<int, 2> a(2, 3);
// a.setValues({{1, 2, 3}, {6, 5, 4}});
// // Reduce it along the second dimension (1)...
// Eigen::array<int, 1> dims({1 /* dimension to reduce */});
// // ...using the "maximum" operator.
// // The result is a tensor with one dimension.  The size of
// // that dimension is the same as the first (non-reduced) dimension of a.
// Eigen::Tensor<int, 1> b = a.sum(dims);
// std::cout << "a" << std::endl << a << std::endl << std::endl;
// std::cout << "b" << std::endl << b << std::endl << std::endl;
//   // Eigen::ArrayXf a = Eigen::ArrayXf::Random(10);
//   // std::cout << typeid(a.sum()).name() << std::endl;
// }


//  (C) 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)

#ifndef BOOST_MATH_POWM1
#define BOOST_MATH_POWM1

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/assert.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T, class Policy>
inline T powm1_imp(const T a, const T z, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if((fabs(a) < 1) || (fabs(z) < 1))
   {
      T p = log(a) * z;
      if(fabs(p) < 2)
         return boost::math::expm1(p, pol);
      // otherwise fall though:
   }
   return pow(a, z) - 1;
}

} // detail

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   powm1(const T1 a, const T2 z)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::powm1_imp(static_cast<result_type>(a), static_cast<result_type>(z), policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   powm1(const T1 a, const T2 z, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::powm1_imp(static_cast<result_type>(a), static_cast<result_type>(z), pol);
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_POWM1


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

#include <boost/simd/swar/functions/cumsum.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         (cumsum_, boost::simd::tag::avx_,
                                    (A0),
                                    ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a000 = _mm256_extractf128_si256(a0, 0);
      svtype a011 = _mm256_extractf128_si256(a0, 1);
      svtype a00 =  cumsum(a000);
      svtype a01 =  cumsum(a011);
      svtype z = splat<svtype>(a00[meta::cardinal_of<svtype>::value-1]);
      __m256i that = _mm256_castsi128_si256(a00);
      return _mm256_insertf128_si256(that, a01+z, 1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////

  BOOST_DISPATCH_IMPLEMENT         (cumsum_, boost::simd::tag::avx_,
                                    (A0),
                                    ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a000 = _mm256_extractf128_pd(a0, 0);
      svtype a011 = _mm256_extractf128_pd(a0, 1);
      svtype a00 =  cumsum(a000);
      svtype a01 =  cumsum(a011);
      svtype z = splat<svtype>(a00[meta::cardinal_of<svtype>::value-1]);
      __m256d that = _mm256_castpd128_pd256(a00);
      return _mm256_insertf128_pd(that, a01+z, 1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////

  BOOST_DISPATCH_IMPLEMENT         (cumsum_, boost::simd::tag::avx_,
                                    (A0),
                                    ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a011;
      a011=  _mm256_extractf128_ps(a0, 1);
      svtype a000;
      a000 =  _mm256_extractf128_ps(a0, 0);
      svtype a00 =  cumsum(a000);
      svtype a01 =  cumsum(a011);
      svtype z = splat<svtype>(a00[meta::cardinal_of<svtype>::value-1]);
      __m256 that = _mm256_castps128_ps256(a00);
      return _mm256_insertf128_ps(that, a01+z, 1);
    }
  };
} } }
#endif
#endif


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

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_int.hpp>
#include "test.hpp"

template <class T, class U>
void check_result_type(const T&, const U&)
{
   BOOST_CHECK(0);
}

void check_result_type(const boost::multiprecision::checked_int1024_t&, const boost::multiprecision::checked_int1024_t&){}

int main()
{
   try{
      typedef boost::multiprecision::checked_int1024_t big_type;
      typedef boost::multiprecision::checked_int512_t  small_type;
      typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<32, 32, boost::multiprecision::signed_magnitude, boost::multiprecision::checked, void>, boost::multiprecision::et_off>  little_type;

      big_type big_val = (big_type(1) << 1000) + 1;
      small_type small_val = 1;
      little_type little_val = 1;

      check_result_type(big_val, big_val + small_val);
      check_result_type(big_val, big_val - small_val);
      check_result_type(big_val, big_val * small_val);
      check_result_type(big_val, big_val / small_val);
      check_result_type(big_val, big_val | small_val);
      check_result_type(big_val, big_val & small_val);
      check_result_type(big_val, big_val ^ small_val);
      check_result_type(big_val, small_val + big_val);
      check_result_type(big_val, small_val - big_val);
      check_result_type(big_val, small_val * big_val);
      check_result_type(big_val, small_val / big_val);
      check_result_type(big_val, small_val | big_val);
      check_result_type(big_val, small_val & big_val);
      check_result_type(big_val, small_val ^ big_val);

      check_result_type(big_val, big_val + little_val);
      check_result_type(big_val, big_val - little_val);
      check_result_type(big_val, big_val * little_val);
      check_result_type(big_val, big_val / little_val);
      check_result_type(big_val, big_val | little_val);
      check_result_type(big_val, big_val & little_val);
      check_result_type(big_val, big_val ^ little_val);
      check_result_type(big_val, little_val + big_val);
      check_result_type(big_val, little_val - big_val);
      check_result_type(big_val, little_val * big_val);
      check_result_type(big_val, little_val / big_val);
      check_result_type(big_val, little_val | big_val);
      check_result_type(big_val, little_val & big_val);
      check_result_type(big_val, little_val ^ big_val);

      BOOST_CHECK_EQUAL(big_val == small_val, false);
      BOOST_CHECK_EQUAL(big_val <= small_val, false);
      BOOST_CHECK_EQUAL(big_val >= small_val, true);
      BOOST_CHECK_EQUAL(big_val < small_val, false);
      BOOST_CHECK_EQUAL(big_val > small_val, true);
      BOOST_CHECK_EQUAL(big_val != small_val, true);
      BOOST_CHECK_EQUAL(small_val == big_val, false);
      BOOST_CHECK_EQUAL(small_val <= big_val, true);
      BOOST_CHECK_EQUAL(small_val >= big_val, false);
      BOOST_CHECK_EQUAL(small_val < big_val, true);
      BOOST_CHECK_EQUAL(small_val > big_val, false);
      BOOST_CHECK_EQUAL(small_val != big_val, true);

      BOOST_CHECK_EQUAL(big_val == little_val, false);
      BOOST_CHECK_EQUAL(big_val <= little_val, false);
      BOOST_CHECK_EQUAL(big_val >= little_val, true);
      BOOST_CHECK_EQUAL(big_val < little_val, false);
      BOOST_CHECK_EQUAL(big_val > little_val, true);
      BOOST_CHECK_EQUAL(big_val != little_val, true);
      BOOST_CHECK_EQUAL(little_val == big_val, false);
      BOOST_CHECK_EQUAL(little_val <= big_val, true);
      BOOST_CHECK_EQUAL(little_val >= big_val, false);
      BOOST_CHECK_EQUAL(little_val < big_val, true);
      BOOST_CHECK_EQUAL(little_val > big_val, false);
      BOOST_CHECK_EQUAL(little_val != big_val, true);
   }
   catch(const std::exception& e)
   {
      std::cout << "Failed with unexpected exception: " << e.what() << std::endl;
      return 1;
   }
   return boost::report_errors();
}


/**
 * \ file FixedDelayLineFilter.cpp
 */

#include <ATK/Delay/FixedDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_line100_delay_1_test )
{
  ATK::FixedDelayLineFilter<float> filter(100);
  BOOST_CHECK_THROW(filter.set_delay(-1), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_line100_delay_100_test )
{
  ATK::FixedDelayLineFilter<float> filter(100);
  BOOST_CHECK_THROW(filter.set_delay(100), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_sinus_line100_delay50_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::FixedDelayLineFilter<float> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(1);
  output.process(49);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(int64_t i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}


#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <cmath>
#include <chrono>
#include <exception>
#include <queue>
#include <boost/math/distributions/normal.hpp>
#include <sys/time.h>
#include "ptss_dse.hpp"
#include "ptss_config.hpp"
#include "ptss_pkmin.hpp"

using namespace std;

void test_gen_bench_id() {
    int k = 10;
    while(k-- > 0) {
        cout << "Bench:"<<gen_bench_id()<<endl;
    }
}

void test_compute_execution_time() {
    /* Create an allocation */
    alloc2_t a;
    double s = 0.0;

    for (int i = 0; i < 6; i++) {
        a.push_back(phase_t(5+i,1));
        // a.push_back(phase_t(409,1));
        // a.push_back(phase_t(408,2));
        // a.push_back(phase_t(406,1));
        // a.push_back(phase_t(410,1));

        double d  = compute_execution_time(a);
        s += d;
        cout << "Execution Time "<< a <<", : " << d << endl;

        a.clear();
    }
    cout << "Total Time : " << s << endl;
}

void test_power() {
    ptss_int_t x = (ptss_int_t)pow(16.0,NPH);
    cout << x << endl;
    // construct_alloc_2();
}

int main() {
    //srand(time(NULL));
    // test_gen_bench_id();
    // test_compute_execution_time();
    test_power();
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Filename    : sphdec.hpp                                                                            *
 * Project     : Planewalker - Schnorr-Euchner sphere decoder simulation for space-time lattice codes  *
 * Authors     : Pasi Pyrrö, Oliver Gnilke                                                             *
 * Version     : 1.0                                                                                *
 * Copyright   : Aalto University ~ School of Science ~ Department of Mathematics and Systems Analysis *
 * Date        : 17.8.2017                                                                             *
 * Language    : C++ (2011 or newer standard)                                                          *
 * Description : Function prototypes for sphdec.cpp                                                    *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SPHDEC_HPP
#define SPHDEC_HPP

#include <armadillo>
#include <vector>
#include <complex>
#include <limits>

std::vector<int> sphdec(const arma::vec &y, const arma::mat &R, const std::vector<int> &S,
						int &counter, double radius=std::numeric_limits<double>::max());
std::vector<int> sphdec_spherical_shaping(const arma::vec &y, const arma::mat &HR, const arma::mat &R, const std::vector<int> &S,
										  int &counter, double P, double radius=std::numeric_limits<double>::max());
std::vector<int> sphdec_wrapper(const std::vector<arma::cx_mat> &bases, const arma::mat Rorig,
								const arma::cx_mat &H, const arma::cx_mat &X, const arma::cx_mat &N,
								const std::vector<int> &symbset, int &visited_nodes, double radius=std::numeric_limits<double>::max());

#endif /* SPHDEC_HPP */


//
// Copyright (c) 2015, J2 Innovations
// Copyright (c) 2012 Brian Frank
// Licensed under the Academic Free License version 3.0
// History:
//   15 Sep 2014  Radu Racariu<radur@2inn.com> Ported to C++
//   06 Jun 2011  Brian Frank  Creation
//
#include "datetimerange.hpp"
#include "date.hpp"
#include "zincreader.hpp"

#include <boost/algorithm/string.hpp>

////////////////////////////////////////////////
// DateTimeRange
////////////////////////////////////////////////
using namespace haystack;

DateTimeRange::DateTimeRange(const Date& date, const TimeZone& tz) :
m_start(*date.midnight(tz)),
m_end(*date.inc_days(1).midnight(tz))
{}

DateTimeRange::DateTimeRange(const Date& s, const Date& e, const TimeZone& tz) : m_start(*s.midnight(tz)), m_end(*e.inc_days(1).midnight(tz)) {}

DateTimeRange::DateTimeRange(const DateTime& s, const DateTime& e) : m_start(s), m_end(e)
{
    if (start().tz != end().tz)
        throw std::runtime_error("start.tz != end.tz");
}

DateTimeRange::auto_ptr_t  DateTimeRange::this_week(const TimeZone& tz)
{
    const Date& today = Date::today();
    const Date& sun = today.dec_days(today.weekday() - SUNDAY);
    const Date& sat = today.inc_days(SATURDAY - today.weekday());
    return auto_ptr_t(new DateTimeRange(sun, sat, tz));
}

DateTimeRange::auto_ptr_t DateTimeRange::this_month(const TimeZone& tz)
{
    const Date& today = Date::today();
    Date first(today.year, today.month, 1);
    Date last(today.year, today.month, Date::days_in_month(today.year, today.month));
    return auto_ptr_t(new DateTimeRange(first, last, tz));
}

DateTimeRange::auto_ptr_t DateTimeRange::this_year(const TimeZone& tz)
{
    const Date& today = Date::today();
    Date first(today.year, 1, 1);
    Date last(today.year, 12, 31);
    return auto_ptr_t(new DateTimeRange(first, last, tz));
}

DateTimeRange::auto_ptr_t DateTimeRange::last_week(const TimeZone& tz)
{
    const Date& today = Date::today();
    const Date& prev = today.dec_days(7);
    const Date& sun = prev.dec_days(prev.weekday() - SUNDAY);
    const Date& sat = prev.inc_days(SATURDAY - prev.weekday());
    return auto_ptr_t(new DateTimeRange(sun, sat, tz));
}

DateTimeRange::auto_ptr_t DateTimeRange::last_month(const TimeZone& tz)
{
    const Date& today = Date::today();
    int year = today.year;
    int month = today.month;

    if (month == 1)
    {
        year--;
        month = 12;
    }
    else
    {
        month--;
    }
    Date first(year, month, 1);
    Date last(year, month, Date::days_in_month(year, month));
    return auto_ptr_t(new DateTimeRange(first, last, tz));
}

DateTimeRange::auto_ptr_t DateTimeRange::last_year(const TimeZone& tz)
{
    const Date& today = Date::today();
    Date first(today.year - 1, 1, 1);
    Date last(today.year - 1, 12, 31);
    return auto_ptr_t(new DateTimeRange(first, last, tz));
}

DateTimeRange::auto_ptr_t DateTimeRange::make(std::string str, const TimeZone& tz)
{
    // handle keywords

    boost::algorithm::trim(str);

    if (str == "today")     return auto_ptr_t(new DateTimeRange(Date::today(), tz));
    if (str == "yesterday") return auto_ptr_t(new DateTimeRange(Date::today().dec_days(1), tz));

    // parse scalars
    size_t comma = str.find(',');
    Val::auto_ptr_t start, end;
    if (comma == str.npos)
    {
        start = ZincReader::make(str)->read_scalar();
    }
    else
    {
        start = ZincReader::make(str.substr(0, comma))->read_scalar();
        end = ZincReader::make(str.substr(comma + 1))->read_scalar();
    }

    // figure out what we parsed for start,end
    if (start->type() == Val::DATE_TYPE)
    {
        if (end.get() == NULL) return auto_ptr_t(new DateTimeRange((Date&)*start, tz));
        if (end->type() == Val::DATE_TYPE) return auto_ptr_t(new DateTimeRange((Date&)*start, (Date&)*end, tz));
    }
    else if (start->type() == Val::DATE_TIME_TYPE)
    {
        if (end.get() == NULL) return auto_ptr_t(new DateTimeRange((DateTime&)*start, DateTime::now(tz)));
        if (end->type() == Val::DATE_TIME_TYPE) return auto_ptr_t(new DateTimeRange((DateTime&)*start, (DateTime&)*end));
    }

    throw std::runtime_error("Invalid DateTimeRange: " + str);
}

////////////////////////////////////////////////
// to string
////////////////////////////////////////////////

const std::string DateTimeRange::to_string() const
{
    return start().to_string() + end().to_string();
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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 LinearSMCimproved.hpp
  \brief General interface to define an actuator
*/

#ifndef LinearSMCimproved_H
#define LinearSMCimproved_H

#include "LinearSMC.hpp"

#include <boost/circular_buffer_fwd.hpp>

typedef std11::shared_ptr<boost::circular_buffer<SP::SiconosVector> > BufferOfVectors;

class LinearSMCimproved : public LinearSMC
{
private:
  /** serialization hooks */
  ACCEPT_SERIALIZATION(LinearSMCimproved);

  /** default constructor */
  LinearSMCimproved() {};

protected:

  /** try to predict the perturbation */
  bool _predictionPerturbation;

  /** boolean to determine if we are in the discrete-time sliding phase */
  bool _inDisceteTimeSlidingPhase;

  /** Vector to store previous values of the perturbation */
  BufferOfVectors _measuredPert;

  /** Vector of predicted values for the perturbation */
  BufferOfVectors _predictedPert;

  /** Upper bound on the norm2 of the perturbation */
  double _ubPerturbation;

  /** Control input to counteract the effect of the perturbation */
  SP::SiconosVector _up;

  /** Predict the effect of the perturnation during the next timestep
   * \param xTk available state at the current time instant
   * \param CBstar matrix \f$CB^{*}\f$
   */
  void predictionPerturbation(const SiconosVector& xTk, SimpleMatrix& CBstar);

public:

  /** Constructor
   * \param sensor the ControlSensor feeding the Actuator
   */
  LinearSMCimproved(SP::ControlSensor sensor);

  /** Constructor with all the data
   * \param sensor the ControlSensor feeding the Actuator
   * \param B the B matrix in the FirstOrderLinearR
   * \param D the D matrix in the FirstOrderLinearR
   */
  LinearSMCimproved(SP::ControlSensor sensor, SP::SimpleMatrix B, SP::SimpleMatrix D = std11::shared_ptr<SimpleMatrix>());

  /** destructor
   */
  virtual ~LinearSMCimproved();

  /** Initialize Controller
   * \param m the Model
   */
  virtual void initialize(const NonSmoothDynamicalSystem& nsds, const Simulation & s);

  /** Compute the new control law at each event
   * Here we are using the following formula:
   * TODO
   */
  virtual void actuate();

  /** Enable perturbation prediction */
  void setPerturbationPrediction(double ub = std::numeric_limits<double>::quiet_NaN())
  {
    _ubPerturbation = ub;
    _predictionPerturbation = true;
  }

  /** Get the control input _up, acting against matched perturbations
   * \return a reference to _up
   */
  inline const SiconosVector& up() const { return *_up; };

  /** Set the order of the prediction
   * - 0 -> the predicted value is the same as the one we measured
   * - 1 -> \f$\widetilde{Cp_{k+1}} = 2Cp_k - Cp_{k-1}\f$
   * - 2 -> \f$\widetilde{Cp_{k+1}} = 3Cp_k - 3Cp_{k-1} + Cp_{k-2}\f$
   * \param order the order of the prediction
   */
  void setPredictionOrder(unsigned int order);
};
#endif