/* +------------------------------------------------------------------------+
   |                     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.;
}


// Copyright (c) 2014-2019 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <net.h>
#include <validation.h>

#include <test/setup_common.h>

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(validation_tests, TestingSetup)

static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    const int maxHalvings = 64;
    const CAmount nInitialSubsidy = consensusParams.initialSubsidy;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        /* In Xaya, we have the special rule that the block reward was set
           to 1 CHI at the beginning of the first halving period (before the
           POST_ICO fork).  Thus we use the *end* of the halving period
           for the purpose of this test.  */
        nHeight += consensusParams.nSubsidyHalvingInterval - 1;

        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK(nSubsidy <= nInitialSubsidy);
        BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
        nPreviousSubsidy = nSubsidy;
    }
    BOOST_CHECK_EQUAL(GetBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
    /* Testing other intervals as is done upstream doesn't work, as we would
       need to craft a ConsensusRules instance for the POST_ICO fork check.  */
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);

    CAmount nSum = 0;
    CAmount nLastSubsidy;
    int nLastHeight = 0;

    /* Updates the current state (contained in the variables this closes over)
       based on the block subsidy at the given height, assuming that it is the
       same from nLastHeight+1 up to height.  */
    const auto updateState = [&] (const int height)
      {
        const int numBlocks = height - nLastHeight;
        BOOST_CHECK(numBlocks > 0);

        const CAmount nSubsidy = GetBlockSubsidy(height, chainParams->GetConsensus());
        BOOST_CHECK(nSubsidy <= 4 * COIN);

        nSum += nSubsidy * numBlocks;
        nLastSubsidy = nSubsidy;
        BOOST_CHECK(MoneyRange(nSum));
        nLastHeight = height;
      };

    updateState(1);
    updateState(439999);
    BOOST_CHECK_EQUAL(nLastSubsidy, 1 * COIN);
    updateState(440000);
    BOOST_CHECK(nLastSubsidy > 3 * COIN);
    updateState(4199999);
    BOOST_CHECK(nLastSubsidy > 3 * COIN);
    updateState(4200000);
    BOOST_CHECK(nLastSubsidy < 2 * COIN);

    /* We need to call updateState always at the *last* block that has
       a certain subsidy.  Thus start iterating on the next block batch
       (100k more than first halving), and always update on height-1.  */
    for (int height = 4300000; height < 140000000; height += 100000)
        updateState(height - 1);

    BOOST_CHECK_EQUAL(nLastSubsidy, CAmount{0});
    /* This is the real PoW coin supply, taking rounding into account.  The
       final coin supply is larger by the presale amount.  */
    BOOST_CHECK_EQUAL(nSum, CAmount{3092157231160000});
}

BOOST_AUTO_TEST_CASE(subsidy_post_ico_fork_test)
{
    const auto main = CreateChainParams(CBaseChainParams::MAIN);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(439999, main->GetConsensus()), COIN);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(440000, main->GetConsensus()), 382934346);

    const auto test = CreateChainParams(CBaseChainParams::TESTNET);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(10999, test->GetConsensus()), COIN);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(11000, test->GetConsensus()), 10 * COIN);

    const auto regtest = CreateChainParams(CBaseChainParams::REGTEST);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(1, regtest->GetConsensus()), 50 * COIN);
}

static bool ReturnFalse() { return false; }
static bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
BOOST_AUTO_TEST_SUITE_END()


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/def.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/return_by_value.hpp>
#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <cctbx/adp_restraints/rigu.h>
#include <boost/multi_array.hpp>

namespace cctbx { namespace adp_restraints {
namespace {

  struct rigu_proxy_wrappers
  {
    typedef rigu_proxy w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>("rigu_proxy", no_init)
        .def(init<af::tiny<unsigned, 2> const&, double>((
           arg("i_seqs"),
           arg("weight"))))
        .add_property("i_seqs", make_getter(&w_t::i_seqs, rbv()))
        .add_property("weight", make_getter(&w_t::weight, rbv()))
      ;
      {
        scitbx::af::boost_python::shared_wrapper<w_t>::wrap(
          "shared_rigu_proxy")
        ;
      }
    }
  };

  // original procedure for tests only
  //! Calculate all the partial derivatives of the adp
  //! in the local cartesian coordinate system.
  af::shared<rigu::sym_mat3d> CalcReferenceGradients(const rigu::mat3d& RM) {
    /** See Parois, et. al. (2018). J. Appl. Cryst. 51, 1059-1068.
    *  for the detail on how it works
    *
    *  The U tensors are vectorized as 9 elements vectors
    *  column by column to simplify the calculations.
    *  Operations like V = A U B translate then to vec(V) =
    *  (B^t @ A) U with @ the kronecker product
    */

    /** dUcart contains the 6 partial derivatives dU_cart/dUij_cart
    *  writen as vectors.
    *  The 6 columns vectors are then stacked column wise to form dUcart
    */
    const int dUcart[9][6] = {
      //dU11 dU22 dU33 dU12 dU13 dU23
      { 1,   0,   0,   0,   0,   0 }, // U11
      { 0,   0,   0,   1,   0,   0 }, // U21
      { 0,   0,   0,   0,   1,   0 }, // U23
      { 0,   0,   0,   1,   0,   0 }, // U21
      { 0,   1,   0,   0,   0,   0 }, // U22
      { 0,   0,   0,   0,   0,   1 }, // U23
      { 0,   0,   0,   0,   1,   0 }, // U31
      { 0,   0,   0,   0,   0,   1 }, // U32
      { 0,   0,   1,   0,   0,   0 }, // U33
    };

    boost::multi_array<double, 2> kron(boost::extents[9][9]);

    //! calulating the kronecker product
    for (int i = 0; i < 3; i++) {
      for (int j = 0; j < 3; j++) {
        int startRow = i * 3;
        int startCol = j * 3;
        for (int k = 0; k < 3; k++) {
          for (int l = 0; l < 3; l++) {
            kron[startRow + k][startCol + l] = RM(i, j)*RM(k, l);
          }
        }
      }
    }

    /** vec(dRUcart) = kron vec(dUcart).
    *  Derivatives of (RUcart = RM Ucart RM^t)
    */
    boost::multi_array<double, 2> dRUcart_(boost::extents[9][6]);
    std::fill(dRUcart_.data(), dRUcart_.data() + dRUcart_.num_elements(), 0);
    for (int i = 0; i < 9; i++) {
      for (int j = 0; j < 6; j++) {
        for (int k = 0; k < 9; k++) {
          dRUcart_[i][j] += kron[i][k] * dUcart[k][j];
        }
      }
    }
    af::shared<rigu::sym_mat3d> dRUcart(3);
    for (int i = 0; i < 3; i++) {
      int r_idx[3] = { 8, 6, 7 };
      for (int j = 0; j < 6; j++) {
        dRUcart[i][j] = dRUcart_[r_idx[i]][j];
      }
    }
    return dRUcart;
  }
}

struct rigu_wrappers {
  typedef rigu w_t;

  static void wrap() {
    using namespace boost::python;
    typedef return_value_policy<return_by_value> rbv;
    class_<w_t>("rigu", no_init)
      .def(init<
        af::tiny<scitbx::vec3<double>, 2> const&,
        af::tiny<scitbx::sym_mat3<double>, 2> const&,
        double>(
        (arg("sites"),
          arg("u_cart"),
          arg("weight"))))
      .def(init<
        adp_restraint_params<double> const &,
        rigu_proxy const&>(
        (arg("params"),
          arg("proxy"))))
      .def_readonly("weight", &w_t::weight)
      .def("delta_33", &w_t::delta_33)
      .def("delta_13", &w_t::delta_13)
      .def("delta_23", &w_t::delta_23)
      .def("residual33", &w_t::residual33)
      .def("residual13", &w_t::residual13)
      .def("residual23", &w_t::residual23)
      .def("gradients33", &w_t::gradients33)
      .def("gradients13", &w_t::gradients13)
      .def("gradients23", &w_t::gradients23)
      .def("RM", &w_t::getRM)
      .def("raw_gradients", &w_t::raw_gradients)
      .def("reference_gradients", &CalcReferenceGradients)
      .staticmethod("reference_gradients")
      ;
  }
};

void wrap_all() {
  using namespace boost::python;
  rigu_wrappers::wrap();

  rigu_proxy_wrappers::wrap();
  def("rigu_residual_sum",
    adp_restraint_residual_sum_aniso<rigu_proxy, rigu>::impl,
    (arg("params"),
      arg("proxies"),
      arg("gradients_aniso_cart")));
  def("rigu_residuals",
    adp_restraint_residuals<rigu_proxy, rigu>::impl,
    (arg("params"),
      arg("proxies")));
}

namespace boost_python {

  void wrap_rigu() { wrap_all(); }

}}}


#include <boost/test/unit_test.hpp>
#include "test_op.hpp"
#include "inputs/minmax.h"
#include "inputs/znorm.h"
#include "libs/model_prediction/prediction_flow.h"
#include "libs/model_prediction/prediction_data.h"
#include "libs/model_prediction/prediction_norm.h"
#include "libs/model_prediction/prediction_status.h"
#include "libs/model_prediction/prediction_norm_task.h"
#include "libs/cleanup_operatiosn/normalized_conf.h"
#include "utils/algorithms.hpp"
#include "utils/math_datatype.h"

namespace
{

inline math::min_max<double>
    minmax_stats()
{
    return math::min_max_values(min_max_data);
}

minmax_normalization_stats minmax_norm()
{
    static const math::min_max<double> minmax{minmax_stats()};
    return minmax_normalization_stats{0u, 0, 1,
            math::minimum(minmax), math::maximum(minmax)};
}

z_normaliztion_conf_stats zrnom()
{
    return z_normaliztion_conf_stats{1u, PsXY[0], PsXY[1]};
}

}   // end of local namespace

BOOST_AUTO_TEST_CASE(test_no_input_for_norm)
{
    // test what would happen in case we don't have row for norm operation
    prediction::data d{prediction::output{prediction::row_number{1}, prediction::status::OK}};
    const auto& before_action = prediction::current_values(d);
    const auto norm_data{prediction::norm(prediction::normalized{}, d)};
    BOOST_TEST_REQUIRE(empty(norm_data));
    BOOST_TEST(size(norm_data) == 0u);
    BOOST_TEST(state(prediction::current_state(norm_data)) == prediction::status::OK);
    const auto& r = prediction::current_values(norm_data);
    BOOST_TEST_REQUIRE((r == before_action), "we are expecting that the call in this case would do nothing");
}

BOOST_AUTO_TEST_CASE(test_no_action_for_norm)
{
    // in this case we would not have any norm actions, so we
    // are expecting to have the same data in the input and the output
    // of the operation, but this should not have any issues with the
    // results of the operation
    const prediction::row::data_type the_data[] = {5.1155f, 67.2244f, 87.234f, 8.83552f, 77.778, 11,442};
    const prediction::row data_before{std::begin(the_data), std::end(the_data), prediction::row_number{16}};
    const auto data_after = prediction::norm(prediction::normalized{}, prediction::data{data_before});
    // we are expecting to not change anything after appllying the operation
    BOOST_TEST_REQUIRE(state(prediction::current_state(data_after)) == prediction::status::OK);
    BOOST_TEST_REQUIRE(state(prediction::current_state(data_after)) != prediction::status::REMOVED);
    const auto after_action{prediction::current_values(data_after)};
    BOOST_TEST(after_action.size() == utils::size(the_data));
    BOOST_TEST(after_action == data_before);
    BOOST_TEST(not empty(data_after));
}

BOOST_AUTO_TEST_CASE(test_normalized_two_columns)
{
    const prediction::row::data_type column_0_min_max[] = {
        (prediction::row::data_type)min_max_data[0],
        (prediction::row::data_type)min_max_data[1],
        (prediction::row::data_type)min_max_data[2],
        (prediction::row::data_type)min_max_data[3],
        (prediction::row::data_type)min_max_data[4],
        (prediction::row::data_type)min_max_data[5]
    };
    const prediction::row::data_type column_1_znrom[] = {
        (prediction::row::data_type)data_mapstd[0],
        (prediction::row::data_type)data_mapstd[1],
        (prediction::row::data_type)data_mapstd[2],
        (prediction::row::data_type)data_mapstd[3],
        (prediction::row::data_type)data_mapstd[4],
        (prediction::row::data_type)data_mapstd[5]
    };
    prediction::row rows[] = {
        prediction::row{prediction::row_number{1}, 3u},
        prediction::row{prediction::row_number{2}, 3u},
        prediction::row{prediction::row_number{3}, 3u},
        prediction::row{prediction::row_number{4}, 3u},
        prediction::row{prediction::row_number{5}, 3u},
        prediction::row{prediction::row_number{6}, 3u}
    };
    for (unsigned int i = 0u; i < utils::size(column_0_min_max); i++) {
        rows[i][0] = column_0_min_max[i];
        rows[i][1] = column_1_znrom[i];
        rows[i][2] = 11.22f;    // just a number to check for later
    }
    // now we would like to see if normalization works
    prediction::normalized norm_op;
    norm_op.assign(prediction::norm_entry{minmax_norm()});
    norm_op.assign(prediction::norm_entry{zrnom()});
    // normalized each row
    for (unsigned int i = 0u; i < utils::size(rows); ++i) {
        const auto data_after = prediction::norm(norm_op, prediction::data{rows[i]});
        BOOST_TEST(not empty(data_after));
        BOOST_TEST_REQUIRE(state(prediction::current_state(data_after)) == prediction::status::OK);
        BOOST_TEST_REQUIRE(state(prediction::current_state(data_after)) != prediction::status::REMOVED);
        const auto after_action{prediction::current_values(data_after)};
        BOOST_TEST(after_action != rows[i]);
        BOOST_TEST(not empty(data_after));
        // check the minmax value
        BOOST_TEST(round_cmp(after_action[0], (prediction::row::data_type)expected_second[i], 0.001f), "value after min max norm "<<after_action[0]<<" != "<<expected_second[i]);
        // check znorm
        BOOST_TEST(round_cmp(after_action[1], (prediction::row::data_type)expected_first_mapstd[i], 0.001f), "value after z norm "<<after_action[0]<<" != "<<expected_first_mapstd[i]);
        // and for the last column make sure that it didn't changed
        BOOST_TEST(after_action[2] == 11.22f);
    }
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_ASUM_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_ASUM_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/has_linear_array.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The BLAS-backend is selected by defining a pre-processor variable,
//  which can be one of
// * for CBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
// * for CUBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
// * netlib-compatible BLAS is the default
//
#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
#include <boost/numeric/bindings/blas/detail/cblas.h>
#include <boost/numeric/bindings/blas/detail/cblas_option.hpp>
#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
#include <boost/numeric/bindings/blas/detail/cublas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#else
#include <boost/numeric/bindings/blas/detail/blas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#endif

namespace boost {
namespace numeric {
namespace bindings {
namespace blas {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end BLAS-routine.
//
namespace detail {

#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * float value-type.
//
inline float asum( const int n, const float* x, const int incx ) {
    return cblas_sasum( n, x, incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * double value-type.
//
inline double asum( const int n, const double* x, const int incx ) {
    return cblas_dasum( n, x, incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<float> value-type.
//
inline float asum( const int n, const std::complex<float>* x,
        const int incx ) {
    return cblas_scasum( n, x, incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<double> value-type.
//
inline double asum( const int n, const std::complex<double>* x,
        const int incx ) {
    return cblas_dzasum( n, x, incx );
}

#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * float value-type.
//
inline float asum( const int n, const float* x, const int incx ) {
    return cublasSasum( n, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * double value-type.
//
inline double asum( const int n, const double* x, const int incx ) {
    return cublasDasum( n, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<float> value-type.
//
inline float asum( const int n, const std::complex<float>* x,
        const int incx ) {
    return cublasScasum( n, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<double> value-type.
//
inline double asum( const int n, const std::complex<double>* x,
        const int incx ) {
    return cublasDzasum( n, x, incx );
}

#else
//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * float value-type.
//
inline float asum( const fortran_int_t n, const float* x,
        const fortran_int_t incx ) {
    return BLAS_SASUM( &n, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * double value-type.
//
inline double asum( const fortran_int_t n, const double* x,
        const fortran_int_t incx ) {
    return BLAS_DASUM( &n, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<float> value-type.
//
inline float asum( const fortran_int_t n, const std::complex<float>* x,
        const fortran_int_t incx ) {
    return BLAS_SCASUM( &n, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<double> value-type.
//
inline double asum( const fortran_int_t n, const std::complex<double>* x,
        const fortran_int_t incx ) {
    return BLAS_DZASUM( &n, x, &incx );
}

#endif

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to asum.
//
template< typename Value >
struct asum_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;
    typedef real_type result_type;

    //
    // Static member function that
    // * Deduces the required arguments for dispatching to BLAS, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorX >
    static result_type invoke( const VectorX& x ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorX >::value) );
        return detail::asum( bindings::size(x),
                bindings::begin_value(x), bindings::stride(x) );
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. Calls
// to these functions are passed to the asum_impl classes. In the
// documentation, the const-overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for asum. Its overload differs for
//
template< typename VectorX >
inline typename asum_impl< typename bindings::value_type<
        VectorX >::type >::result_type
asum( const VectorX& x ) {
    return asum_impl< typename bindings::value_type<
            VectorX >::type >::invoke( x );
}

} // namespace blas
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include <chrono>
#include <fstream>
#include <iostream>
#include <boost/exception/diagnostic_information.hpp>
#include <g3log/logworker.hpp>
#include "bdm/device/demultiplexer.hxx"
#include "bdm/device/mcp3004.hxx"
#include "bdm/device/gpio/gpio_wrapper.hxx"
#include "bdm/device/spi/spidev.hxx"
#include "bdm/task/delay_estimation.hxx"
#include "bdm/task/position_estimation.hxx"
#include "bdm/task/sound_buffer.hxx"
#include "bdmbin/logger.hxx"

int main(int, char**) {
	constexpr std::size_t FRAMELEN = 16384;
	constexpr double sample_time = 0.1;
	bdmbin::logger logger;
	logger.install_logger();
	try {
		FILE* fpDAT;

		if (NULL == (fpDAT = fopen("a.raw", "r"))) {
			exit(1);
		}
		auto sigdata1 = std::make_unique<short[]>(FRAMELEN);
		auto sigdata2 = std::make_unique<short[]>(FRAMELEN);
		auto sigdata3 = std::make_unique<short[]>(FRAMELEN);
		auto sigdata4 = std::make_unique<short[]>(FRAMELEN);
		fseek(fpDAT, 15 * sizeof(short), SEEK_SET);
		fread(sigdata1.get(), sizeof(short), FRAMELEN, fpDAT);
		fseek(fpDAT, 0 * sizeof(short), SEEK_SET);
		fread(sigdata2.get(), sizeof(short), FRAMELEN, fpDAT);
		fseek(fpDAT, 0 * sizeof(short), SEEK_SET);
		fread(sigdata3.get(), sizeof(short), FRAMELEN, fpDAT);
		fseek(fpDAT, 5 * sizeof(short), SEEK_SET);
		fread(sigdata4.get(), sizeof(short), FRAMELEN, fpDAT);
		fclose(fpDAT);
		bdm::task::delay_estimation de;
		double T1 = de.get_delay(FRAMELEN, sigdata1.get(), sigdata2.get()) * sample_time;
		double T2 = de.get_delay(FRAMELEN, sigdata1.get(), sigdata3.get()) * sample_time;
		double T3 = de.get_delay(FRAMELEN, sigdata1.get(), sigdata4.get()) * sample_time;

		bdm::task::position_estimation pe;
		double p[3];
		pe.estimate_position(0, T1, T2, T3, p);
			printf("%lf %lf %lf\n", p[0], p[1], p[2]);
	} catch (boost::exception& ex) {
		LOG(ERROR) << "uncaught exception";
		std::exception* stdex = dynamic_cast<std::exception*>(&ex);
		if (stdex) {
			LOG(ERROR) << "what: " << stdex->what();
		}
		LOG(ERROR) << boost::diagnostic_information_what(ex);
		return 1;
	} catch (std::exception& ex) {
		LOG(ERROR) << "uncaught exception";
		LOG(ERROR) << "what: " << ex.what();
		return 1;
	}
}


// Boost.Geometry Index
// OpenGL visualization

// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.

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

#include <GL/glut.h>

#include <boost/foreach.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/index/detail/rtree/utilities/gl_draw.hpp>
#include <boost/geometry/index/detail/rtree/utilities/print.hpp>
#include <boost/geometry/index/detail/rtree/utilities/are_boxes_ok.hpp>
#include <boost/geometry/index/detail/rtree/utilities/are_levels_ok.hpp>
#include <boost/geometry/index/detail/rtree/utilities/statistics.hpp>

#include <boost/variant.hpp>

#define ENABLE_POINTS_AND_SEGMENTS

namespace bg = boost::geometry;
namespace bgi = bg::index;

// used types

typedef bg::model::point<float, 2, boost::geometry::cs::cartesian> P;
typedef bg::model::box<P> B;
typedef bg::model::linestring<P> LS;
typedef bg::model::segment<P> S;
typedef bg::model::ring<P> R;
typedef bg::model::polygon<P> Poly;
typedef bg::model::multi_polygon<Poly> MPoly;

// containers variant

template <typename V>
struct containers
{
    containers & operator=(containers const& c)
    {
        tree = c.tree;
        values = c.values;
        result = c.result;
        return *this;
    }

    bgi::rtree< V, bgi::rstar<4, 2> > tree;
    std::vector<V> values;
    std::vector<V> result;
};

boost::variant<
    containers<B>
#ifdef ENABLE_POINTS_AND_SEGMENTS
  , containers<P>
  , containers<S>
#endif
> cont;

// visitors

template <typename Pred>
struct query_v : boost::static_visitor<size_t>
{
    Pred m_pred;
    query_v(Pred const& pred) : m_pred(pred) {}

    template <typename C>
    size_t operator()(C & c) const
    {
        c.result.clear();
        return c.tree.query(m_pred, std::back_inserter(c.result));
    }
};
template <typename Cont, typename Pred>
inline size_t query(Cont & cont, Pred const& pred)
{
    return boost::apply_visitor(query_v<Pred>(pred), cont);
}

struct print_result_v : boost::static_visitor<>
{
    template <typename C>
    void operator()(C & c) const
    {
        for ( size_t i = 0 ; i < c.result.size() ; ++i )
        {
            bgi::detail::utilities::print_indexable(std::cout, c.result[i]);
            std::cout << '\n';
        }
    }
};
template <typename Cont>
inline void print_result(Cont const& cont)
{
    boost::apply_visitor(print_result_v(), cont);
}

struct bounds_v : boost::static_visitor<B>
{
    template <typename C>
    B operator()(C & c) const
    {
        return c.tree.bounds();
    }
};
template <typename Cont>
inline B bounds(Cont const& cont)
{
    return boost::apply_visitor(bounds_v(), cont);
}

struct depth_v : boost::static_visitor<size_t>
{
    template <typename C>
    size_t operator()(C & c) const
    {
        return get(c.tree);
    }
    template <typename RTree>
    static size_t get(RTree const& t)
    {
        return bgi::detail::rtree::utilities::view<RTree>(t).depth();
    }
};
template <typename Cont>
inline size_t depth(Cont const& cont)
{
    return boost::apply_visitor(depth_v(), cont);
}

struct draw_tree_v : boost::static_visitor<>
{
    template <typename C>
    void operator()(C & c) const
    {
        bgi::detail::rtree::utilities::gl_draw(c.tree);
    }
};
template <typename Cont>
inline void draw_tree(Cont const& cont)
{
    return boost::apply_visitor(draw_tree_v(), cont);
}

struct draw_result_v : boost::static_visitor<>
{
    template <typename C>
    void operator()(C & c) const
    {
        for ( size_t i = 0 ; i < c.result.size() ; ++i )
        {
            bgi::detail::utilities::gl_draw_indexable(c.result[i], depth_v::get(c.tree));
        }
    }
};
template <typename Cont>
inline void draw_result(Cont const& cont)
{
    return boost::apply_visitor(draw_result_v(), cont);
}

struct print_tree_v : boost::static_visitor<>
{
    template <typename C>
    void operator()(C & c) const
    {
        bgi::detail::rtree::utilities::print(std::cout, c.tree);
    }
};
template <typename Cont>
inline void print_tree(Cont const& cont)
{
    return boost::apply_visitor(print_tree_v(), cont);
}

// globals used in querying

size_t found_count = 0;
size_t count = 5;

P search_point;
B search_box;
R search_ring;
Poly search_poly;
MPoly search_multi_poly;
S search_segment;
LS search_linestring;
LS search_path;

enum query_mode_type {
    qm_knn, qm_knnb, qm_knns, qm_c, qm_d, qm_i, qm_o, qm_w, qm_nc, qm_nd, qm_ni, qm_no, qm_nw, qm_all, qm_ri, qm_pi, qm_mpi, qm_si, qm_lsi, qm_path
} query_mode = qm_knn;

bool search_valid = false;

// various queries

void query_knn()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;

    if ( query_mode == qm_knn )
    {
        search_point = P(x, y);
        found_count = query(cont, bgi::nearest(search_point, count));
    }
    else if ( query_mode == qm_knnb )
    {
        float w = 2 + ( rand() % 1000 ) / 500.0f;
        float h = 2 + ( rand() % 1000 ) / 500.0f;
        search_box = B(P(x - w, y - h), P(x + w, y + h));
        found_count = query(cont, bgi::nearest(search_box, count));
    }
    else if ( query_mode == qm_knns )
    {
        int signx = rand() % 2 ? 1 : -1;
        int signy = rand() % 2 ? 1 : -1;
        float w = (10 + ( rand() % 1000 ) / 100.0f) * signx;
        float h = (10 + ( rand() % 1000 ) / 100.0f) * signy;
        search_segment = S(P(x - w, y - h), P(x + w, y + h));
        found_count = query(cont, bgi::nearest(search_segment, count));
    }
    else
    {
        BOOST_ASSERT(false);
    }

    if ( found_count > 0 )
    {
        if ( query_mode == qm_knn )
        {
            std::cout << "search point: ";
            bgi::detail::utilities::print_indexable(std::cout, search_point);
        }
        else if ( query_mode == qm_knnb )
        {
            std::cout << "search box: ";
            bgi::detail::utilities::print_indexable(std::cout, search_box);
        }
        else if ( query_mode == qm_knns )
        {
            std::cout << "search segment: ";
            bgi::detail::utilities::print_indexable(std::cout, search_segment);
        }
        else
        {
            BOOST_ASSERT(false);
        }
        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "nearest not found\n";
}

#ifndef ENABLE_POINTS_AND_SEGMENTS
void query_path()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 20 + ( rand() % 1000 ) / 100.0f;
    float h = 20 + ( rand() % 1000 ) / 100.0f;

    search_path.resize(10);
    float yy = y-h;
    for ( int i = 0 ; i < 5 ; ++i, yy += h / 2 )
    {
        search_path[2 * i] = P(x-w, yy);
        search_path[2 * i + 1] = P(x+w, yy);
    }

    found_count = query(cont, bgi::detail::path<LS>(search_path, count));

    if ( found_count > 0 )
    {
        std::cout << "search path: ";
        BOOST_FOREACH(P const& p, search_path)
            bgi::detail::utilities::print_indexable(std::cout, p);
        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "values on path not found\n";
}
#endif

template <typename Predicate>
void query()
{
    if ( query_mode != qm_all )
    {
        float x = ( rand() % 1000 ) / 10.0f;
        float y = ( rand() % 1000 ) / 10.0f;
        float w = 10 + ( rand() % 1000 ) / 100.0f;
        float h = 10 + ( rand() % 1000 ) / 100.0f;

        search_box = B(P(x - w, y - h), P(x + w, y + h));
    }
    else
    {
        search_box = bounds(cont);
    }

    found_count = query(cont, Predicate(search_box));

    if ( found_count > 0 )
    {
        std::cout << "search box: ";
        bgi::detail::utilities::print_indexable(std::cout, search_box);
        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_ring()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10 + ( rand() % 1000 ) / 100.0f;
    float h = 10 + ( rand() % 1000 ) / 100.0f;

    search_ring.clear();
    search_ring.push_back(P(x - w, y - h));
    search_ring.push_back(P(x - w/2, y - h));
    search_ring.push_back(P(x, y - 3*h/2));
    search_ring.push_back(P(x + w/2, y - h));
    search_ring.push_back(P(x + w, y - h));
    search_ring.push_back(P(x + w, y - h/2));
    search_ring.push_back(P(x + 3*w/2, y));
    search_ring.push_back(P(x + w, y + h/2));
    search_ring.push_back(P(x + w, y + h));
    search_ring.push_back(P(x + w/2, y + h));
    search_ring.push_back(P(x, y + 3*h/2));
    search_ring.push_back(P(x - w/2, y + h));
    search_ring.push_back(P(x - w, y + h));
    search_ring.push_back(P(x - w, y + h/2));
    search_ring.push_back(P(x - 3*w/2, y));
    search_ring.push_back(P(x - w, y - h/2));
    search_ring.push_back(P(x - w, y - h));

    found_count = query(cont, Predicate(search_ring));

    if ( found_count > 0 )
    {
        std::cout << "search ring: ";
        BOOST_FOREACH(P const& p, search_ring)
        {
            bgi::detail::utilities::print_indexable(std::cout, p);
            std::cout << ' ';
        }
        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_poly()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10 + ( rand() % 1000 ) / 100.0f;
    float h = 10 + ( rand() % 1000 ) / 100.0f;

    search_poly.clear();
    search_poly.outer().push_back(P(x - w, y - h));
    search_poly.outer().push_back(P(x - w/2, y - h));
    search_poly.outer().push_back(P(x, y - 3*h/2));
    search_poly.outer().push_back(P(x + w/2, y - h));
    search_poly.outer().push_back(P(x + w, y - h));
    search_poly.outer().push_back(P(x + w, y - h/2));
    search_poly.outer().push_back(P(x + 3*w/2, y));
    search_poly.outer().push_back(P(x + w, y + h/2));
    search_poly.outer().push_back(P(x + w, y + h));
    search_poly.outer().push_back(P(x + w/2, y + h));
    search_poly.outer().push_back(P(x, y + 3*h/2));
    search_poly.outer().push_back(P(x - w/2, y + h));
    search_poly.outer().push_back(P(x - w, y + h));
    search_poly.outer().push_back(P(x - w, y + h/2));
    search_poly.outer().push_back(P(x - 3*w/2, y));
    search_poly.outer().push_back(P(x - w, y - h/2));
    search_poly.outer().push_back(P(x - w, y - h));

    search_poly.inners().push_back(Poly::ring_type());
    search_poly.inners()[0].push_back(P(x - w/2, y - h/2));
    search_poly.inners()[0].push_back(P(x + w/2, y - h/2));
    search_poly.inners()[0].push_back(P(x + w/2, y + h/2));
    search_poly.inners()[0].push_back(P(x - w/2, y + h/2));
    search_poly.inners()[0].push_back(P(x - w/2, y - h/2));

    found_count = query(cont, Predicate(search_poly));

    if ( found_count > 0 )
    {
        std::cout << "search poly outer: ";
        BOOST_FOREACH(P const& p, search_poly.outer())
        {
            bgi::detail::utilities::print_indexable(std::cout, p);
            std::cout << ' ';
        }
        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_multi_poly()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10 + ( rand() % 1000 ) / 100.0f;
    float h = 10 + ( rand() % 1000 ) / 100.0f;

    search_multi_poly.clear();

    search_multi_poly.push_back(Poly());
    search_multi_poly[0].outer().push_back(P(x - w, y - h));
    search_multi_poly[0].outer().push_back(P(x - w/2, y - h));
    search_multi_poly[0].outer().push_back(P(x, y - 3*h/2));
    search_multi_poly[0].outer().push_back(P(x + w/2, y - h));
    search_multi_poly[0].outer().push_back(P(x + w, y - h));
    search_multi_poly[0].outer().push_back(P(x + w, y - h/2));
    search_multi_poly[0].outer().push_back(P(x + 3*w/2, y));
    search_multi_poly[0].outer().push_back(P(x + w, y + h/2));
    search_multi_poly[0].outer().push_back(P(x + w, y + h));
    search_multi_poly[0].outer().push_back(P(x + w/2, y + h));
    search_multi_poly[0].outer().push_back(P(x, y + 3*h/2));
    search_multi_poly[0].outer().push_back(P(x - w/2, y + h));
    search_multi_poly[0].outer().push_back(P(x - w, y + h));
    search_multi_poly[0].outer().push_back(P(x - w, y + h/2));
    search_multi_poly[0].outer().push_back(P(x - 3*w/2, y));
    search_multi_poly[0].outer().push_back(P(x - w, y - h/2));
    search_multi_poly[0].outer().push_back(P(x - w, y - h));

    search_multi_poly[0].inners().push_back(Poly::ring_type());
    search_multi_poly[0].inners()[0].push_back(P(x - w/2, y - h/2));
    search_multi_poly[0].inners()[0].push_back(P(x + w/2, y - h/2));
    search_multi_poly[0].inners()[0].push_back(P(x + w/2, y + h/2));
    search_multi_poly[0].inners()[0].push_back(P(x - w/2, y + h/2));
    search_multi_poly[0].inners()[0].push_back(P(x - w/2, y - h/2));

    search_multi_poly.push_back(Poly());
    search_multi_poly[1].outer().push_back(P(x - 2*w, y - 2*h));
    search_multi_poly[1].outer().push_back(P(x - 6*w/5, y - 2*h));
    search_multi_poly[1].outer().push_back(P(x - 6*w/5, y - 6*h/5));
    search_multi_poly[1].outer().push_back(P(x - 2*w, y - 6*h/5));
    search_multi_poly[1].outer().push_back(P(x - 2*w, y - 2*h));

    search_multi_poly.push_back(Poly());
    search_multi_poly[2].outer().push_back(P(x + 6*w/5, y + 6*h/5));
    search_multi_poly[2].outer().push_back(P(x + 2*w, y + 6*h/5));
    search_multi_poly[2].outer().push_back(P(x + 2*w, y + 2*h));
    search_multi_poly[2].outer().push_back(P(x + 6*w/5, y + 2*h));
    search_multi_poly[2].outer().push_back(P(x + 6*w/5, y + 6*h/5));

    found_count = query(cont, Predicate(search_multi_poly));

    if ( found_count > 0 )
    {
        std::cout << "search multi_poly[0] outer: ";
        BOOST_FOREACH(P const& p, search_multi_poly[0].outer())
        {
            bgi::detail::utilities::print_indexable(std::cout, p);
            std::cout << ' ';
        }
        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_segment()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10.0f - ( rand() % 1000 ) / 50.0f;
    float h = 10.0f - ( rand() % 1000 ) / 50.0f;
    w += 0 <= w ? 10 : -10;
    h += 0 <= h ? 10 : -10;

    boost::geometry::set<0, 0>(search_segment, x - w);
    boost::geometry::set<0, 1>(search_segment, y - h);
    boost::geometry::set<1, 0>(search_segment, x + w);
    boost::geometry::set<1, 1>(search_segment, y + h);

    found_count = query(cont, Predicate(search_segment));

    if ( found_count > 0 )
    {
        std::cout << "search segment: ";
        bgi::detail::utilities::print_indexable(std::cout, P(x-w, y-h));
        bgi::detail::utilities::print_indexable(std::cout, P(x+w, y+h));

        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_linestring()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10 + ( rand() % 1000 ) / 100.0f;
    float h = 10 + ( rand() % 1000 ) / 100.0f;

    search_linestring.clear();
    float a = 0;
    float d = 0;
    for ( size_t i = 0 ; i < 300 ; ++i, a += 0.05, d += 0.005 )
    {
        float xx = x + w * d * ::cos(a);
        float yy = y + h * d * ::sin(a);
        search_linestring.push_back(P(xx, yy));
    }

    found_count = query(cont, Predicate(search_linestring));

    if ( found_count > 0 )
    {
        std::cout << "search linestring: ";
        BOOST_FOREACH(P const& p, search_linestring)
        {
            bgi::detail::utilities::print_indexable(std::cout, p);
            std::cout << ' ';
        }
        std::cout << "\nfound: ";
        print_result(cont);
    }
    else
        std::cout << "boxes not found\n";
}

// the function running the correct query based on the query_mode

void search()
{
    namespace d = bgi::detail;

    if ( query_mode == qm_knn || query_mode == qm_knnb || query_mode == qm_knns )
        query_knn();
    else if ( query_mode == qm_d )
        query< d::spatial_predicate<B, d::disjoint_tag, false> >();
    else if ( query_mode == qm_i )
        query< d::spatial_predicate<B, d::intersects_tag, false> >();
    else if ( query_mode == qm_nd )
        query< d::spatial_predicate<B, d::disjoint_tag, true> >();
    else if ( query_mode == qm_ni )
        query< d::spatial_predicate<B, d::intersects_tag, true> >();
    else if ( query_mode == qm_all )
        query< d::spatial_predicate<B, d::intersects_tag, false> >();
#ifdef ENABLE_POINTS_AND_SEGMENTS
    else
        std::cout << "query disabled\n";
#else
    else if ( query_mode == qm_c )
        query< d::spatial_predicate<B, d::covered_by_tag, false> >();
    else if ( query_mode == qm_o )
        query< d::spatial_predicate<B, d::overlaps_tag, false> >();
    else if ( query_mode == qm_w )
        query< d::spatial_predicate<B, d::within_tag, false> >();
    else if ( query_mode == qm_nc )
        query< d::spatial_predicate<B, d::covered_by_tag, true> >();
    else if ( query_mode == qm_no )
        query< d::spatial_predicate<B, d::overlaps_tag, true> >();
    else if ( query_mode == qm_nw )
        query< d::spatial_predicate<B, d::within_tag, true> >();
    else if ( query_mode == qm_ri )
        query_ring< d::spatial_predicate<R, d::intersects_tag, false> >();
    else if ( query_mode == qm_pi )
        query_poly< d::spatial_predicate<Poly, d::intersects_tag, false> >();
    else if ( query_mode == qm_mpi )
        query_multi_poly< d::spatial_predicate<MPoly, d::intersects_tag, false> >();
    else if ( query_mode == qm_si )
        query_segment< d::spatial_predicate<S, d::intersects_tag, false> >();
    else if ( query_mode == qm_lsi )
        query_linestring< d::spatial_predicate<LS, d::intersects_tag, false> >();
    else if ( query_mode == qm_path )
        query_path();
#endif

    search_valid = true;
}

// various drawing functions

void draw_point(P const& p)
{
    float x = boost::geometry::get<0>(p);
    float y = boost::geometry::get<1>(p);
    float z = depth(cont);

    glBegin(GL_QUADS);
    glVertex3f(x+1, y, z);
    glVertex3f(x, y+1, z);
    glVertex3f(x-1, y, z);
    glVertex3f(x, y-1, z);
    glEnd();
}

void draw_knn_area(float min_distance, float max_distance)
{
    float x = boost::geometry::get<0>(search_point);
    float y = boost::geometry::get<1>(search_point);
    float z = depth(cont);

    draw_point(search_point);

    // search min circle

    glBegin(GL_LINE_LOOP);
    for(float a = 0 ; a < 3.14158f * 2 ; a += 3.14158f / 180)
        glVertex3f(x + min_distance * ::cos(a), y + min_distance * ::sin(a), z);
    glEnd();

    // search max circle

    glBegin(GL_LINE_LOOP);
    for(float a = 0 ; a < 3.14158f * 2 ; a += 3.14158f / 180)
        glVertex3f(x + max_distance * ::cos(a), y + max_distance * ::sin(a), z);
    glEnd();
}

void draw_linestring(LS const& ls)
{
    glBegin(GL_LINE_STRIP);

    BOOST_FOREACH(P const& p, ls)
    {
        float x = boost::geometry::get<0>(p);
        float y = boost::geometry::get<1>(p);
        float z = depth(cont);
        glVertex3f(x, y, z);
    }

    glEnd();
}

void draw_segment(S const& s)
{
    float x1 = boost::geometry::get<0, 0>(s);
    float y1 = boost::geometry::get<0, 1>(s);
    float x2 = boost::geometry::get<1, 0>(s);
    float y2 = boost::geometry::get<1, 1>(s);
    float z = depth(cont);

    glBegin(GL_LINES);
    glVertex3f(x1, y1, z);
    glVertex3f(x2, y2, z);
    glEnd();
}

template <typename Box>
void draw_box(Box const& box)
{
    float x1 = boost::geometry::get<bg::min_corner, 0>(box);
    float y1 = boost::geometry::get<bg::min_corner, 1>(box);
    float x2 = boost::geometry::get<bg::max_corner, 0>(box);
    float y2 = boost::geometry::get<bg::max_corner, 1>(box);
    float z = depth(cont);

    // search box
    glBegin(GL_LINE_LOOP);
        glVertex3f(x1, y1, z);
        glVertex3f(x2, y1, z);
        glVertex3f(x2, y2, z);
        glVertex3f(x1, y2, z);
    glEnd();
}

template <typename Range>
void draw_ring(Range const& range)
{
    float z = depth(cont);

    // search box
    glBegin(GL_LINE_LOOP);

    BOOST_FOREACH(P const& p, range)
    {
        float x = boost::geometry::get<0>(p);
        float y = boost::geometry::get<1>(p);

        glVertex3f(x, y, z);
    }
    glEnd();
}

template <typename Polygon>
void draw_polygon(Polygon const& polygon)
{
    draw_ring(polygon.outer());
    BOOST_FOREACH(Poly::ring_type const& r, polygon.inners())
        draw_ring(r);
}

template <typename MultiPolygon>
void draw_multi_polygon(MultiPolygon const& multi_polygon)
{
    BOOST_FOREACH(Poly const& p, multi_polygon)
        draw_polygon(p);
}

// render the scene -> tree, if searching data available also the query geometry and result

void render_scene(void)
{
    glClear(GL_COLOR_BUFFER_BIT);

    draw_tree(cont);

    if ( search_valid )
    {
        glColor3f(1.0f, 0.25f, 0.0f);

        if ( query_mode == qm_knn )
            draw_knn_area(0, 0);
        else if ( query_mode == qm_knnb )
            draw_box(search_box);
        else if ( query_mode == qm_knns )
            draw_segment(search_segment);
        else if ( query_mode == qm_ri )
            draw_ring(search_ring);
        else if ( query_mode == qm_pi )
            draw_polygon(search_poly);
        else if ( query_mode == qm_mpi )
            draw_multi_polygon(search_multi_poly);
        else if ( query_mode == qm_si )
            draw_segment(search_segment);
        else if ( query_mode == qm_lsi )
            draw_linestring(search_linestring);
        else if ( query_mode == qm_path )
            draw_linestring(search_path);
        else
            draw_box(search_box);

        glColor3f(1.0f, 0.5f, 0.0f);

        draw_result(cont);
    }

    glFlush();
}

void resize(int w, int h)
{
    if ( h == 0 )
        h = 1;

    //float ratio = float(w) / h;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    glViewport(0, 0, w, h);

    //gluPerspective(45, ratio, 1, 1000);
    glOrtho(-150, 150, -150, 150, -150, 150);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    /*gluLookAt(
        120.0f, 120.0f, 120.0f,
        50.0f, 50.0f, -1.0f,
        0.0f, 1.0f, 0.0f);*/
    gluLookAt(
        50.0f, 50.0f, 75.0f,
        50.0f, 50.0f, -1.0f,
        0.0f, 1.0f, 0.0f);

    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(1.5f);

    srand(1);
}

// randomize various indexables

inline void rand_val(B & b)
{
    float x = ( rand() % 100 );
    float y = ( rand() % 100 );
    float w = ( rand() % 2 ) + 1;
    float h = ( rand() % 2 ) + 1;
    b = B(P(x - w, y - h),P(x + w, y + h));
}
inline void rand_val(P & p)
{
    float x = ( rand() % 100 );
    float y = ( rand() % 100 );
    p = P(x, y);
}
inline void rand_val(S & s)
{
    float x = ( rand() % 100 );
    float y = ( rand() % 100 );
    float w = ( rand() % 2 + 1) * (rand() % 2 ? 1.0f : -1.0f);
    float h = ( rand() % 2 + 1) * (rand() % 2 ? 1.0f : -1.0f);
    s = S(P(x - w, y - h),P(x + w, y + h));
}

// more higher-level visitors

struct insert_random_value_v : boost::static_visitor<>
{
    template <typename V>
    void operator()(containers<V> & c) const
    {
        V v;
        rand_val(v);

        boost::geometry::index::insert(c.tree, v);
        c.values.push_back(v);

        std::cout << "inserted: ";
        bgi::detail::utilities::print_indexable(std::cout, v);
        std::cout << '\n';

        std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(c.tree) ? "boxes OK\n" : "WRONG BOXES!\n" );
        std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(c.tree) ? "levels OK\n" : "WRONG LEVELS!\n" );
        std::cout << "\n";
    }
};
template <typename Cont>
inline void insert_random_value(Cont & cont)
{
    return boost::apply_visitor(insert_random_value_v(), cont);
}

struct remove_random_value_v : boost::static_visitor<>
{
    template <typename V>
    void operator()(containers<V> & c) const
    {
        if ( c.values.empty() )
            return;

        size_t i = rand() % c.values.size();
        V v = c.values[i];

        c.tree.remove(v);
        c.values.erase(c.values.begin() + i);

        std::cout << "removed: ";
        bgi::detail::utilities::print_indexable(std::cout, v);
        std::cout << '\n';

        std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(c.tree) ? "boxes OK\n" : "WRONG BOXES!\n" );
        std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(c.tree) ? "levels OK\n" : "WRONG LEVELS!\n" );
        std::cout << "\n";
    }
};
template <typename Cont>
inline void remove_random_value(Cont & cont)
{
    return boost::apply_visitor(remove_random_value_v(), cont);
}

// handle mouse input

void mouse(int button, int state, int /*x*/, int /*y*/)
{
    if ( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN )
    {
        insert_random_value(cont);
        search_valid = false;
    }
    else if ( button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN )
    {
        remove_random_value(cont);
        search_valid = false;
    }
    else if ( button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN )
    {
        search();
    }

    glutPostRedisplay();
}

// more higher-level visitors

struct insert_random_values_v : boost::static_visitor<>
{
    template <typename V>
    void operator()(containers<V> & c) const
    {
        for ( size_t i = 0 ; i < 35 ; ++i )
        {
            V v;
            rand_val(v);

            c.tree.insert(v);
            c.values.push_back(v);

            std::cout << "inserted: ";
            bgi::detail::utilities::print_indexable(std::cout, v);
            std::cout << '\n';
        }

        std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(c.tree) ? "boxes OK\n" : "WRONG BOXES!\n" );
        std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(c.tree) ? "levels OK\n" : "WRONG LEVELS!\n" );
        std::cout << "\n";
    }
};
template <typename Cont>
inline void insert_random_values(Cont & cont)
{
    return boost::apply_visitor(insert_random_values_v(), cont);
}

struct bulk_insert_random_values_v : boost::static_visitor<>
{
    template <typename V>
    void operator()(containers<V> & c) const
    {
        c.values.clear();

        for ( size_t i = 0 ; i < 35 ; ++i )
        {
            V v;
            rand_val(v);

            c.values.push_back(v);

            std::cout << "inserted: ";
            bgi::detail::utilities::print_indexable(std::cout, v);
            std::cout << '\n';
        }

        create(c.tree, c.values);

        std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(c.tree) ? "boxes OK\n" : "WRONG BOXES!\n" );
        std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(c.tree) ? "levels OK\n" : "WRONG LEVELS!\n" );
        std::cout << "\n";
    }

    template <typename Tree, typename Values>
    void create(Tree & tree, Values const& values) const
    {
        Tree t(values);
        tree = boost::move(t);
    }
};
template <typename Cont>
inline void bulk_insert_random_values(Cont & cont)
{
    return boost::apply_visitor(bulk_insert_random_values_v(), cont);
}

// handle keyboard input

std::string current_line;

void keyboard(unsigned char key, int /*x*/, int /*y*/)
{
    if ( key == '\r' || key == '\n' )
    {
        if ( current_line == "storeb" )
        {
            cont = containers<B>();
            glutPostRedisplay();
        }
#ifdef ENABLE_POINTS_AND_SEGMENTS
        else if ( current_line == "storep" )
        {
            cont = containers<P>();
            glutPostRedisplay();
        }
        else if ( current_line == "stores" )
        {
            cont = containers<S>();
            glutPostRedisplay();
        }
#endif
        else if ( current_line == "t" )
        {
            std::cout << "\n";
            print_tree(cont);
            std::cout << "\n";
        }
        else if ( current_line == "rand" )
        {
            insert_random_values(cont);
            search_valid = false;

            glutPostRedisplay();
        }
        else if ( current_line == "bulk" )
        {
            bulk_insert_random_values(cont);
            search_valid = false;

            glutPostRedisplay();
        }
        else
        {
            if ( current_line == "knn" )
                query_mode = qm_knn;
            else if ( current_line == "knnb" )
                query_mode = qm_knnb;
            else if ( current_line == "knns" )
                query_mode = qm_knns;
            else if ( current_line == "c" )
                query_mode = qm_c;
            else if ( current_line == "d" )
                query_mode = qm_d;
            else if ( current_line == "i" )
                query_mode = qm_i;
            else if ( current_line == "o" )
                query_mode = qm_o;
            else if ( current_line == "w" )
                query_mode = qm_w;
            else if ( current_line == "nc" )
                query_mode = qm_nc;
            else if ( current_line == "nd" )
                query_mode = qm_nd;
            else if ( current_line == "ni" )
                query_mode = qm_ni;
            else if ( current_line == "no" )
                query_mode = qm_no;
            else if ( current_line == "nw" )
                query_mode = qm_nw;
            else if ( current_line == "all" )
                query_mode = qm_all;
            else if ( current_line == "ri" )
                query_mode = qm_ri;
            else if ( current_line == "pi" )
                query_mode = qm_pi;
            else if ( current_line == "mpi" )
                query_mode = qm_mpi;
            else if ( current_line == "si" )
                query_mode = qm_si;
            else if ( current_line == "lsi" )
                query_mode = qm_lsi;
            else if ( current_line == "path" )
                query_mode = qm_path;

            search();
            glutPostRedisplay();
        }

        current_line.clear();
        std::cout << '\n';
    }
    else
    {
        current_line += key;
        std::cout << key;
    }
}

// main function

int main(int argc, char **argv)
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DEPTH | GLUT_SINGLE | GLUT_RGBA);
    glutInitWindowPosition(100,100);
    glutInitWindowSize(600, 600);
    glutCreateWindow("boost::geometry::index::rtree GLUT test");

    glutDisplayFunc(render_scene);
    glutReshapeFunc(resize);
    glutMouseFunc(mouse);
    glutKeyboardFunc(keyboard);

    glutMainLoop();

    return 0;
}