/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/config.h>  // for HAVE_SINCOS
#include <mrpt/core/bits_math.h>  // for square
#include <mrpt/math/CMatrixDynamic.h>  // for CMatrixD...
#include <mrpt/math/CMatrixF.h>  // for CMatrixF
#include <mrpt/math/CMatrixFixed.h>  // for CMatrixF...
#include <mrpt/math/CQuaternion.h>  // for CQuatern...
#include <mrpt/math/CVectorFixed.h>  // for CArrayDo...
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/geometry.h>  // for skew_sym...
#include <mrpt/math/homog_matrices.h>  // for homogene...
#include <mrpt/math/matrix_serialization.h>  // for operator>>
#include <mrpt/math/ops_containers.h>  // for dotProduct
#include <mrpt/math/utils_matlab.h>
#include <mrpt/math/wrap2pi.h>  // for wrapToPi
#include <mrpt/poses/CPoint2D.h>  // for CPoint2D
#include <mrpt/poses/CPoint3D.h>  // for CPoint3D
#include <mrpt/poses/CPose2D.h>  // for CPose2D
#include <mrpt/poses/CPose3D.h>  // for CPose3D
#include <mrpt/poses/CPose3DQuat.h>  // for CPose3DQuat
#include <mrpt/poses/Lie/SO.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/CSchemeArchiveBase.h>
#include <mrpt/serialization/CSerializable.h>  // for CSeriali...
#include <Eigen/Dense>
#include <algorithm>  // for move
#include <cmath>  // for fabs
#include <iomanip>  // for operator<<
#include <limits>  // for numeric_...
#include <ostream>  // for operator<<
#include <string>  // for allocator

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;

IMPLEMENTS_SERIALIZABLE(CPose3D, CSerializable, mrpt::poses)

/*---------------------------------------------------------------
	Constructors
  ---------------------------------------------------------------*/
CPose3D::CPose3D()
{
	m_coords[0] = m_coords[1] = m_coords[2] = 0;
	m_ROT.setIdentity();
}

CPose3D::CPose3D(
	const double x, const double y, const double z, const double yaw,
	const double pitch, const double roll)
	: m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
	setFromValues(x, y, z, yaw, pitch, roll);
}

CPose3D::CPose3D(const mrpt::math::TPose3D& o) : m_ypr_uptodate(false)
{
	setFromValues(o.x, o.y, o.z, o.yaw, o.pitch, o.roll);
}

CPose3D::CPose3D(const CPose2D& p) : m_ypr_uptodate(false)
{
	setFromValues(p.x(), p.y(), 0, p.phi(), 0, 0);
}

CPose3D::CPose3D(const CPoint3D& p)
	: m_ypr_uptodate(false), m_yaw(), m_pitch(), m_roll()
{
	setFromValues(p.x(), p.y(), p.z());
}

CPose3D::CPose3D(const math::CMatrixDouble& m)
	: m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
	ASSERT_ABOVEEQ_(m.rows(), 3);
	ASSERT_ABOVEEQ_(m.cols(), 4);
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) m_ROT(r, c) = m(r, c);
	for (int r = 0; r < 3; r++) m_coords[r] = m(r, 3);
}

CPose3D::CPose3D(const math::CMatrixDouble44& m)
	: m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) m_ROT(r, c) = m(r, c);
	for (int r = 0; r < 3; r++) m_coords[r] = m(r, 3);
}

/** Constructor from a quaternion (which only represents the 3D rotation part)
 * and a 3D displacement. */
CPose3D::CPose3D(
	const mrpt::math::CQuaternionDouble& q, const double _x, const double _y,
	const double _z)
	: m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
	double yaw, pitch, roll;
	q.rpy(roll, pitch, yaw);
	this->setFromValues(_x, _y, _z, yaw, pitch, roll);
}

/** Constructor from a quaternion-based full pose. */
CPose3D::CPose3D(const CPose3DQuat& p)
	: m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
	// Extract XYZ + ROT from quaternion:
	m_coords[0] = p.x();
	m_coords[1] = p.y();
	m_coords[2] = p.z();
	p.quat().rotationMatrixNoResize(m_ROT);
}

uint8_t CPose3D::serializeGetVersion() const { return 3; }
void CPose3D::serializeTo(mrpt::serialization::CArchive& out) const
{
	// v2 serialized the equivalent CPose3DQuat representation.
	// But this led to (**really** tiny) numerical differences between the
	// original and reconstructed poses. To ensure bit-by-bit equivalence before
	// and after serialization, let's get back to serializing the actual SO(3)
	// matrix in serialization v3:
	for (int i = 0; i < 3; i++) out << m_coords[i];
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) out << m_ROT(r, c);
}
void CPose3D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			// The coordinates:
			CMatrixF HM2;
			in >> HM2;
			ASSERT_(HM2.rows() == 4 && HM2.isSquare());

			m_ROT = HM2.block<3, 3>(0, 0).cast<double>();

			m_coords[0] = HM2(0, 3);
			m_coords[1] = HM2(1, 3);
			m_coords[2] = HM2(2, 3);
		}
		break;
		case 1:
		{
			// The coordinates:
			CMatrixDouble44 HM;
			in >> HM;

			m_ROT = HM.block<3, 3>(0, 0);

			m_coords[0] = HM(0, 3);
			m_coords[1] = HM(1, 3);
			m_coords[2] = HM(2, 3);
		}
		break;
		case 2:
		{
			// An equivalent CPose3DQuat
			CPose3DQuat p(UNINITIALIZED_QUATERNION);
			in >> p[0] >> p[1] >> p[2] >> p[3] >> p[4] >> p[5] >> p[6];

			// Extract XYZ + ROT from quaternion:
			m_coords[0] = p.x();
			m_coords[1] = p.y();
			m_coords[2] = p.z();
			p.quat().rotationMatrixNoResize(m_ROT);
		}
		break;
		case 3:
		{
			for (int i = 0; i < 3; i++) in >> m_coords[i];
			for (int r = 0; r < 3; r++)
				for (int c = 0; c < 3; c++) in >> m_ROT(r, c);
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	m_ypr_uptodate = false;
}

void CPose3D::serializeTo(mrpt::serialization::CSchemeArchiveBase& out) const
{
	SCHEMA_SERIALIZE_DATATYPE_VERSION(1);
	out["x"] = m_coords[0];
	out["y"] = m_coords[1];
	out["z"] = m_coords[2];
	out["rot"] = CMatrixD(m_ROT);
}
void CPose3D::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in)
{
	uint8_t version;
	SCHEMA_DESERIALIZE_DATATYPE_VERSION();
	switch (version)
	{
		case 1:
		{
			m_coords[0] = static_cast<double>(in["x"]);
			m_coords[1] = static_cast<double>(in["y"]);
			m_coords[2] = static_cast<double>(in["z"]);
			CMatrixD m;
			in["rot"].readTo(m);
			m_ROT = m;
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

/**  Textual output stream function.
 */
std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3D& p)
{
	const std::streamsize old_pre = o.precision();
	const std::ios_base::fmtflags old_flags = o.flags();
	o << "(x,y,z,yaw,pitch,roll)=(" << std::fixed << std::setprecision(4)
	  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
	  << std::setprecision(2) << RAD2DEG(p.yaw()) << "deg,"
	  << RAD2DEG(p.pitch()) << "deg," << RAD2DEG(p.roll()) << "deg)";
	o.flags(old_flags);
	o.precision(old_pre);
	return o;
}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM(mrpt::poses::CPose3D)
#endif

mxArray* CPose3D::writeToMatlab() const
{
#if MRPT_HAS_MATLAB
	const char* fields[] = {"R", "t"};
	mexplus::MxArray pose_struct(
		mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
	pose_struct.set("R", mrpt::math::convertToMatlab(this->m_ROT));
	pose_struct.set("t", mrpt::math::convertToMatlab(this->m_coords));
	return pose_struct.release();
#else
	THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
#endif
}

/*---------------------------------------------------------------
				normalizeAngles
---------------------------------------------------------------*/
void CPose3D::normalizeAngles() { updateYawPitchRoll(); }
/*---------------------------------------------------------------
 Set the pose from 3D point and yaw/pitch/roll angles, in radians.
---------------------------------------------------------------*/
void CPose3D::setFromValues(
	const double x0, const double y0, const double z0, const double yaw,
	const double pitch, const double roll)
{
	m_coords[0] = x0;
	m_coords[1] = y0;
	m_coords[2] = z0;
	this->m_yaw = mrpt::math::wrapToPi(yaw);
	this->m_pitch = mrpt::math::wrapToPi(pitch);
	this->m_roll = mrpt::math::wrapToPi(roll);

	m_ypr_uptodate = true;

	rebuildRotationMatrix();
}

void CPose3D::rebuildRotationMatrix()
{
	m_ROT = Lie::SO<3>::fromYPR(m_yaw, m_pitch, m_roll);
}

/*---------------------------------------------------------------
		Scalar multiplication.
---------------------------------------------------------------*/
void CPose3D::operator*=(const double s)
{
	updateYawPitchRoll();
	m_coords[0] *= s;
	m_coords[1] *= s;
	m_coords[2] *= s;
	m_yaw *= s;
	m_pitch *= s;
	m_roll *= s;
	rebuildRotationMatrix();
}

/*---------------------------------------------------------------
		getYawPitchRoll
---------------------------------------------------------------*/
void CPose3D::getYawPitchRoll(double& yaw, double& pitch, double& roll) const
{
	TPose3D::SO3_to_yaw_pitch_roll(m_ROT, yaw, pitch, roll);
}

/*---------------------------------------------------------------
		sphericalCoordinates
---------------------------------------------------------------*/
void CPose3D::sphericalCoordinates(
	const TPoint3D& point, double& out_range, double& out_yaw,
	double& out_pitch) const
{
	// Pass to coordinates as seen from this 6D pose:
	TPoint3D local;
	this->inverseComposePoint(
		point.x, point.y, point.z, local.x, local.y, local.z);

	// Range:
	out_range = local.norm();

	// Yaw:
	if (local.y != 0 || local.x != 0)
		out_yaw = atan2(local.y, local.x);
	else
		out_yaw = 0;

	// Pitch:
	if (out_range != 0)
		out_pitch = -asin(local.z / out_range);
	else
		out_pitch = 0;
}

CPose3D CPose3D::getOppositeScalar() const
{
	return CPose3D(
		-m_coords[0], -m_coords[1], -m_coords[2], -m_yaw, -m_pitch, -m_roll);
}

/*---------------------------------------------------------------
		addComponents
---------------------------------------------------------------*/
void CPose3D::addComponents(const CPose3D& p)
{
	updateYawPitchRoll();
	m_coords[0] += p.m_coords[0];
	m_coords[1] += p.m_coords[1];
	m_coords[2] += p.m_coords[2];
	m_yaw += p.m_yaw;
	m_pitch += p.m_pitch;
	m_roll += p.m_roll;
	rebuildRotationMatrix();
}

/*---------------------------------------------------------------
		distanceEuclidean6D
---------------------------------------------------------------*/
double CPose3D::distanceEuclidean6D(const CPose3D& o) const
{
	updateYawPitchRoll();
	o.updateYawPitchRoll();
	return sqrt(
		square(o.m_coords[0] - m_coords[0]) +
		square(o.m_coords[1] - m_coords[1]) +
		square(o.m_coords[2] - m_coords[2]) +
		square(wrapToPi(o.m_yaw - m_yaw)) +
		square(wrapToPi(o.m_pitch - m_pitch)) +
		square(wrapToPi(o.m_roll - m_roll)));
}

/*---------------------------------------------------------------
		composePoint
---------------------------------------------------------------*/
void CPose3D::composePoint(
	double lx, double ly, double lz, double& gx, double& gy, double& gz,
	mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint,
	mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose,
	mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3,
	bool use_small_rot_approx) const
{
	// Jacob: df/dpoint
	if (out_jacobian_df_dpoint) out_jacobian_df_dpoint.value().get() = m_ROT;

	// Jacob: df/dpose
	if (out_jacobian_df_dpose)
	{
		if (use_small_rot_approx)
		{
			// Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
				1, 0, 0, -ly, lz, 0, 0, 1, 0, lx, 0, -lz, 0, 0, 1, 0, -lx, ly};
			out_jacobian_df_dpose.value().get().loadFromArray(nums);
		}
		else
		{
			// Exact Jacobians:
			updateYawPitchRoll();
#ifdef HAVE_SINCOS
			double cy, sy;
			::sincos(m_yaw, &sy, &cy);
			double cp, sp;
			::sincos(m_pitch, &sp, &cp);
			double cr, sr;
			::sincos(m_roll, &sr, &cr);
#else
			const double cy = cos(m_yaw);
			const double sy = sin(m_yaw);
			const double cp = cos(m_pitch);
			const double sp = sin(m_pitch);
			const double cr = cos(m_roll);
			const double sr = sin(m_roll);
#endif

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
				1,
				0,
				0,
				-lx * sy * cp + ly * (-sy * sp * sr - cy * cr) +
					lz * (-sy * sp * cr + cy * sr),  // d_x'/d_yaw
				-lx * cy * sp + ly * (cy * cp * sr) +
					lz * (cy * cp * cr),  // d_x'/d_pitch
				ly * (cy * sp * cr + sy * sr) +
					lz * (-cy * sp * sr + sy * cr),  // d_x'/d_roll
				0,
				1,
				0,
				lx * cy * cp + ly * (cy * sp * sr - sy * cr) +
					lz * (cy * sp * cr + sy * sr),  // d_y'/d_yaw
				-lx * sy * sp + ly * (sy * cp * sr) +
					lz * (sy * cp * cr),  // d_y'/d_pitch
				ly * (sy * sp * cr - cy * sr) +
					lz * (-sy * sp * sr - cy * cr),  // d_y'/d_roll
				0,
				0,
				1,
				0,  // d_z' / d_yaw
				-lx * cp - ly * sp * sr - lz * sp * cr,  // d_z' / d_pitch
				ly * cp * cr - lz * cp * sr  // d_z' / d_roll
			};
			out_jacobian_df_dpose.value().get().loadFromArray(nums);
		}
	}

	gx = m_ROT(0, 0) * lx + m_ROT(0, 1) * ly + m_ROT(0, 2) * lz + m_coords[0];
	gy = m_ROT(1, 0) * lx + m_ROT(1, 1) * ly + m_ROT(1, 2) * lz + m_coords[1];
	gz = m_ROT(2, 0) * lx + m_ROT(2, 1) * ly + m_ROT(2, 2) * lz + m_coords[2];

	// Jacob: df/dse3
	if (out_jacobian_df_dse3)
	{
		alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
			1, 0, 0, 0, gz, -gy, 0, 1, 0, -gz, 0, gx, 0, 0, 1, gy, -gx, 0};
		out_jacobian_df_dse3.value().get().loadFromArray(nums);
	}
}

mrpt::math::TVector3D CPose3D::rotateVector(
	const mrpt::math::TVector3D& l) const
{
	mrpt::math::TVector3D g;
	g.x = m_ROT(0, 0) * l.x + m_ROT(0, 1) * l.y + m_ROT(0, 2) * l.z;
	g.y = m_ROT(1, 0) * l.x + m_ROT(1, 1) * l.y + m_ROT(1, 2) * l.z;
	g.z = m_ROT(2, 0) * l.x + m_ROT(2, 1) * l.y + m_ROT(2, 2) * l.z;
	return g;
}

mrpt::math::TVector3D CPose3D::inverseRotateVector(
	const mrpt::math::TVector3D& g) const
{
	mrpt::math::TVector3D l;
	l.x = m_ROT(0, 0) * g.x + m_ROT(1, 0) * g.y + m_ROT(2, 0) * g.z;
	l.y = m_ROT(0, 1) * g.x + m_ROT(1, 1) * g.y + m_ROT(2, 1) * g.z;
	l.z = m_ROT(0, 2) * g.x + m_ROT(1, 2) * g.y + m_ROT(2, 2) * g.z;
	return l;
}

// TODO: Use SSE2? OTOH, this forces mem align...
#if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
/*static inline __m128 transformSSE(const __m128* matrix, const __m128& in)
{
	ASSERT_(((size_t)matrix & 15) == 0);
	__m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)),
_mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
	__m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)),
_mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
	__m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)),
_mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));

	return _mm_add_ps(_mm_add_ps(a0,a1),a2);
}*/
#endif  // SSE2

void CPose3D::asVector(vector_t& r) const
{
	updateYawPitchRoll();
	r[0] = m_coords[0];
	r[1] = m_coords[1];
	r[2] = m_coords[2];
	r[3] = m_yaw;
	r[4] = m_pitch;
	r[5] = m_roll;
}

/*---------------------------------------------------------------
		unary -
---------------------------------------------------------------*/
CPose3D mrpt::poses::operator-(const CPose3D& b)
{
	CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
	b.getInverseHomogeneousMatrix(B_INV);
	return CPose3D(B_INV);
}

void CPose3D::getAsQuaternion(
	mrpt::math::CQuaternionDouble& q,
	mrpt::optional_ref<mrpt::math::CMatrixDouble43> out_dq_dr) const
{
	updateYawPitchRoll();
	mrpt::math::TPose3D(0, 0, 0, m_yaw, m_pitch, m_roll)
		.getAsQuaternion(q, out_dq_dr);
}

bool mrpt::poses::operator==(const CPose3D& p1, const CPose3D& p2)
{
	return (p1.m_coords == p2.m_coords) &&
		   ((p1.getRotationMatrix() - p2.getRotationMatrix())
				.array()
				.abs()
				.maxCoeff() < 1e-6);
}

bool mrpt::poses::operator!=(const CPose3D& p1, const CPose3D& p2)
{
	return (p1.m_coords != p2.m_coords) ||
		   ((p1.getRotationMatrix() - p2.getRotationMatrix())
				.array()
				.abs()
				.maxCoeff() >= 1e-6);
}

/*---------------------------------------------------------------
				point3D = pose3D + point3D
  ---------------------------------------------------------------*/
CPoint3D CPose3D::operator+(const CPoint3D& b) const
{
	return CPoint3D(
		m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y() +
			m_ROT(0, 2) * b.z(),
		m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y() +
			m_ROT(1, 2) * b.z(),
		m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y() +
			m_ROT(2, 2) * b.z());
}

/*---------------------------------------------------------------
				point3D = pose3D + point2D
  ---------------------------------------------------------------*/
CPoint3D CPose3D::operator+(const CPoint2D& b) const
{
	return CPoint3D(
		m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y(),
		m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y(),
		m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y());
}

/*---------------------------------------------------------------
				this = A + B
  ---------------------------------------------------------------*/
void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B)
{
	// The translation part HM(0:3,3)
	if (this == &B)
	{
		// we need to make a temporary copy of the vector:
		const CVectorFixedDouble<3> B_coords = B.m_coords;
		for (int r = 0; r < 3; r++)
			m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B_coords[0] +
						  A.m_ROT(r, 1) * B_coords[1] +
						  A.m_ROT(r, 2) * B_coords[2];
	}
	else
	{
		for (int r = 0; r < 3; r++)
			m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B.m_coords[0] +
						  A.m_ROT(r, 1) * B.m_coords[1] +
						  A.m_ROT(r, 2) * B.m_coords[2];
	}

	// Important: Make this multiplication AFTER the translational part, to cope
	// with the case when A==this
	m_ROT = A.m_ROT * B.m_ROT;

	m_ypr_uptodate = false;
}

/** Convert this pose into its inverse, saving the result in itself. */
void CPose3D::inverse()
{
	CMatrixDouble33 inv_rot(UNINITIALIZED_MATRIX);
	CVectorFixedDouble<3> inv_xyz;

	mrpt::math::homogeneousMatrixInverse(m_ROT, m_coords, inv_rot, inv_xyz);

	m_ROT = inv_rot;
	m_coords = inv_xyz;
	m_ypr_uptodate = false;
}

/*---------------------------------------------------------------
						isHorizontal
 ---------------------------------------------------------------*/
bool CPose3D::isHorizontal(const double tolerance) const
{
	updateYawPitchRoll();
	return (fabs(m_pitch) <= tolerance || M_PI - fabs(m_pitch) <= tolerance) &&
		   (fabs(m_roll) <= tolerance ||
			fabs(mrpt::math::wrapToPi(m_roll - M_PI)) <= tolerance);
}

/**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
 * than "this= A - B;" since it avoids the temporary object.
 *  \note A or B can be "this" without problems.
 * \sa composeFrom, composePoint
 */
void CPose3D::inverseComposeFrom(const CPose3D& A, const CPose3D& B)
{
	// this    =    A  (-)  B
	// HM_this = inv(HM_B) * HM_A
	//
	// [  R_b  | t_b ] -1   [  R_a  | t_a ]    [ R_b^t * Ra |    ..    ]
	// [ ------+-----]    * [ ------+-----]  = [ ---------- +----------]
	// [ 0 0 0 |  1  ]      [ 0 0 0 |  1  ]    [  0  0   0  |      1   ]
	//

	// XYZ part:
	CMatrixDouble33 R_b_inv(UNINITIALIZED_MATRIX);
	CVectorFixedDouble<3> t_b_inv;
	mrpt::math::homogeneousMatrixInverse(B.m_ROT, B.m_coords, R_b_inv, t_b_inv);

	for (int i = 0; i < 3; i++)
		m_coords[i] = t_b_inv[i] + R_b_inv(i, 0) * A.m_coords[0] +
					  R_b_inv(i, 1) * A.m_coords[1] +
					  R_b_inv(i, 2) * A.m_coords[2];

	// Rot part:
	m_ROT = R_b_inv * A.m_ROT;
	m_ypr_uptodate = false;
}

/**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
 * \sa composePoint, composeFrom
 */
void CPose3D::inverseComposePoint(
	const double gx, const double gy, const double gz, double& lx, double& ly,
	double& lz,
	mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint,
	mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose,
	mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3) const
{
	CMatrixDouble33 R_inv(UNINITIALIZED_MATRIX);
	CVectorFixedDouble<3> t_inv;
	mrpt::math::homogeneousMatrixInverse(m_ROT, m_coords, R_inv, t_inv);

	// Jacob: df/dpoint
	if (out_jacobian_df_dpoint) out_jacobian_df_dpoint.value().get() = R_inv;

	// Jacob: df/dpose
	if (out_jacobian_df_dpose)
	{
		// TODO: Perhaps this and the sin/cos's can be avoided if all needed
		// terms are already in m_ROT ???
		updateYawPitchRoll();

#ifdef HAVE_SINCOS
		double cy, sy;
		::sincos(m_yaw, &sy, &cy);
		double cp, sp;
		::sincos(m_pitch, &sp, &cp);
		double cr, sr;
		::sincos(m_roll, &sr, &cr);
#else
		const double cy = cos(m_yaw);
		const double sy = sin(m_yaw);
		const double cp = cos(m_pitch);
		const double sp = sin(m_pitch);
		const double cr = cos(m_roll);
		const double sr = sin(m_roll);
#endif

		const double m11_dy = -sy * cp;
		const double m12_dy = cy * cp;
		const double m13_dy = 0;
		const double m11_dp = -cy * sp;
		const double m12_dp = -sy * sp;
		const double m13_dp = -cp;
		const double m11_dr = 0;
		const double m12_dr = 0;
		const double m13_dr = 0;

		const double m21_dy = (-sy * sp * sr - cy * cr);
		const double m22_dy = (cy * sp * sr - sy * cr);
		const double m23_dy = 0;
		const double m21_dp = (cy * cp * sr);
		const double m22_dp = (sy * cp * sr);
		const double m23_dp = -sp * sr;
		const double m21_dr = (cy * sp * cr + sy * sr);
		const double m22_dr = (sy * sp * cr - cy * sr);
		const double m23_dr = cp * cr;

		const double m31_dy = (-sy * sp * cr + cy * sr);
		const double m32_dy = (cy * sp * cr + sy * sr);
		const double m33_dy = 0;
		const double m31_dp = (cy * cp * cr);
		const double m32_dp = (sy * cp * cr);
		const double m33_dp = -sp * cr;
		const double m31_dr = (-cy * sp * sr + sy * cr);
		const double m32_dr = (-sy * sp * sr - cy * cr);
		const double m33_dr = -cp * sr;

		const double Ax = gx - m_coords[0];
		const double Ay = gy - m_coords[1];
		const double Az = gz - m_coords[2];

		alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
			-m_ROT(0, 0),
			-m_ROT(1, 0),
			-m_ROT(2, 0),
			Ax * m11_dy + Ay * m12_dy + Az * m13_dy,  // d_x'/d_yaw
			Ax * m11_dp + Ay * m12_dp + Az * m13_dp,  // d_x'/d_pitch
			Ax * m11_dr + Ay * m12_dr + Az * m13_dr,  // d_x'/d_roll

			-m_ROT(0, 1),
			-m_ROT(1, 1),
			-m_ROT(2, 1),
			Ax * m21_dy + Ay * m22_dy + Az * m23_dy,  // d_x'/d_yaw
			Ax * m21_dp + Ay * m22_dp + Az * m23_dp,  // d_x'/d_pitch
			Ax * m21_dr + Ay * m22_dr + Az * m23_dr,  // d_x'/d_roll

			-m_ROT(0, 2),
			-m_ROT(1, 2),
			-m_ROT(2, 2),
			Ax * m31_dy + Ay * m32_dy + Az * m33_dy,  // d_x'/d_yaw
			Ax * m31_dp + Ay * m32_dp + Az * m33_dp,  // d_x'/d_pitch
			Ax * m31_dr + Ay * m32_dr + Az * m33_dr,  // d_x'/d_roll
		};
		out_jacobian_df_dpose.value().get().loadFromArray(nums);
	}

	lx = t_inv[0] + R_inv(0, 0) * gx + R_inv(0, 1) * gy + R_inv(0, 2) * gz;
	ly = t_inv[1] + R_inv(1, 0) * gx + R_inv(1, 1) * gy + R_inv(1, 2) * gz;
	lz = t_inv[2] + R_inv(2, 0) * gx + R_inv(2, 1) * gy + R_inv(2, 2) * gz;

	// Jacob: df/dse3
	if (out_jacobian_df_dse3)
	{
		alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
			-1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
		out_jacobian_df_dse3.value().get().loadFromArray(nums);
	}
}

void CPose3D::setToNaN()
{
	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++)
			m_ROT(i, j) = std::numeric_limits<double>::quiet_NaN();

	for (int i = 0; i < 3; i++)
		m_coords[i] = std::numeric_limits<double>::quiet_NaN();
}

mrpt::math::TPose3D CPose3D::asTPose() const
{
	return mrpt::math::TPose3D(x(), y(), z(), yaw(), pitch(), roll());
}

void CPose3D::fromString(const std::string& s)
{
	using mrpt::DEG2RAD;
	mrpt::math::CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(m.rows() == 1 && m.cols() == 6, "Expected vector length=6");
	this->setFromValues(
		m(0, 0), m(0, 1), m(0, 2), DEG2RAD(m(0, 3)), DEG2RAD(m(0, 4)),
		DEG2RAD(m(0, 5)));
}

void CPose3D::fromStringRaw(const std::string& s)
{
	this->fromString("[" + s + "]");
}

void CPose3D::getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const
{
	auto M = out_HM.asEigen();
	M.block<3, 3>(0, 0) = m_ROT.asEigen();
	for (int i = 0; i < 3; i++) out_HM(i, 3) = m_coords[i];
	out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0.;
	out_HM(3, 3) = 1.;
}


/*
 * DIPlib 3.0
 * This file contains definitions of functions for adaptive Gaussian filtering.
 *
 * (c)2018, Erik Schuitema.
 * Based on original DIPlib code: (c)1995-2014, Delft University of Technology.
 *
 * 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.
 */

#include "diplib/nonlinear.h"
#include "diplib/framework.h"
#include "diplib/generation.h"
#include "diplib/overload.h"
#include "diplib/pixel_table.h"
#include "diplib/private/constfor.h"

#if defined(__GNUG__) || defined(__clang__)
// For this file, turn off -Wsign-conversion, Eigen is really bad at this!
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#if __GNUC__ >= 7 || __clang_major__ >= 12
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#if __GNUC__ >= 9
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#endif

#include <Eigen/Geometry>

namespace dip {

namespace {

// KernelTransform: transforms kernel pixel coordinates according to one or more parameter images
// The base class performs no specific transformation,
// other than simply adding kernel coordinates to the current image coordinates
class KernelTransform {
   public:
      // Virtual destructor
      virtual ~KernelTransform() = default;

      // Clone the kernel transform. This is done when creating a copy for each thread
      // to avoid unwanted sharing between threads of the members altered inside SetImageCoords().
      virtual KernelTransform* Clone() const { return new KernelTransform( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) {
         // Cast-copy
         imgCoords_.resize( imgCoords.size() );
         for( UnsignedArray::size_type ii = 0; ii < imgCoords.size(); ++ii ) {
            imgCoords_[ ii ] = static_cast< dfloat >( imgCoords[ ii ] );
         }
         // Note for writing derived classes: perform any parameter computation or other preparation here,
         // so it is done only once per input pixel.
      }

      // Transforms kernel coordinates to input image coordinates
      virtual void Transform( IntegerArray const& kernelCoords, dip::uint /*tensorIndex*/, FloatArray& transformedCoords ) const {
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] );
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 1 ] );
      }

   protected:
      FloatArray imgCoords_;
      // Makes a quick copy of a parameter image and expands its tensor if necessary
      static void ParamImageQuickCopyAndExpandTensor( Image const& paramImage, Image& expandedParamImage, dip::uint inputTensorElements ) {
         expandedParamImage = paramImage.QuickCopy();
         if( expandedParamImage.TensorElements() != inputTensorElements ) {
            expandedParamImage.ExpandSingletonTensor( inputTensorElements );
         }
      }
};

// Scaling helper class
template< dip::uint nDims >
class KernelTransformScale {
   public:
      // The `kernelScale` image tensor need not be expanded beforehand
      KernelTransformScale( Image const& kernelScale, dip::uint inputTensorElements )
            : scaleAtImgCoords_( inputTensorElements ), inputTensorElements_( inputTensorElements ) {
         // The kernel scale image must be a COL_MAJOR_MATRIX tensor image with the following tensor size:
         // Rows == input tensor size (the input image has a column vector tensor)
         // Cols == input dimensionality == kernelScaleDimensionality
         kernelScale_ = kernelScale.QuickCopy();
         dip::uint tensorRows = inputTensorElements;
         dip::uint tensorCols = kernelScale.Dimensionality();
         // See if we need to reshape the tensor
         if( kernelScale_.TensorElements() == 1 ) {
            // Singleton expansion to COL_MAJOR_MATRIX tensor
            kernelScale_.ExpandSingletonTensor( tensorRows * tensorCols );
            kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( kernelScale_.TensorColumns() == tensorCols && kernelScale_.TensorRows() == tensorRows ) {
            // Dimensions are ok, only create LUT
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( inputTensorElements == 1 && kernelScale_.TensorShape() == Tensor::Shape::COL_VECTOR && kernelScale_.TensorRows() == tensorCols ) {
            // The scale tensor is a col vector but must be a row vector
            kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( kernelScale_.TensorColumns() == 1 && kernelScale_.TensorRows() == tensorRows ) {
            // Only the row size matches: tensor contains a scalar per input tensor element -> create special LUT but leave the tensor itself unchanged
            // LUT is indexed as: [col * NUM_ROWS + row]
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable(); // LUT for column vector
            //kernelScale_.ExpandSingletonTensor(tensorRows * tensorCols);
            //kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            // Complete LUT by adding the same indices for each column
            dip::sint LUTColSize = static_cast< dip::sint >( scaleTensorLUT_.size() );
            for( dip::uint iDim = 1; iDim < nDims; ++iDim ) {
               scaleTensorLUT_.insert( scaleTensorLUT_.end(), scaleTensorLUT_.begin(), scaleTensorLUT_.begin() + LUTColSize );
            }
         } else {
            DIP_THROW( "Scale parameter image tensor has wrong size, must have " + std::to_string( inputTensorElements ) + " rows and " + std::to_string( nDims ) + " columns" );
         }
      }

      // Computes scaleAtImgCoords_.
      // Prerequisite: SetImageCoords() must have been called to populate the image coords
      void SetScaleAtImgCoords( UnsignedArray const& imgCoords ) {
         // Given a tensor with `M` rows and `N` columns, tensor element `(m,n)` has the linear
         // index given by `scaleTensorLUT_[n*M+m] = Tensor::LookUpTable()[n*M+m]`.
         Image::Pixel scalePixel = kernelScale_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < inputTensorElements_; ++iTE ) {
            // scaleTensorRowIndex = iTE : The scale tensor has a row for each input tensor element.
            for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
               // scaleTensorColIndex = iDim : The scale tensor has a column for each kernel/input dimension ( 0 for X, 1 for Y ).
               scaleAtImgCoords_[ iTE ][ iDim ] = scalePixel[ scaleTensorLUT_[ iDim * inputTensorElements_ + iTE ]].template As< dfloat >();
            }
         }
      }

   protected:
      std::vector< std::array< dfloat, nDims >> scaleAtImgCoords_; // The vector is over the input tensor elements; the array is over the kernel dimensions
      Image kernelScale_;
      dip::uint inputTensorElements_;
      std::vector< dip::sint > scaleTensorLUT_;
};

// Kernel transformation: 2D rotation
class KernelTransform2DRotation : public KernelTransform {
   public:
      // The `orientation` image tensor need not be expanded beforehand
      KernelTransform2DRotation( Image const& orientation, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( orientation, orientation_, inputTensorElements );
         csn_.resize( inputTensorElements );
         sn_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DRotation( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel dirPixel = orientation_.At( imgCoords );
         // Iterate over tensor elements
         for( dip::uint iTE = 0; iTE < orientation_.TensorElements(); ++iTE ) {
            csn_[ iTE ] = std::cos( dip::pi * 0.5 - dirPixel[ iTE ].As< dfloat >() );
            sn_[ iTE ] = std::sin( dip::pi * 0.5 - dirPixel[ iTE ].As< dfloat >() );
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * csn_[ tensorIndex ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - static_cast< dfloat >( kernelCoords[ 0 ] ) * sn_[ tensorIndex ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * csn_[ tensorIndex ];
      }

   protected:
      std::vector< dfloat > csn_, sn_;   // Length is equal to number of input tensor elements
      Image orientation_;
};

class KernelTransform2DScaledRotation : public KernelTransform2DRotation, public KernelTransformScale< 2 > {
   public:
      // The `orientation` and `kernelScale` image tensors need not be expanded beforehand
      KernelTransform2DScaledRotation( Image const& orientation, Image const& kernelScale, dip::uint inputTensorElements )
            : KernelTransform2DRotation( orientation, inputTensorElements ), KernelTransformScale< 2 >( kernelScale, inputTensorElements ) {}

      virtual KernelTransform* Clone() const override { return new KernelTransform2DScaledRotation( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DRotation::SetImageCoords( imgCoords );
         SetScaleAtImgCoords( imgCoords );
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         // First scale, then rotate
         dfloat scaledKernelCoords[ 2 ] = {
               scaleAtImgCoords_[ tensorIndex ][ 0 ] * static_cast< dfloat >( kernelCoords[ 0 ] ),
               scaleAtImgCoords_[ tensorIndex ][ 1 ] * static_cast< dfloat >( kernelCoords[ 1 ] )
         };
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + scaledKernelCoords[ 0 ] * csn_[ tensorIndex ]
                                  + scaledKernelCoords[ 1 ] * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - scaledKernelCoords[ 0 ] * sn_[ tensorIndex ]
                                  + scaledKernelCoords[ 1 ] * csn_[ tensorIndex ];
      }
};

// Kernel transformation: 3D rotation using phi3 and theta3
class KernelTransform3DRotationZ : public KernelTransform {
   public:
      // The `phi3` and `theta3` image tensors need not be expanded beforehand
      KernelTransform3DRotationZ( Image const& phi3, Image const& theta3, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( phi3, phi3_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta3, theta3_, inputTensorElements );
         R_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform3DRotationZ( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel phiPixel = phi3_.At( imgCoords );
         Image::Pixel thetaPixel = theta3_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < phi3_.TensorElements(); ++iTE ) {
            dfloat phi = phiPixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta = thetaPixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat cs_p = std::cos( phi );
            dfloat sn_p = std::sin( phi );
            dfloat cs_t = std::cos( theta );
            dfloat sn_t = std::sin( theta );

            RotMatrix& R = R_[ iTE ];
            R[ 0 ] = cs_p * cs_t;   R[ 1 ] = -sn_p;   R[ 2 ] = cs_p * sn_t;
            R[ 3 ] = sn_p * cs_t;   R[ 4 ] = cs_p;    R[ 5 ] = sn_p * sn_t;
            R[ 6 ] = -sn_t;         R[ 7 ] = 0;       R[ 8 ] = cs_t;
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         RotMatrix const& R = R_[ tensorIndex ];
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 0 ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * R[ 1 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 2 ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 3 ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * R[ 4 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 5 ];
         transformedCoords[ 2 ] = imgCoords_[ 2 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 6 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 8 ];
      }

   protected:
      Image phi3_;
      Image theta3_;
      using RotMatrix = std::array< dfloat, 9 >;
      std::vector< RotMatrix > R_;  // Rotation matrix. The vector is over the input tensor elements. // TODO: use Matrix class?
};

// Kernel transformation: 3D rotation using phi2, theta2, phi3 and theta3
class KernelTransform3DRotationXY : public KernelTransform {
   public:
      // The `phi2`, `theta2`, `phi3` and `theta3` image tensors need not be expanded beforehand
      KernelTransform3DRotationXY( Image const& phi2, Image const& theta2, Image const& phi3, Image const& theta3, dip::uint inputTensorElements ) {
         // Expand parameter images if necessary
         ParamImageQuickCopyAndExpandTensor( phi2, phi2_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta2, theta2_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( phi3, phi3_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta3, theta3_, inputTensorElements );
         T_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform3DRotationXY( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         Image::Pixel phi2Pixel = phi2_.At( imgCoords );
         Image::Pixel theta2Pixel = theta2_.At( imgCoords );
         Image::Pixel phi3Pixel = phi3_.At( imgCoords );
         Image::Pixel theta3Pixel = theta3_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < phi2_.TensorElements(); ++iTE ) {
            dfloat phi2 = phi2Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta2 = theta2Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat phi3 = phi3Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta3 = theta3Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements

            // Create transformation matrix from basis vectors
            Eigen::Vector3d xAxis( GetAxis( phi2, theta2 ));
            Eigen::Vector3d yAxis( GetAxis( phi3, theta3 ));
            Eigen::Vector3d zAxis( xAxis.cross( yAxis ).normalized() ); // TODO: do we need to normalize? Only needed if xAxis and zAxis are not perpendicular..
            // Set rotation part
            T_[ iTE ].linear() << xAxis, yAxis, zAxis;
            // Set translation part
            T_[ iTE ].translation() << Eigen::Vector3d( imgCoords_[ 0 ], imgCoords_[ 1 ], imgCoords_[ 2 ] );
         }
      }

      virtual void Transform( IntegerArray const& /*kernelCoords*/, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         Eigen::Map< Eigen::Vector3d > output( transformedCoords.data() );   // Create an Eigen map that directly accesses transformedCoords
         output = T_[ tensorIndex ] * output;
      }

   protected:
      Eigen::Vector3d GetAxis( dfloat phi, dfloat theta ) {
         dfloat sin_phi = std::sin( phi );
         dfloat cos_phi = std::cos( phi );
         dfloat sin_theta = std::sin( theta );
         dfloat cos_theta = std::cos( theta );
         return Eigen::Vector3d( sin_phi * cos_theta, sin_phi * sin_theta, cos_phi );
      }

      Image phi2_;
      Image theta2_;
      Image phi3_;
      Image theta3_;
      using CompactTransform = Eigen::Transform< dfloat, 3, Eigen::AffineCompact >;
      std::vector< CompactTransform, Eigen::aligned_allocator< CompactTransform >> T_;  // Transformation matrix. The vector is over the input tensor elements.
};

// Kernel transformation: 2D skew
class KernelTransform2DSkew : public KernelTransform {
   public:
      // The `skew` image tensor need not be expanded beforehand
      KernelTransform2DSkew( Image const& skew, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( skew, skew_, inputTensorElements );
         s_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DSkew( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel skewPixel = skew_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < skew_.TensorElements(); ++iTE ) {
            s_[ iTE ] = skewPixel[ iTE ].As< dfloat >();
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         dfloat kernelCoordX = static_cast< dfloat >( kernelCoords[ 0 ] );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX;
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 1 ] ) + s_[ tensorIndex ] * kernelCoordX;
      }

   protected:
      Image skew_;
      std::vector< dfloat > s_;  // The vector is over the input tensor elements
};

// Kernel transformation: 2D banana
class KernelTransform2DBanana : public KernelTransform2DRotation {
   public:
      // The `orientation` and `hcurvature` image tensors need not be expanded beforehand
      KernelTransform2DBanana( Image const& orientation, Image const& hcurvature, dip::uint inputTensorElements )
            : KernelTransform2DRotation( orientation, inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( hcurvature, hcurvature_, inputTensorElements );
         hcurv_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DBanana( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DRotation::SetImageCoords( imgCoords );
         Image::Pixel cPixel = hcurvature_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < orientation_.TensorElements(); ++iTE ) {
            hcurv_[ iTE ] = -0.5 * cPixel[ iTE ].As< dfloat >();
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         dfloat kernelCoordX = static_cast< dfloat> ( kernelCoords[ 0 ] );
         dfloat kernelCoordY = static_cast< dfloat >( kernelCoords[ 1 ] ) + ( hcurv_[ tensorIndex ] * kernelCoordX * kernelCoordX );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX * csn_[ tensorIndex ] + kernelCoordY * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - kernelCoordX * sn_[ tensorIndex ] + kernelCoordY * csn_[ tensorIndex ];
      }

   protected:
      Image hcurvature_;
      std::vector< dfloat > hcurv_; // The vector is over the input tensor elements
};

class KernelTransform2DScaledBanana : public KernelTransform2DBanana, public KernelTransformScale< 2 > {
   public:
      // The `orientation`, `hcurvature` and `kernelScale` image tensors need not be expanded beforehand
      KernelTransform2DScaledBanana( Image const& orientation, Image const& hcurvature, Image const& kernelScale, dip::uint inputTensorElements )
            : KernelTransform2DBanana( orientation, hcurvature, inputTensorElements ), KernelTransformScale< 2 >( kernelScale, inputTensorElements ) {}

      virtual KernelTransform* Clone() const override { return new KernelTransform2DScaledBanana( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DBanana::SetImageCoords( imgCoords );
         SetScaleAtImgCoords( imgCoords );
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         // First scale, then curve, then rotate
         dfloat kernelCoordX = scaleAtImgCoords_[ tensorIndex ][ 0 ] * static_cast< dfloat >( kernelCoords[ 0 ] );
         dfloat kernelCoordY = scaleAtImgCoords_[ tensorIndex ][ 1 ] * static_cast< dfloat >( kernelCoords[ 1 ] ) + ( hcurv_[ tensorIndex ] * kernelCoordX * kernelCoordX );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX * csn_[ tensorIndex ] + kernelCoordY * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - kernelCoordX * sn_[ tensorIndex ] + kernelCoordY * csn_[ tensorIndex ];
      }
};

// Input interpolation class
template< typename TPI, typename TPO >
class InputInterpolator {
   public:
      explicit InputInterpolator( Image const& in )
            : in_( in ), inOrigin_( static_cast< TPI* >( in_.Origin() )), inTensorStride_( in_.TensorStride() ) {}

      virtual ~InputInterpolator() = default;

      virtual TPO GetInputValue( FloatArray& /*coords*/, dip::uint /*tensorIndex*/, bool /*mirrorAtImageBoundaries*/ ) const { return 0; }

   protected:
      // Maps coords to a location inside the image using mirroring at the image boundaries.
      // Returns false if coordinates don't fall inside the image even after mirroring.
      template< dip::uint nDims >
      bool MapCoords_Mirror( dfloat* coords ) const {
         // Make sure all coordinates are within the image. Mirror at the boundaries.
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            const dfloat cMax = static_cast< dfloat >( in_.Size( iDim ) - 1 );
            dfloat& c = coords[ iDim ];
            if( c < 0 ) {
               c = -c;
               if( c > cMax ) {
                  return false; // Mirroring exceeds border
               }
            } else if( c > cMax ) {
               c = cMax - ( c - cMax );
               if( c < 0 ) {
                  return false; // Mirroring exceeds border
               }
            }
         }
         return true;
      }

      Image const& in_; // Input image
      TPI* inOrigin_;   // Input image origin
      dip::sint inTensorStride_;   // Input image tensor stride
};

// Input interpolation class, templated in the number of input dimensions
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorFixedDims : public InputInterpolator< TPI, TPO > {
   public:
      explicit InputInterpolatorFixedDims( Image const& in ) : InputInterpolator< TPI, TPO >( in ) {
         // Verify input dimensionality
         DIP_THROW_IF( in_.Dimensionality() != nDims, "Interpolation dimensionality incorrect" );
         // Cache strides and sizes
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            inStrides_[ iDim ] = in_.Stride( iDim );
            inSizes_[ iDim ] = in_.Size( iDim );
         }
      }

   protected:
      using InputInterpolator< TPI, TPO >::in_;
      std::array< dip::sint, nDims > inStrides_;
      std::array< dip::uint, nDims > inSizes_;
};

// Zero order hold input interpolator
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorZOH : public InputInterpolatorFixedDims< nDims, TPI, TPO > {
   public:
      explicit InputInterpolatorZOH( Image const& in ) : InputInterpolatorFixedDims< nDims, TPI, TPO >( in ) {}

      // Get zero-order-hold input value at floating point coordinates, arbitrary input dimensionality
      TPO GetInputValue( FloatArray& coords, dip::uint tensorIndex, bool mirrorAtImageBoundaries ) const override {
         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords )) {
               return 0;
            }
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< nDims >( &coords[ 0 ] )) {
               return 0;
            }
         }

         // Compute pixel offset
         dip::sint pixelOffset = 0;
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            dip::sint integerCoord = floor_cast( coords[ iDim ] );
            pixelOffset += integerCoord * inStrides_[ iDim ];
         }

         // Add tensor offset
         pixelOffset += static_cast< dip::sint >( tensorIndex ) * inTensorStride_;

         // Output single input value
         TPI inValue = *( inOrigin_ + pixelOffset );
         return static_cast< TPO >(inValue);
      }

      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inStrides_;
      using InputInterpolator< TPI, TPO >::inTensorStride_;
      using InputInterpolator< TPI, TPO >::inOrigin_;
      using InputInterpolator< TPI, TPO >::in_;
};

// First order hold input interpolator
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorFOH : public InputInterpolatorFixedDims< nDims, TPI, TPO > {
   public:
      explicit InputInterpolatorFOH( Image const& in ) : InputInterpolatorFixedDims< nDims, TPI, TPO >( in ) {}

      // Get linearly interpolated input value at floating point coordinates, arbitrary input dimensionality
      TPO GetInputValue( FloatArray& coords, dip::uint tensorIndex, bool mirrorAtImageBoundaries ) const override {
         using InputAsFloat = FloatType< TPI >;

         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords )) {
               return 0;
            }
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< nDims >( &coords[ 0 ] )) {
               return 0;
            }
         }

         // Start linear interpolation
         // Compute lower bound index and interpolation factor for all coordinates
         //  E.g.: if coords[i] == 1.1, then loBoundIndices[i] == 1.0 and factors[i] == 0.1
         std::array< dip::sint, nDims > loBoundIndices;
         std::array< InputAsFloat, nDims > factors; // Linear interpolation factors
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            loBoundIndices[ iDim ] = floor_cast( coords[ iDim ] );   // Take lower bound using floor_cast
            if( loBoundIndices[ iDim ] == static_cast< dip::sint >( inSizes_[ iDim ] - 1 )) { // Because we interpolate between loBound and loBound+1, make sure we don't go beyond the image borders
               loBoundIndices[ iDim ]--;
            }
            factors[ iDim ] = static_cast< InputAsFloat >( coords[ iDim ] ) - static_cast< InputAsFloat >( loBoundIndices[ iDim ] );
         }

         // Compute pixel offset for first pixel of the interpolation input window, including tensor offset
         dip::sint interpOriginOffset = 0;
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            interpOriginOffset += loBoundIndices[ iDim ] * inStrides_[ iDim ];
         }

         // Compute pixel offsets and composite interpolation factors for a total of 2 ^ nDims pixels
         // We can avoid an ND-loop by exploiting the fact that only 2 coordinates are processed per dimension.
         constexpr dip::uint numInterpPixels = dip::uint( 1 ) << nDims;

         TPO result = 0.0;
         //for( dip::uint iOffset = 0; iOffset < numInterpPixels; ++iOffset ) {
         const_for< numInterpPixels >(
               [ & ]( dip::uint iOffset ) {
                  dip::sint pixelOffset = interpOriginOffset;
                  InputAsFloat pixelFactor = 1.0;
                  //for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
                  const_for< nDims >(
                        [ & ]( dip::uint iDim ) {
                           // To process all permutations of lowerbound and upperbound (= lowerbound + 1) in each dimension,
                           // decide whether to process the upperbound in this dimension by testing the bits in iOffset
                           bool useUpperBound = ( iOffset & ( dip::uint( 1 ) << iDim )) != 0;
                           if( useUpperBound ) {
                              pixelOffset += inStrides_[ iDim ];
                              pixelFactor *= factors[ iDim ];
                           } else {
                              pixelFactor *= 1 - factors[ iDim ];
                           }
                        } );
                  // Add tensor offset
                  pixelOffset += static_cast< dip::sint >( tensorIndex ) * inTensorStride_;
                  // Multiply pixel input values with composite interpolation factors
                  TPI inValue = *( inOrigin_ + pixelOffset );
                  result += static_cast< TPO >(inValue) * pixelFactor;
               } );

         return result;
      }

      /*
      // GetInputValue_LinearInterpolation() 2D reference implementation for speed comparison
      TPO GetInputValue_LinearInterpolation_2D( FloatArray& coords, bool mirrorAtImageBoundaries ) const {
         using InputAsFloat = FloatType< TPI >;
         assert( in_.Dimensionality() == nDims );

         // Cache strides
         std::array< dip::sint, 2 > strides = { in_.Stride( 0 ), in_.Stride( 1 ) };

         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords ) )
               return 0;
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< 2 >( &coords[ 0 ] ) )
               return 0;
         }

         // Start linear interpolation
         // Compute lower bound index and interpolation factor for all coordinates
         //  E.g.: if coords[i] == 1.1, then loBoundIndices[i] == 1.0 and factors[i] == 0.1
         std::array< dip::uint, 2> loBoundIndices;
         for( dip::uint iDim = 0; iDim < 2; ++iDim ) {
            loBoundIndices[ iDim ] = (floor_cast( coords[ iDim ] ));   // Take lower bound using floor_cast
            if( coords[ iDim ] == in_.Size( iDim ) - 1 )  // Because we interpolate between loBound and loBound+1, make sure we don't go beyond the image borders
               loBoundIndices[ iDim ]--;
         }

         // Compute pixel offset of p00
         dip::sint offset = loBoundIndices[ 0 ] * strides[ 0 ] + loBoundIndices[ 1 ] * strides[ 1 ];

         TPI* origin = static_cast<TPI*>(in_.Origin());
         TPO p00 = static_cast<TPO>(*(origin + offset));
         TPO p01 = static_cast<TPO>(*(origin + offset + strides[ 1 ]));
         TPO p10 = static_cast<TPO>(*(origin + offset + strides[ 0 ]));
         TPO p11 = static_cast<TPO>(*(origin + offset + strides[ 0 ] + strides[ 1 ]));
         InputAsFloat xo = coords[ 0 ] - loBoundIndices[ 0 ];
         InputAsFloat yo = coords[ 1 ] - loBoundIndices[ 1 ];
         InputAsFloat xs = 1 - xo;
         InputAsFloat ys = 1 - yo;

         return (TPO)((p00 * xs * ys)
            + (p10 * xo * ys)
            + (p01 * xs * yo)
            + (p11 * xo * yo));
      }
      */

      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inSizes_;
      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inStrides_;
      using InputInterpolator< TPI, TPO >::inTensorStride_;
      using InputInterpolator< TPI, TPO >::inOrigin_;
      using InputInterpolator< TPI, TPO >::in_;
};

// The adaptive window convolution filter for adaptive gauss and its variants
template< typename TPI, typename TPO = FlexType< TPI >>
class AdaptiveWindowConvolutionLineFilter : public Framework::FullLineFilter {
   public:
      AdaptiveWindowConvolutionLineFilter( Image const& in, Kernel const& kernel, ImageArray const& params, String const& interpolation, BoundaryCondition bc, String const& transform )
            : in_( in ), kernel_( kernel ) {
         // Determine kernel transformation
         if( in_.Dimensionality() == 2 ) {
            // === 2D ===
            // Construct input interpolator
            ConstructInputInterpolator< 2 >( in, interpolation );

            // Construct kernel transformation
            ConstructKernelTransform2D( transform, params, in.TensorElements() );

         } else if( in_.Dimensionality() == 3 ) {
            // === 3D ===
            // Construct input interpolator
            ConstructInputInterpolator< 3 >( in, interpolation );

            // Determine kernel transformation
            ConstructKernelTransform3D( transform, params, in.TensorElements() );
         } else {
            DIP_THROW( "No transform \"" + transform + "\" known for input dimensionality " + std::to_string( in_.Dimensionality() ));
         }

         // Store boundary condition. We only support mirroring or zeros for now.
         DIP_THROW_IF( bc != BoundaryCondition::SYMMETRIC_MIRROR && bc != BoundaryCondition::ADD_ZEROS, "Unsupported boundary condition" );
         mirrorAtInputBoundaries_ = ( bc == BoundaryCondition::SYMMETRIC_MIRROR );
      }

      virtual void SetNumberOfThreads( dip::uint numThreads, PixelTableOffsets const& pixelTable ) override {
         offsets_ = pixelTable.Offsets();
         kernelTransforms_.resize( numThreads - 1 );  // kernelTransform_ is used for thread 0
      }

      virtual void Filter( Framework::FullLineFilterParameters const& params ) override {
         TPI* in = static_cast< TPI* >( params.inBuffer.buffer );
         dip::sint inStride = params.inBuffer.stride;
         TPO* out = static_cast< TPO* >( params.outBuffer.buffer );
         dip::sint outStride = params.outBuffer.stride;
         dip::sint outTensorStride = params.outBuffer.tensorStride;
         dip::uint length = params.bufferLength;
         PixelTableOffsets const& pixelTableOffsets = params.pixelTable;
         std::vector< dfloat > const& weights = pixelTableOffsets.Weights();
         UnsignedArray inCoords( params.position );
         PixelTable pixelTable = kernel_.PixelTable( in_.Dimensionality(), params.dimension );  // TODO: move to constructor
         FloatArray transformedKernelCoords( in_.Dimensionality() );

         // Obtain kernel transform for this thread
         std::unique_ptr< KernelTransform >& kernelTransform = params.thread == 0 ? kernelTransform_ : kernelTransforms_[ params.thread - 1 ];
         // Clone kernelTransform_ for this thread if not done already.
         // Needed because the KernelTransform computes and stores values per input pixel and uses those repeatedly for each kernel element.
         if( !kernelTransform ) {
            kernelTransform.reset( kernelTransform_->Clone() );
         }

         for( dip::uint ii = 0; ii < length; ++ii ) {
            for( dip::uint iTE = 0; iTE < in_.TensorElements(); ++iTE ) {
               *( out + static_cast< dip::sint >( iTE ) * outTensorStride ) = 0;
            }
            auto itWeight = weights.begin();
            // Prepare kernel transform for current input coordinates
            kernelTransform->SetImageCoords( inCoords );
            // Apply kernel
            for( PixelTable::iterator itPT = pixelTable.begin(); itPT != pixelTable.end(); ++itPT ) {
               for( dip::uint iTE = 0; iTE < in_.TensorElements(); ++iTE ) {
                  // Apply kernel transformation
                  kernelTransform->Transform( *itPT, iTE, transformedKernelCoords );
                  // Obtain input value at the transformed kernel coords
                  *( out + static_cast< dip::sint >( iTE ) * outTensorStride ) +=
                        inputInterpolator_->GetInputValue( transformedKernelCoords, iTE, mirrorAtInputBoundaries_ ) * static_cast< FloatType< TPO >>( *itWeight );
               }
               ++itWeight;
            }
            // Prepare next buffer element
            inCoords[ params.dimension ]++;
            in += inStride;
            out += outStride;
         }
      }

   private:
      template< dip::uint nDims >
      void ConstructInputInterpolator( Image const& in, String const& interpolation ) {
         // Determine input interpolator
         if( interpolation == S::ZERO_ORDER ) {
            inputInterpolator_ = std::make_unique< InputInterpolatorZOH< nDims, TPI, TPO >>( in );
         } else if( interpolation == S::LINEAR ) {
            inputInterpolator_ = std::make_unique< InputInterpolatorFOH< nDims, TPI, TPO >>( in );
         } else {
            DIP_THROW( "Unknown interpolation \"" + interpolation + "\"" );
         }
      }

      void ConstructKernelTransform2D( String const& transform, ImageArray const& params, dip::uint inputTensorElements ) {
         // Determine kernel transformation
         if( transform == "none" ) {
            kernelTransform_ = std::make_unique< KernelTransform >();
         } else if( transform == "ellipse" ) {
            if( params.size() == 1 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DRotation >( params[ 0 ], inputTensorElements );
            } else if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DScaledRotation >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else {
               DIP_THROW( E::ARRAY_PARAMETER_WRONG_LENGTH );
            }
         } else if( transform == "banana" ) {
            if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DBanana >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else if( params.size() == 3 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DScaledBanana >( params[ 0 ], params[ 1 ], params[ 2 ], inputTensorElements );
            } else {
               DIP_THROW( E::ARRAY_PARAMETER_WRONG_LENGTH );
            }
         } else if( transform == "skew" ) {
            DIP_THROW_IF( params.size() != 1, E::ARRAY_PARAMETER_WRONG_LENGTH );
            kernelTransform_ = std::make_unique< KernelTransform2DSkew >( params[ 0 ], inputTensorElements );
         } else {
            DIP_THROW( "Unknown 2D transform \"" + transform + "\"" );
         }
      }

      void ConstructKernelTransform3D( String const& transform, ImageArray const& params, dip::uint inputTensorElements ) {
         if( transform == "none" ) {
            kernelTransform_ = std::make_unique< KernelTransform >();
         } else if( transform == "ellipse" ) {
            if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform3DRotationZ >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else if( params.size() == 4 ) {
               kernelTransform_ = std::make_unique< KernelTransform3DRotationXY >( params[ 0 ], params[ 1 ], params[ 2 ], params[ 3 ], inputTensorElements );
            }
         } else {
            DIP_THROW( "Unknown 3D transform \"" + transform + "\"" );
         }
      }

      std::vector< dip::sint > offsets_;  // Pixel table offsets
      Image const& in_; // Input image
      Kernel const& kernel_;  // Kernel
      std::unique_ptr< KernelTransform > kernelTransform_;  // Kernel transform for thread 0
      std::vector< std::unique_ptr< KernelTransform >> kernelTransforms_; // Kernel transforms for remaining threads
      std::unique_ptr< InputInterpolator< TPI, TPO >> inputInterpolator_; // Input interpolator
      bool mirrorAtInputBoundaries_;   // Boundary condition: either mirror or zeros
};

} // namespace

void AdaptiveFilter(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition,
      String const& transform
) {
   DIP_THROW_IF( !in.IsForged(), E::IMAGE_NOT_FORGED );

   // Prepare parameter images: expand singleton dimensions, including the tensor
   ImageArray paramImages( params.size() );
   for( dip::uint iP = 0; iP < params.size(); ++iP ) {
      paramImages[ iP ] = params[ iP ].get().QuickCopy();
      paramImages[ iP ].ExpandSingletonDimensions( in.Sizes() );
      // The param image's tensor is expanded while constructing the kernel transformation
      // TODO: ExpandSingletonDimensions() could also be done while constructing the kernel transformation, so this loop is removed altogether
   }
   DIP_STACK_TRACE_THIS( ArrayUseParameter( sigmas, in.Dimensionality(), 1.0 ));

   DIP_START_STACK_TRACE
      // Create gaussian kernel
      Kernel kernel{ CreateGauss( sigmas, orders, truncation, exponents ) };

      BoundaryCondition bc = StringToBoundaryCondition( boundaryCondition );
      DataType outputType = DataType::SuggestFlex( in.DataType() );
      std::unique_ptr< Framework::FullLineFilter > lineFilter;
      DIP_OVL_NEW_ALL( lineFilter, AdaptiveWindowConvolutionLineFilter, ( in, kernel, paramImages, interpolationMethod, bc, transform ), in.DataType() );
      // We use the full framework to allow multi-threading. Its parameters prevent input or output buffering to minimize overhead. Border expansion is not used either.
      Framework::Full( in, out, in.DataType(), outputType, outputType, in.TensorElements(), { bc }, kernel, *lineFilter, Framework::FullOption::BorderAlreadyExpanded );// for performance comparisons: +Framework::FullOption::NoMultiThreading );

   DIP_END_STACK_TRACE
}

void AdaptiveGauss(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray const& sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition
) {
   AdaptiveFilter( in, params, out, sigmas, orders, truncation, exponents, interpolationMethod, boundaryCondition, "ellipse" );
}

void AdaptiveBanana(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray const& sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition
) {
   AdaptiveFilter( in, params, out, sigmas, orders, truncation, exponents, interpolationMethod, boundaryCondition, "banana" );
}

} // namespace dip

#if defined(__GNUG__) || defined(__clang__)
#pragma GCC diagnostic pop
#endif


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

/**
 * @file rk.hpp
 * @brief Integration of ODEs by Runge-Kutta
 * @author Ben Allanach, Alexander Voigt
 *
 * The implementation of the Runge-Kutta routines have been derived
 * from SOFTSUSY [hep-ph/0104145, Comp. Phys. Comm. 143 (2002) 305].
 */

#ifndef RK_H
#define RK_H

#include <algorithm>
#include <cmath>
#include <functional>

#include <Eigen/Core>

#include "logger.hpp"
#include "error.hpp"

namespace flexiblesusy {

namespace runge_kutta {

namespace {
/// Returns |a| with sign of b in front
inline double sign(double a, double b) noexcept {
   return b >= 0 ? std::fabs(a) : -std::fabs(a);
}
} // anonymous namespace

/// A single step of Runge Kutta (5th order), input:
/// y and dydx (derivative of y), x is independent variable. yout is value
/// after step. derivs is a user-supplied function
template <typename ArrayType, typename Derivs>
void rungeKuttaStep(const ArrayType& y, const ArrayType& dydx, double x,
		    double h, ArrayType& yout, ArrayType& yerr, Derivs derivs)
{
   const double a2 = 0.2;
   const double a3 = 0.3;
   const double a4 = 0.6;
   const double a5 = 1.0;
   const double a6 = 0.875;
   const double b21 = 0.2;
   const double b31 = 3.0 / 40.0;
   const double b32 = 9.0 / 40.0;
   const double b41 = 0.3;
   const double b42 = -0.9;
   const double b43 = 1.2;
   const double b51 = -11.0 / 54.0;
   const double b52 = 2.5;
   const double b53 = -70.0 / 27.0;
   const double b54 = 35.0 / 27.0;
   const double b61 = 1631.0 / 55296.0;
   const double b62 = 175.0 / 512.0;
   const double b63 = 575.0 / 13824.0;
   const double b64 = 44275.0 / 110592.0;
   const double b65 = 253.0 / 4096.0;
   const double c1 = 37.0 / 378.0;
   const double c3 = 250.0 / 621.0;
   const double c4 = 125.0 / 594.0;
   const double c6 = 512.0 / 1771.0;
   const double dc5 = -277.00 / 14336.0;
   const double dc1 = c1 - 2825.0 / 27648.0;
   const double dc3 = c3 - 18575.0 / 48384.0;
   const double dc4 = c4 - 13525.0 / 55296.0;
   const double dc6 = c6 - 0.25;

   ArrayType ytemp = b21 * h * dydx + y;
   const ArrayType ak2 = derivs(x + a2 * h, ytemp);

   // Allowing piece-wise calculating of ytemp for speed reasons
   ytemp = y + h * (b31 * dydx + b32 * ak2);
   const ArrayType ak3 = derivs(x + a3 * h, ytemp);

   ytemp = y + h * (b41 * dydx + b42 * ak2 + b43 * ak3);
   const ArrayType ak4 = derivs(x+a4*h,ytemp);

   ytemp = y + h * (b51 * dydx + b52 * ak2 + b53 * ak3 + b54 * ak4);
   const ArrayType ak5 = derivs(x + a5 * h, ytemp);

   ytemp = y + h * (b61 * dydx + b62 * ak2 + b63 * ak3 + b64 * ak4 + b65 * ak5);
   const ArrayType ak6 = derivs(x + a6 * h, ytemp);

   yout = y + h * (c1 * dydx + c3 * ak3 + c4 * ak4 + c6 * ak6);
   yerr = h * (dc1 * dydx + dc3 * ak3 + dc4 * ak4 + dc5 * ak5 + dc6 * ak6);
}

/// organises the variable step-size for Runge-Kutta evolution
template <typename ArrayType, typename Derivs>
double odeStepper(ArrayType& y, const ArrayType& dydx, double& x, double htry,
                  double eps, const ArrayType& yscal, Derivs derivs,
                  int& max_step_dir)
{
   const double SAFETY = 0.9;
   const double PGROW = -0.2;
   const double PSHRNK = -0.25;
   const double ERRCON = 1.89e-4;
   const int n = y.size();
   double errmax;
   double h = htry;
   ArrayType yerr(n);
   ArrayType ytemp(n);

   for (;;) {
      rungeKuttaStep(y, dydx, x, h, ytemp, yerr, derivs);
      errmax = (yerr / yscal).abs().maxCoeff(&max_step_dir);
      errmax  /= eps;
      if (!std::isfinite(errmax)) {
#ifdef ENABLE_VERBOSE
         ERROR("odeStepper: non-perturbative running at Q = "
               << std::exp(x) << " GeV of parameter y(" << max_step_dir
               << ") = " << y(max_step_dir) << ", dy(" << max_step_dir
               << ")/dx = " << dydx(max_step_dir));
#endif
         throw NonPerturbativeRunningError(std::exp(x), max_step_dir, y(max_step_dir));
      }
      if (errmax <= 1.0) {
         break;
      }
      const double htemp = SAFETY * h * std::pow(errmax, PSHRNK);
      h = (h >= 0.0 ? std::max(htemp, 0.1 * h) : std::min(htemp, 0.1 * h));
      if (x + h == x) {
#ifdef ENABLE_VERBOSE
         ERROR("At Q = " << std::exp(x) << " GeV "
               "stepsize underflow in odeStepper in parameter y("
               << max_step_dir << ") = " << y(max_step_dir) << ", dy("
               << max_step_dir << ")/dx = " << dydx(max_step_dir));
#endif
         throw NonPerturbativeRunningError(std::exp(x), max_step_dir, y(max_step_dir));
      }
   }
   x += h;
   y = ytemp;

   return errmax > ERRCON ? SAFETY * h * std::pow(errmax,PGROW) : 5.0 * h;
}

/// Organises integration of 1st order system of ODEs
template <typename ArrayType, typename Derivs,
          typename Stepper = decltype(runge_kutta::odeStepper<ArrayType,Derivs>)>
void integrateOdes(ArrayType& ystart, double from, double to, double eps,
                   double h1, double hmin, Derivs derivs,
                   Stepper rkqs = runge_kutta::odeStepper<ArrayType,Derivs>, int max_steps = 400)
{
   const int nvar = ystart.size();
   const double TINY = 1.0e-16;
   double x = from;
   double h = sign(h1, to - from);
   ArrayType yscal(nvar);
   ArrayType y(ystart);
   ArrayType dydx;
   int max_step_dir;

   for (int nstp = 0; nstp < max_steps; ++nstp) {
      dydx = derivs(x, y);
      yscal = y.abs() + (dydx * h).abs() + TINY;
      if ((x + h - to) * (x + h - from) > 0.0) {
         h = to - x;
      }

      const double hnext = rkqs(y, dydx, x, h, eps, yscal, derivs, max_step_dir);

      if ((x - to) * (to - from) >= 0.0) {
         ystart = y;
         return;
      }

      h = hnext;

      if (std::fabs(hnext) <= hmin) {
         break;
      }
   }

#ifdef ENABLE_VERBOSE
   ERROR("Bailed out of rk.cpp:too many steps in integrateOdes\n"
         "********** Q = " << std::exp(x) << " *********");
   ERROR("max step in direction of " << max_step_dir);
   for (int i = 0; i < nvar; i++)
      ERROR("y(" << i << ") = " << y(i) << " dydx(" << i <<
            ") = " << dydx(i));
#endif

   throw NonPerturbativeRunningError(std::exp(x), max_step_dir, y(max_step_dir));
}

} // namespace runge_kutta

} // namespace flexiblesusy

#endif // RK_H