/* +------------------------------------------------------------------------+
   |                     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 "vision-precomp.h"  // Precompiled headers

#include <mrpt/core/round.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/vision/CDifodo.h>
#include <Eigen/Dense>
#include <iostream>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::math;
using namespace std;
using namespace Eigen;
using mrpt::round;
using mrpt::square;

CDifodo::CDifodo()
{
	rows = 60;
	cols = 80;
	fovh = M_PIf * 58.6f / 180.0f;
	fovv = M_PIf * 45.6f / 180.0f;
	cam_mode = 1;  // (1 - 640 x 480, 2 - 320 x 240, 4 - 160 x 120)
	downsample = 1;
	ctf_levels = 1;
	m_width = 640 / (cam_mode * downsample);
	m_height = 480 / (cam_mode * downsample);
	fast_pyramid = true;

	// Resize pyramid
	const unsigned int pyr_levels =
		round(log(float(m_width / cols)) / log(2.f)) + ctf_levels;
	depth.resize(pyr_levels);
	depth_old.resize(pyr_levels);
	depth_inter.resize(pyr_levels);
	depth_warped.resize(pyr_levels);
	xx.resize(pyr_levels);
	xx_inter.resize(pyr_levels);
	xx_old.resize(pyr_levels);
	xx_warped.resize(pyr_levels);
	yy.resize(pyr_levels);
	yy_inter.resize(pyr_levels);
	yy_old.resize(pyr_levels);
	yy_warped.resize(pyr_levels);
	transformations.resize(pyr_levels);

	for (unsigned int i = 0; i < pyr_levels; i++)
	{
		unsigned int s = pow(2.f, int(i));
		cols_i = m_width / s;
		rows_i = m_height / s;
		depth[i].resize(rows_i, cols_i);
		depth_inter[i].resize(rows_i, cols_i);
		depth_old[i].resize(rows_i, cols_i);
		depth[i].fill(0.0f);
		depth_old[i].fill(0.0f);
		xx[i].resize(rows_i, cols_i);
		xx_inter[i].resize(rows_i, cols_i);
		xx_old[i].resize(rows_i, cols_i);
		xx[i].fill(0.0f);
		xx_old[i].fill(0.0f);
		yy[i].resize(rows_i, cols_i);
		yy_inter[i].resize(rows_i, cols_i);
		yy_old[i].resize(rows_i, cols_i);
		yy[i].fill(0.0f);
		yy_old[i].fill(0.0f);
		transformations[i].resize(4, 4);

		if (cols_i <= cols)
		{
			depth_warped[i].resize(rows_i, cols_i);
			xx_warped[i].resize(rows_i, cols_i);
			yy_warped[i].resize(rows_i, cols_i);
		}
	}

	depth_wf.setSize(m_height, m_width);

	fps = 30.f;  // In Hz

	previous_speed_const_weight = 0.05f;
	previous_speed_eig_weight = 0.5f;
	kai_loc_old = TTwist3D();
	num_valid_points = 0;

	// Compute gaussian mask
	VectorXf v_mask(4);
	v_mask(0) = 1.f;
	v_mask(1) = 2.f;
	v_mask(2) = 2.f;
	v_mask(3) = 1.f;
	for (unsigned int i = 0; i < 4; i++)
		for (unsigned int j = 0; j < 4; j++)
			f_mask(i, j) = v_mask(i) * v_mask(j) / 36.f;

	// Compute gaussian mask
	float v_mask2[5] = {1, 4, 6, 4, 1};
	for (unsigned int i = 0; i < 5; i++)
		for (unsigned int j = 0; j < 5; j++)
			g_mask[i][j] = v_mask2[i] * v_mask2[j] / 256.f;
}

void CDifodo::buildCoordinatesPyramid()
{
	const float max_depth_dif = 0.1f;

	// Push coordinates back
	depth_old.swap(depth);
	xx_old.swap(xx);
	yy_old.swap(yy);

	// The number of levels of the pyramid does not match the number of levels
	// used
	// in the odometry computation (because we might want to finish with lower
	// resolutions)

	unsigned int pyr_levels =
		round(log(float(m_width / cols)) / log(2.f)) + ctf_levels;

	// Generate levels
	for (unsigned int i = 0; i < pyr_levels; i++)
	{
		unsigned int s = pow(2.f, int(i));
		cols_i = m_width / s;
		rows_i = m_height / s;
		const int rows_i2 = 2 * rows_i;
		const int cols_i2 = 2 * cols_i;
		const int i_1 = i - 1;

		if (i == 0) depth[i].swap(depth_wf);

		//                              Downsampling
		//-----------------------------------------------------------------------------
		else
		{
			for (unsigned int u = 0; u < cols_i; u++)
				for (unsigned int v = 0; v < rows_i; v++)
				{
					const int u2 = 2 * u;
					const int v2 = 2 * v;
					const float dcenter = depth[i_1](v2, u2);

					// Inner pixels
					if ((v > 0) && (v < rows_i - 1) && (u > 0) &&
						(u < cols_i - 1))
					{
						if (dcenter > 0.f)
						{
							float sum = 0.f;
							float weight = 0.f;

							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const float abs_dif = abs(
										depth[i_1](v2 + k, u2 + l) - dcenter);
									if (abs_dif < max_depth_dif)
									{
										const float aux_w =
											g_mask[2 + k][2 + l] *
											(max_depth_dif - abs_dif);
										weight += aux_w;
										sum +=
											aux_w * depth[i_1](v2 + k, u2 + l);
									}
								}
							depth[i](v, u) = sum / weight;
						}
						else
						{
							float min_depth = 10.f;
							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const float d = depth[i_1](v2 + k, u2 + l);
									if ((d > 0.f) && (d < min_depth))
										min_depth = d;
								}

							if (min_depth < 10.f)
								depth[i](v, u) = min_depth;
							else
								depth[i](v, u) = 0.f;
						}
					}

					// Boundary
					else
					{
						if (dcenter > 0.f)
						{
							float sum = 0.f;
							float weight = 0.f;

							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const int indv = v2 + k, indu = u2 + l;
									if ((indv >= 0) && (indv < rows_i2) &&
										(indu >= 0) && (indu < cols_i2))
									{
										const float abs_dif = abs(
											depth[i_1](indv, indu) - dcenter);
										if (abs_dif < max_depth_dif)
										{
											const float aux_w =
												g_mask[2 + k][2 + l] *
												(max_depth_dif - abs_dif);
											weight += aux_w;
											sum +=
												aux_w * depth[i_1](indv, indu);
										}
									}
								}
							depth[i](v, u) = sum / weight;
						}
						else
						{
							float min_depth = 10.f;
							for (int l = -2; l < 3; l++)
								for (int k = -2; k < 3; k++)
								{
									const int indv = v2 + k, indu = u2 + l;
									if ((indv >= 0) && (indv < rows_i2) &&
										(indu >= 0) && (indu < cols_i2))
									{
										const float d = depth[i_1](indv, indu);
										if ((d > 0.f) && (d < min_depth))
											min_depth = d;
									}
								}

							if (min_depth < 10.f)
								depth[i](v, u) = min_depth;
							else
								depth[i](v, u) = 0.f;
						}
					}
				}
		}

		// Calculate coordinates "xy" of the points
		const float inv_f_i = 2.f * tan(0.5f * fovh) / float(cols_i);
		const float disp_u_i = 0.5f * (cols_i - 1);
		const float disp_v_i = 0.5f * (rows_i - 1);

		for (unsigned int u = 0; u < cols_i; u++)
			for (unsigned int v = 0; v < rows_i; v++)
				if (depth[i](v, u) > 0.f)
				{
					xx[i](v, u) = (u - disp_u_i) * depth[i](v, u) * inv_f_i;
					yy[i](v, u) = (v - disp_v_i) * depth[i](v, u) * inv_f_i;
				}
				else
				{
					xx[i](v, u) = 0.f;
					yy[i](v, u) = 0.f;
				}
	}
}

void CDifodo::buildCoordinatesPyramidFast()
{
	const float max_depth_dif = 0.1f;

	// Push coordinates back
	depth_old.swap(depth);
	xx_old.swap(xx);
	yy_old.swap(yy);

	// The number of levels of the pyramid does not match the number of levels
	// used
	// in the odometry computation (because we might want to finish with lower
	// resolutions)

	unsigned int pyr_levels =
		round(log(float(m_width / cols)) / log(2.f)) + ctf_levels;

	// Generate levels
	for (unsigned int i = 0; i < pyr_levels; i++)
	{
		unsigned int s = pow(2.f, int(i));
		cols_i = m_width / s;
		rows_i = m_height / s;
		// const int rows_i2 = 2*rows_i;
		// const int cols_i2 = 2*cols_i;
		const int i_1 = i - 1;

		if (i == 0) depth[i].swap(depth_wf);

		//                              Downsampling
		//-----------------------------------------------------------------------------
		else
		{
			for (unsigned int u = 0; u < cols_i; u++)
				for (unsigned int v = 0; v < rows_i; v++)
				{
					const int u2 = 2 * u;
					const int v2 = 2 * v;

					// Inner pixels
					if ((v > 0) && (v < rows_i - 1) && (u > 0) &&
						(u < cols_i - 1))
					{
						const Matrix4f d_block =
							depth[i_1].block<4, 4>(v2 - 1, u2 - 1);
						float depths[4] = {d_block(5), d_block(6), d_block(9),
										   d_block(10)};
						float dcenter;

						// Sort the array (try to find a good/representative
						// value)
						for (signed char k = 2; k >= 0; k--)
							if (depths[k + 1] < depths[k])
								std::swap(depths[k + 1], depths[k]);
						for (unsigned char k = 1; k < 3; k++)
							if (depths[k] > depths[k + 1])
								std::swap(depths[k + 1], depths[k]);
						if (depths[2] < depths[1])
							dcenter = depths[1];
						else
							dcenter = depths[2];

						if (dcenter > 0.f)
						{
							float sum = 0.f;
							float weight = 0.f;

							for (unsigned char k = 0; k < 16; k++)
							{
								const float abs_dif = abs(d_block(k) - dcenter);
								if (abs_dif < max_depth_dif)
								{
									const float aux_w =
										f_mask[k] * (max_depth_dif - abs_dif);
									weight += aux_w;
									sum += aux_w * d_block(k);
								}
							}
							if (weight > 0) depth[i](v, u) = sum / weight;
						}
						else
							depth[i](v, u) = 0.f;
					}

					// Boundary
					else
					{
						const Matrix2f d_block = depth[i_1].block<2, 2>(v2, u2);
						const float new_d = 0.25f * d_block.array().sum();
						if (new_d < 0.4f)
							depth[i](v, u) = 0.f;
						else
							depth[i](v, u) = new_d;
					}
				}
		}

		// Calculate coordinates "xy" of the points
		const float inv_f_i = 2.f * tan(0.5f * fovh) / float(cols_i);
		const float disp_u_i = 0.5f * (cols_i - 1);
		const float disp_v_i = 0.5f * (rows_i - 1);

		for (unsigned int u = 0; u < cols_i; u++)
			for (unsigned int v = 0; v < rows_i; v++)
				if (depth[i](v, u) > 0.f)
				{
					xx[i](v, u) = (u - disp_u_i) * depth[i](v, u) * inv_f_i;
					yy[i](v, u) = (v - disp_v_i) * depth[i](v, u) * inv_f_i;
				}
				else
				{
					xx[i](v, u) = 0.f;
					yy[i](v, u) = 0.f;
				}
	}
}

void CDifodo::performWarping()
{
	// Camera parameters (which also depend on the level resolution)
	const float f = float(cols_i) / (2.f * tan(0.5f * fovh));
	const float disp_u_i = 0.5f * float(cols_i - 1);
	const float disp_v_i = 0.5f * float(rows_i - 1);

	// Rigid transformation estimated up to the present level
	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 1; i <= level; i++)
		acu_trans = transformations[i - 1].asEigen() * acu_trans;

	MatrixXf wacu(rows_i, cols_i);
	wacu.fill(0.f);
	depth_warped[image_level].fill(0.f);

	const auto cols_lim = float(cols_i - 1);
	const auto rows_lim = float(rows_i - 1);

	//						Warping loop
	//---------------------------------------------------------
	for (unsigned int j = 0; j < cols_i; j++)
		for (unsigned int i = 0; i < rows_i; i++)
		{
			const float z = depth[image_level](i, j);

			if (z > 0.f)
			{
				// Transform point to the warped reference frame
				const float depth_w = acu_trans(0, 0) * z +
									  acu_trans(0, 1) * xx[image_level](i, j) +
									  acu_trans(0, 2) * yy[image_level](i, j) +
									  acu_trans(0, 3);
				const float x_w = acu_trans(1, 0) * z +
								  acu_trans(1, 1) * xx[image_level](i, j) +
								  acu_trans(1, 2) * yy[image_level](i, j) +
								  acu_trans(1, 3);
				const float y_w = acu_trans(2, 0) * z +
								  acu_trans(2, 1) * xx[image_level](i, j) +
								  acu_trans(2, 2) * yy[image_level](i, j) +
								  acu_trans(2, 3);

				// Calculate warping
				const float uwarp = f * x_w / depth_w + disp_u_i;
				const float vwarp = f * y_w / depth_w + disp_v_i;

				// The warped pixel (which is not integer in general)
				// contributes to all the surrounding ones
				if ((uwarp >= 0.f) && (uwarp < cols_lim) && (vwarp >= 0.f) &&
					(vwarp < rows_lim))
				{
					const int uwarp_l = uwarp;
					const int uwarp_r = uwarp_l + 1;
					const int vwarp_d = vwarp;
					const int vwarp_u = vwarp_d + 1;
					const float delta_r = float(uwarp_r) - uwarp;
					const float delta_l = uwarp - float(uwarp_l);
					const float delta_u = float(vwarp_u) - vwarp;
					const float delta_d = vwarp - float(vwarp_d);

					// Warped pixel very close to an integer value
					if (abs(round(uwarp) - uwarp) + abs(round(vwarp) - vwarp) <
						0.05f)
					{
						depth_warped[image_level](round(vwarp), round(uwarp)) +=
							depth_w;
						wacu(round(vwarp), round(uwarp)) += 1.f;
					}
					else
					{
						const float w_ur = square(delta_l) + square(delta_d);
						depth_warped[image_level](vwarp_u, uwarp_r) +=
							w_ur * depth_w;
						wacu(vwarp_u, uwarp_r) += w_ur;

						const float w_ul = square(delta_r) + square(delta_d);
						depth_warped[image_level](vwarp_u, uwarp_l) +=
							w_ul * depth_w;
						wacu(vwarp_u, uwarp_l) += w_ul;

						const float w_dr = square(delta_l) + square(delta_u);
						depth_warped[image_level](vwarp_d, uwarp_r) +=
							w_dr * depth_w;
						wacu(vwarp_d, uwarp_r) += w_dr;

						const float w_dl = square(delta_r) + square(delta_u);
						depth_warped[image_level](vwarp_d, uwarp_l) +=
							w_dl * depth_w;
						wacu(vwarp_d, uwarp_l) += w_dl;
					}
				}
			}
		}

	// Scale the averaged depth and compute spatial coordinates
	const float inv_f_i = 1.f / f;
	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i; v++)
		{
			if (wacu(v, u) > 0.f)
			{
				depth_warped[image_level](v, u) /= wacu(v, u);
				xx_warped[image_level](v, u) =
					(u - disp_u_i) * depth_warped[image_level](v, u) * inv_f_i;
				yy_warped[image_level](v, u) =
					(v - disp_v_i) * depth_warped[image_level](v, u) * inv_f_i;
			}
			else
			{
				depth_warped[image_level](v, u) = 0.f;
				xx_warped[image_level](v, u) = 0.f;
				yy_warped[image_level](v, u) = 0.f;
			}
		}
}

void CDifodo::calculateCoord()
{
	null.resize(rows_i, cols_i);
	null.fill(false);
	num_valid_points = 0;

	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i; v++)
		{
			if ((depth_old[image_level](v, u)) == 0.f ||
				(depth_warped[image_level](v, u) == 0.f))
			{
				depth_inter[image_level](v, u) = 0.f;
				xx_inter[image_level](v, u) = 0.f;
				yy_inter[image_level](v, u) = 0.f;
				null(v, u) = true;
			}
			else
			{
				depth_inter[image_level](v, u) =
					0.5f * (depth_old[image_level](v, u) +
							depth_warped[image_level](v, u));
				xx_inter[image_level](v, u) =
					0.5f *
					(xx_old[image_level](v, u) + xx_warped[image_level](v, u));
				yy_inter[image_level](v, u) =
					0.5f *
					(yy_old[image_level](v, u) + yy_warped[image_level](v, u));
				null(v, u) = false;
				if ((u > 0) && (v > 0) && (u < cols_i - 1) && (v < rows_i - 1))
					num_valid_points++;
			}
		}
}

void CDifodo::calculateDepthDerivatives()
{
	dt.resize(rows_i, cols_i);
	dt.fill(0.f);
	du.resize(rows_i, cols_i);
	du.fill(0.f);
	dv.resize(rows_i, cols_i);
	dv.fill(0.f);

	// Compute connectivity
	MatrixXf rx_ninv(rows_i, cols_i);
	MatrixXf ry_ninv(rows_i, cols_i);
	rx_ninv.fill(1.f);
	ry_ninv.fill(1.f);

	for (unsigned int u = 0; u < cols_i - 1; u++)
		for (unsigned int v = 0; v < rows_i; v++)
			if (null(v, u) == false)
			{
				rx_ninv(v, u) = sqrtf(
					square(
						xx_inter[image_level](v, u + 1) -
						xx_inter[image_level](v, u)) +
					square(
						depth_inter[image_level](v, u + 1) -
						depth_inter[image_level](v, u)));
			}

	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i - 1; v++)
			if (null(v, u) == false)
			{
				ry_ninv(v, u) = sqrtf(
					square(
						yy_inter[image_level](v + 1, u) -
						yy_inter[image_level](v, u)) +
					square(
						depth_inter[image_level](v + 1, u) -
						depth_inter[image_level](v, u)));
			}

	// Spatial derivatives
	for (unsigned int v = 0; v < rows_i; v++)
	{
		for (unsigned int u = 1; u < cols_i - 1; u++)
			if (null(v, u) == false)
				du(v, u) =
					(rx_ninv(v, u - 1) * (depth_inter[image_level](v, u + 1) -
										  depth_inter[image_level](v, u)) +
					 rx_ninv(v, u) * (depth_inter[image_level](v, u) -
									  depth_inter[image_level](v, u - 1))) /
					(rx_ninv(v, u) + rx_ninv(v, u - 1));

		du(v, 0) = du(v, 1);
		du(v, cols_i - 1) = du(v, cols_i - 2);
	}

	for (unsigned int u = 0; u < cols_i; u++)
	{
		for (unsigned int v = 1; v < rows_i - 1; v++)
			if (null(v, u) == false)
				dv(v, u) =
					(ry_ninv(v - 1, u) * (depth_inter[image_level](v + 1, u) -
										  depth_inter[image_level](v, u)) +
					 ry_ninv(v, u) * (depth_inter[image_level](v, u) -
									  depth_inter[image_level](v - 1, u))) /
					(ry_ninv(v, u) + ry_ninv(v - 1, u));

		dv(0, u) = dv(1, u);
		dv(rows_i - 1, u) = dv(rows_i - 2, u);
	}

	// Temporal derivative
	for (unsigned int u = 0; u < cols_i; u++)
		for (unsigned int v = 0; v < rows_i; v++)
			if (null(v, u) == false)
				dt(v, u) = fps * (depth_warped[image_level](v, u) -
								  depth_old[image_level](v, u));
}

void CDifodo::computeWeights()
{
	weights.resize(rows_i, cols_i);
	weights.fill(0.f);

	// Obtain the velocity associated to the rigid transformation estimated up
	// to the present level
	CVectorFixedFloat<6> kai_level;
	kai_level.setFromMatrixLike(kai_loc_old);

	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 0; i < level; i++)
		acu_trans = transformations[i].asEigen() * acu_trans;

	// Alternative way to compute the log
	auto mat_aux = CMatrixDouble44(acu_trans.cast<double>().eval());

	auto trans_vec = poses::Lie::SE<3>::log(poses::CPose3D(mat_aux));
	trans_vec *= fps;
	const auto kai_level_acu = CVectorFixedDouble<6>(trans_vec);

	kai_level -= kai_level_acu.cast_float();

	// Parameters for the measurement error
	const float f_inv = float(cols_i) / (2.f * tan(0.5f * fovh));
	const float kz2 = 8.122e-12f;  // square(1.425e-5) / 25

	// Parameters for linearization error
	const float kduv = 20e-5f;
	const float kdt = kduv / square(fps);
	const float k2dt = 5e-6f;
	const float k2duv = 5e-6f;

	for (unsigned int u = 1; u < cols_i - 1; u++)
		for (unsigned int v = 1; v < rows_i - 1; v++)
			if (null(v, u) == false)
			{
				//					Compute measurment error (simplified)
				//-----------------------------------------------------------------------
				const float z = depth_inter[image_level](v, u);
				const float inv_d = 1.f / z;
				// const float dycomp = du2(v,u)*f_inv_y*inv_d;
				// const float dzcomp = dv2(v,u)*f_inv_z*inv_d;
				const float z2 = z * z;
				const float z4 = z2 * z2;

				// const float var11 = kz2*z4;
				// const float var12 =
				// kz2*xx_inter[image_level](v,u)*z2*depth_inter[image_level](v,u);
				// const float var13 =
				// kz2*yy_inter[image_level](v,u)*z2*depth_inter[image_level](v,u);
				// const float var22 =
				// kz2*square(xx_inter[image_level](v,u))*z2;
				// const float var23 =
				// kz2*xx_inter[image_level](v,u)*yy_inter[image_level](v,u)*z2;
				// const float var33 =
				// kz2*square(yy_inter[image_level](v,u))*z2;
				const float var44 = kz2 * z4 * square(fps);
				const float var55 = kz2 * z4 * 0.25f;
				const float var66 = var55;

				// const float j1 =
				// -2.f*inv_d*inv_d*(xx_inter[image_level](v,u)*dycomp +
				// yy_inter[image_level](v,u)*dzcomp)*(kai_level[0] +
				// yy_inter[image_level](v,u)*kai_level[4] -
				// xx_inter[image_level](v,u)*kai_level[5])
				//				+ inv_d*dycomp*(kai_level[1] -
				// yy_inter[image_level](v,u)*kai_level[3]) +
				// inv_d*dzcomp*(kai_level[2] +
				// xx_inter[image_level](v,u)*kai_level[3]);
				// const float j2 = inv_d*dycomp*(kai_level[0] +
				// yy_inter[image_level](v,u)*kai_level[4] -
				// 2.f*xx_inter[image_level](v,u)*kai_level[5]) -
				// dzcomp*kai_level[3];
				// const float j3 = inv_d*dzcomp*(kai_level[0] +
				// 2.f*yy_inter[image_level](v,u)*kai_level[4] -
				// xx_inter[image_level](v,u)*kai_level[5]) +
				// dycomp*kai_level[3];

				const float j4 = 1.f;
				const float j5 =
					xx_inter[image_level](v, u) * inv_d * inv_d * f_inv *
						(kai_level[0] +
						 yy_inter[image_level](v, u) * kai_level[4] -
						 xx_inter[image_level](v, u) * kai_level[5]) +
					inv_d * f_inv *
						(-kai_level[1] - z * kai_level[5] +
						 yy_inter[image_level](v, u) * kai_level[3]);
				const float j6 =
					yy_inter[image_level](v, u) * inv_d * inv_d * f_inv *
						(kai_level[0] +
						 yy_inter[image_level](v, u) * kai_level[4] -
						 xx_inter[image_level](v, u) * kai_level[5]) +
					inv_d * f_inv *
						(-kai_level[2] + z * kai_level[4] -
						 xx_inter[image_level](v, u) * kai_level[3]);

				// error_measurement(v,u) = j1*(j1*var11+j2*var12+j3*var13) +
				// j2*(j1*var12+j2*var22+j3*var23)
				//						+j3*(j1*var13+j2*var23+j3*var33) +
				// j4*j4*var44
				//+
				// j5*j5*var55 + j6*j6*var66;

				const float error_m =
					j4 * j4 * var44 + j5 * j5 * var55 + j6 * j6 * var66;

				//					Compute linearization error
				//-----------------------------------------------------------------------
				const float ini_du = depth_old[image_level](v, u + 1) -
									 depth_old[image_level](v, u - 1);
				const float ini_dv = depth_old[image_level](v + 1, u) -
									 depth_old[image_level](v - 1, u);
				const float final_du = depth_warped[image_level](v, u + 1) -
									   depth_warped[image_level](v, u - 1);
				const float final_dv = depth_warped[image_level](v + 1, u) -
									   depth_warped[image_level](v - 1, u);

				const float dut = ini_du - final_du;
				const float dvt = ini_dv - final_dv;
				const float duu = du(v, u + 1) - du(v, u - 1);
				const float dvv = dv(v + 1, u) - dv(v - 1, u);
				const float dvu =
					dv(v, u + 1) -
					dv(v, u - 1);  // Completely equivalent to compute duv

				const float error_l =
					kdt * square(dt(v, u)) +
					kduv * (square(du(v, u)) + square(dv(v, u))) +
					k2dt * (square(dut) + square(dvt)) +
					k2duv * (square(duu) + square(dvv) + square(dvu));

				// Weight
				weights(v, u) = sqrt(1.f / (error_m + error_l));
			}

	// Normalize weights in the range [0,1]
	const float inv_max = 1.f / weights.maxCoeff();
	weights *= inv_max;
}

void CDifodo::solveOneLevel()
{
	MatrixXf A(num_valid_points, 6);
	MatrixXf B(num_valid_points, 1);
	unsigned int cont = 0;

	// Fill the matrix A and the vector B
	// The order of the unknowns is (vz, vx, vy, wz, wx, wy)
	// The points order will be (1,1), (1,2)...(1,cols-1), (2,1),
	// (2,2)...(row-1,cols-1).

	const float f_inv = float(cols_i) / (2.f * tan(0.5f * fovh));

	for (unsigned int u = 1; u < cols_i - 1; u++)
		for (unsigned int v = 1; v < rows_i - 1; v++)
			if (null(v, u) == false)
			{
				// Precomputed expressions
				const float d = depth_inter[image_level](v, u);
				const float inv_d = 1.f / d;
				const float x = xx_inter[image_level](v, u);
				const float y = yy_inter[image_level](v, u);
				const float dycomp = du(v, u) * f_inv * inv_d;
				const float dzcomp = dv(v, u) * f_inv * inv_d;
				const float tw = weights(v, u);

				// Fill the matrix A
				A(cont, 0) =
					tw * (1.f + dycomp * x * inv_d + dzcomp * y * inv_d);
				A(cont, 1) = tw * (-dycomp);
				A(cont, 2) = tw * (-dzcomp);
				A(cont, 3) = tw * (dycomp * y - dzcomp * x);
				A(cont, 4) = tw * (y + dycomp * inv_d * y * x +
								   dzcomp * (y * y * inv_d + d));
				A(cont, 5) = tw * (-x - dycomp * (x * x * inv_d + d) -
								   dzcomp * inv_d * y * x);
				B(cont, 0) = tw * (-dt(v, u));

				cont++;
			}

	// Solve the linear system of equations using weighted least squares
	const MatrixXf AtA = A.transpose() * A;
	const MatrixXf AtB = A.transpose() * B;
	VectorXf Var = AtA.ldlt().solve(AtB);

	// Covariance matrix calculation
	MatrixXf res = -B;
	for (unsigned int k = 0; k < 6; k++) res += Var(k) * A.col(k);

	est_cov =
		(1.f / float(num_valid_points - 6)) * AtA.inverse() * res.squaredNorm();

	// Update last velocity in local coordinates
	// (vx, vy, vz, wx, wy, wz)
	kai_loc_level.fromVector(Var);
}

void CDifodo::odometryCalculation()
{
	// Clock to measure the runtime
	mrpt::system::CTicTac clock;
	clock.Tic();

	// Build the gaussian pyramid
	if (fast_pyramid)
		buildCoordinatesPyramidFast();
	else
		buildCoordinatesPyramid();

	// Coarse-to-fines scheme
	for (unsigned int i = 0; i < ctf_levels; i++)
	{
		// Previous computations
		transformations[i].setIdentity();

		level = i;
		unsigned int s = pow(2.f, int(ctf_levels - (i + 1)));
		cols_i = cols / s;
		rows_i = rows / s;
		image_level =
			ctf_levels - i + round(log(float(m_width / cols)) / log(2.f)) - 1;

		// 1. Perform warping
		if (i == 0)
		{
			depth_warped[image_level] = depth[image_level];
			xx_warped[image_level] = xx[image_level];
			yy_warped[image_level] = yy[image_level];
		}
		else
			performWarping();

		// 2. Calculate inter coords and find null measurements
		calculateCoord();

		// 3. Compute derivatives
		calculateDepthDerivatives();

		// 4. Compute weights
		computeWeights();

		// 5. Solve odometry
		if (num_valid_points > 6) solveOneLevel();

		// 6. Filter solution
		filterLevelSolution();
	}

	// Update poses
	poseUpdate();

	// Save runtime
	execution_time = 1000.f * clock.Tac();
}

void CDifodo::filterLevelSolution()
{
	//		Calculate Eigenvalues and Eigenvectors
	//----------------------------------------------------------
	CMatrixFloat66 Bii;
	std::vector<float> eigenVals;

	if (est_cov.eig_symmetric(Bii, eigenVals))
	{
		std::cerr
			<< "\n Eigensolver couldn't find a solution. Pose is not updated\n";
		return;
	}

	// First, we have to describe both the new linear and angular velocities in
	// the "eigenvector" basis
	//-------------------------------------------------------------------------------------------------
	auto kai_b = CVectorFixedFloat<6>(Bii.asEigen().colPivHouseholderQr().solve(
		kai_loc_level.asVector<Matrix<float, 6, 1>>()));

	// Second, we have to describe both the old linear and angular velocities in
	// the "eigenvector" basis
	//-------------------------------------------------------------------------------------------------
	auto kai_loc_sub = kai_loc_old.asVector<Eigen::Matrix<float, 6, 1>>();

	// Important: we have to substract the previous levels' solutions from the
	// old velocity.
	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 0; i < level; i++)
		acu_trans = transformations[i].asEigen() * acu_trans;

	auto mat_aux = CMatrixDouble44(acu_trans.cast<double>());
	auto acu_trans_vec = poses::Lie::SE<3>::log(poses::CPose3D(mat_aux));
	acu_trans_vec *= fps;
	const auto kai_level_acu = CVectorFixedDouble<6>(acu_trans_vec);

	kai_loc_sub -= kai_level_acu.asEigen().cast<float>();

	// Matrix<float, 4, 4> log_trans = fps*acu_trans.log();
	// kai_loc_sub(0) -= log_trans(0,3); kai_loc_sub(1) -= log_trans(1,3);
	// kai_loc_sub(2) -= log_trans(2,3);
	// kai_loc_sub(3) += log_trans(1,2); kai_loc_sub(4) -= log_trans(0,2);
	// kai_loc_sub(5) += log_trans(0,1);

	// Transform that local representation to the "eigenvector" basis
	const Matrix<float, 6, 1> kai_b_old =
		Bii.asEigen().colPivHouseholderQr().solve(kai_loc_sub);

	//									Filter velocity
	//--------------------------------------------------------------------------------
	const float cf = previous_speed_eig_weight * expf(-int(level)),
				df = previous_speed_const_weight * expf(-int(level));
	Matrix<float, 6, 1> kai_b_fil;
	for (unsigned int i = 0; i < 6; i++)
		kai_b_fil(i) =
			(kai_b.asEigen()(i) + (cf * eigenVals[i] + df) * kai_b_old(i)) /
			(1.f + cf * eigenVals[i] + df);

	// Transform filtered velocity to the local reference frame
	Matrix<float, 6, 1> kai_loc_fil =
		Bii.asEigen().inverse().colPivHouseholderQr().solve(kai_b_fil);

	// Compute the rigid transformation
	auto aux_vel =
		mrpt::math::CVectorFixedDouble<6>(kai_loc_fil.cast<double>() / fps);
	const poses::CPose3D aux2 = mrpt::poses::Lie::SE<3>::exp(aux_vel);

	CMatrixDouble44 trans;
	aux2.getHomogeneousMatrix(trans);
	transformations[level] = trans.cast_float();
}

void CDifodo::poseUpdate()
{
	// First, compute the overall transformation
	//---------------------------------------------------
	Matrix4f acu_trans;
	acu_trans.setIdentity();
	for (unsigned int i = 1; i <= ctf_levels; i++)
		acu_trans = transformations[i - 1].asEigen() * acu_trans;

	// Compute the new estimates in the local and absolutes reference frames
	//---------------------------------------------------------------------
	auto mat_aux = CMatrixDouble44(acu_trans.cast<double>().eval());
	auto acu_trans_vec = poses::Lie::SE<3>::log(poses::CPose3D(mat_aux));
	acu_trans_vec *= fps;
	const CVectorFixedDouble<6> kai_level_acu(acu_trans_vec);
	kai_loc.fromVector(kai_level_acu.cast_float());

	//---------------------------------------------------------------------------------------------
	// Directly from Eigen:
	//- Eigen 3.1.0 needed for Matrix::log()
	//- The line "#include <unsupported/Eigen/MatrixFunctions>" should be
	// uncommented (CDifodo.h)
	//
	// Matrix<float, 4, 4> log_trans = fps*acu_trans.log();
	// kai_loc(0) = log_trans(0,3); kai_loc(1) = log_trans(1,3); kai_loc(2) =
	// log_trans(2,3);
	// kai_loc(3) = -log_trans(1,2); kai_loc(4) = log_trans(0,2); kai_loc(5) =
	// -log_trans(0,1);
	//---------------------------------------------------------------------------------------------

	CMatrixDouble33 inv_trans;

	cam_pose.getRotationMatrix(inv_trans);
	const auto v_abs =
		(inv_trans.asEigen() *
		 (kai_loc.asVector<Eigen::Matrix<double, 6, 1>>().topRows(3)))
			.eval();
	const auto w_abs =
		(inv_trans.asEigen() *
		 (kai_loc.asVector<Eigen::Matrix<double, 6, 1>>().bottomRows(3)))
			.eval();
	kai_abs.vx = v_abs.x();
	kai_abs.vy = v_abs.y();
	kai_abs.vz = v_abs.z();

	kai_abs.wx = w_abs.x();
	kai_abs.wy = w_abs.y();
	kai_abs.wz = w_abs.z();

	//						Update poses
	//-------------------------------------------------------
	cam_oldpose = cam_pose;
	const auto pose_aux = poses::CPose3D(CMatrixDouble44(acu_trans));
	cam_pose = cam_pose + pose_aux;

	// Compute the velocity estimate in the new ref frame (to be used by the
	// filter in the next iteration)
	//---------------------------------------------------------------------------------------------------
	cam_pose.getRotationMatrix(inv_trans);
	const auto old_vtrans =
		(inv_trans.asEigen().inverse() *
		 (kai_abs.asVector<Eigen::Matrix<double, 6, 1>>().topRows(3)))
			.eval();
	const auto old_w =
		(inv_trans.asEigen().inverse() *
		 (kai_abs.asVector<Eigen::Matrix<double, 6, 1>>().bottomRows(3)))
			.eval();

	kai_loc_old.vx = old_vtrans.x();
	kai_loc_old.vy = old_vtrans.y();
	kai_loc_old.vz = old_vtrans.z();

	kai_loc_old.wx = old_w.x();
	kai_loc_old.wy = old_w.y();
	kai_loc_old.wz = old_w.z();
}

void CDifodo::setFOV(float new_fovh, float new_fovv)
{
	fovh = M_PI * new_fovh / 180.0;
	fovv = M_PI * new_fovv / 180.0;
}

void CDifodo::getPointsCoord(CMatrixFloat& x, CMatrixFloat& y, CMatrixFloat& z)
{
	x.resize(rows, cols);
	y.resize(rows, cols);
	z.resize(rows, cols);

	z = depth_inter[0];
	x = xx_inter[0];
	y = yy_inter[0];
}

void CDifodo::getDepthDerivatives(
	CMatrixFloat& cur_du, CMatrixFloat& cur_dv, CMatrixFloat& cur_dt)
{
	cur_du.resize(rows, cols);
	cur_dv.resize(rows, cols);
	cur_dt.resize(rows, cols);

	cur_du = du;
	cur_dv = dv;
	cur_dt = dt;
}

void CDifodo::getWeights(CMatrixFloat& w)
{
	w.resize(rows, cols);
	w = weights;
}


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#include <core/random.hpp>
#include <math/polygon.hpp>
#include <math/aabb2dpolygon.hpp>
#include <math/math.hpp>

#include <boost/random.hpp>
#include <cstdint>

namespace eXl
{
  class Serializer;

  class MathTools
  {
  public:

    //template <typename Real>
    //static Err Stream(Vector2<Real> const& iPoint, Serializer iStreamer);

    //template <typename Real>
    //static Err Unstream(Vector2<Real>& oPoint, Unstreamer& iUnstreamer);

    //template <typename Real>
    //static Err Stream(Polygon<Real> const& iPolygon, Streamer& iStreamer);
    //
    //template <typename Real>
    //static Err Unstream(Polygon<Real>& oPolygon, Unstreamer& iUnstreamer);
    //
    //template <typename Real>
    //static Err Stream(AABB2DPolygon<Real> const& iPolygon, Streamer& iStreamer);
    //
    //template <typename Real>
    //static Err Unstream(AABB2DPolygon<Real>& oPolygon, Unstreamer& iUnstreamer);

    //Clamp angle between 0 and 2*PI
    inline static float ClampAngle(float iAngle)
    {
      int div = iAngle / (Mathf::PI * 2);
      if(iAngle < 0)
      {
        --div;
      }
      return iAngle - div * Mathf::PI * 2;
    }

    // AngleDist between -PI and PI
    inline static float AngleDist(float iAngle1, float iAngle2)
    {
      float dist = ClampAngle(iAngle1) - ClampAngle(iAngle2);
      if(dist > Mathf::PI)
      {
        return 2.0 * Mathf::PI - dist;
      }
      else if(dist < -Mathf::PI)
      {
        return dist + 2.0 * Mathf::PI;
      }
      return dist;
    }

    template <typename Real>
    static inline Vector2<Real> Perp(Vector2<Real> const& iVec)
    {
      return Vector2<Real>(-iVec.Y(), iVec.X());
    }

    static inline uint64_t SquaredLength(Vector2i const& iVect)
    {
      return (int64_t)iVect.X() * (int64_t)iVect.X() + (int64_t)iVect.Y() * (int64_t)iVect.Y();
    }

    template <typename Real>
    static inline Vector2i ToIVec(Vector2<Real> const& iVec)
    {
      return Vector2i(Math<Real>::Round(iVec.X()), Math<Real>::Round(iVec.Y()));
    }

    template <typename Real>
    static inline Vector2f ToFVec(Vector2<Real> const& iVec)
    {
      return Vector2f(iVec.X(), iVec.Y());
    }

    template <typename Real>
    static inline Vector2d ToDVec(Vector2<Real> const& iVec)
    {
      return Vector2d(iVec.X(), iVec.Y());
    }

    template <typename Real>
    static inline Real Sign(Real iValue)
    {
      return iValue > 0 ? 1 : -1;
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, unsigned int &oSegNum, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true)
    {
      int dummyHole;
      Vector2<Real2> dummy;
      return NearestPointOnPoly(iPoint,iPoly, iMinSegLen, dummy, oSegNum, dummyHole, oPoint, oDir, iCheckExt, false);
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true)
    {
      int dummyHole;
      unsigned int dummy;
      return NearestPointOnPoly(iPoint,iPoly, iMinSegLen, oSeg, dummy, dummyHole, oPoint, oDir, iCheckExt, false);
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], unsigned int& oSegNum, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true)
    {
      int dummyHole;
      return NearestPointOnPoly(iPoint,iPoly, iMinSegLen, oSeg, oSegNum, dummyHole, oPoint, oDir, iCheckExt, false);
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], unsigned int& oSegNum, int& oHole, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true, bool iCheckHoles = false)
    {
      Real1 minDist;
      oHole = -1;
      bool res = _NearestPointOnPoly(iPoint, iPoly.Border(), iMinSegLen, oSeg, oSegNum, oPoint, oDir, minDist, iCheckExt);
      if(iCheckHoles)
      {
        for(unsigned int i = 0 ; i< iPoly.Holes().size(); ++i)
        {
          unsigned int segNum;
          Vector2<Real1> point;
          Vector2<Real2> seg[2];
          Vector2<Real1>    dir;
          Real1 curMin;
          bool newRes = _NearestPointOnPoly(iPoint, iPoly.Holes()[i], iMinSegLen, seg, segNum, point, dir, curMin, iCheckExt);
          if(newRes)
          {
            if(!res || curMin < minDist)
            {
              oHole = i;
              res = newRes;
              oSegNum = segNum;
              oPoint = point;
              oDir = dir;
              oSeg[0] = seg[0];
              oSeg[1] = seg[1];
              minDist = curMin;
            }
          }
        }
      }
      return res;
    }

    template <typename Real1, typename Real2>
    static inline bool _NearestPointOnPoly(Vector2<Real1> const& iPoint, Vector<Vector2<Real2> > const& polyBorder, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], unsigned int& oSegNum, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, Real1& oDist, bool iCheckExt)
    {
      Vector2<Real1> polyCenter;
      for(auto point : polyBorder)
      {
        polyCenter += Vector2<Real1>(point.X(), point.Y());
      }

      if(polyBorder.front() == polyBorder.back())
      {
        polyCenter -= Vector2<Real1>(polyBorder.front().X(), polyBorder.front().Y());
        polyCenter = polyCenter / (polyBorder.size() - 1);
      }
      else
      {
        polyCenter = polyCenter / polyBorder.size();
      }

      //Vector2<Real1> iDir = polyCenter - iPoint;
      //Real1 minDist = iDir.Normalize();

      Vector2<Real1> prevPoint(polyBorder.back().X(), polyBorder.back().Y());

      Vector2<Real1> iDir = polyCenter - iPoint;
      iDir.Normalize();
      Real1 minDist = (iPoint - prevPoint).Length() + 1;

      for (unsigned int i = 0; i < polyBorder.size(); ++i)
      {
        Vector2<Real1> curPoint(polyBorder[i].X(), polyBorder[i].Y());

        if (curPoint != prevPoint)
        {
          Vector2<Real1> dir = curPoint - prevPoint;
          Vector2<Real1> extDir(MathTools::Perp(dir));

          if (!iCheckExt || (iDir).Dot(extDir) < 0)
          {
            Real1 segLen = dir.Normalize();
            if (segLen > iMinSegLen)
            {
              Vector2<Real1> prevPointOff = prevPoint + dir * 0.5 * iMinSegLen;
              Vector2<Real1> curPointOff = curPoint - dir * 0.5 * iMinSegLen;
              segLen -= iMinSegLen;
              Real1 dirPoj = (iPoint - prevPointOff).Dot(dir);
              Vector2<Real1> candidate;

              if (dirPoj < -Math<Real1>::ZERO_TOLERANCE)
              {
                candidate = prevPointOff;
              }
              else if (dirPoj > segLen - Math<Real1>::ZERO_TOLERANCE)
              {
                candidate = curPointOff;
              }
              else
              {
                candidate = prevPointOff + (dir * dirPoj);
              }

              Real1 curDist = (candidate - iPoint).Length();
              if (curDist < minDist)
              {
                oSeg[0] = Vector2<Real2>(prevPoint.X(), prevPoint.Y());
                oSeg[1] = Vector2<Real2>(curPoint.X(), curPoint.Y());
                if(i == 0)
                  oSegNum = polyBorder.size() - 1;
                else
                  oSegNum = i - 1;
                oPoint = candidate;
                oDir = extDir;
                oDir.Normalize();
                minDist = curDist;
              }
            }
          }
        }

        prevPoint = curPoint;
      }

      oDist = minDist;

      if(minDist != (iPoint - polyCenter).Length())
        return true;
      else
        return false;
    }

    template <typename Real>
    static void SimplifyPolygon(Polygon<Real> const& iPoly1, double iEpsilon, Polygon<Real>& oPoly);

    template <typename Real1, typename Real2>
    static inline unsigned int FindPolyIntersection(Vector2<Real1> const& iPoint1, Vector2<Real1> const& iPoint2, Polygon<Real2> const& iPoly, Vector2<Real1>(&oPoints)[2])
    {
      unsigned int curInter = 0;
      Vector<Vector2<Real2> > const& border = iPoly.Border();
      Vector2<Real1> prevPoint(border.back().X(), border.back().Y());
      for (unsigned int i = 0; i < border.size() && curInter < 2; ++i)
      {
        Vector2<Real1> curPoint(border[i].X(), border[i].Y());

        unsigned int res = Segment<Real1>::Intersect(prevPoint, curPoint, iPoint1, iPoint2, oPoints[curInter]);
        if (res & Segmentd::PointOnSegment1)
        {
          if ((curInter == 0 || (oPoints[0] - oPoints[curInter]).Length() > Math<Real1>::ZERO_TOLERANCE))
          {
            ++curInter;
          }
        }
        prevPoint = curPoint;
      }
      return curInter;
    }

    template <typename Real1, typename Real2>
    static bool FindCommonSegment(Polygon<Real2> const& iPoly1, Polygon<Real2> const& iPoly2, Vector2<Real1>& oSegPt1, Vector2<Real1>& oSegPt2, Real1 iMinLen = Math<Real1>::ZERO_TOLERANCE)
    {
      bool foundCommon = false;
      Vector2<Real1> prevPt1 = Vector2<Real1>(iPoly1.Border().back().X(),iPoly1.Border().back().Y());
      for(auto curPt1I : iPoly1.Border())
      {
        Vector2<Real1> curPt1 = Vector2<Real1>(curPt1I.X(),curPt1I.Y());
        if(prevPt1 != curPt1)
        {
          Vector2<Real1> prevPt2 = Vector2<Real1>(iPoly2.Border().back().X(), iPoly2.Border().back().Y());
          for(auto curPt2I : iPoly2.Border())
          {
            Vector2<Real1> curPt2 = Vector2<Real1>(curPt2I.X(),curPt2I.Y());
            if(prevPt2 != curPt2)
            {
                unsigned int res = Segment<Real1>::Intersect(prevPt1, curPt1, prevPt2, curPt2);
                if(res & Segment<Real1>::AlignedSegments
                && (res & Segment<Real1>::ConfoundSegments || res & Segmentd::PointOnSegments))
                {
                  Vector2<Real1> dir = curPt1 - prevPt1;
                  AABB2D<Real1> box1(0.0,0.0, dir.Normalize(), 0.0);
                  double val1 = (prevPt2 - prevPt1).Dot(dir);
                  double val2 = (curPt2 - prevPt1).Dot(dir);
                  AABB2D<Real1> box2 = val1 > val2 ? AABB2D<Real1>(val2, 0.0, val1, 0.0) : AABB2D<Real1>(val1, 0.0, val2, 0.0);
                  AABB2D<Real1> result;
                  result.SetCommonBox(box1, box2);
                  double commonLen = result.m_Data[1].X() - result.m_Data[0].X();
                  if(commonLen >= iMinLen)
                  {
                    foundCommon = true;
                    oSegPt1 = prevPt1 + dir * result.m_Data[0].X();
                    oSegPt2 = prevPt1 + dir * (result.m_Data[0].X() + commonLen);
                    break;
                  }
                }
            }
            prevPt2 = curPt2;
          }
        }
        if(foundCommon)
          break;
        prevPt1 = curPt1;
      }
      return foundCommon;
    }

    template <typename Real>
    static inline Real GetAngleFromVec(Vector2<Real> const& iDir)
    {
      return Math<Real>::Abs(iDir.X()) < Math<Real>::EPSILON ? (iDir.Y() > 0.0 ? Math<Real>::PI * 0.5 : Math<Real>::PI * 1.5)
        : (iDir.X() > 0.0 ? Math<Real>::ATan(iDir.Y() / iDir.X()) : Math<Real>::PI - Math<Real>::ATan(iDir.Y() / (-1.0 * iDir.X())));
    }

    template<typename Real>
    static inline Vector2<Real> GetPerp(Vector2<Real> const& iVec)
    {
      return Vector2<Real>(iVec.Y() * -1.0, iVec.X());
    }

    template<typename Real>
    static inline Vector2<Real> GetLocal(Vector2<Real> const& iBase, Vector2<Real> const& iVec)
    {
      return Vector2<Real>(iBase.X() * iVec.X() + iBase.Y() * iVec.Y(), iBase.X() * iVec.Y() - iBase.Y() * iVec.X());
    }

    template<typename Real>
    static inline Vector2i RoundVector(Vector2<Real> const& iVec)
    {
      return Vector2i(Math<Real>::Round(iVec.X()), Math<Real>::Round(iVec.Y()));
    }

    template <typename Real>
    static inline Real ModAngle(Real iAngle)
    {
      iAngle = fmod(iAngle, 2.0*Math<Real>::PI);
      iAngle -= Mathf::PI;
      return iAngle;
    }

    template <typename Real>
    static inline Real RandFloatIn(Random& randGen, Real iMin, Real iMax)
    {
      return Real(randGen() % 10000) / 10000.0 * (iMax - iMin) + iMin;
    };

    template <typename Real>
    static inline Real RandNormFloatIn(Random& randGen, Real iMin, Real iMax)
    {
      boost::random::normal_distribution<Real> distrib;
      RandomWrapper randW(&randGen);
      Real rangeValNorm = distrib(randW);
      Real rangeVal = rangeValNorm * ((iMax-iMin) * 0.33);
      Real offsetVal = rangeVal + (iMax + iMin) * 0.5;

      return Math<Real>::Clamp( offsetVal , iMin, iMax);
    };

    static inline int RandIntIn(Random& randGen, int iMin, int iMax)
    {
      return (randGen() % (iMax - iMin)) + iMin;
    };

    static inline Vector2f Perturbate(Random& iRand, Vector2f const& iPos, float iRadius)
    {
      float const angle = ((iRand.Generate() % 10000) / 10000.0 - 1.0) * Mathf::PI;
      float const dist = ((iRand.Generate() % 10000) / 10000.0 ) * iRadius;

      return iPos + Vector2f(Mathd::Cos(angle), Mathd::Sin(angle)) * dist;
    }

    template <typename Real>
    static inline Vector2<Real> const& As2DVec(Vector3<Real> const& iVec)
    {
      return reinterpret_cast<Vector2<Real> const&>(iVec);
    }

    template <typename Real>
    static inline Vector2<Real>& As2DVec(Vector3<Real>& iVec)
    {
      return reinterpret_cast<Vector2<Real>&>(iVec);
    }

    template <typename Real>
    static inline Vector3<Real> To3DVec(Vector2<Real> const& iVec, Real iZComp = 0)
    {
      return Vector3<Real>(iVec.X(), iVec.Y(), iZComp);
    }

    template <typename Real>
    static inline Vector3<Real> const& GetPosition(Matrix4<Real> const& iMat)
    {
      return *reinterpret_cast<Vector3<Real> const*>(iMat.m_Data + 12);
    }

    template <typename Real>
    static inline Vector2<Real> const& GetPosition2D(Matrix4<Real> const& iMat)
    {
      return *reinterpret_cast<Vector2<Real> const*>(iMat.m_Data + 12);
    }

    template <typename Real>
    static inline Vector3<Real>& GetPosition(Matrix4<Real>& iMat)
    {
      return *reinterpret_cast<Vector3<Real>*>(iMat.m_Data + 12);
    }

    template <typename Real>
    static inline Vector2<Real>& GetPosition2D(Matrix4<Real>& iMat)
    {
      return *reinterpret_cast<Vector2<Real>*>(iMat.m_Data + 12);
    }

    // Prereq : points have to be in CCW order
    template <typename Real>
    static Vector2<Real> ConeGetMidSegment(Vector2<Real> const (&iRange)[2])
    {
      Vector2<Real> mid = iRange[0] + iRange[1];
      Real dist = mid.Normalize();
      if (dist < Math<Real>::ZERO_TOLERANCE)
      {
        mid = GetPerp(iRange[0]);
      }
      mid *= Math<Real>::Sign(Segment<Real>::Cross(iRange[0], mid));

      return mid;
    }

    template <typename Real>
    static void ConeUpdateRange(Vector2<Real> const (&iRange)[2], Vector2<Real>& oMid, Real& oLowLimit)
    {
      oMid = ConeGetMidSegment(iRange);
      oLowLimit = iRange[0].Dot(oMid) / iRange[0].Length();
    }

    template <typename Real>
    static bool IsInCone(Vector2<Real> const& iDir, Vector2<Real> const& iMidSeg, Real iLowLimit, Real iEpsilon = Math<Real>::EPSILON)
    {
      return iDir.Dot(iMidSeg) > (iLowLimit - iEpsilon);
    }

    template <typename Real>
    static Vector3<Real> Reflect(Vector3<Real> const& iIncomingDir, Vector3<Real> const& iNormal)
    {
      return iIncomingDir - iNormal * iIncomingDir.Dot(iNormal) * 2;
    }

    template <typename Real>
    static void ClampToBox(Vector2<Real>& ioPoint, AABB2D<Real> const& iBox)
    {
      for (uint32_t i = 0; i < 2; ++i)
      {
        ioPoint.m_Data[i] = Math<Real>::Clamp(ioPoint.m_Data[i], iBox.m_Data[0].m_Data[i], iBox.m_Data[1].m_Data[i]);
      }
    }

    //static void AlignVector(Vector2d& ioDir, Vector2i& oCanonicalVector, unsigned int iFactor)
    //{
    //  if (Mathd::Abs(ioDir.X()) > Mathd::ZERO_TOLERANCE)
    //  {
    //    if (Mathd::Abs(ioDir.Y()) > Mathd::ZERO_TOLERANCE)
    //    {
    //      double ratio = (ioDir.X() / ioDir.Y()));
    //      int sign = Sign(ratio);
    //      ratio = ratio * sign;
    //      if (ratio > 1)
    //      {
    //        ratio = 1 / ratio;
    //        unsigned int ratioI = Mathi::Round(ratio * iFactor);
    //        oCanonicalVector = Vector2i(Sign(ioDir.X()) * iFactor, Sign(ioDir.Y()) * ratioI);
    //      }
    //      else
    //      {
    //        unsigned int ratioI = Mathi::Round(ratio * iFactor);
    //        oCanonicalVector = Vector2i(Sign(ioDir.X()) * ratioI, Sign(ioDir.Y()) * iFactor);
    //      }
    //      ioDir = MathTools::ToDVec(oCanonicalVector) * ioDir.Length() / Mathd::Sqrt(ratioI*ratioI + iFactor*iFactor);
    //      return;
    //    }
    //  }
    //  ioDir = Vector2d::ZERO;
    //  oCanonicalVector = Vector2i::ZERO;
    //}
    //
    //static void AlignSegment(Vector2d& ioPoint1, Vector2d ioPoint2, unsigned int iFactor)
    //{
    //
    //}

  };

  //template <>
  //EXL_MATH_API Err MathTools::Stream<float>(Vector2f const& iPoint, Serializer iStreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Unstream<float>(Vector2f& oPoint, Unstreamer& iUnstreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Stream<int>(Vector2i const& iPoint, Serializer iStreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Unstream<int>(Vector2i& oPoint, Unstreamer& iUnstreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Stream<int>(Polygoni const& iPoint, Streamer& iStreamer);
  //
  //template <>
  //EXL_MATH_API Err MathTools::Unstream<int>(Polygoni& oPoint, Unstreamer& iUnstreamer);
  //
  //template <>
  //EXL_MATH_API Err MathTools::Stream<int>(AABB2DPolygoni const& iPoint, Streamer& iStreamer);
  //
  //template <>
  //EXL_MATH_API Err MathTools::Unstream<int>(AABB2DPolygoni& oPoint, Unstreamer& iUnstreamer);

  template <>
  EXL_MATH_API void MathTools::SimplifyPolygon<int>(Polygoni const& iPoly1, double iEpsilon, Polygoni& oPoly);
  //template <>
  //EXL_GEN_API void MathTools::SimplifyPolygon<float>(Polygonf const& iPoly1, double iEpsilon, Polygonf& oPoly);
  //template <>
  //EXL_GEN_API void MathTools::SimplifyPolygon<double>(Polygond const& iPoly1, double iEpsilon, Polygond& oPoly);

  //template <typename Real>
  //struct StreamerTemplateHandler<Polygon<Real>>
  //{
  //  static Err Do(Streamer& iStreamer, Polygon<Real> const* iObj)
  //  {
  //    return MathTools::Stream<Real>(*iObj, iStreamer);
  //  }
  //};
  //
  //template <typename Real>
  //struct UnstreamerTemplateHandler<Polygon<Real>>
  //{
  //  static Err Do(Unstreamer& iUnstreamer, Polygon<Real>* iObj)
  //  {
  //    return MathTools::Unstream<Real>(*iObj, iUnstreamer);
  //  }
  //};

  EXL_MATH_API uint32_t Factorial(uint32_t i);

  struct EXL_MATH_API CombinationHelper_Iter
  {
    // Compute K amongst N with as an iterator adaptor.
    CombinationHelper_Iter(uint32_t iSize, uint32_t iNum, bool iIsEnd);

    void ComputeMaxStep(uint32_t iSize, uint32_t iNum);

    Vector<uint32_t> const& operator*() const { return m_Stack; }

    CombinationHelper_Iter& operator ++()
    {
      Advance();
      return *this;
    }

    bool operator == (CombinationHelper_Iter const& iOther) const
    {
      return m_Step == iOther.m_Step;
    }
    bool operator != (CombinationHelper_Iter const& iOther) const
    {
      return !(*this == iOther);
    }

    void Advance();

    Vector<uint32_t> m_Stack;
    uint32_t m_Size;
    uint32_t m_Num;
    uint32_t m_Step;
    uint32_t m_MaxStep;
  };

  struct CombinationHelper
  {
    CombinationHelper(uint32_t iSize, uint32_t iNum)
      : m_Size(iSize)
      , m_Num(iNum)
    {

    }

    CombinationHelper_Iter begin() const { return CombinationHelper_Iter(m_Size, m_Num, false); }
    CombinationHelper_Iter end() const { return CombinationHelper_Iter(m_Size, m_Num, true); }

    uint32_t m_Size;
    uint32_t m_Num;
  };

  template <typename T>
  struct TCombinationHelper_Set_Iter
  {
    TCombinationHelper_Set_Iter(Vector<T> const& iElements, CombinationHelper_Iter const& iIter, bool iEnd)
      : m_Elements(iElements)
      , m_Iter(iIter)
    {
      if (iEnd)
      {
        m_Cur = iIter.m_Num;
      }
      else
      {
        m_Cur = 0;
      }
    }

    T const& operator*()
    {
      return m_Elements[(*m_Iter)[m_Cur]];
    }

    TCombinationHelper_Set_Iter& operator ++()
    {
      ++m_Cur;
      return *this;
    }

    bool operator == (TCombinationHelper_Set_Iter const& iOther) const
    {
      return m_Cur == iOther.m_Cur;
    }
    bool operator != (TCombinationHelper_Set_Iter const& iOther) const
    {
      return !(*this == iOther);
    }

    uint32_t m_Cur;
    Vector<T> const& m_Elements;
    CombinationHelper_Iter const& m_Iter;
  };

  template <typename T>
  struct TCombinationHelper_Set
  {
    TCombinationHelper_Set(Vector<T> const& iElements, CombinationHelper_Iter const& iIter)
      : m_Elements(iElements)
      , m_Iter(iIter)
    {

    }

    TCombinationHelper_Set_Iter<T> begin() const { return TCombinationHelper_Set_Iter<T>(m_Elements, m_Iter, false); }
    TCombinationHelper_Set_Iter<T> end() const { return TCombinationHelper_Set_Iter<T>(m_Elements, m_Iter, true); }

    Vector<T> const& m_Elements;
    CombinationHelper_Iter const& m_Iter;
  };

  template <typename T>
  struct TCombinationHelper_Iter
  {
    TCombinationHelper_Iter(Vector<T> const& iElements, uint32_t iNum, bool iEnd)
      : m_Elements(iElements)
      , m_Iter(iElements.size(), iNum, iEnd)
    {

    }

    TCombinationHelper_Set<T> operator*()
    {
      return TCombinationHelper_Set<T>(m_Elements, m_Iter);
    }

    TCombinationHelper_Iter& operator ++()
    {
      ++m_Iter;
      return *this;
    }

    bool operator == (TCombinationHelper_Iter const& iOther) const
    {
      return m_Iter == iOther.m_Iter;
    }
    bool operator != (TCombinationHelper_Iter const& iOther) const
    {
      return !(*this == iOther);
    }

    Vector<T> const& m_Elements;
    CombinationHelper_Iter m_Iter;
  };

  template <typename T>
  struct TCombinationHelper
  {
    TCombinationHelper(Vector<T> const& iElements, uint32_t iNum)
      : m_Elements(iElements)
      , m_Num(iNum)
    {

    }

    TCombinationHelper_Iter<T> begin() const { return TCombinationHelper_Iter<T>(m_Elements, m_Num, false); }
    TCombinationHelper_Iter<T> end() const { return TCombinationHelper_Iter<T>(m_Elements, m_Num, true); }

    Vector<T> const& m_Elements;
    uint32_t m_Num;
  };
}


#include "refill/system_models/system_model_base.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include "refill/distributions/gaussian_distribution.h"

namespace refill {

class SystemModelMockClass : public SystemModelBase {
 public:
  SystemModelMockClass(const size_t& state_dim,
                       const GaussianDistribution& system_noise)
      : SystemModelBase(state_dim, system_noise) {
  }
  SystemModelMockClass(const size_t& state_dim,
                       const GaussianDistribution& system_noise,
                       const size_t& input_dim)
      : SystemModelBase(state_dim, system_noise, input_dim) {
  }
  Eigen::VectorXd propagate(const Eigen::VectorXd& state,
                            const Eigen::VectorXd& input,
                            const Eigen::VectorXd& noise) const {
    return state + input + noise;
  }
  void setModelParameters(const size_t& state_dim,
                          const DistributionInterface& system_noise) {
    this->setSystemModelBaseParameters(state_dim, system_noise);
  }
  void setModelParameters(const size_t& state_dim,
                          const DistributionInterface& system_noise,
                          const size_t& input_dim) {
    this->setSystemModelBaseParameters(state_dim, system_noise, input_dim);
  }
};

TEST(SystemModelBaseTest, NoInputTest) {
  GaussianDistribution system_noise(Eigen::Vector3d::Zero(),
                                    Eigen::Matrix3d::Identity());

  // Test constructor with two parameters
  SystemModelMockClass system_model(3, system_noise);

  ASSERT_EQ(3, system_model.getStateDim());
  ASSERT_EQ(0, system_model.getInputDim());
  ASSERT_EQ(3, system_model.getNoiseDim());
  ASSERT_EQ(Eigen::Vector3d::Zero(), system_model.getNoise()->mean());
  ASSERT_EQ(Eigen::Matrix3d::Identity(), system_model.getNoise()->cov());

  system_noise.setDistributionParameters(Eigen::Vector2d::Ones(),
                                         Eigen::Matrix2d::Identity() * 2.0);

  system_model.setModelParameters(2, system_noise);

  ASSERT_EQ(2, system_model.getStateDim());
  ASSERT_EQ(0, system_model.getInputDim());
  ASSERT_EQ(2, system_model.getNoiseDim());
  ASSERT_EQ(Eigen::Vector2d::Ones(), system_model.getNoise()->mean());
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 2.0,
            system_model.getNoise()->cov());

  Eigen::Matrix2d state_samples = Eigen::Matrix2d::Ones();
  Eigen::Matrix2d noise_samples = Eigen::Matrix2d::Ones();
  Eigen::Vector2d input = Eigen::Vector2d::Ones();

  Eigen::MatrixXd vectorized_propagation = system_model.propagateVectorized(
      state_samples, input, noise_samples);

  ASSERT_EQ(2, vectorized_propagation.rows());
  ASSERT_EQ(4, vectorized_propagation.cols());
  ASSERT_EQ(Eigen::MatrixXd::Ones(2, 4) * 3.0, vectorized_propagation);
}

TEST(SystemModelBaseTest, WithInputTest) {
  GaussianDistribution system_noise(Eigen::Vector3d::Zero(),
                                    Eigen::Matrix3d::Identity());

  // Test constructor with two parameters
  SystemModelMockClass system_model(3, system_noise, 3);

  ASSERT_EQ(3, system_model.getStateDim());
  ASSERT_EQ(3, system_model.getInputDim());
  ASSERT_EQ(3, system_model.getNoiseDim());
  ASSERT_EQ(Eigen::Vector3d::Zero(), system_model.getNoise()->mean());
  ASSERT_EQ(Eigen::Matrix3d::Identity(), system_model.getNoise()->cov());

  system_noise.setDistributionParameters(Eigen::Vector2d::Ones(),
                                         Eigen::Matrix2d::Identity() * 2.0);

  system_model.setModelParameters(2, system_noise, 2);

  ASSERT_EQ(2, system_model.getStateDim());
  ASSERT_EQ(2, system_model.getInputDim());
  ASSERT_EQ(2, system_model.getNoiseDim());
  ASSERT_EQ(Eigen::Vector2d::Ones(), system_model.getNoise()->mean());
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 2.0,
            system_model.getNoise()->cov());

  Eigen::Matrix2d state_samples = Eigen::Matrix2d::Ones();
  Eigen::Matrix2d noise_samples = Eigen::Matrix2d::Ones();
  Eigen::Vector2d input = Eigen::Vector2d::Ones();

  Eigen::MatrixXd vectorized_propagation = system_model.propagateVectorized(
      state_samples, input, noise_samples);

  ASSERT_EQ(2, vectorized_propagation.rows());
  ASSERT_EQ(4, vectorized_propagation.cols());
  ASSERT_EQ(Eigen::MatrixXd::Ones(2, 4) * 3.0, vectorized_propagation);
}

}  // namespace refill


#define BOOST_TEST_MODULE "test_tabulated_wca_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/global/TabulatedWCAPotential.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>

BOOST_AUTO_TEST_CASE(WCA_double)
{
    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = typename traits_type::real_type;
    using molecule_id_type = mjolnir::Topology::molecule_id_type;
    using group_id_type    = mjolnir::Topology::group_id_type;
    using potential_type   = mjolnir::TabulatedWCAPotential<traits_type>;
    using parameter_type   = potential_type::pair_parameter_type;

    constexpr static std::size_t N = 10000;
    constexpr static real_type   h = 1e-6;

    const real_type sigma   = 3.0;
    const real_type epsilon = 1.0;
    const parameter_type param{sigma, epsilon};
    potential_type wca{
        potential_type::default_cutoff(),
        {{"A:B", {sigma, epsilon}}},
        {{0, "A"}, {1, "B"}}, {},
        mjolnir::IgnoreMolecule<molecule_id_type>("Nothing"),
        mjolnir::IgnoreGroup   <group_id_type   >({})
    };

    const real_type x_min = 0.8 * sigma;
    const real_type x_max = wca.cutoff_ratio() * sigma - 2 * h;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = wca.potential(0, 1, x + h);
        const real_type pot2 = wca.potential(0, 1, x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = wca.derivative(0, 1, x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x   = x_max + i * dx;
        const real_type pot = wca.potential(0, 1, x);
        BOOST_TEST(pot == 0.0, boost::test_tools::tolerance(h));
    }
}


//==================================================================================================
/*!
    @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_ARCH_X86_SSE2_SIMD_FUNCTION_NBTRUE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_NBTRUE_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/popcnt.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/simd/detail/make_dependent.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( nbtrue_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::integer_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE std::size_t operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      using i8type = typename detail::make_dependent<pack<int8_t,16>, A0>::type;
      i8type tmp = bitwise_cast<i8type>(genmask(a0));
      return bs::popcnt(_mm_movemask_epi8(tmp)) * bs::cardinal_of<A0>::value >> 4;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( nbtrue_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bs::logical_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE std::size_t operator() ( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      using i8type = typename detail::make_dependent<pack<int8_t,16>, A0>::type;
      i8type tmp = bitwise_cast<i8type>(genmask(a0));
      return bs::popcnt(_mm_movemask_epi8(tmp)) * bs::cardinal_of<A0>::value >> 4;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( nbtrue_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE std::size_t operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return bs::popcnt(_mm_movemask_pd(genmask(a0)));
    }
  };

} } }

#endif


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

#ifndef NETKET_METROPOLISHAMILTONIAN_PT_HPP
#define NETKET_METROPOLISHAMILTONIAN_PT_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

namespace netket {

// Metropolis sampling generating transitions using the Hamiltonian
template <class H>
class MetropolisHamiltonianPt : public AbstractSampler {
  H &hamiltonian_;

  // number of visible units
  const int nv_;

  const int nrep_;
  std::vector<double> beta_;

  // states of visible units
  std::vector<Eigen::VectorXd> v_;
  Eigen::VectorXd v1_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  int mynode_;
  int totalnodes_;

  // Look-up tables
  std::vector<typename AbstractMachine::LookupType> lt_;

  std::vector<std::vector<int>> tochange_;
  std::vector<std::vector<double>> newconfs_;
  std::vector<Complex> mel_;

  std::vector<std::vector<int>> tochange1_;
  std::vector<std::vector<double>> newconfs1_;
  std::vector<Complex> mel1_;

  int sweep_size_;

 public:
  MetropolisHamiltonianPt(AbstractMachine &psi, H &hamiltonian, int nrep)
      : AbstractSampler(psi),
        hamiltonian_(hamiltonian),
        nv_(GetHilbert().Size()),
        nrep_(nrep) {
    Init();
  }

  void Init() {
    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    if (!GetHilbert().IsDiscrete()) {
      throw InvalidInputError(
          "Hamiltonian Metropolis sampler works only for discrete "
          "Hilbert spaces");
    }

    v_.resize(nrep_);
    for (int i = 0; i < nrep_; i++) {
      v_[i].resize(nv_);
    }

    for (int i = 0; i < nrep_; i++) {
      beta_.push_back(1. - double(i) / double(nrep_));
    }

    accept_.resize(2 * nrep_);
    moves_.resize(2 * nrep_);

    lt_.resize(nrep_);

    Reset(true);

    // Always use odd sweep size to avoid possible ergodicity problems
    if (nv_ % 2 == 0) {
      sweep_size_ = nv_ + 1;
    } else {
      sweep_size_ = nv_;
    }

    InfoMessage() << "Hamiltonian Metropolis sampler with parallel tempering "
                     "is ready "
                  << std::endl;
    InfoMessage() << nrep_ << " replicas are being used" << std::endl;
  }

  void Reset(bool initrandom = false) override {
    if (initrandom) {
      for (int i = 0; i < nrep_; i++) {
        GetHilbert().RandomVals(v_[i], this->GetRandomEngine());
      }
    }

    for (int i = 0; i < nrep_; i++) {
      GetMachine().InitLookup(v_[i], lt_[i]);
    }

    accept_ = Eigen::VectorXd::Zero(2 * nrep_);
    moves_ = Eigen::VectorXd::Zero(2 * nrep_);
  }

  void LocalSweep(int rep) {
    for (int i = 0; i < sweep_size_; i++) {
      hamiltonian_.FindConn(v_[rep], mel_, tochange_, newconfs_);

      const double w1 = tochange_.size();

      std::uniform_int_distribution<int> distrs(0, tochange_.size() - 1);
      std::uniform_real_distribution<double> distu(0, 1);

      // picking a random state to transit to
      int si = distrs(this->GetRandomEngine());

      // Inverse transition
      v1_ = v_[rep];
      GetHilbert().UpdateConf(v1_, tochange_[si], newconfs_[si]);

      hamiltonian_.FindConn(v1_, mel1_, tochange1_, newconfs1_);

      double w2 = tochange1_.size();

      const auto lvd = GetMachine().LogValDiff(v_[rep], tochange_[si],
                                               newconfs_[si], lt_[rep]);
      double ratio =
          this->GetMachineFunc()(std::exp(beta_[rep] * lvd)) * w1 / w2;

#ifndef NDEBUG
      const auto psival1 = GetMachine().LogVal(v_[rep]);
      if (std::abs(std::exp(GetMachine().LogVal(v_[rep]) -
                            GetMachine().LogVal(v_[rep], lt_[rep])) -
                   1.) > 1.0e-8) {
        std::cerr << GetMachine().LogVal(v_[rep]) << "  and LogVal with Lt is "
                  << GetMachine().LogVal(v_[rep], lt_[rep]) << std::endl;
        std::abort();
      }
#endif

      // Metropolis acceptance test
      if (ratio > distu(this->GetRandomEngine())) {
        accept_(rep) += 1;
        GetMachine().UpdateLookup(v_[rep], tochange_[si], newconfs_[si],
                                  lt_[rep]);
        v_[rep] = v1_;

#ifndef NDEBUG
        const auto psival2 = GetMachine().LogVal(v_[rep]);
        if (std::abs(std::exp(psival2 - psival1 - lvd) - 1.) > 1.0e-8) {
          std::cerr << psival2 - psival1 << " and logvaldiff is " << lvd
                    << std::endl;
          std::cerr << psival2 << " and LogVal with Lt is "
                    << GetMachine().LogVal(v_[rep], lt_[rep]) << std::endl;
          std::abort();
        }
#endif
      }
      moves_(rep) += 1;
    }
  }

  void Sweep() override {
    // First we do local exchange sweeps
    for (int i = 0; i < nrep_; i++) {
      LocalSweep(i);
    }

    // Tempearture exchanges
    std::uniform_real_distribution<double> distribution(0, 1);

    for (int r = 1; r < nrep_; r += 2) {
      if (ExchangeProb(r, r - 1) > distribution(this->GetRandomEngine())) {
        Exchange(r, r - 1);
        accept_(nrep_ + r) += 1.;
        accept_(nrep_ + r - 1) += 1;
      }
      moves_(nrep_ + r) += 1.;
      moves_(nrep_ + r - 1) += 1;
    }

    for (int r = 2; r < nrep_; r += 2) {
      if (ExchangeProb(r, r - 1) > distribution(this->GetRandomEngine())) {
        Exchange(r, r - 1);
        accept_(nrep_ + r) += 1.;
        accept_(nrep_ + r - 1) += 1;
      }
      moves_(nrep_ + r) += 1.;
      moves_(nrep_ + r - 1) += 1;
    }
  }

  // computes the probability to exchange two replicas
  double ExchangeProb(int r1, int r2) {
    const double lf1 = 2 * std::real(GetMachine().LogVal(v_[r1], lt_[r1]));
    const double lf2 = 2 * std::real(GetMachine().LogVal(v_[r2], lt_[r2]));

    return std::exp((beta_[r1] - beta_[r2]) * (lf2 - lf1));
  }

  void Exchange(int r1, int r2) {
    std::swap(v_[r1], v_[r2]);
    std::swap(lt_[r1], lt_[r2]);
  }

  const Eigen::VectorXd &Visible() const noexcept override { return v_[0]; }

  void SetVisible(const Eigen::VectorXd &v) override { v_[0] = v; }

  AbstractMachine::VectorType DerLogVisible() override {
    return GetMachine().DerLog(v_[0], lt_[0]);
  }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < acc.size(); i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }
};

}  // namespace netket

#endif