// Rotations conversion library
// File: rot_conv.cpp
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Includes
#include <rot_conv/rot_conv.h>
#include <Eigen/Eigenvalues>
#include <cmath>

// Defines
#define M_2PI (2.0*M_PI)

// Rotations conversion namespace
namespace rot_conv
{
	// ########################
	// #### Rotation types ####
	// ########################

	// Euler angles constants
	const EulerAngles EulerAngles::Identity(0.0, 0.0, 0.0);

	// Fused angles constants
	const FusedAngles FusedAngles::Identity(0.0, 0.0, 0.0, true);

	// Tilt angles constants
	const TiltAngles TiltAngles::Identity(0.0, 0.0, 0.0);

	// ##########################################
	// #### Rotation checking and validation ####
	// ##########################################

	// Check and validate: Rotation matrix
	bool ValidateRotmat(Rotmat& R, double tol)
	{
		// Make a copy of the input
		Rotmat Rorig = R;

		// Find the closest orthogonal matrix to the input rotation matrix
		Rotmat nonOrth = R.transpose() * R;
		R *= Eigen::SelfAdjointEigenSolver<Rotmat>(nonOrth).operatorInverseSqrt();

		// Filter out invalid left hand coordinate systems
		if(R.determinant() < 0.0)
			R.setIdentity();

		// Return whether the rotation matrix was valid within the given tolerance
		return (R - Rorig).isZero(tol);
	}

	// Check and validate: Quaternion
	bool ValidateQuat(Quat& q, double tol, bool unique)
	{
		// Make a copy of the input
		Quat qorig = q;

		// Renormalise the quaternion
		double normsq = q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z();
		if(normsq <= 0.0)
		{
			q.w() = 1.0;
			q.x() = q.y() = q.z() = 0.0;
		}
		else
		{
			double norm = sqrt(normsq);
			q.w() /= norm;
			q.x() /= norm;
			q.y() /= norm;
			q.z() /= norm;
		}

		// Make the quaternion unique
		if(unique && q.w() < 0.0)
		{
			q.w() = -q.w();
			q.x() = -q.x();
			q.y() = -q.y();
			q.z() = -q.z();
		}

		// Return whether the quaternion was valid within the given tolerance
		return (fabs(q.w() - qorig.w()) <= tol && fabs(q.x() - qorig.x()) <= tol && fabs(q.y() - qorig.y()) <= tol && fabs(q.z() - qorig.z()) <= tol);
	}

	// Check and validate: Euler angles
	bool ValidateEuler(EulerAngles& e, double tol, bool unique)
	{
		// Make a copy of the input
		EulerAngles eorig = e;

		// Wrap the pitch to (-pi,pi] and then collapse it to the [-pi/2,pi/2] interval
		e.pitch += M_2PI*std::floor((M_PI - e.pitch) / M_2PI);
		if(fabs(e.pitch) > M_PI_2)
		{
			e.yaw += M_PI;
			e.pitch = (e.pitch >= 0.0 ? M_PI - e.pitch : -M_PI - e.pitch);
			e.roll += M_PI;
		}

		// Make the positive and negative gimbal lock representations unique
		if(unique)
		{
			double spitch = sin(e.pitch);
			if(fabs(spitch - 1.0) <= tol)
			{
				e.roll -= e.yaw;
				e.yaw = 0.0;
			}
			else if(fabs(spitch + 1.0) <= tol)
			{
				e.roll += e.yaw;
				e.yaw = 0.0;
			}
		}

		// Wrap yaw and roll to (-pi,pi]
		e.yaw += M_2PI*std::floor((M_PI - e.yaw) / M_2PI);
		e.roll += M_2PI*std::floor((M_PI - e.roll) / M_2PI);

		// Return whether the Euler angles were valid within the given tolerance
		return (fabs(e.yaw - eorig.yaw) <= tol && fabs(e.pitch - eorig.pitch) <= tol && fabs(e.roll - eorig.roll) <= tol);
	}

	// Check and validate: Fused angles
	bool ValidateFused(FusedAngles& f, double tol, bool unique)
	{
		// Make a copy of the input
		FusedAngles forig = f;

		// Wrap the angles to (-pi,pi]
		f.fusedYaw += M_2PI*std::floor((M_PI - f.fusedYaw) / M_2PI);
		f.fusedPitch += M_2PI*std::floor((M_PI - f.fusedPitch) / M_2PI);
		f.fusedRoll += M_2PI*std::floor((M_PI - f.fusedRoll) / M_2PI);

		// Coerce the L1 norm
		double L1Norm = fabs(f.fusedPitch) + fabs(f.fusedRoll);
		if(L1Norm > M_PI_2)
		{
			double scale = M_PI_2 / L1Norm;
			f.fusedPitch *= scale;
			f.fusedRoll *= scale;
		}

		// Make the representation unique if required
		if(unique)
		{
			double spitch = sin(f.fusedPitch);
			double sroll = sin(f.fusedRoll);
			double sineSum = spitch*spitch + sroll*sroll;
			if(sineSum >= 1.0 - tol)
				f.hemi = true;
			L1Norm = fabs(f.fusedPitch) + fabs(f.fusedRoll);
			if(L1Norm <= tol && !f.hemi)
				f.fusedYaw = 0.0;
		}

		// Return whether the fused angles were valid within the given tolerance
		return (fabs(f.fusedYaw - forig.fusedYaw) <= tol && fabs(f.fusedPitch - forig.fusedPitch) <= tol && fabs(f.fusedRoll - forig.fusedRoll) <= tol && f.hemi == forig.hemi);
	}

	// Check and validate: Tilt angles
	bool ValidateTilt(TiltAngles& t, double tol, bool unique)
	{
		// Make a copy of the input
		TiltAngles torig = t;

		// Wrap the angles to (-pi,pi]
		t.fusedYaw += M_2PI*std::floor((M_PI - t.fusedYaw) / M_2PI);
		t.tiltAxisAngle += M_2PI*std::floor((M_PI - t.tiltAxisAngle) / M_2PI);
		t.tiltAngle += M_2PI*std::floor((M_PI - t.tiltAngle) / M_2PI);

		// Handle the case of a negative tilt angle
		if(t.tiltAngle < 0.0)
		{
			t.tiltAxisAngle = (t.tiltAxisAngle > 0.0 ? -M_PI + t.tiltAxisAngle : M_PI + t.tiltAxisAngle);
			t.tiltAngle = -t.tiltAngle;
		}

		// Make the representation unique if required
		if(unique)
		{
			double ctilt = cos(t.tiltAngle);
			bool near0 = (fabs(ctilt - 1.0) <= tol);
			bool near180 = (fabs(ctilt + 1.0) <= tol);
			if(near0 || near180)
				t.tiltAxisAngle = 0.0;
			if(near180)
				t.fusedYaw = 0.0;
		}

		// Return whether the tilt angles were valid within the given tolerance
		return (fabs(t.fusedYaw - torig.fusedYaw) <= tol && fabs(t.tiltAxisAngle - torig.tiltAxisAngle) <= tol && fabs(t.tiltAngle - torig.tiltAngle) <= tol);
	}

	// ###########################
	// #### Rotation equality ####
	// ###########################

	// Check equality: Rotation matrix
	bool RotmatEqual(const Rotmat& Ra, const Rotmat& Rb, double tol)
	{
		// Return whether none of the elements of the rotation matrices differ by more than the tolerance
		return (Ra - Rb).isZero(tol);
	}

	// Check equality: Quaternion
	bool QuatEqual(const Quat& qa, const Quat& qb, double tol)
	{
		// Return whether to the specified tolerance the quaternions are the same
		bool isSame = (fabs(qa.w() - qb.w()) <= tol && fabs(qa.x() - qb.x()) <= tol && fabs(qa.y() - qb.y()) <= tol && fabs(qa.z() - qb.z()) <= tol);
		bool isOpp  = (fabs(qa.w() + qb.w()) <= tol && fabs(qa.x() + qb.x()) <= tol && fabs(qa.y() + qb.y()) <= tol && fabs(qa.z() + qb.z()) <= tol);
		return (isSame || isOpp);
	}

	// Check equality: Euler angles
	bool EulerEqual(const EulerAngles& ea, const EulerAngles& eb, double tol)
	{
		// Convert both Euler angles to their unique representations
		EulerAngles eau = ea, ebu = eb;
		ValidateEuler(eau, tol, true);
		ValidateEuler(ebu, tol, true);

		// Handle angle wrapping issues
		if(fabs(eau.yaw - ebu.yaw) > M_PI)
		{
			if(eau.yaw > ebu.yaw)
				ebu.yaw += M_2PI;
			else
				eau.yaw += M_2PI;
		}
		if(fabs(eau.roll - ebu.roll) > M_PI)
		{
			if(eau.roll > ebu.roll)
				ebu.roll += M_2PI;
			else
				eau.roll += M_2PI;
		}

		// Return whether to the specified tolerance the Euler angles are the same
		return (fabs(eau.yaw - ebu.yaw) <= tol && fabs(sin(eau.pitch) - sin(ebu.pitch)) <= tol && fabs(eau.roll - ebu.roll) <= tol); // The pitch suffers from the numerical insensitivity of asin, so the sine thereof is checked
	}

	// Check equality: Fused angles
	bool FusedEqual(const FusedAngles& fa, const FusedAngles& fb, double tol)
	{
		// Convert both fused angles to their unique representations
		FusedAngles fau = fa, fbu = fb;
		ValidateFused(fau, tol, true);
		ValidateFused(fbu, tol, true);

		// Handle angle wrapping issues
		if(fabs(fau.fusedYaw - fbu.fusedYaw) > M_PI)
		{
			if(fau.fusedYaw > fbu.fusedYaw)
				fbu.fusedYaw += M_2PI;
			else
				fau.fusedYaw += M_2PI;
		}

		// Return whether to the specified tolerance the fused angles are the same
		return (fabs(fau.fusedYaw - fbu.fusedYaw) <= tol && fabs(sin(fau.fusedPitch) - sin(fbu.fusedPitch)) <= tol && fabs(sin(fau.fusedRoll) - sin(fbu.fusedRoll)) <= tol && fau.hemi == fbu.hemi); // The fused pitch and roll suffer from the numerical insensitivity of asin, so the sine's thereof are checked
	}

	// Check equality: Tilt angles
	bool TiltEqual(const TiltAngles& ta, const TiltAngles& tb, double tol)
	{
		// Convert both tilt angles to their unique representations
		TiltAngles tau = ta, tbu = tb;
		ValidateTilt(tau, tol, true);
		ValidateTilt(tbu, tol, true);

		// Handle angle wrapping issues
		if(fabs(tau.fusedYaw - tbu.fusedYaw) > M_PI)
		{
			if(tau.fusedYaw > tbu.fusedYaw)
				tbu.fusedYaw += M_2PI;
			else
				tau.fusedYaw += M_2PI;
		}

		// Return whether to the specified tolerance the fused angles are the same
		double stilta = sin(tau.tiltAngle);
		double stiltb = sin(tbu.tiltAngle);
		double stiltasq = stilta*stilta;
		double stiltbsq = stiltb*stiltb;
		return (fabs(tau.fusedYaw - tbu.fusedYaw) <= tol && fabs(stiltasq*cos(tau.tiltAxisAngle) - stiltbsq*cos(tbu.tiltAxisAngle)) <= tol && fabs(stiltasq*sin(tau.tiltAxisAngle) - stiltbsq*sin(tbu.tiltAxisAngle)) <= tol && fabs(cos(tau.tiltAngle) - cos(tbu.tiltAngle)) <= tol); // The tilt angle suffers from the numerical insensitivity of acos, so the cosine thereof is checked / The tilt axis angle has a singularity when the tilt angle is zero, so two geometrically relevant terms are checked instead of the tilt axis angle directly
	}

	// #########################
	// #### Yaw of rotation ####
	// #########################

	// Euler yaw of: Rotation matrix
	double EYawOfRotmat(const Rotmat& R)
	{
		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(R.coeff(1,0), R.coeff(0,0));
	}

	// Fused yaw of: Rotation matrix
	double FYawOfRotmat(const Rotmat& R)
	{
		// Calculate, wrap and return the fused yaw
		double fusedYaw, trace = R.coeff(0,0) + R.coeff(1,1) + R.coeff(2,2);
		if(trace >= 0.0)
			fusedYaw = 2.0*atan2(R.coeff(1,0) - R.coeff(0,1), 1.0 + trace);
		else if(R.coeff(2,2) >= R.coeff(1,1) && R.coeff(2,2) >= R.coeff(0,0))
			fusedYaw = 2.0*atan2(1.0 - R.coeff(0,0) - R.coeff(1,1) + R.coeff(2,2), R.coeff(1,0) - R.coeff(0,1));
		else if(R.coeff(1,1) >= R.coeff(0,0))
			fusedYaw = 2.0*atan2(R.coeff(2,1) + R.coeff(1,2), R.coeff(0,2) - R.coeff(2,0));
		else
			fusedYaw = 2.0*atan2(R.coeff(0,2) + R.coeff(2,0), R.coeff(2,1) - R.coeff(1,2));
		if(fusedYaw > M_PI) fusedYaw -= M_2PI;   // fusedYaw is now in [-2*pi,pi]
		if(fusedYaw <= -M_PI) fusedYaw += M_2PI; // fusedYaw is now in (-pi,pi]
		return fusedYaw;
	}

	// Euler yaw of: Quaternion
	double EYawOfQuat(const Quat& q)
	{
		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(q.w()*q.z() + q.x()*q.y(), 0.5 - (q.y()*q.y() + q.z()*q.z()));
	}

	// Fused yaw of: Quaternion
	double FYawOfQuat(const Quat& q)
	{
		// Calculate, wrap and return the fused yaw
		double fusedYaw = 2.0*atan2(q.z(), q.w()); // Output of atan2 is [-pi,pi], so this expression is in [-2*pi,2*pi]
		if(fusedYaw > M_PI) fusedYaw -= M_2PI;     // fusedYaw is now in [-2*pi,pi]
		if(fusedYaw <= -M_PI) fusedYaw += M_2PI;   // fusedYaw is now in (-pi,pi]
		return fusedYaw;
	}

	// Fused yaw of: Euler angles
	double FYawOfEuler(const EulerAngles& e)
	{
		// Calculate and return the fused yaw of the rotation
		return FYawOfRotmat(RotmatFromEuler(e));
	}

	// Euler yaw of: Fused angles
	double EYawOfFused(const FusedAngles& f)
	{
		// Precalculate the sin values
		double sth  = sin(f.fusedPitch);
		double sphi = sin(f.fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the cosine of the tilt angle alpha
		double calpha;
		if(crit >= 1.0)
			calpha = 0.0;
		else
			calpha = (f.hemi ? sqrt(1.0-crit) : -sqrt(1.0-crit));

		// Calculate the tilt axis gamma
		double gamma = atan2(sth,sphi);

		// Precalculate trigonometric terms
		double psigam = f.fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double A = sgam*calpha;

		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam);
	}

	// Euler yaw of: Tilt angles
	double EYawOfTilt(const TiltAngles& t)
	{
		// Precalculate trigonometric terms
		double psigam = t.fusedYaw + t.tiltAxisAngle;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double cgam = cos(t.tiltAxisAngle);
		double sgam = sin(t.tiltAxisAngle);
		double calpha = cos(t.tiltAngle);
		double A = sgam*calpha;

		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam);
	}

	// ##################################
	// #### Remove yaw from rotation ####
	// ##################################

	// Remove Euler yaw from: Rotation matrix
	void RotmatNoEYaw(const Rotmat& R, Rotmat& Rout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfRotmat(R);

		// Precalculate trigonometric terms
		double cEYaw = cos(EYaw);
		double sEYaw = sin(EYaw);

		// Construct the Euler ZYX yaw component of the rotation
		Rotmat REYawTrans;
		REYawTrans << cEYaw, sEYaw, 0.0, -sEYaw, cEYaw, 0.0, 0.0, 0.0, 1.0;

		// Remove the Euler ZYX yaw component of the rotation
		Rout = REYawTrans * R;
	}

	// Remove fused yaw from: Rotation matrix
	void RotmatNoFYaw(const Rotmat& R, Rotmat& Rout)
	{
		// Calculate the fused yaw of the input
		double FYaw = FYawOfRotmat(R);

		// Precalculate trigonometric terms
		double cFYaw = cos(FYaw);
		double sFYaw = sin(FYaw);

		// Construct the fused yaw component of the rotation
		Rotmat RFYawTrans;
		RFYawTrans << cFYaw, sFYaw, 0.0, -sFYaw, cFYaw, 0.0, 0.0, 0.0, 1.0;

		// Remove the fused yaw component of the rotation
		Rout = RFYawTrans * R;
	}

	// Remove Euler yaw from: Quaternion
	void QuatNoEYaw(const Quat& q, Quat& qout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfQuat(q);

		// Construct the Euler ZYX yaw component of the rotation
		double hcEYaw = cos(0.5*EYaw);
		double hsEYaw = sin(0.5*EYaw);

		// Remove the Euler ZYX yaw component of the rotation
		qout.w() = hcEYaw*q.w() + hsEYaw*q.z();
		qout.x() = hcEYaw*q.x() + hsEYaw*q.y();
		qout.y() = hcEYaw*q.y() - hsEYaw*q.x();
		qout.z() = hcEYaw*q.z() - hsEYaw*q.w();
	}

	// Remove fused yaw from: Quaternion
	void QuatNoFYaw(const Quat& q, Quat& qout)
	{
		// Calculate the fused yaw of the input
		double FYaw = FYawOfQuat(q);

		// Construct the fused yaw component of the rotation
		double hcFYaw = cos(0.5*FYaw);
		double hsFYaw = sin(0.5*FYaw);

		// Remove the fused yaw component of the rotation
		qout.w() = hcFYaw*q.w() + hsFYaw*q.z();
		qout.x() = hcFYaw*q.x() + hsFYaw*q.y();
		qout.y() = hcFYaw*q.y() - hsFYaw*q.x();
		qout.z() = hcFYaw*q.z() - hsFYaw*q.w();
	}

	// Remove yaw from: Euler angles
	void EulerNoFYaw(const EulerAngles& e, EulerAngles& eout)
	{
		// Calculate the fused yaw of the input
		double FYaw = FYawOfEuler(e);

		// Remove the fused yaw component of the rotation
		eout.yaw = e.yaw - FYaw;
		eout.pitch = e.pitch;
		eout.roll = e.roll;
	}

	// Remove yaw from: Fused angles
	void FusedNoEYaw(const FusedAngles& f, FusedAngles& fout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfFused(f);

		// Remove the Euler ZYX yaw component of the rotation
		fout.fusedYaw = f.fusedYaw - EYaw;
		fout.fusedPitch = f.fusedPitch;
		fout.fusedRoll = f.fusedRoll;
		fout.hemi = f.hemi;
	}

	// Remove yaw from: Tilt angles
	void TiltNoEYaw(const TiltAngles& t, TiltAngles& tout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfTilt(t);

		// Remove the Euler ZYX yaw component of the rotation
		tout.fusedYaw = t.fusedYaw - EYaw;
		tout.tiltAxisAngle = t.tiltAxisAngle;
		tout.tiltAngle = t.tiltAngle;
	}

	// ###########################
	// #### Rotation inverses ####
	// ###########################

	// Inverse: Rotation matrix
	void RotmatInv(const Rotmat& R, Rotmat& Rinv)
	{
		// Calculate the inverse of the rotation
		Rinv = R.transpose();
	}

	// Inverse: Quaternion
	void QuatInv(const Quat& q, Quat& qinv)
	{
		// Calculate the inverse of the rotation
		qinv.w() = q.w();
		qinv.x() = -q.x();
		qinv.y() = -q.y();
		qinv.z() = -q.z();
	}

	// Inverse: Euler angles
	void EulerInv(const EulerAngles& e, EulerAngles& einv)
	{
		// Precalculate the required sin and cos values
		double cpsi = cos(e.yaw);
		double spsi = sin(e.yaw);
		double cth = cos(e.pitch);
		double sth = sin(e.pitch);
		double cphi = cos(e.roll);
		double sphi = sin(e.roll);

		// Calculate the sine of the inverse pitch angle
		double sthinv = -(cpsi*sth*cphi + spsi*sphi);
		sthinv = (sthinv >= 1.0 ? 1.0 : (sthinv <= -1.0 ? -1.0 : sthinv)); // Coerce sthinv to [-1,1]

		// Calculate the required inverse Euler angles representation
		einv.yaw = atan2(cpsi*sth*sphi - spsi*cphi, cpsi*cth);
		einv.pitch = asin(sthinv);
		einv.roll = atan2(spsi*sth*cphi - cpsi*sphi, cth*cphi);
	}

	// Inverse: Fused angles
	void FusedInv(const FusedAngles& f, FusedAngles& finv)
	{
		// Precalculate the sin values
		double sth  = sin(f.fusedPitch);
		double sphi = sin(f.fusedRoll);

		// Calculate the sine of the tilt angle alpha
		double crit = sth*sth + sphi*sphi;
		double salpha = (crit >= 1.0 ? 1.0 : sqrt(crit));

		// Calculate the tilt axis gamma
		double gamma = atan2(sth,sphi);

		// Precalculate trigonometric values
		double psigam = f.fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);

		// Calculate the inverse fused pitch and roll
		double thinv = asin(-salpha*spsigam);
		double phinv = asin(-salpha*cpsigam);

		// Construct the inverse fused angles rotation
		finv.fusedYaw = -f.fusedYaw;
		finv.fusedPitch = thinv;
		finv.fusedRoll = phinv;
		finv.hemi = f.hemi;
	}

	// Inverse: Tilt angles
	void TiltInv(const TiltAngles& t, TiltAngles& tinv)
	{
		// Calculate the inverse tilt axis angle
		double gammainv = t.fusedYaw + t.tiltAxisAngle - M_PI;
		gammainv += M_2PI*std::floor((M_PI - gammainv) / M_2PI);

		// Construct the inverse tilt angles rotation
		tinv.fusedYaw = -t.fusedYaw;
		tinv.tiltAxisAngle = gammainv;
		tinv.tiltAngle = t.tiltAngle;
	}

	// ##########################
	// #### Vector rotations ####
	// ##########################

	// Rotate vector by: Rotation matrix
	Vec3 RotmatRotVec(const Rotmat& R, const Vec3& v)
	{
		// Return the required rotated vector
		return R*v;
	}

	// Rotate vector by: Quaternion
	Vec3 QuatRotVec(const Quat& q, const Vec3& v)
	{
		// Precalculate an intermediate vector term
		double tx = 2.0*(q.y()*v.z() - v.y()*q.z());
		double ty = 2.0*(q.z()*v.x() - v.z()*q.x());
		double tz = 2.0*(q.x()*v.y() - v.x()*q.y());

		// Calculate and return the required vector
		Vec3 vout = v;
		vout.x() += q.w()*tx + q.y()*tz - ty*q.z();
		vout.y() += q.w()*ty + q.z()*tx - tz*q.x();
		vout.z() += q.w()*tz + q.x()*ty - tx*q.y();
		return vout;
	}

	// Rotate vector by: Euler angles
	Vec3 EulerRotVec(const EulerAngles& e, const Vec3& v)
	{
		// Return the required rotated vector
		return RotmatFromEuler(e)*v;
	}

	// Rotate vector by: Fused angles
	Vec3 FusedRotVec(const FusedAngles& f, const Vec3& v)
	{
		// Return the required rotated vector
		return RotmatFromFused(f)*v;
	}

	// Rotate vector by: Tilt angles
	Vec3 TiltRotVec(const TiltAngles& t, const Vec3& v)
	{
		// Return the required rotated vector
		return RotmatFromTilt(t)*v;
	}

	// ##############################
	// #### Pure yaw conversions ####
	// ##############################

	// Conversion: Pure yaw --> Rotation matrix
	void RotmatFromYaw(double yaw, Rotmat& R)
	{
		// Precalculate trigonometric values
		double cyaw = cos(yaw);
		double syaw = sin(yaw);

		// Set the required rotation matrix
		R << cyaw, -syaw, 0.0, syaw, cyaw, 0.0, 0.0, 0.0, 1.0;
	}

	// Conversion: Pure yaw --> Quaternion
	void QuatFromYaw(double yaw, Quat& q)
	{
		// Precalculate the sin values
		double hpsi = 0.5*yaw;
		double chpsi = cos(hpsi);
		double shpsi = sin(hpsi);

		// Set the required quaternion orientation
		q.w() = chpsi;
		q.x() = 0.0;
		q.y() = 0.0;
		q.z() = shpsi;
	}

	// ############################################
	// #### Conversions from rotation matrices ####
	// ############################################

	//
	// Conversion: Rotation matrix --> Quaternion
	//

	// Conversion: Rotation matrix --> Quaternion
	void QuatFromRotmat(const Rotmat& R, Quat& q)
	{
		// Perform the required conversion in a numerically stable manner
		double r, s, t = R.coeff(0,0) + R.coeff(1,1) + R.coeff(2,2);
		if(t >= 0.0)
		{
			r = sqrt(1.0 + t);
			s = 0.5/r;
			q.w() = 0.5*r;
			q.x() = s*(R.coeff(2,1) - R.coeff(1,2));
			q.y() = s*(R.coeff(0,2) - R.coeff(2,0));
			q.z() = s*(R.coeff(1,0) - R.coeff(0,1));
		}
		else if(R.coeff(2,2) >= R.coeff(1,1) && R.coeff(2,2) >= R.coeff(0,0))
		{
			r = sqrt(1.0 - (R.coeff(0,0) + R.coeff(1,1) - R.coeff(2,2)));
			s = 0.5/r;
			q.w() = s*(R.coeff(1,0) - R.coeff(0,1));
			q.x() = s*(R.coeff(0,2) + R.coeff(2,0));
			q.y() = s*(R.coeff(2,1) + R.coeff(1,2));
			q.z() = 0.5*r;
		}
		else if(R.coeff(1,1) >= R.coeff(0,0))
		{
			r = sqrt(1.0 - (R.coeff(0,0) - R.coeff(1,1) + R.coeff(2,2)));
			s = 0.5/r;
			q.w() = s*(R.coeff(0,2) - R.coeff(2,0));
			q.x() = s*(R.coeff(1,0) + R.coeff(0,1));
			q.y() = 0.5*r;
			q.z() = s*(R.coeff(2,1) + R.coeff(1,2));
		}
		else
		{
			r = sqrt(1.0 + (R.coeff(0,0) - R.coeff(1,1) - R.coeff(2,2)));
			s = 0.5/r;
			q.w() = s*(R.coeff(2,1) - R.coeff(1,2));
			q.x() = 0.5*r;
			q.y() = s*(R.coeff(1,0) + R.coeff(0,1));
			q.z() = s*(R.coeff(0,2) + R.coeff(2,0));
		}
	}

	//
	// Conversion: Rotation matrix --> Euler angles
	//

	// Conversion: Rotation matrix --> Euler angles
	void EulerFromRotmat(const Rotmat& R, double& yaw, double& pitch, double& roll)
	{
		// Calculate the sine of the pitch angle
		double sth = -R.coeff(2,0);
		sth = (sth >= 1.0 ? 1.0 : (sth <= -1.0 ? -1.0 : sth)); // Coerce sth to [-1,1]

		// Calculate the required Euler angles
		yaw = atan2(R.coeff(1,0), R.coeff(0,0));
		pitch = asin(sth);
		roll = atan2(R.coeff(2,1), R.coeff(2,2));
	}

	//
	// Conversion: Rotation matrix --> Fused angles
	//

	// Conversion: Rotation matrix --> Fused angles (2D)
	void FusedFromRotmat(const Rotmat& R, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused pitch and roll
		double stheta = -R.coeff(2,0);
		double sphi   = R.coeff(2,1);
		stheta = (stheta >= 1.0 ? 1.0 : (stheta <= -1.0 ? -1.0 : stheta)); // Coerce stheta to [-1,1]
		sphi   = (sphi   >= 1.0 ? 1.0 : (sphi   <= -1.0 ? -1.0 : sphi  )); // Coerce sphi   to [-1,1]
		fusedPitch = asin(stheta);
		fusedRoll  = asin(sphi);
	}

	// Conversion: Rotation matrix --> Fused angles (3D)
	void FusedFromRotmat(const Rotmat& R, double& fusedYaw, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused yaw, pitch and roll
		fusedYaw = FYawOfRotmat(R);
		FusedFromRotmat(R, fusedPitch, fusedRoll);
	}

	// Conversion: Rotation matrix --> Fused angles (4D)
	void FusedFromRotmat(const Rotmat& R, double& fusedYaw, double& fusedPitch, double& fusedRoll, bool& hemi)
	{
		// Calculate the fused yaw, pitch and roll
		fusedYaw = FYawOfRotmat(R);
		FusedFromRotmat(R, fusedPitch, fusedRoll);

		// Calculate the hemisphere of the rotation
		hemi = (R.coeff(2,2) >= 0.0);
	}

	//
	// Conversion: Rotation matrix --> Tilt angles
	//

	// Conversion: Rotation matrix --> Tilt angles (2D)
	void TiltFromRotmat(const Rotmat& R, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the tilt axis angle
		tiltAxisAngle = atan2(-R.coeff(2,0), R.coeff(2,1));

		// Calculate the tilt angle
		double calpha = R.coeff(2,2);
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		tiltAngle = acos(calpha);
	}

	// Conversion: Rotation matrix --> Tilt angles (3D)
	void TiltFromRotmat(const Rotmat& R, double& fusedYaw, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the fused yaw, tilt axis angle and tilt angle
		fusedYaw = FYawOfRotmat(R);
		TiltFromRotmat(R, tiltAxisAngle, tiltAngle);
	}

	// ######################################
	// #### Conversions from quaternions ####
	// ######################################

	//
	// Conversion: Quaternion --> Rotation matrix
	//

	// Conversion: Quaternion --> Rotation matrix
	void RotmatFromQuat(const Quat& q, Rotmat& R)
	{
		// Construct the required rotation matrix
		R << 1.0 - 2.0*(q.y()*q.y() + q.z()*q.z()),       2.0*(q.x()*q.y() - q.z()*q.w()),       2.0*(q.x()*q.z() + q.y()*q.w()),
		           2.0*(q.x()*q.y() + q.z()*q.w()), 1.0 - 2.0*(q.x()*q.x() + q.z()*q.z()),       2.0*(q.y()*q.z() - q.x()*q.w()),
		           2.0*(q.x()*q.z() - q.y()*q.w()),       2.0*(q.y()*q.z() + q.x()*q.w()), 1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
	}

	//
	// Conversion: Quaternion --> Euler angles
	//

	// Conversion: Quaternion --> Euler angles
	void EulerFromQuat(const Quat& q, double& yaw, double& pitch, double& roll)
	{
		// Calculate the sine of the pitch angle
		double sth = 2.0*(q.w()*q.y() - q.x()*q.z());
		sth = (sth >= 1.0 ? 1.0 : (sth <= -1.0 ? -1.0 : sth)); // Coerce sth to [-1,1]

		// Calculate the required Euler angles
		double qysq = q.y()*q.y();
		yaw = atan2(q.x()*q.y() + q.z()*q.w(), 0.5 - (qysq + q.z()*q.z()));
		pitch = asin(sth);
		roll = atan2(q.y()*q.z() + q.x()*q.w(), 0.5 - (q.x()*q.x() + qysq));
	}

	//
	// Conversion: Quaternion --> Fused angles
	//

	// Conversion: Quaternion --> Fused angles (2D)
	void FusedFromQuat(const Quat& q, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused pitch and roll
		double stheta = 2.0*(q.y()*q.w() - q.x()*q.z());
		double sphi   = 2.0*(q.y()*q.z() + q.x()*q.w());
		stheta = (stheta >= 1.0 ? 1.0 : (stheta <= -1.0 ? -1.0 : stheta)); // Coerce stheta to [-1,1]
		sphi   = (sphi   >= 1.0 ? 1.0 : (sphi   <= -1.0 ? -1.0 : sphi  )); // Coerce sphi   to [-1,1]
		fusedPitch = asin(stheta);
		fusedRoll  = asin(sphi);
	}

	// Conversion: Quaternion --> Fused angles (3D)
	void FusedFromQuat(const Quat& q, double& fusedYaw, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused yaw, pitch and roll
		FusedFromQuat(q, fusedYaw);
		FusedFromQuat(q, fusedPitch, fusedRoll);
	}

	// Conversion: Quaternion --> Fused angles (4D)
	void FusedFromQuat(const Quat& q, double& fusedYaw, double& fusedPitch, double& fusedRoll, bool& hemi)
	{
		// Calculate the fused yaw, pitch and roll
		FusedFromQuat(q, fusedYaw);
		FusedFromQuat(q, fusedPitch, fusedRoll);

		// Calculate the hemisphere of the rotation
		hemi = (0.5 - (q.x()*q.x() + q.y()*q.y()) >= 0.0);
	}

	//
	// Conversion: Quaternion --> Tilt angles
	//

	// Conversion: Quaternion --> Tilt angles (2D)
	void TiltFromQuat(const Quat& q, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the tilt axis angle
		tiltAxisAngle = atan2(q.w()*q.y() - q.x()*q.z(), q.w()*q.x() + q.y()*q.z());

		// Calculate the tilt angle
		double calpha = 1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		tiltAngle = acos(calpha);
	}

	// Conversion: Quaternion --> Tilt angles (3D)
	void TiltFromQuat(const Quat& q, double& fusedYaw, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the fused yaw, tilt axis angle and tilt angle
		FusedFromQuat(q, fusedYaw);
		TiltFromQuat(q, tiltAxisAngle, tiltAngle);
	}

	//
	// Conversion: Quaternion --> Z vector
	//

	// Conversion: Quaternion --> Z vector
	void ZVecFromQuat(const Quat& q, ZVec& z)
	{
		// Calculate the required Z vector
		z.x() = 2.0*(q.x()*q.z() - q.y()*q.w());
		z.y() = 2.0*(q.y()*q.z() + q.x()*q.w());
		z.z() = 1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
	}

	// #######################################
	// #### Conversions from Euler angles ####
	// #######################################

	// Conversion: Euler angles --> Rotation matrix
	Rotmat RotmatFromEuler(double yaw, double pitch, double roll)
	{
		// Precalculate the trigonometric values
		double cpsi = cos(yaw);
		double spsi = sin(yaw);
		double cth  = cos(pitch);
		double sth  = sin(pitch);
		double cphi = cos(roll);
		double sphi = sin(roll);

		// Calculate and return the required rotation matrix
		Rotmat R;
		R << cpsi*cth, cpsi*sth*sphi - spsi*cphi, cpsi*sth*cphi + spsi*sphi,
		     spsi*cth, spsi*sth*sphi + cpsi*cphi, spsi*sth*cphi - cpsi*sphi,
		         -sth,                  cth*sphi,                  cth*cphi;
		return R;
	}

	// Conversion: Euler angles --> Quaternion
	Quat QuatFromEuler(double yaw, double pitch, double roll)
	{
		// Halve the Euler angles
		double hpsi = 0.5*yaw;
		double hth = 0.5*pitch;
		double hphi = 0.5*roll;

		// Precalculate the trigonometric values
		double hcpsi = cos(hpsi);
		double hspsi = sin(hpsi);
		double hcth  = cos(hth);
		double hsth  = sin(hth);
		double hcphi = cos(hphi);
		double hsphi = sin(hphi);

		// Calculate and return the required quaternion
		return Quat(hcphi*hcth*hcpsi + hsphi*hsth*hspsi, hsphi*hcth*hcpsi - hcphi*hsth*hspsi, hcphi*hsth*hcpsi + hsphi*hcth*hspsi, hcphi*hcth*hspsi - hsphi*hsth*hcpsi);
	}

	// Conversion: Euler angles --> Fused angles
	FusedAngles FusedFromEuler(double yaw, double pitch, double roll)
	{
		// Construct a fused angles object
		FusedAngles f;

		// Calculation of the fused yaw in a numerically stable manner requires the complete rotation matrix representation
		Rotmat R = RotmatFromEuler(yaw, pitch, roll);

		// Calculate the fused yaw
		f.fusedYaw = FYawOfRotmat(R);

		// Calculate the fused pitch
		f.fusedPitch = pitch; // ZYX Euler pitch is equivalent to fused pitch!

		// Calculate the fused roll
		double sphi = R.coeff(2,1);
		sphi = (sphi >= 1.0 ? 1.0 : (sphi <= -1.0 ? -1.0 : sphi)); // Coerce sphi to [-1,1]
		f.fusedRoll  = asin(sphi);

		// See which hemisphere we're in
		f.hemi = (R.coeff(2,2) >= 0.0);

		// Return the calculated fused angles
		return f;
	}

	// Conversion: Euler angles --> Tilt angles
	TiltAngles TiltFromEuler(double yaw, double pitch, double roll)
	{
		// Construct a tilt angles object
		TiltAngles t;

		// Calculation of the fused yaw in a numerically stable manner requires the complete rotation matrix representation
		Rotmat R = RotmatFromEuler(yaw, pitch, roll);

		// Calculate the fused yaw
		t.fusedYaw = FYawOfRotmat(R);

		// Calculate the tilt axis angle
		t.tiltAxisAngle = atan2(-R.coeff(2,0), R.coeff(2,1));

		// Calculate the tilt angle
		double calpha = R.coeff(2,2);
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		t.tiltAngle = acos(calpha);

		// Return the calculated tilt angles
		return t;
	}

	// Conversion: Euler angles --> Z vector
	ZVec ZVecFromEuler(double pitch, double roll)
	{
		// Precalculate the trigonometric values
		double cth  = cos(pitch);
		double sth  = sin(pitch);
		double cphi = cos(roll);
		double sphi = sin(roll);

		// Calculate and return the required Z vector
		return ZVec(-sth, cth*sphi, cth*cphi);
	}

	// #######################################
	// #### Conversions from fused angles ####
	// #######################################

	//
	// Conversion: Fused angles --> Rotation matrix
	//

	// Conversion: Fused angles (2D) --> Rotation matrix
	Rotmat RotmatFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;
		double calpha = (crit >= 1.0 ? 0.0 : sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms involved in the rotation matrix expression
		double calphabar = 1.0 - calpha;
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double A = calpha + calphabar*cgam*cgam;
		double B = calpha + calphabar*sgam*sgam;
		double C = calphabar*cgam*sgam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R <<    A,    C,    sth,
		        C,    B,  -sphi,
		     -sth, sphi, calpha;
		return R;
	}

	// Conversion: Fused angles (3D/4D) --> Rotation matrix
	Rotmat RotmatFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the tilt angle alpha
		double calpha, salpha;
		if(crit >= 1.0)
		{
			calpha = 0.0;
			salpha = 1.0;
		}
		else
		{
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));
			salpha = sqrt(crit);
		}

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms involved in the rotation matrix expression
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double psigam = fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = cgam*cpsigam;
		double B = sgam*cpsigam;
		double C = cgam*spsigam;
		double D = sgam*spsigam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R << A + D*calpha, B - C*calpha,  salpha*spsigam,
		     C - B*calpha, D + A*calpha, -salpha*cpsigam,
		          -sth,          sphi,    calpha;
		return R;
	}

	//
	// Conversion: Fused angles --> Quaternion
	//

	// Conversion: Fused angles (2D) --> Quaternion
	Quat QuatFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the tilt angle alpha
		double alpha = (crit >= 1.0 ? M_PI_2 : acos(sqrt(1.0 - crit)));
		double halpha = 0.5*alpha;
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);
		double cgamma = cos(gamma);
		double sgamma = sin(gamma);

		// Return the required quaternion orientation (a rotation about (cgamma, sgamma, 0) by angle alpha)
		return Quat(chalpha, cgamma*shalpha, sgamma*shalpha, 0.0); // Order: (w,x,y,z)
	}

	// Conversion: Fused angles (3D/4D) --> Quaternion
	Quat QuatFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the tilt angle alpha
		double alpha;
		if(crit >= 1.0)
			alpha = M_PI_2;
		else
			alpha = acos(hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Evaluate the required intermediate angles
		double halpha = 0.5*alpha;
		double hpsi = 0.5*fusedYaw;
		double hgampsi = gamma + hpsi;

		// Precalculate trigonometric terms involved in the quaternion expression
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);
		double chpsi = cos(hpsi);
		double shpsi = sin(hpsi);
		double chgampsi = cos(hgampsi);
		double shgampsi = sin(hgampsi);

		// Calculate and return the required quaternion
		return Quat(chalpha*chpsi, shalpha*chgampsi, shalpha*shgampsi, chalpha*shpsi); // Order: (w,x,y,z)
	}

	//
	// Conversion: Fused angles --> Euler angles
	//

	// Conversion: Fused angles (2D) --> Euler angles
	EulerAngles EulerFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = (crit >= 1.0 ? 0.0 : sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double A = cgam*(1.0 - calpha);

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(A*sgam, calpha + A*cgam), fusedPitch, atan2(sphi, calpha));
	}

	// Conversion: Fused angles (3D/4D) --> Euler angles
	EulerAngles EulerFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = 0.0;
		if(crit < 1.0)
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double psigam = fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = sgam*calpha;

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam), fusedPitch, atan2(sphi, calpha));
	}

	//
	// Conversion: Fused angles --> Tilt angles
	//

	// Conversion: Fused angles (2D) --> Tilt angles
	TiltAngles TiltFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Construct a tilt angles object
		TiltAngles t;

		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = (crit >= 1.0 ? 0.0 : sqrt(1.0 - crit));

		// Calculate and return the tilt angles representation
		t.fusedYaw = 0.0;
		t.tiltAxisAngle = atan2(sth,sphi);
		t.tiltAngle = acos(calpha);
		return t;
	}

	// Conversion: Fused angles (3D/4D) --> Tilt angles
	TiltAngles TiltFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Construct a tilt angles object
		TiltAngles t;

		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = 0.0;
		if(crit < 1.0)
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Calculate and return the tilt angles representation
		t.fusedYaw = fusedYaw;
		t.tiltAxisAngle = atan2(sth,sphi);
		t.tiltAngle = acos(calpha);
		return t;
	}

	//
	// Conversion: Fused angles --> Z vector
	//

	// Conversion: Fused angles --> Z vector
	ZVec ZVecFromFused(double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sin values
		double sth  = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = 0.0;
		if(crit < 1.0)
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Return the required Z vector
		return ZVec(-sth, sphi, calpha);
	}

	// ######################################
	// #### Conversions from tilt angles ####
	// ######################################

	//
	// Conversion: Tilt angles --> Rotation matrix
	//

	// Conversion: Tilt angles (2D) --> Rotation matrix
	Rotmat RotmatFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Precalculate terms involved in the rotation matrix expression
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double calphabar = 1.0 - calpha;
		double sth = salpha*sgam;
		double sphi = salpha*cgam;
		double A = calpha + calphabar*cgam*cgam;
		double B = calpha + calphabar*sgam*sgam;
		double C = calphabar*cgam*sgam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R <<    A,    C,    sth,
		        C,    B,  -sphi,
		     -sth, sphi, calpha;
		return R;
	}

	// Conversion: Tilt angles (3D) --> Rotation matrix
	Rotmat RotmatFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate terms involved in the rotation matrix expression
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double psigam = fusedYaw + tiltAxisAngle;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = cgam*cpsigam;
		double B = sgam*cpsigam;
		double C = cgam*spsigam;
		double D = sgam*spsigam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R << A + D*calpha, B - C*calpha,  salpha*spsigam,
		     C - B*calpha, D + A*calpha, -salpha*cpsigam,
		     -sgam*salpha,  cgam*salpha,  calpha;
		return R;
	}

	//
	// Conversion: Tilt angles --> Quaternion
	//

	// Conversion: Tilt angles (2D) --> Quaternion
	Quat QuatFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Precalculate the required angles
		double halpha = 0.5*tiltAngle;

		// Precalculate the required trigonometric values
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);
		double cgamma = cos(tiltAxisAngle);
		double sgamma = sin(tiltAxisAngle);

		// Return the required quaternion orientation
		return Quat(chalpha, shalpha*cgamma, shalpha*sgamma, 0.0); // Order: (w,x,y,z)
	}

	// Conversion: Tilt angles (3D) --> Quaternion
	Quat QuatFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate the required angles
		double hpsi = 0.5*fusedYaw;
		double halpha = 0.5*tiltAngle;
		double hgampsi = tiltAxisAngle + hpsi;

		// Precalculate the required trigonometric values
		double chpsi = cos(hpsi);
		double shpsi = sin(hpsi);
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);
		double chgampsi = cos(hgampsi);
		double shgampsi = sin(hgampsi);

		// Return the required quaternion orientation
		return Quat(chalpha*chpsi, shalpha*chgampsi, shalpha*shgampsi, chalpha*shpsi); // Order: (w,x,y,z)
	}

	//
	// Conversion: Tilt angles --> Euler angles
	//

	// Conversion: Tilt angles (2D) --> Euler angles
	EulerAngles EulerFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Precalculate terms
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double sth = sgam*salpha;
		double sphi = cgam*salpha;
		double A = cgam*(1.0 - calpha);

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(A*sgam, calpha + A*cgam), asin(sth), atan2(sphi, calpha));
	}

	// Conversion: Tilt angles (3D) --> Euler angles
	EulerAngles EulerFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate terms
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double sth = sgam*salpha;
		double sphi = cgam*salpha;
		double psigam = fusedYaw + tiltAxisAngle;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = sgam*calpha;

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam), asin(sth), atan2(sphi, calpha));
	}

	//
	// Conversion: Tilt angles --> Fused angles
	//

	// Conversion: Tilt angles (2D) --> Fused angles
	FusedAngles FusedFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Calculate and return the fused angles representation
		return FusedFromTilt(0.0, tiltAxisAngle, tiltAngle);
	}

	// Conversion: Tilt angles (3D) --> Fused angles
	FusedAngles FusedFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Construct a fused angles object
		FusedAngles f;

		// Precalculate terms
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double salpha = sin(tiltAngle);

		// Calculate and return the fused angles representation
		f.fusedYaw = fusedYaw;
		f.fusedPitch = asin(salpha*sgam);
		f.fusedRoll = asin(salpha*cgam);
		f.hemi = (tiltAngle <= M_PI_2);
		return f;
	}

	//
	// Conversion: Tilt angles --> Z vector
	//

	// Conversion: Tilt angles --> Z vector
	ZVec ZVecFromTilt(double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate the required trigonometric terms
		double cgamma = cos(tiltAxisAngle);
		double sgamma = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);

		// Return the required Z vector
		return ZVec(-salpha*sgamma, salpha*cgamma, calpha);
	}

	// ####################################
	// #### Conversions from Z vectors ####
	// ####################################

	//
	// Conversion: Z vector --> Fused angles
	//

	// Conversion: Z vector --> Fused angles (2D)
	void FusedFromZVec(const ZVec& z, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused pitch and roll
		double stheta = -z.x();
		double sphi   = z.y();
		stheta = (stheta >= 1.0 ? 1.0 : (stheta <= -1.0 ? -1.0 : stheta)); // Coerce stheta to [-1,1]
		sphi   = (sphi   >= 1.0 ? 1.0 : (sphi   <= -1.0 ? -1.0 : sphi  )); // Coerce sphi   to [-1,1]
		fusedPitch = asin(stheta);
		fusedRoll  = asin(sphi);
	}

	// Conversion: Z vector --> Fused angles (3D)
	void FusedFromZVec(const ZVec& z, double& fusedPitch, double& fusedRoll, bool& hemi)
	{
		// Calculate the fused pitch and roll
		FusedFromZVec(z, fusedPitch, fusedRoll);

		// Calculate the hemisphere
		hemi = (z.z() >= 0.0);
	}

	//
	// Conversion: Z vector --> Tilt angles
	//

	// Conversion: Z vector --> Tilt angles (2D)
	void TiltFromZVec(const ZVec& z, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the tilt axis angle
		tiltAxisAngle = atan2(-z.x(), z.y());

		// Calculate the tilt angle
		double calpha = z.z();
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		tiltAngle = acos(calpha);
	}
}
// EOF


#define BOOST_TEST_MODULE pcraster geo average_filter
#include <boost/test/unit_test.hpp>
#include "geo_averagefilter.h"
#include "geo_filterengine.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace geo;

  //  1  2  3  4  5
  //  6  7  8  9 10
  // 11 12 13 14 15
  // 16 17 MV 19 20
  // 21 22 23 24 25

  {
    // Create source raster.
    SimpleRaster<double> source(5, 5);
    source.cell(0, 0) = 1;
    source.cell(0, 1) = 2;
    source.cell(0, 2) = 3;
    source.cell(0, 3) = 4;
    source.cell(0, 4) = 5;
    source.cell(1, 0) = 6;
    source.cell(1, 1) = 7;
    source.cell(1, 2) = 8;
    source.cell(1, 3) = 9;
    source.cell(1, 4) = 10;
    source.cell(2, 0) = 11;
    source.cell(2, 1) = 12;
    source.cell(2, 2) = 13;
    source.cell(2, 3) = 14;
    source.cell(2, 4) = 15;
    source.cell(3, 0) = 16;
    source.cell(3, 1) = 17;
    pcr::setMV(source.cell(3, 2));
    source.cell(3, 3) = 19;
    source.cell(3, 4) = 20;
    source.cell(4, 0) = 21;
    source.cell(4, 1) = 22;
    source.cell(4, 2) = 23;
    source.cell(4, 3) = 24;
    source.cell(4, 4) = 25;

    // Filter for calculating average within kernel.
    SimpleRaster<double> weights(3, 3, 1.0);
    AverageFilter filter(weights);

    // Destination raster.
    SimpleRaster<double> destination(5, 5);

    FilterEngine<double, double> engine(source, filter, destination);
    engine.calc();

    BOOST_CHECK(destination.cell(0, 0) == 4.0);
    BOOST_CHECK(destination.cell(0, 1) == 4.5);
    BOOST_CHECK(destination.cell(0, 2) == 5.5);
    BOOST_CHECK(destination.cell(0, 3) == 6.5);
    BOOST_CHECK(destination.cell(0, 4) == 7.0);

    BOOST_CHECK(destination.cell(1, 0) == 6.5);
    BOOST_CHECK(destination.cell(1, 1) == 7.0);
    BOOST_CHECK(destination.cell(1, 2) == 8.0);
    BOOST_CHECK(destination.cell(1, 3) == 9.0);
    BOOST_CHECK(destination.cell(1, 4) == 9.5);

    BOOST_CHECK(destination.cell(2, 0) == 11.5);
    BOOST_CHECK(destination.cell(2, 1) == 11.25);
    BOOST_CHECK(destination.cell(2, 2) == 12.375);
    BOOST_CHECK(destination.cell(2, 3) == 13.5);
    BOOST_CHECK(destination.cell(2, 4) == 14.5);

    BOOST_CHECK(destination.cell(3, 0) == 16.5);
    BOOST_CHECK(destination.cell(3, 1) == 16.875);
    BOOST_CHECK(pcr::isMV(destination.cell(3, 2)));
    BOOST_CHECK(destination.cell(3, 3) == 19.125);
    BOOST_CHECK(destination.cell(3, 4) == 19.5);

    BOOST_CHECK(destination.cell(4, 0) == 19.0);
    BOOST_CHECK(destination.cell(4, 1) == 19.8);
    BOOST_CHECK(destination.cell(4, 2) == 21);
    BOOST_CHECK(destination.cell(4, 3) == 22.2);
    BOOST_CHECK(destination.cell(4, 4) == 22);
  }

  {
    // Create raster:
    // MV MV MV
    // MV MV  6
    // MV MV MV
    SimpleRaster<double> source(3, 3);
    pcr::setMV(source.cell(0, 0));
    pcr::setMV(source.cell(0, 1));
    pcr::setMV(source.cell(0, 2));
    pcr::setMV(source.cell(1, 0));
    pcr::setMV(source.cell(1, 1));
    source.cell(1, 2) = 6.0;
    pcr::setMV(source.cell(2, 0));
    pcr::setMV(source.cell(2, 1));
    pcr::setMV(source.cell(2, 2));

    // Filter for calculating average within kernel.
    SimpleRaster<double> weights(3, 3, 1.0);
    AverageFilter filter(weights);

    // Destination raster.
    SimpleRaster<double> destination(3, 3);

    FilterEngine<double, double> engine(source, filter, destination);
    engine.calc();

    BOOST_CHECK(pcr::isMV(destination.cell(0, 0)));
    BOOST_CHECK(pcr::isMV(destination.cell(0, 1)));
    BOOST_CHECK(pcr::isMV(destination.cell(0, 2)));
    BOOST_CHECK(pcr::isMV(destination.cell(1, 0)));
    BOOST_CHECK(pcr::isMV(destination.cell(1, 1)));
    BOOST_CHECK(destination.cell(1, 2) == 6.0);
    BOOST_CHECK(pcr::isMV(destination.cell(2, 0)));
    BOOST_CHECK(pcr::isMV(destination.cell(2, 1)));
    BOOST_CHECK(pcr::isMV(destination.cell(2, 2)));
  }

  {
    // Create raster:
    // MV MV MV
    // MV MV  6
    // MV MV MV
    SimpleRaster<double> source(3, 3);
    pcr::setMV(source.cell(0, 0));
    pcr::setMV(source.cell(0, 1));
    pcr::setMV(source.cell(0, 2));
    pcr::setMV(source.cell(1, 0));
    pcr::setMV(source.cell(1, 1));
    source.cell(1, 2) = 6.0;
    pcr::setMV(source.cell(2, 0));
    pcr::setMV(source.cell(2, 1));
    pcr::setMV(source.cell(2, 2));

    // Calculate average values for current and cell to the right.
    // 0 0 0
    // 0 1 1
    // 0 0 0
    SimpleRaster<double> weights(3, 3, 0.0);
    weights.cell(1, 1) = 1.0;
    weights.cell(1, 2) = 1.0;
    AverageFilter filter(weights);

    // Destination raster.
    SimpleRaster<double> destination(3, 3);

    FilterEngine<double, double> engine(source, filter, destination);
    engine.calc();

    BOOST_CHECK(pcr::isMV(destination.cell(0, 0)));
    BOOST_CHECK(pcr::isMV(destination.cell(0, 1)));
    BOOST_CHECK(pcr::isMV(destination.cell(0, 2)));
    BOOST_CHECK(pcr::isMV(destination.cell(1, 0)));
    BOOST_CHECK(pcr::isMV(destination.cell(1, 1)));
    BOOST_CHECK(destination.cell(1, 2) == 6.0);
    BOOST_CHECK(pcr::isMV(destination.cell(2, 0)));
    BOOST_CHECK(pcr::isMV(destination.cell(2, 1)));
    BOOST_CHECK(pcr::isMV(destination.cell(2, 2)));
  }
}


#include <iostream>
#include <chrono>
#include <FreeImage.h>
#include <Eigen/Core>

#define WIDTH 1920
#define HEIGHT 1080
#define BITS_PER_PIXEL 24
#define OUTPUT_FILE "output.png"

using namespace std;
typedef FIBITMAP Image;
typedef RGBQUAD Colour;

int main() {
    FreeImage_Initialise();

    Image * image = FreeImage_Allocate(WIDTH, HEIGHT, BITS_PER_PIXEL);
    if (!image) exit(EXIT_FAILURE);

    Colour colour{0,0,0,1};

    auto start = chrono::steady_clock::now();

    //
    // INSERT IMAGE RENDERING CODE HERE!
    //

    Eigen::MatrixXf m = Eigen::MatrixXf::Random(HEIGHT, WIDTH);

    for (unsigned int y = 0; y < HEIGHT; ++y)
        for (unsigned int x = 0; x < WIDTH; ++x)
        {
            colour.rgbRed   = m(y,x) * 255;
            colour.rgbGreen = m(y,x) * 255;
            colour.rgbBlue  = m(y,x) * 255;
            FreeImage_SetPixelColor(image, x, y, &colour);
        }

    //
    // REPLACE THE IMAGE RENDERING CODE ABOVE!
    //

    auto end = chrono::steady_clock::now();
    auto diff = end - start;
    cout << "Render time = " << chrono::duration <double, milli> (diff).count() / 1000.0 << "s" << endl;

    if (FreeImage_Save(FIF_PNG, image, OUTPUT_FILE, 0))
        cout << "Image saved!" << endl;

    FreeImage_DeInitialise();
    exit(EXIT_SUCCESS);
}


/**
 * Test for the KalmanFilter class with 1D projectile motion.
 *
 * @author: Hayk Martirosyan
 * @date: 2014.11.15
 */
#include <ros/ros.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include "kalman.hpp"

#include <people_msgs/PositionMeasurement.h>
#include <visualization_msgs/Marker.h>

template <typename T> std::string tostr(const T& t)
{
    std::ostringstream os; os<<t; return os.str();
}

static std::string fixed_frame = "odom";

class KalmanTracker{
private:
    ros::Rate r;
    ros::Subscriber person_pos_sub;
    ros::Publisher markers_pub,text_pub,estimate_pos_pub;
    ros::NodeHandle nh1;
    ros::Time start,now;
    Eigen::VectorXd x,y;
    Eigen::MatrixXd A; // System dynamics matrix
    Eigen::MatrixXd C; // Output matrix
    Eigen::MatrixXd Q; // Process noise covariance
    Eigen::MatrixXd R; // Measurement noise covariance
    Eigen::MatrixXd P; // Estimate error covariance
    KalmanFilter estimate_X,estimate_Y;
    int n; // Number of states
    int m; // Number of measurements
    int init_f,update_f;
    double dt; // Time step
    double t; // Time
public:
    KalmanTracker(ros::NodeHandle nh):
    nh1(nh),r(10),n(3),m(1),dt(1.0/10),init_f(0)
    {
        x = Eigen::VectorXd(n);
        y = Eigen::VectorXd(n);
        A = Eigen::MatrixXd(n,n);
        C = Eigen::MatrixXd(m,n);
        Q = Eigen::MatrixXd(n,n);
        R = Eigen::MatrixXd(m,m);
        P = Eigen::MatrixXd(n,n);
        // Discrete LTI projectile motion, measuring position only
        A << 1, dt, 1/2*dt*dt, 0, 1, dt, 0, 0, 1;
        C << 1, 0, 0;
        // Reasonable covariance matrices
        Q << .05, .05, .0, .05, .05, .0, .0, .0, .0;
        R << 5;
        P << .1, .1, .1, .1, 10000, 10, .1, 10, 100;
        estimate_X=KalmanFilter(dt,A, C, Q, R, P);
        estimate_Y=KalmanFilter(dt,A, C, Q, R, P);
        estimate_X.init(0,x);
        estimate_Y.init(0,y);
        //start = now = ros::Time::now();
        person_pos_sub = nh1.subscribe("filter_measurement",1,&KalmanTracker::msgCallback_PeopleTracker, this);
        markers_pub = nh1.advertise<visualization_msgs::Marker>("filter_marker", 20);//Output of KF
        estimate_pos_pub = nh1.advertise<people_msgs::PositionMeasurement>("estimate_pos", 20);
        //text_pub = nh1.advertise<visualization_msgs::Marker>("tracking_text", 20);
        //estimate_pos_pub = nh1.advertise<geometry_msgs::PoseStamped>("human_pose", 20);

        std::cout << "A: \n" << A << std::endl;
        std::cout << "C: \n" << C << std::endl;
        std::cout << "Q: \n" << Q << std::endl;
        std::cout << "R: \n" << R << std::endl;
        std::cout << "P: \n" << P << std::endl;
  }

    void msgCallback_PeopleTracker(const people_msgs::PositionMeasurement::ConstPtr& msg )
    {
        if(msg->initialization == 1){
        // Best guess of initial states
            std::cout<<"---Initialization---"<<std::endl;
            //start = now = ros::Time::now();
            x <<msg->pos.x, 0.0, 0.0;
            estimate_X.init(t,x);
            y << msg->pos.y, 0.0, 0.0;
            estimate_Y.init(t,y);
            init_f=1;
        }
        else if(init_f==1){
            //now = ros::Time::now();
            std::cout<<"Update position"<<std::endl;
            Eigen::VectorXd z(m);
            z <<msg->pos.x;
            estimate_X.update(z);
            z <<msg->pos.y;
            estimate_Y.update(z);
        }

        //std::cout <<"x_hat=" << estimate_X.state().transpose()<<std::endl;
        //std::cout <<"y_hat=" << estimate_Y.state().transpose()<<std::endl;

        visualization_msgs::Marker m;
        m.header.stamp = ros::Time::now();
        m.header.frame_id = fixed_frame;
        m.type = m.SPHERE;
        m.scale.x = .6;
        m.scale.y = .6;
        m.scale.z = .6;
        m.color.a = 0.2;
        m.color.g = 0.5;
        m.pose.position.x = estimate_X.state()[0];
        m.pose.position.y = estimate_Y.state()[0];

        people_msgs::PositionMeasurement estimate_pos;
        estimate_pos.header.stamp = ros::Time::now();
        estimate_pos.header.frame_id = fixed_frame;
        estimate_pos.name = "tracking";
        estimate_pos.pos.x = estimate_X.state()[0];
        estimate_pos.pos.y = estimate_Y.state()[0];
        estimate_pos.pos.z = 0;
        //std::cout<<estimate_X.cov()<<std::endl;
        estimate_pos.covariance[0] = (estimate_X.cov()(0,0)+estimate_Y.cov()(0,0))/2;
        estimate_pos.covariance[1] = 0.0;
        estimate_pos.covariance[2] = 0.0;
        estimate_pos.covariance[3] = 0.0;
        estimate_pos.covariance[4] = (estimate_X.cov()(1,1)+estimate_Y.cov()(1,1))/2;
        estimate_pos.covariance[5] = 0.0;
        estimate_pos.covariance[6] = 0.0;
        estimate_pos.covariance[7] = 0.0;
        estimate_pos.covariance[8] = (estimate_X.cov()(2,2)+estimate_Y.cov()(2,2))/2;

        if(init_f==1){
            markers_pub.publish(m);
            estimate_pos_pub.publish(estimate_pos);
        }
    //r.sleep();
    }
};

// 購読者ノードのメイン関数
int main(int argc, char **argv)
{
    // ノード名の初期化
    ros::init(argc, argv, "person_tracking_filter_kalman");
    // ROSシステムとの通信のためのノードのハンドルを宣言
    ros::NodeHandle nh;
    KalmanTracker kt(nh);
    ros::spin();
    return 0;
}


#include <iostream>
#include <chrono>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main ()
{
    VectorXd vec1;

    // Testing Eigen matrix
    // Copied from: https://gitlab.com/libeigen/eigen/-/blob/master/doc/examples/QuickStart_example.cpp

    auto tsim1 = std::chrono::high_resolution_clock::now();

    MatrixXd m(2,2); // matrix composed by doubles.
    m(0,0) = 3;
    m(1,0) = 2.5;
    m(0,1) = -1;
    m(1,1) = m(1,0) + m(0,1);
    cout << m << endl;

    VectorXd v(2);
    v(0) = 4;
    v(1) = v(0) - 1;
    cout << "Here is the vector v:\n" << v << endl;

    cout << "Here is the vector v:\n" << v << endl;

    auto tsim2 = chrono::high_resolution_clock::now();
    chrono::duration<double, milli> elapsed_time = tsim2 - tsim1;
    cout << "measure time: " << elapsed_time.count() << endl;

    return 0;
}


#include <boost/test/unit_test.hpp>
#include <shz/math/frustum.hpp>

BOOST_AUTO_TEST_CASE(frustumConstructors)
{
    shz::math::matrix<shz::math::f32, 4, 4> projection;
	shz::math::frustum<shz::math::f32> f;

    projection = shz::math::matrix<shz::math::f32, 4, 4> ::ortho(-1, 1, -1, 1, 0.1f, 100.f);

    f.generate_planes_from_projection(projection);

    BOOST_CHECK(f.num_planes == 6);
}


#include <Rcpp.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include "bckPotentialsAsymp.hpp"
#include "interp_1d.hpp"

using namespace Rcpp;
using namespace std;
using namespace boost::numeric::odeint ;

// The stiff algorith only accepts these data types
typedef boost::numeric::ublas::vector< long double > state_type;
typedef boost::numeric::ublas::matrix< long double > matrix_type;

struct VQCD
{
    long double x;
    long double tau0;
    long double W0;
    long double V0;
    long double lambda0;
    VQCD():x(1.0), tau0(1.0), W0(12.0/11), V0(12.0), lambda0(8.0 * M_PI * M_PI){}
    VQCD(long double xi, long double ti, long double w0, long double v0, long double l0):x(xi),tau0(ti), W0(w0), V0(v0), lambda0(l0){}
    void operator()(const state_type &X , state_type &dXdt , long double A)
    {
        long double e2A = exp(2.0 * A);
        long double kNr = k(X[1], x) ;
        long double dkNr = dk(X[1],x);
        long double vf = Vf(X[1], X[2], x, W0) ;
        long double dvfdl = dVfdl(X[1], X[2], x, W0) ;
        long double dlogvfdt = dLogVfdt(X[2], x, W0) ;
        long double dlogvfdl = dLogVfdl(X[1], x, W0) ;
        long double G = sqrt(1.0 + kNr * pow(X[4], 2.0) / ( e2A * pow(X[0], 2.0 ) ) ) ;
        // 1st Aeom in Mathematica Notebook
        dXdt[0] = - X[0] + (4.0 / 9.0) * X[0] * pow(X[3], 2.0) / pow(X[1], 2.0) + x * e2A * pow(X[0], 3.0) * kNr * vf * pow(X[4], 2.0) * G / ( 6.0 * ( e2A * pow( X[0], 2.0 ) + kNr * pow(X[4], 2.0) ) ) ;
        dXdt[1] = X[3] ;
        dXdt[2] = X[4] ;
        // 2nd Aeom in Mathematica Notebook
        dXdt[3] = - (3.0 / 8) * e2A * pow( X[0] * X[1], 2.0 ) * dVg(X[1], V0, lambda0) + 9.0 * pow(X[1], 2.0) / X[3] \
        - 3 * e2A * pow(X[0] * X[1], 2.0) * Vg(X[1], V0, lambda0) / ( 4.0 * X[3] ) - 5.0 * X[3] + pow( X[3], 2.0) / X[1] \
        + ( 4.0 / 9) * pow(X[3], 3.0) / pow(X[1], 2.0) + 3 * e2A * x * pow( X[0] * X[1], 2.0) * vf / (4.0 * X[3] * G) \
        + x * kNr * vf * X[3] * pow( X[4], 2.0) / ( 6.0 * G) + 3.0 * x * vf * pow(X[1] * X[4], 2.0) * dkNr / ( 16.0 * G) \
        + 3 * e2A * x * pow(X[0] * X[1], 2.0) * dvfdl / ( 8.0 * G) + 3.0 * x * kNr * pow(X[1] * X[4], 2.0) * dvfdl / ( 8.0 * G) ;
        // 3rd Aeom in Mathematica Notebook
        dXdt[4] = - 4.0 * X[4] + ( 4.0 / 9 ) * pow( X[3] / X[1] , 2.0) * X[4] - 4.0 * kNr * pow(X[4], 3.0) / ( e2A * X[0] * X[0]) \
        + e2A * x * pow( X[0], 2.0 ) * kNr * vf * pow( X[4], 3.0) * G / ( 6 * ( e2A * pow(X[0],2.0) + kNr * pow(X[4], 2.0) ) )\
        - X[3] * X[4] * dkNr / kNr - X[3] * pow(X[4], 3.0) * dkNr / (2.0 * e2A * pow( X[0], 2.0) ) \
        + e2A * pow(X[0], 2.0) * dlogvfdt / kNr + pow(X[4], 2.0) * dlogvfdt - X[3] * X[4] * dlogvfdl \
        - kNr * X[3] * pow(X[4], 3.0) * dlogvfdl / ( e2A * pow(X[0], 2.0) ) ;
    }
};

// [[Rcpp::export]]
List solveHVQCD(long double xi = 1, long double ti = 1, long double W0 = 12/11.0, long double V0 = 12, long double lambda0 = 8 * M_PI * M_PI)
{
    // Computes dr/dA, tau, lambda, dtau/dA and dlambda/dA given x and tau0
    // x - long double. Physically it means x = N_f / N_c when N_f, N_c -> inf but with fixed coefficient
    // tau0 - > parameter related with the exponential behaviour of the tachyon in the IR
    // Returns a list with quantitites that depend on A, mq and zIR

    // Create a vectors containing the values of the fields
    vector< long double > Z, AA, dZ, L, T;
    // Boundary conditions in the IR
    long double zIR = log(70.0 / ti) / CI(xi, W0, V0, lambda0) ;
    long double aIR = AIR(zIR, V0, lambda0) ;
    AA.push_back(aIR) ;
    long double lambdair = lambdaIR(zIR, lambda0) ;
    L.push_back(lambdair);
    long double tauir = 70.0 ;
    T.push_back(tauir) ;
    // dA/dz at zIR
    long double daIR =  173.0 / ( 1728.0 * pow(zIR, 3.0)) + 0.5 /zIR - 2.0 * zIR  ;
    // dtau/dz at zIR
    long double dtauir = CI(xi, W0, V0, lambda0) * tauir ;
    // dlambda/dz at zIR. Expression got from eom2 in Mathematica notebook.
    long double dlambdair = sqrt(1.5) * lambdair * sqrt( 6 * pow(daIR, 2.0) + exp( 2.0 * aIR) * xi * Vf(lambdair,tauir, xi, W0) \
         / (2.0 * sqrt(1+ dtauir * dtauir * k(lambdair, xi) / exp(2 * aIR ) ) ) - 0.5 * exp( 2.0 * aIR) * Vg(lambdair, V0, lambda0) );
    long double dzIR = 1.0 / daIR ;
    dZ.push_back(dzIR) ;
    dlambdair = dlambdair / daIR;
    dtauir = dtauir / daIR;
    // Define the type of the state. We have X = {dz, lambda, tau, dlambda, dtau}
    state_type X (5);
    X <<= dzIR, lambdair, tauir, dlambdair, dtauir;
    // Now compute the starting value of dX
    cout << "Solving HVQCD for x = " << xi << ", tau0 = " << ti << ", W0 = " << W0 << ", V0 = " << V0 << ", lambda0 = " << lambda0 << endl;
    long double Amax = 100.0;
    long double h = 0.1 ;
    dense_output_runge_kutta< controlled_runge_kutta< runge_kutta_dopri5< state_type > > > stepper;
    stepper.initialize( X , aIR , h );
    long double A = aIR;
    while ( A < Amax )
    {
        stepper.do_step( VQCD(xi, ti, W0, V0, lambda0) ) ;
        X = stepper.current_state();
        A = stepper.current_time();
        //cout << A << '\t' <<  X(0) << '\t' <<  X(1) << '\t' <<  X(2) << endl;
        AA.push_back(A) ;
        dZ.push_back(X(0)) ;
        L.push_back(X(1)) ;
        T.push_back(X(2)) ;
    }
    double mq = X(4) / X(0) ;
    Spline_Interp<long double> dzfun = Spline_Interp<long double>(AA, dZ);
    int n = AA.size() ;
    long double zmin = zIR + dzfun.integrate(AA[ n - 1]) ;
    for(int i = 0; i < n; i++)
    {
        Z.push_back( zIR + dzfun.integrate(AA[i]) - zmin ) ;
    }
    // We need to reverse the lists because later
    // we will use them to compute the spectra of vector mesons.
    reverse(AA.begin(),AA.end()) ;
    reverse(Z.begin(),Z.end()) ;
    reverse(L.begin(),L.end()) ;
    reverse(T.begin(),T.end()) ;
    // Return A, Z, L(A), T(A), zIR and mq
    return List::create(Named("z") = Z, Named("A") = AA, Named("lambda") = L, Named("tau") = T, Named("zIR") = zIR - zmin, Named("mq") = mq );
} ;


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    using mtl::Collection;
    A.change_dim(5, 5); A= 0.0;
    {
	mtl::mat::inserter<Matrix>   ins(A);
	ins[0][0] << 7; ins[1][1] << 8; ins[1][3] << 2; ins[1][4] << 3;
	ins[2][2] << 2; ins[3][3] << 4; ins[4][4] << 9;
     }

    cout << name << "\nA is set to \n" << A;
    invert_diagonal(A);

    cout << "\nAfter inverting the diagonal, it is: \n" << A;
    MTL_THROW_IF(std::abs(A[0][0] - 1.0 / 7.0) > 0.0001, mtl::runtime_error("Wrong value after inverting diagonal!"));
    MTL_THROW_IF(std::abs(A[1][3] - 2.0) > 0.0001, mtl::runtime_error("Wrong value after inverting diagonal!"));

}

int main(int, char**)
{
    mtl::dense2D<double>                                                dr;
    mtl::dense2D<double, mtl::mat::parameters<mtl::col_major> >      dc;
    mtl::morton_dense<double, mtl::recursion::morton_z_mask>            mzd;
    mtl::morton_dense<double, mtl::recursion::doppled_2_row_mask>       d2r;
    mtl::compressed2D<double>                                           cr;
    mtl::compressed2D<double, mtl::mat::parameters<mtl::col_major> > cc;

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");

    return 0;
}


/* ----------------------------------------------------------------------------
 * Copyright 2017, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Luca Carlone, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file   testParallelPlaneRegularTangentSpaceFactor.cpp
 * @brief  test ParallelPlaneRegularTangentSpaceFactor
 * @author Antoni Rosinol Vidal
 */

#include <algorithm>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <random>

#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind.hpp>

#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "kimera-vio/factors/ParallelPlaneRegularFactor.h"
#include "kimera-vio/factors/PointPlaneFactor.h"

using namespace std;
using namespace gtsam;

static const double tol = 1e-5;
static const double der_tol = 1e-5;

/**
 * Test that error does give the right result when it is zero.
 */
TEST(testParallelPlaneRegularTangentSpaceFactor, ErrorIsZero) {
  /// Plane keys.
  Key plane_key_1(1);
  Key plane_key_2(2);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  ParallelPlaneRegularTangentSpaceFactor factor(plane_key_1, plane_key_2,
                                                parallel_plane_noise);

  /// Planes.
  OrientedPlane3 plane_1(0.1, 0.1, 0.9, 0.9);
  OrientedPlane3 plane_2(0.1, 0.1, 0.9, 0.1);

  /// Calculate error.
  Vector error = factor.evaluateError(plane_1, plane_2);

  /// Expected error.
  Vector2 expected_error = Vector2::Constant(0.0);

  ASSERT_TRUE(assert_equal(expected_error, error, tol));
}

/**
 * Test that error does give the right result when it is not zero.
 */
TEST(testParallelPlaneRegularTangentSpaceFactor, ErrorOtherThanZero) {
  /// Plane keys.
  Key plane_key_1(1);
  Key plane_key_2(2);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  ParallelPlaneRegularTangentSpaceFactor factor(plane_key_1, plane_key_2,
                                                parallel_plane_noise);

  /// Planes.
  OrientedPlane3 plane_1(0.0, 0.0, 1.0, 0.9);
  OrientedPlane3 plane_2(0.1, 0.1, 0.9, 0.1);

  /// Calculate error.
  Vector error = factor.evaluateError(plane_1, plane_2);

  /// Expected error.
  Vector2 expected_error;
  expected_error << 0.109764, -0.109764;

  ASSERT_TRUE(assert_equal(expected_error, error, tol));
}

/**
 * Test that analytical jacobians equal numerical ones.
 *
 */
TEST(testParallelPlaneRegularTangentSPaceFactor, Jacobians) {
  /// Plane keys.
  Key plane_key_1(1);
  Key plane_key_2(2);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  ParallelPlaneRegularTangentSpaceFactor factor(plane_key_1, plane_key_2,
                                                parallel_plane_noise);

  /// Planes.
  OrientedPlane3 plane_1(0.3, 0.2, 1.9, 0.9);
  OrientedPlane3 plane_2(0.1, 0.1, 0.9, 0.1);

  // Use the factor to calculate the Jacobians
  gtsam::Matrix H1Actual, H2Actual;
  factor.evaluateError(plane_1, plane_2, H1Actual, H2Actual);

  // Calculate numerical derivatives
  Matrix H1Expected =
      numericalDerivative21<Vector, OrientedPlane3, OrientedPlane3>(
          boost::bind(&ParallelPlaneRegularTangentSpaceFactor::evaluateError,
                      &factor, _1, _2, boost::none, boost::none),
          plane_1, plane_2, der_tol);

  Matrix H2Expected =
      numericalDerivative22<Vector, OrientedPlane3, OrientedPlane3>(
          boost::bind(&ParallelPlaneRegularTangentSpaceFactor::evaluateError,
                      &factor, _1, _2, boost::none, boost::none),
          plane_1, plane_2, der_tol);

  // Verify the Jacobians are correct
  ASSERT_TRUE(assert_equal(H1Expected, H1Actual, tol));
  ASSERT_TRUE(assert_equal(H2Expected, H2Actual, tol));
}

/**
 * Test that optimization works.
 * A plane and a landmark with prior factors, and a second plane constrained
 * together with the first plane using the ParallelPlaneRegularTangentSpace
 * factor.
 *
 *              Prior                      +-------+    +-+
 *               +-+                       | Lmk 1 +----+ | Prior
 *               +-+        Parallelism    +---+---+    +-+
 *                |           factor           |
 *            +---+---+        +-+         +---+---+
 *            |Plane 1+--------+ +---------+Plane 2|
 *            +-------+        +-+         +-------+
 *
 */
TEST(testParallelPlaneRegularTangentSpaceFactor, PlaneOptimization) {
  NonlinearFactorGraph graph;

  /// Keys
  Key landmark_key = 1;
  Key plane_key_1 = 2;
  Key plane_key_2 = 3;

  /// Shared noise for all landmarks.
  noiseModel::Diagonal::shared_ptr prior_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  Point3 priorMeanLandmark1(0.0, 0.0, 0.0);
  // TODO change for push_back or add.
  graph.emplace_shared<PriorFactor<Point3> >(landmark_key, priorMeanLandmark1,
                                             prior_noise);

  OrientedPlane3 priorMeanPlane1(0.0, 0.0, 1.0, 1.0);
  graph.emplace_shared<PriorFactor<OrientedPlane3> >(
      plane_key_1, priorMeanPlane1, prior_noise);

  /// Shared noise for all constraints between landmarks and planes.
  noiseModel::Isotropic::shared_ptr regularity_noise =
      noiseModel::Isotropic::Sigma(1, 0.5);

  /// Plane 2 to landmark.
  graph.emplace_shared<PointPlaneFactor>(landmark_key, plane_key_2,
                                         regularity_noise);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  graph.emplace_shared<ParallelPlaneRegularTangentSpaceFactor>(
      plane_key_1, plane_key_2, parallel_plane_noise);

  // graph.print("\nFactor Graph:\n");

  Values initial;
  initial.insert(landmark_key, Point3(0.0, 0.2, 0.1));
  initial.insert(plane_key_1, OrientedPlane3(0.1, 0.1, 0.9, 0.9));
  initial.insert(plane_key_2, OrientedPlane3(0.1, 0.1, 0.8, 0.1));

  // GaussianFactorGraph gfg = *graph.linearize(initial);
  // gfg.print("\nFactor Graph:\n");

  GaussNewtonParams params;
  // params.setVerbosity("LINEAR");
  params.setMaxIterations(20);
  params.setRelativeErrorTol(-std::numeric_limits<double>::max());
  // params.setErrorTol(-std::numeric_limits<double>::max());
  params.setAbsoluteErrorTol(-std::numeric_limits<double>::max());

  Values result = GaussNewtonOptimizer(graph, initial, params).optimize();
  // Values result = LevenbergMarquardtOptimizer(graph, initial,
  // params).optimize();

  Values expected;
  expected.insert(landmark_key, priorMeanLandmark1);
  expected.insert(plane_key_1, priorMeanPlane1);
  expected.insert(plane_key_2, OrientedPlane3(0.0, 0.0, 1.0, 0.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-euler
    This function object computes the natural logarithm of the absolute
    value of the Gamma function:
     \f$\displaystyle \log |\Gamma(x)|\f$

    @par Header <boost/simd/function/gammaln.hpp>

    @par Notes

    - The accuracy of the function is not uniformly good for negative entries
      The algorithm used is currently an adapted vesion of the cephes one.
      For better accuracy in the negative entry case, one can use the extern
      boost_math gammaln function but at a loss of speed.

      However, as stated in boost math:

      "While the relative errors near the positive roots of lgamma are very low,
       the  function has an infinite number of irrational roots for negative arguments:
       very close to these negative roots only a low absolute error can be guaranteed."

    - The call `gammaln(x, sgn)` also returns the sign of gamma in the output parameter @c sgn.

       Be aware that POSIX version of @c lgamma is not thread-safe: each execution of the function
       stores the sign of the gamma function of @c x in the static external variable signgam.

       boost.simd also provides @ref signgam which independantly computes the sign.

    @par Decorators

      - std_ fcalls @c std::lgamma

    @see gamma, signgam

    @par Example:

      @snippet gammaln.cpp gammaln

    @par Possible output:

      @snippet gammaln.txt gammaln
  **/
  IEEEValue gammaln(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/gammaln.hpp>
#include <boost/simd/function/simd/gammaln.hpp>

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

using namespace std;
int main()
{
  // For a more realistic example set size to 1000 or larger
  const int size = 8, N = size * size;

  //typedef mtl::dense2D<double>  matrix_type;
  typedef mtl::compressed2D<double>  matrix_type;
  matrix_type                      A(N, N), dr(5,5);

  laplacian_setup(A, size, size);

  dr= 1,  1,  1,   0,  0,
      1, -1, -2,   0,  0,
      1, -2,  1,   0,  0,
      0,  0,  0, -10,  0,
      0,  0,  0,   0, 22;

  itl::pc::diagonal<matrix_type>     P(A), Pb(dr);
  mtl::dense_vector<double>          x(N, 1.0), b(N), xb(5, 1.0), bb(5);
  mtl::dense_vector<complex<double> > xz(5,complex<double>(1.0, 0.0)), bz(5);

  bb= dr * xb;
  xb= 0;

  b= A * x;
  x= 0;

  itl::cyclic_iteration<double> iter(b, 500, 1.e-6, 0.0, 1);
  qmr(A, x, b, P, P, iter);
  std::cout<< "x=" << x << "\n";

  itl::cyclic_iteration<double> iterb(bb, 500, 1.e-6, 0.0, 1);
  qmr(dr, xb, bb, Pb, Pb, iterb);
  std::cout<< "xb=" << xb << "\n";

  return 0;
}


//  (C) Copyright Matt Borland 2021.
//  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_CCMATH_LOGB_HPP
#define BOOST_MATH_CCMATH_LOGB_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/frexp.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/abs.hpp>

namespace boost::math::ccmath {

namespace detail {

// The value of the exponent returned by std::logb is always 1 less than the exponent returned by
// std::frexp because of the different normalization requirements: for the exponent e returned by std::logb,
// |arg*r^-e| is between 1 and r (typically between 1 and 2), but for the exponent e returned by std::frexp,
// |arg*2^-e| is between 0.5 and 1.
template <typename T>
inline constexpr T logb_impl(T arg) noexcept
{
    int exp = 0;
    boost::math::ccmath::frexp(arg, &exp);

    return exp - 1;
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real logb(Real arg) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? -std::numeric_limits<Real>::infinity() :
               boost::math::ccmath::isinf(arg) ? std::numeric_limits<Real>::infinity() :
               boost::math::ccmath::isnan(arg) ? std::numeric_limits<Real>::quiet_NaN() :
               boost::math::ccmath::detail::logb_impl(arg);
    }
    else
    {
        using std::logb;
        return logb(arg);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double logb(Z arg) noexcept
{
    return boost::math::ccmath::logb(static_cast<double>(arg));
}

inline constexpr float logbf(float arg) noexcept
{
    return boost::math::ccmath::logb(arg);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double logbl(long double arg) noexcept
{
    return boost::math::ccmath::logb(arg);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_LOGB_HPP


/* test_uniform_int_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/uniform_int_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::uniform_int_distribution<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 0
#define BOOST_RANDOM_ARG2_DEFAULT 0x7fffffff
#define BOOST_RANDOM_ARG1_VALUE 100
#define BOOST_RANDOM_ARG2_VALUE 250

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX 0x7fffffff
#define BOOST_RANDOM_DIST1_MIN 100
#define BOOST_RANDOM_DIST1_MAX 0x7fffffff
#define BOOST_RANDOM_DIST2_MIN 100
#define BOOST_RANDOM_DIST2_MAX 250

#define BOOST_RANDOM_TEST1_PARAMS (0, 9)
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 9

#define BOOST_RANDOM_TEST2_PARAMS (10, 19)
#define BOOST_RANDOM_TEST2_MIN 10
#define BOOST_RANDOM_TEST2_MAX 19

#include "test_distribution.ipp"

#define BOOST_RANDOM_UNIFORM_INT boost::random::uniform_int_distribution

#include "test_uniform_int.ipp"