#include "MeshCuboidFusion.h"

#include "MeshCuboidParameters.h"
#include "ICP.h"
#include "Utilities.h"

#include <Eigen/Core>
#include <MRFEnergy.h>

MeshCuboidVoxelGrid::MeshCuboidVoxelGrid(MyMesh::Point _min, MyMesh::Point _max, Real _unit_size)
	: min_(_min)
	, max_(_max)
{
	MyMesh::Normal diff = max_ - min_;
	for (unsigned int i = 0; i < 3; ++i)
	{
		assert(diff[i] > 0);
		n_voxels_[i] = static_cast<int>(std::round(diff[i] / _unit_size));
		if (n_voxels_[i] < 2)
		{
			n_voxels_[i] = 2;
			Real center = 0.5 * (min_[i] + max_[i]);
			min_[i] = center - _unit_size;
			max_[i] = center + _unit_size;
		}
	}
}

MeshCuboidVoxelGrid::~MeshCuboidVoxelGrid()
{
}

int MeshCuboidVoxelGrid::n_voxels() const
{
	return (n_voxels_[0] * n_voxels_[1] * n_voxels_[2]);
}

int MeshCuboidVoxelGrid::n_axis_voxels(const int _axis_index) const
{
	assert(_axis_index < 3);
	return n_voxels_[_axis_index];
}

int MeshCuboidVoxelGrid::get_voxel_index(MeshCuboidVoxelIndex3D _xyz_index) const
{
	assert(_xyz_index[0] >= 0);
	assert(_xyz_index[1] >= 0);
	assert(_xyz_index[2] >= 0);
	assert(_xyz_index[0] < n_voxels_[0]);
	assert(_xyz_index[1] < n_voxels_[1]);
	assert(_xyz_index[2] < n_voxels_[2]);

	int voxel_index =
		_xyz_index[0] * (n_voxels_[1] * n_voxels_[2])
		+ _xyz_index[1] * n_voxels_[2]
		+ _xyz_index[2];

	assert(voxel_index >= 0);
	assert(voxel_index < n_voxels());

	// DEBUG.
	MeshCuboidVoxelIndex3D test_xyz_index = get_voxel_index(voxel_index);
	assert(_xyz_index[0] == test_xyz_index[0]);
	assert(_xyz_index[1] == test_xyz_index[1]);
	assert(_xyz_index[2] == test_xyz_index[2]);

	return voxel_index;
}

MeshCuboidVoxelIndex3D MeshCuboidVoxelGrid::get_voxel_index(const int _voxel_index) const
{
	assert(_voxel_index >= 0);
	assert(_voxel_index < n_voxels());
	MeshCuboidVoxelIndex3D xyz_index;

	int temp = _voxel_index;
	xyz_index[0] = temp / (n_voxels_[1] * n_voxels_[2]);
	temp -= (xyz_index[0] * n_voxels_[1] * n_voxels_[2]);

	xyz_index[1] = temp / n_voxels_[2];
	temp -= (xyz_index[1] * n_voxels_[2]);

	xyz_index[2] = temp;

	assert(xyz_index[0] >= 0);
	assert(xyz_index[1] >= 0);
	assert(xyz_index[2] >= 0);
	assert(xyz_index[0] < n_voxels_[0]);
	assert(xyz_index[1] < n_voxels_[1]);
	assert(xyz_index[2] < n_voxels_[2]);
	return xyz_index;
}

int MeshCuboidVoxelGrid::get_voxel_index(const MyMesh::Point _point) const
{
	MyMesh::Normal diff = max_ - min_;
	MeshCuboidVoxelIndex3D xyz_index;

	for (unsigned int i = 0; i < 3; ++i)
	{
		assert(diff[i] > 0);
		if (_point[i] < min_[i] || _point[i] > max_[i])
		{
			// Out of voxel grid range.
			return -1;
		}
		else
		{
			xyz_index[i] = static_cast<int>((_point[i] - min_[i]) / diff[i] * n_voxels_[i]);
			xyz_index[i] = std::max(xyz_index[i], 0);
			xyz_index[i] = std::min(xyz_index[i], n_voxels_[i] - 1);
		}
	}

	return get_voxel_index(xyz_index);
}

MyMesh::Point MeshCuboidVoxelGrid::get_center(const int _voxel_index) const
{
	MeshCuboidVoxelIndex3D xyz_index = get_voxel_index(_voxel_index);
	MyMesh::Normal diff = max_ - min_;
	MyMesh::Point center;

	for (unsigned int i = 0; i < 3; ++i)
	{
		assert(diff[i] > 0);
		Real ratio = static_cast<Real>(xyz_index[i] + 0.5) / n_voxels_[i];
		center[i] = min_[i] + ratio * diff[i];
	}

	return center;
}

void MeshCuboidVoxelGrid::get_centers(std::vector<MyMesh::Point> &_centers) const
{
	_centers.clear();
	_centers.resize(n_voxels());
	for (int voxel_index = 0; voxel_index < n_voxels(); ++voxel_index)
		_centers[voxel_index] = get_center(voxel_index);
}

void MeshCuboidVoxelGrid::get_point_correspondences(
	const std::vector<MyMesh::Point> &_points,
	std::vector<int> &_points_to_voxels,
	std::vector< std::list<int> > &_voxels_to_points) const
{
	const unsigned int num_points = _points.size();
	_points_to_voxels.clear();
	_voxels_to_points.clear();
	_points_to_voxels.resize(num_points);
	_voxels_to_points.resize(n_voxels());

	for (unsigned int point_index = 0; point_index < num_points; ++point_index)
	{
		int voxel_index = get_voxel_index(_points[point_index]);

		// Out of voxel grid range.
		if (voxel_index < 0)
			continue;

		assert(voxel_index < n_voxels());
		_points_to_voxels[point_index] = voxel_index;
		_voxels_to_points[voxel_index].push_back(point_index);
	}
}

void MeshCuboidVoxelGrid::get_voxel_occupancies(const std::vector<MyMesh::Point> &_points,
	Eigen::VectorXd &_voxel_occupancies) const
{
	const unsigned int num_points = _points.size();
	_voxel_occupancies = Eigen::VectorXd::Zero(n_voxels());

	for (unsigned int point_index = 0; point_index < num_points; ++point_index)
	{
		int voxel_index = get_voxel_index(_points[point_index]);

		// Out of voxel grid range.
		if (voxel_index < 0)
			continue;

		assert(voxel_index < n_voxels());
		_voxel_occupancies[voxel_index] = 1;
	}
}

void MeshCuboidVoxelGrid::get_distance_map(ANNpointArray &_ann_points, ANNkd_tree *__ann_kd_tree,
	Eigen::VectorXd &_voxel_to_point_distances) const
{
	std::vector<MyMesh::Point> center_points;
	get_centers(center_points);
	assert(center_points.size() == n_voxels());

	Eigen::MatrixXd center_points_mat(3, center_points.size());
	for (unsigned int voxel_index = 0; voxel_index < center_points.size(); ++voxel_index)
	{
		for (int i = 0; i < 3; ++i)
			center_points_mat.col(voxel_index)[i] = center_points[voxel_index][i];
	}

	ICP::get_closest_points(__ann_kd_tree, center_points_mat, _voxel_to_point_distances);
}

void run_part_ICP(MeshCuboidStructure &_input, const MeshCuboidStructure &_ground_truth)
{
	const Real neighbor_distance = FLAGS_param_sparse_neighbor_distance
		* _ground_truth.mesh_->get_object_diameter();

	unsigned int num_labels = _ground_truth.num_labels();
	for (LabelIndex label_index = 0; label_index < num_labels; ++label_index)
	{
		if (_ground_truth.label_cuboids_[label_index].empty()
			|| _input.label_cuboids_[label_index].empty())
			continue;

		MeshCuboid *ground_truth_cuboid = _ground_truth.label_cuboids_[label_index].front();
		MeshCuboid *input_cuboid = _input.label_cuboids_[label_index].front();
		assert(ground_truth_cuboid);
		assert(input_cuboid);

		if (ground_truth_cuboid->num_sample_points() == 0
			|| input_cuboid->num_sample_points() == 0)
			continue;

		Eigen::MatrixXd ground_truth_sample_points(3, ground_truth_cuboid->num_sample_points());
		Eigen::MatrixXd input_sample_points(3, input_cuboid->num_sample_points());

		for (SamplePointIndex sample_point_index = 0; sample_point_index < ground_truth_cuboid->num_sample_points();
			++sample_point_index)
		{
			MeshSamplePoint *sample_point = ground_truth_cuboid->get_sample_point(sample_point_index);
			assert(sample_point);
			MyMesh::Point point = ground_truth_cuboid->get_sample_point(sample_point_index)->point_;
			for (unsigned int i = 0; i < 3; ++i)
				ground_truth_sample_points.col(sample_point_index)(i) = point[i];

			//
			//is_ground_truth_point_visited[sample_point->sample_point_index_] = true;
			//
		}

		for (SamplePointIndex sample_point_index = 0; sample_point_index < input_cuboid->num_sample_points();
			++sample_point_index)
		{
			MeshSamplePoint *sample_point = input_cuboid->get_sample_point(sample_point_index);
			assert(sample_point);
			MyMesh::Point point = input_cuboid->get_sample_point(sample_point_index)->point_;
			for (unsigned int i = 0; i < 3; ++i)
				input_sample_points.col(sample_point_index)(i) = point[i];
		}

		Eigen::Matrix3d rotation_mat;
		Eigen::Vector3d translation_vec;
		double icp_error = ICP::run_iterative_closest_points(input_sample_points, ground_truth_sample_points,
			rotation_mat, translation_vec, &neighbor_distance);

		if (translation_vec.norm() > 2 * neighbor_distance)
			icp_error = -1;

		// If ICP failed, recover the original input point cloud.
		if (icp_error < 0)
		{
			for (SamplePointIndex sample_point_index = 0; sample_point_index < input_cuboid->num_sample_points();
				++sample_point_index)
			{
				MeshSamplePoint *sample_point = input_cuboid->get_sample_point(sample_point_index);
				assert(sample_point);
				MyMesh::Point point = input_cuboid->get_sample_point(sample_point_index)->point_;
				for (unsigned int i = 0; i < 3; ++i)
					input_sample_points.col(sample_point_index)(i) = point[i];
			}
		}

		for (SamplePointIndex sample_point_index = 0; sample_point_index < input_cuboid->num_sample_points();
			++sample_point_index)
		{
			MeshSamplePoint* input_sample_point = input_cuboid->get_sample_point(sample_point_index);
			assert(input_sample_point);

			for (unsigned int i = 0; i < 3; ++i)
				input_sample_point->point_[i] = input_sample_points.col(sample_point_index)[i];
		}
	}
}

void create_voxel_grid(
	const MeshCuboid *_symmetry_cuboid,
	const MeshCuboid *_database_cuboid,
	MyMesh::Point &_bbox_min, MyMesh::Point &_bbox_max)
{
	Eigen::MatrixXd symmetry_points, database_points;
	_symmetry_cuboid->get_sample_points(symmetry_points);
	_database_cuboid->get_sample_points(database_points);

	Eigen::Vector3d symmetry_bbox_min_vec = symmetry_points.rowwise().minCoeff();
	Eigen::Vector3d symmetry_bbox_max_vec = symmetry_points.rowwise().maxCoeff();
	Eigen::Vector3d database_bbox_min_vec = database_points.rowwise().minCoeff();
	Eigen::Vector3d database_bbox_max_vec = database_points.rowwise().maxCoeff();

	for (unsigned int i = 0; i < 3; ++i)
	{
		_bbox_min[i] = std::min(symmetry_bbox_min_vec[i], database_bbox_min_vec[i]);
		_bbox_max[i] = std::max(symmetry_bbox_max_vec[i], database_bbox_max_vec[i]);
	}
}

void get_smoothed_voxel_visibility(
	const MeshCuboidVoxelGrid &_voxels,
	const MeshCuboid *_ground_truth_cuboid,
	const double *_occlusion_modelview_matrix,
	const MeshCuboidStructure &_original_cuboid_structure,
	const Real &_occlusion_radius,
	const Real &_smoothing_parameter,
	std::vector<Real> &_voxel_visibility)
{
	// Smoothing using MRF.
	MRFEnergy<TypeBinary>* mrf;
	MRFEnergy<TypeBinary>::NodeId* nodes;
	MRFEnergy<TypeBinary>::Options options;
	TypeBinary::REAL energy, lowerBound;

	const int nodeNum = _voxels.n_voxels(); // number of nodes
	mrf = new MRFEnergy<TypeBinary>(TypeBinary::GlobalSize());
	nodes = new MRFEnergy<TypeBinary>::NodeId[nodeNum];

	// construct energy
	for (unsigned int voxel_index = 0; voxel_index < _voxels.n_voxels(); ++voxel_index)
	{
		nodes[voxel_index] = mrf->AddNode(TypeBinary::LocalSize(), TypeBinary::NodeData(
			_voxel_visibility[voxel_index], 1.0 - _voxel_visibility[voxel_index]));
	}

	for (unsigned int voxel_index = 0; voxel_index < _voxels.n_voxels(); ++voxel_index)
	{
		MeshCuboidVoxelIndex3D xyz_index = _voxels.get_voxel_index(voxel_index);

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		{
			MeshCuboidVoxelIndex3D n_xyz_index = xyz_index;
			if (n_xyz_index[axis_index] + 1 == _voxels.n_axis_voxels(axis_index))
				continue;

			++n_xyz_index[axis_index];
			unsigned int n_voxel_index = _voxels.get_voxel_index(n_xyz_index);
			assert(voxel_index < n_voxel_index);
			assert(n_voxel_index < _voxels.n_voxels());
			mrf->AddEdge(nodes[voxel_index], nodes[n_voxel_index], TypeBinary::EdgeData(
				0, _smoothing_parameter, _smoothing_parameter, 0));
		}
	}

	/////////////////////// TRW-S algorithm //////////////////////
	options.m_iterMax = 30; // maximum number of iterations
	mrf->Minimize_TRW_S(options, lowerBound, energy);

	// read solution
	_voxel_visibility.resize(_voxels.n_voxels(), 0.0);
	for (unsigned int voxel_index = 0; voxel_index < _voxels.n_voxels(); ++voxel_index)
	{
		_voxel_visibility[voxel_index] =
			static_cast<Real>(mrf->GetSolution(nodes[voxel_index]));
	}

	delete nodes;
	delete mrf;
}

void merge_symmetric_cuboids_visibility(
	const MeshCuboidSymmetryGroup *_symmetry_group,
	const MeshCuboidVoxelGrid &_voxels_1,
	const MeshCuboidVoxelGrid &_voxels_2,
	std::vector<Real> &_voxel_visibility_1,
	std::vector<Real> &_voxel_visibility_2)
{
	assert(_symmetry_group);

	const int num_voxels_1 = _voxels_1.n_voxels();
	assert(_voxel_visibility_1.size() == num_voxels_1);

	for (int voxel_index_1 = 0; voxel_index_1 < num_voxels_1; ++voxel_index_1)
	{
		MyMesh::Point center_1 = _voxels_1.get_center(voxel_index_1);

		for (unsigned int symmetry_order = 1; symmetry_order < _symmetry_group->num_symmetry_orders(); ++symmetry_order)
		{
			assert(symmetry_order > 0);

			MyMesh::Point center_2 = _symmetry_group->get_symmetric_point(center_1, symmetry_order);
			int voxel_index_2 = _voxels_2.get_voxel_index(center_2);
			if (voxel_index_2 < 0 || voxel_index_2 >= num_voxels_1)
				continue;

			// A voxel is visible if at least one of voxels in symmetric cuboids is visible.
			Real visibility = std::max(_voxel_visibility_1[voxel_index_1], _voxel_visibility_2[voxel_index_2]);
			_voxel_visibility_1[voxel_index_1] = visibility;
			_voxel_visibility_2[voxel_index_2] = visibility;
		}
	}
}

void fill_voxels_using_visibility(
	const MeshCuboidVoxelGrid &_voxels,
	const std::vector<Real> &_voxel_visibility_values,
	const MeshCuboid *_symmetry_cuboid,
	const MeshCuboid *_database_cuboid,
	MeshCuboidStructure &_output_cuboid_structure,
	MeshCuboid *_output_cuboid)
{
	std::vector<MyMesh::Point> symmetry_points;
	std::vector<int> symmetry_points_to_voxels;
	std::vector< std::list<int> > symmetry_voxels_to_points;

	_symmetry_cuboid->get_sample_points(symmetry_points);
	_voxels.get_point_correspondences(symmetry_points,
		symmetry_points_to_voxels, symmetry_voxels_to_points);

	std::vector<MyMesh::Point> database_points;
	std::vector<int> database_points_to_voxels;
	std::vector< std::list<int> > database_voxels_to_points;

	_database_cuboid->get_sample_points(database_points);
	_voxels.get_point_correspondences(database_points,
		database_points_to_voxels, database_voxels_to_points);

	for (unsigned int voxel_index = 0; voxel_index < _voxels.n_voxels(); ++voxel_index)
	{
		assert(voxel_index < symmetry_voxels_to_points.size());
		assert(voxel_index < database_voxels_to_points.size());
		MeshCuboidVoxelIndex3D xyz_index = _voxels.get_voxel_index(voxel_index);

		const std::list<int>& symmetry_voxel_point_indices = symmetry_voxels_to_points[voxel_index];
		const std::list<int>& database_voxel_point_indices = database_voxels_to_points[voxel_index];

		if (_voxel_visibility_values[voxel_index] > 0.5)
		{
			for (std::list<int>::const_iterator it = symmetry_voxel_point_indices.begin();
				it != symmetry_voxel_point_indices.end(); ++it)
			{
				MeshSamplePoint *sample_point = _symmetry_cuboid->get_sample_point(*it);
				assert(sample_point);

				MeshSamplePoint *new_sample_point = _output_cuboid_structure.add_sample_point(
					sample_point->point_, sample_point->normal_);
				_output_cuboid->add_sample_point(new_sample_point);
			}
		}
		else
		{
			for (std::list<int>::const_iterator it = database_voxel_point_indices.begin();
				it != database_voxel_point_indices.end(); ++it)
			{
				MeshSamplePoint *sample_point = _database_cuboid->get_sample_point(*it);
				assert(sample_point);

				MeshSamplePoint *new_sample_point = _output_cuboid_structure.add_sample_point(
					sample_point->point_, sample_point->normal_);
				_output_cuboid->add_sample_point(new_sample_point);
			}
		}
	}
}

void reconstruct_fusion_simple(
	const MeshCuboidStructure &_symmetry_cuboid_structure,
	const MeshCuboidStructure &_database_cuboid_structure,
	MeshCuboidStructure &_output_cuboid_structure)
{
	assert(_symmetry_cuboid_structure.num_labels() == _database_cuboid_structure.num_labels());

	const Real neighbor_distance = FLAGS_param_sparse_neighbor_distance
		* _output_cuboid_structure.mesh_->get_object_diameter();

	_output_cuboid_structure = _symmetry_cuboid_structure;
	unsigned int num_labels = _symmetry_cuboid_structure.num_labels();

	for (LabelIndex label_index = 0; label_index < num_labels; ++label_index)
	{
		if (_symmetry_cuboid_structure.label_cuboids_[label_index].empty()
			|| _database_cuboid_structure.label_cuboids_[label_index].empty()
			|| _output_cuboid_structure.label_cuboids_[label_index].empty())
			continue;

		MeshCuboid *symmetry_cuboid = _symmetry_cuboid_structure.label_cuboids_[label_index].front();
		MeshCuboid *database_cuboid = _database_cuboid_structure.label_cuboids_[label_index].front();
		MeshCuboid *output_cuboid = _output_cuboid_structure.label_cuboids_[label_index].front();
		assert(symmetry_cuboid);
		assert(database_cuboid);
		assert(output_cuboid);

		if (symmetry_cuboid->num_sample_points() == 0
			|| database_cuboid->num_sample_points() == 0)
			continue;

		Eigen::MatrixXd symmetry_sample_points(3, symmetry_cuboid->num_sample_points());
		Eigen::MatrixXd database_sample_points(3, database_cuboid->num_sample_points());

		for (SamplePointIndex sample_point_index = 0; sample_point_index < symmetry_cuboid->num_sample_points();
			++sample_point_index)
		{
			assert(symmetry_cuboid->get_sample_point(sample_point_index));
			MyMesh::Point point = symmetry_cuboid->get_sample_point(sample_point_index)->point_;
			for (unsigned int i = 0; i < 3; ++i)
				symmetry_sample_points.col(sample_point_index)(i) = point[i];
		}

		for (SamplePointIndex sample_point_index = 0; sample_point_index < database_cuboid->num_sample_points();
			++sample_point_index)
		{
			assert(database_cuboid->get_sample_point(sample_point_index));
			MyMesh::Point point = database_cuboid->get_sample_point(sample_point_index)->point_;
			for (unsigned int i = 0; i < 3; ++i)
				database_sample_points.col(sample_point_index)(i) = point[i];
		}

		Eigen::Matrix3d rotation_mat;
		Eigen::Vector3d translation_vec;
		double icp_error = ICP::run_iterative_closest_points(database_sample_points, symmetry_sample_points,
			rotation_mat, translation_vec, &neighbor_distance);
		//std::cout << "ICP Error = " << icp_error << std::endl;

		for (SamplePointIndex sample_point_index = 0; sample_point_index < database_cuboid->num_sample_points();
			++sample_point_index)
		{
			MeshSamplePoint *sample_point = database_cuboid->get_sample_point(sample_point_index);
			assert(sample_point);

			MyMesh::Point new_point;
			for (unsigned int i = 0; i < 3; ++i)
				new_point[i] = database_sample_points.col(sample_point_index)[i];

			MeshSamplePoint *new_sample_point = _output_cuboid_structure.add_sample_point(
				new_point, sample_point->normal_);
			output_cuboid->add_sample_point(new_sample_point);
		}
	}

	/*
	ret = cuboid_structure_.load_dense_sample_points(dense_sample_filepath.c_str());
	assert(ret);
	set_modelview_matrix(_occlusion_modelview_matrix, false);
	remove_occluded_points();
	set_modelview_matrix(_snapshot_modelview_matrix);

	cuboid_structure_.copy_sample_points_to_symmetric_position();

	std::vector<LabelIndex> reconstructed_label_indices;
	reconstructed_label_indices.push_back(0);
	reconstructed_label_indices.push_back(1);
	reconstruct_using_database(&reconstructed_label_indices);
	*/
}

bool get_fusion_cuboids(const LabelIndex _label_index,
	const MeshCuboidStructure &_symmetry_cuboid_structure,
	const MeshCuboidStructure &_database_cuboid_structure,
	const MeshCuboidStructure &_output_cuboid_structure,
	MeshCuboid *&_symmetry_cuboid, MeshCuboid *&_database_cuboid, MeshCuboid *&_output_cuboid)
{
	if (_symmetry_cuboid_structure.label_cuboids_[_label_index].empty()
		|| _database_cuboid_structure.label_cuboids_[_label_index].empty()
		|| _output_cuboid_structure.label_cuboids_[_label_index].empty())
	{
		_symmetry_cuboid = NULL;
		_database_cuboid = NULL;
		_output_cuboid = NULL;
		return false;
	}

	assert(_symmetry_cuboid_structure.label_cuboids_[_label_index].size() <= 1);
	assert(_database_cuboid_structure.label_cuboids_[_label_index].size() <= 1);
	assert(_output_cuboid_structure.label_cuboids_[_label_index].size() <= 1);

	_symmetry_cuboid = _symmetry_cuboid_structure.label_cuboids_[_label_index].front();
	_database_cuboid = _database_cuboid_structure.label_cuboids_[_label_index].front();
	_output_cuboid = _output_cuboid_structure.label_cuboids_[_label_index].front();

	assert(_symmetry_cuboid);
	assert(_database_cuboid);
	assert(_output_cuboid);

	if (_symmetry_cuboid->num_sample_points() == 0) _symmetry_cuboid = NULL;
	if (_database_cuboid->num_sample_points() == 0) _database_cuboid = NULL;
	return (_symmetry_cuboid && _database_cuboid);
}

void mark_cuboid_sample_points(
	const MeshCuboid *_cuboid,
	const MeshCuboidStructure &_cuboid_structure,
	bool *_is_sample_point_visited)
{
	assert(_cuboid);
	for (SamplePointIndex sample_point_index = 0; sample_point_index < _cuboid->num_sample_points();
		++sample_point_index)
	{
		MeshSamplePoint *sample_point = _cuboid->get_sample_point(sample_point_index);
		assert(sample_point);
		assert(sample_point->sample_point_index_ < _cuboid_structure.num_sample_points());
		_is_sample_point_visited[sample_point->sample_point_index_] = true;
	}
}

void copy_sample_points(const MeshCuboid *_from_cuboid,
	MeshCuboidStructure &_to_cuboid_structure, MeshCuboid *_to_cuboid)
{
	assert(_from_cuboid);
	assert(_to_cuboid);

	for (unsigned int i = 0; i < _from_cuboid->num_sample_points(); ++i)
	{
		MeshSamplePoint *sample_point = _from_cuboid->get_sample_point(i);
		assert(sample_point);

		MeshSamplePoint *new_sample_point = _to_cuboid_structure.add_sample_point(
			sample_point->point_, sample_point->normal_);
		_to_cuboid->add_sample_point(new_sample_point);
	}
}

void fusing_single_symmetry_cuboid(LabelIndex _label_index,
	const MeshCuboidSymmetryGroup *_symmetry_group,
	const double *_occlusion_modelview_matrix,
	const MeshCuboidStructure &_original_cuboid_structure,
	const MeshCuboidStructure &_symmetry_cuboid_structure,
	const MeshCuboidStructure &_database_cuboid_structure,
	MeshCuboidStructure &_output_cuboid_structure,
	bool *_is_symmetry_point_visited,
	bool *_is_label_index_visited)
{
	assert(_occlusion_modelview_matrix);
	assert(_is_symmetry_point_visited);
	assert(_is_label_index_visited);

	const Real occlusion_radius = FLAGS_param_fusion_grid_size;
	const Real visibility_smoothing_prior = FLAGS_param_fusion_visibility_smoothing_prior;

	MeshCuboid *symmetry_cuboid = NULL, *database_cuboid = NULL, *output_cuboid = NULL;
	bool ret = get_fusion_cuboids(_label_index,
		_symmetry_cuboid_structure, _database_cuboid_structure, _output_cuboid_structure,
		symmetry_cuboid, database_cuboid, output_cuboid);

	if (!ret)
	{
		if (output_cuboid)
		{
			if (symmetry_cuboid)
				copy_sample_points(symmetry_cuboid, _output_cuboid_structure, output_cuboid);
			else if (database_cuboid)
				copy_sample_points(database_cuboid, _output_cuboid_structure, output_cuboid);
		}
		return;
	}

	_is_label_index_visited[_label_index] = true;
	std::cout << "Single symmetry: (" << _label_index << ")" << std::endl;

	// Mark visited symmetry sample points.
	mark_cuboid_sample_points(symmetry_cuboid, _symmetry_cuboid_structure, _is_symmetry_point_visited);

	// Define local coordinates voxel grid.
	MyMesh::Point bbox_min, bbox_max;
	create_voxel_grid(symmetry_cuboid, database_cuboid, bbox_min, bbox_max);
	MeshCuboidVoxelGrid voxels(bbox_min, bbox_max, occlusion_radius);
	std::vector<MyMesh::Point> voxel_centers;
	voxels.get_centers(voxel_centers);

	std::cout << "Computing visibility values... ";

	std::vector<Real> voxel_visibility;
	MeshCuboid::compute_cuboid_surface_point_visibility(
		_occlusion_modelview_matrix, occlusion_radius, _original_cuboid_structure.sample_points_,
		voxel_centers, NULL, voxel_visibility);

	// Merge visibility values for voxels in symmetric cuboids.
	merge_symmetric_cuboids_visibility(_symmetry_group, voxels, voxels, voxel_visibility, voxel_visibility);

	// Smoothing.
	get_smoothed_voxel_visibility(
		voxels, symmetry_cuboid, _occlusion_modelview_matrix, _original_cuboid_structure,
		occlusion_radius, visibility_smoothing_prior, voxel_visibility);

	std::cout << "Done." << std::endl;

	std::cout << "Filling voxels... ";
	fill_voxels_using_visibility(
		voxels, voxel_visibility, symmetry_cuboid, database_cuboid,
		_output_cuboid_structure, output_cuboid);
	std::cout << "Done." << std::endl;
}

void fusing_pair_symmetry_cuboids(LabelIndex _label_index_1, LabelIndex _label_index_2,
	const MeshCuboidSymmetryGroup *_symmetry_group,
	const double *_occlusion_modelview_matrix,
	const MeshCuboidStructure &_original_cuboid_structure,
	const MeshCuboidStructure &_symmetry_cuboid_structure,
	const MeshCuboidStructure &_database_cuboid_structure,
	MeshCuboidStructure &_output_cuboid_structure,
	bool *_is_symmetry_point_visited,
	bool *_is_label_index_visited)
{
	assert(_occlusion_modelview_matrix);
	assert(_is_symmetry_point_visited);
	assert(_is_label_index_visited);

	const Real occlusion_radius = FLAGS_param_fusion_grid_size;
	const Real visibility_smoothing_prior = FLAGS_param_fusion_visibility_smoothing_prior;

	MeshCuboid *symmetry_cuboid_1 = NULL, *database_cuboid_1 = NULL, *output_cuboid_1 = NULL;
	bool ret_1 = get_fusion_cuboids(_label_index_1,
		_symmetry_cuboid_structure, _database_cuboid_structure, _output_cuboid_structure,
		symmetry_cuboid_1, database_cuboid_1, output_cuboid_1);

	MeshCuboid *symmetry_cuboid_2 = NULL, *database_cuboid_2 = NULL, *output_cuboid_2 = NULL;
	bool ret_2 = get_fusion_cuboids(_label_index_2,
		_symmetry_cuboid_structure, _database_cuboid_structure, _output_cuboid_structure,
		symmetry_cuboid_2, database_cuboid_2, output_cuboid_2);

	if (!ret_1 || !ret_2)
	{
		if (output_cuboid_1 && output_cuboid_2)
		{
			if (symmetry_cuboid_1 && symmetry_cuboid_2)
			{
				copy_sample_points(symmetry_cuboid_1, _output_cuboid_structure, output_cuboid_1);
				copy_sample_points(symmetry_cuboid_2, _output_cuboid_structure, output_cuboid_2);
			}
			else if (database_cuboid_1 && database_cuboid_2)
			{
				copy_sample_points(database_cuboid_1, _output_cuboid_structure, output_cuboid_1);
				copy_sample_points(database_cuboid_2, _output_cuboid_structure, output_cuboid_2);
			}
		}
		return;
	}

	_is_label_index_visited[_label_index_1] = true;
	_is_label_index_visited[_label_index_2] = true;
	std::cout << "Pair symmetry: (" << _label_index_1 << ", " << _label_index_2 << ")" << std::endl;

	// Mark visited symmetry sample points.
	mark_cuboid_sample_points(symmetry_cuboid_1, _symmetry_cuboid_structure, _is_symmetry_point_visited);
	mark_cuboid_sample_points(symmetry_cuboid_2, _symmetry_cuboid_structure, _is_symmetry_point_visited);

	// Define local coordinates voxel grid.
	MyMesh::Point bbox_min_1, bbox_max_1;
	create_voxel_grid(symmetry_cuboid_1, database_cuboid_1, bbox_min_1, bbox_max_1);
	MeshCuboidVoxelGrid voxels_1(bbox_min_1, bbox_max_1, occlusion_radius);
	std::vector<MyMesh::Point> voxel_centers_1;
	voxels_1.get_centers(voxel_centers_1);

	MyMesh::Point bbox_min_2, bbox_max_2;
	create_voxel_grid(symmetry_cuboid_2, database_cuboid_2, bbox_min_2, bbox_max_2);
	MeshCuboidVoxelGrid voxels_2(bbox_min_2, bbox_max_2, occlusion_radius);
	std::vector<MyMesh::Point> voxel_centers_2;
	voxels_2.get_centers(voxel_centers_2);

	std::cout << "Computing visibility values... ";

	std::vector<Real> voxel_visibility_1;
	MeshCuboid::compute_cuboid_surface_point_visibility(
		_occlusion_modelview_matrix, occlusion_radius, _original_cuboid_structure.sample_points_,
		voxel_centers_1, NULL, voxel_visibility_1);

	std::vector<Real> voxel_visibility_2;
	MeshCuboid::compute_cuboid_surface_point_visibility(
		_occlusion_modelview_matrix, occlusion_radius, _original_cuboid_structure.sample_points_,
		voxel_centers_2, NULL, voxel_visibility_2);

	// Merge visibility values for voxels in symmetric cuboids.
	merge_symmetric_cuboids_visibility(_symmetry_group, voxels_1, voxels_2, voxel_visibility_1, voxel_visibility_2);
	merge_symmetric_cuboids_visibility(_symmetry_group, voxels_2, voxels_1, voxel_visibility_2, voxel_visibility_1);

	// Smoothing.
	get_smoothed_voxel_visibility(
		voxels_1, symmetry_cuboid_1, _occlusion_modelview_matrix, _original_cuboid_structure,
		occlusion_radius, visibility_smoothing_prior, voxel_visibility_1);
	get_smoothed_voxel_visibility(
		voxels_2, symmetry_cuboid_2, _occlusion_modelview_matrix, _original_cuboid_structure,
		occlusion_radius, visibility_smoothing_prior, voxel_visibility_2);

	std::cout << "Done." << std::endl;

	std::cout << "Filling voxels... ";
	fill_voxels_using_visibility(
		voxels_1, voxel_visibility_1, symmetry_cuboid_1, database_cuboid_1, _output_cuboid_structure, output_cuboid_1);
	fill_voxels_using_visibility(
		voxels_2, voxel_visibility_2, symmetry_cuboid_2, database_cuboid_2, _output_cuboid_structure, output_cuboid_2);
	std::cout << "Done." << std::endl;
}

void reconstruct_fusion(const char *_mesh_filepath,
	const double *_snapshot_modelview_matrix,
	const double *_occlusion_modelview_matrix,
	const MeshCuboidStructure &_original_cuboid_structure,
	const MeshCuboidStructure &_symmetry_cuboid_structure,
	const MeshCuboidStructure &_database_cuboid_structure,
	MeshCuboidStructure &_output_cuboid_structure,
	bool _add_outliers)
{
	assert(_symmetry_cuboid_structure.num_labels() == _database_cuboid_structure.num_labels());

	const Real occlusion_radius = FLAGS_param_fusion_grid_size;
	const Real visibility_smoothing_prior = FLAGS_param_fusion_visibility_smoothing_prior;

	_output_cuboid_structure.clear_sample_points();

	std::cout << "Computing ICP for each part... ";
	MeshCuboidStructure aligned_database_cuboid_structure = _database_cuboid_structure;
	run_part_ICP(aligned_database_cuboid_structure, _symmetry_cuboid_structure);
	std::cout << "Done." << std::endl;

	// Check whether a sample point in the symmetry reconstruction is visited or not.
	bool *is_symmetry_point_visited = new bool[_symmetry_cuboid_structure.num_sample_points()];
	memset(is_symmetry_point_visited, false, _symmetry_cuboid_structure.num_sample_points() * sizeof(bool));

	bool *is_label_index_visited = new bool[_symmetry_cuboid_structure.num_labels()];
	memset(is_label_index_visited, false, _symmetry_cuboid_structure.num_labels() * sizeof(bool));

	for (std::vector< MeshCuboidReflectionSymmetryGroup* >::const_iterator it = _symmetry_cuboid_structure.reflection_symmetry_groups_.begin();
		it != _symmetry_cuboid_structure.reflection_symmetry_groups_.end(); ++it)
	{
		MeshCuboidReflectionSymmetryGroup* symmetry_group = (*it);
		assert(symmetry_group);
		MeshCuboidSymmetryGroupInfo symmetry_group_info = symmetry_group->get_symmetry_group_info();

		// Single symmetric cuboids.
		for (std::vector<LabelIndex>::const_iterator jt = symmetry_group_info.single_label_indices_.begin();
			jt != symmetry_group_info.single_label_indices_.end(); ++jt)
		{
			LabelIndex label_index = (*jt);
			fusing_single_symmetry_cuboid(label_index, symmetry_group,
				_occlusion_modelview_matrix, _original_cuboid_structure,
				_symmetry_cuboid_structure, aligned_database_cuboid_structure, _output_cuboid_structure,
				is_symmetry_point_visited, is_label_index_visited);
		}

		// Pair symmetric cuboids.
		for (std::vector< std::pair<LabelIndex, LabelIndex> >::const_iterator jt = symmetry_group_info.pair_label_indices_.begin();
			jt != symmetry_group_info.pair_label_indices_.end(); ++jt)
		{
			LabelIndex label_index_1 = (*jt).first;
			LabelIndex label_index_2 = (*jt).second;
			fusing_pair_symmetry_cuboids(label_index_1, label_index_2, symmetry_group,
				_occlusion_modelview_matrix, _original_cuboid_structure,
				_symmetry_cuboid_structure, aligned_database_cuboid_structure, _output_cuboid_structure,
				is_symmetry_point_visited, is_label_index_visited);
		}
	}

	for (std::vector< MeshCuboidRotationSymmetryGroup* >::const_iterator it = _symmetry_cuboid_structure.rotation_symmetry_groups_.begin();
		it != _symmetry_cuboid_structure.rotation_symmetry_groups_.end(); ++it)
	{
		MeshCuboidRotationSymmetryGroup* symmetry_group = (*it);
		assert(symmetry_group);
		MeshCuboidSymmetryGroupInfo symmetry_group_info = symmetry_group->get_symmetry_group_info();

		// Single symmetric cuboids.
		for (std::vector<LabelIndex>::const_iterator jt = symmetry_group_info.single_label_indices_.begin();
			jt != symmetry_group_info.single_label_indices_.end(); ++jt)
		{
			LabelIndex label_index = (*jt);
			fusing_single_symmetry_cuboid(label_index, symmetry_group,
				_occlusion_modelview_matrix, _original_cuboid_structure,
				_symmetry_cuboid_structure, aligned_database_cuboid_structure, _output_cuboid_structure,
				is_symmetry_point_visited, is_label_index_visited);
		}

		// NOTE:
		// Pairwise rotational symmetry is not considered.
	}

	// Other single cuboids.
	unsigned int num_labels = _symmetry_cuboid_structure.num_labels();
	for (LabelIndex label_index = 0; label_index < num_labels; ++label_index)
	{
		if (is_label_index_visited[label_index])
			continue;

		MeshCuboid *symmetry_cuboid, *database_cuboid, *output_cuboid;
		bool ret = get_fusion_cuboids(label_index,
			_symmetry_cuboid_structure, aligned_database_cuboid_structure, _output_cuboid_structure,
			symmetry_cuboid, database_cuboid, output_cuboid);
		if (!ret) continue;

		is_label_index_visited[label_index] = true;
		std::cout << "Asymmetry: (" << label_index << ")" << std::endl;

		// Mark visited symmetry sample points.
		mark_cuboid_sample_points(symmetry_cuboid, _symmetry_cuboid_structure, is_symmetry_point_visited);

		// Define local coordinates voxel grid.
		MyMesh::Point bbox_min, bbox_max;
		create_voxel_grid(symmetry_cuboid, database_cuboid,
			bbox_min, bbox_max);
		MeshCuboidVoxelGrid voxels(bbox_min, bbox_max, occlusion_radius);
		std::vector<MyMesh::Point> voxel_centers;
		voxels.get_centers(voxel_centers);

		std::cout << "Computing visibility values... ";
		std::vector<Real> voxel_visibility;
		MeshCuboid::compute_cuboid_surface_point_visibility(
			_occlusion_modelview_matrix, occlusion_radius, _original_cuboid_structure.sample_points_,
			voxel_centers, NULL, voxel_visibility);

		get_smoothed_voxel_visibility(
			voxels, symmetry_cuboid, _occlusion_modelview_matrix, _original_cuboid_structure,
			occlusion_radius, visibility_smoothing_prior, voxel_visibility);
		std::cout << "Done." << std::endl;

		std::cout << "Filling voxels... ";
		fill_voxels_using_visibility(
			voxels, voxel_visibility, symmetry_cuboid, database_cuboid, _output_cuboid_structure, output_cuboid);
		std::cout << "Done." << std::endl;
	}

	// Add unvisited (unsegmented) sample points in symmetry reconstruction.
	for (SamplePointIndex sample_point_index = 0; sample_point_index < _symmetry_cuboid_structure.num_sample_points();
		++sample_point_index)
	{
		if (!is_symmetry_point_visited[sample_point_index])
		{
			const MeshSamplePoint *sample_point = _symmetry_cuboid_structure.sample_points_[sample_point_index];
			assert(sample_point);
			MeshSamplePoint *new_sample_point = _output_cuboid_structure.add_sample_point(
				sample_point->point_, sample_point->normal_);
		}
	}

	delete[] is_symmetry_point_visited;
	delete[] is_label_index_visited;
}


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

#include <nt2/linalg/functions/clinsolve.hpp>

#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/gelsy.hpp>
#include <nt2/include/functions/gesv.hpp>
#include <nt2/include/functions/gbsv.hpp>
#include <nt2/include/functions/posv.hpp>
#include <nt2/include/functions/sysv.hpp>
#include <nt2/include/functions/lange.hpp>
#include <nt2/include/functions/lansy.hpp>
#include <nt2/include/functions/langb.hpp>
#include <nt2/include/functions/gecon.hpp>
#include <nt2/include/functions/sycon.hpp>
#include <nt2/include/functions/pocon.hpp>
#include <nt2/include/functions/gbcon.hpp>
#include <nt2/include/functions/trsolve.hpp>

#include <nt2/include/functions/tie.hpp>

#include <nt2/linalg/options.hpp>
#include <nt2/linalg/functions/details/eval_linsolve.hpp>
#include <nt2/sdk/meta/settings_of.hpp>
#include <boost/dispatch/meta/hierarchy_of.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <boost/dispatch/meta/terminal_of.hpp>
#include <nt2/core/container/table/table.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  // LINSOLVE classic
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( clinsolve_, tag::cpu_
                            , (A0)(A1)(A2)(N2)
                            , ((ast_<A0, nt2::container::domain>))   // A
                              ((ast_<A1, nt2::container::domain>))   // B
                              ((node_<A2, nt2::tag::tie_             // X-R
                                    , N2, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename A0::value_type ctype_t;
    typedef typename nt2::meta::as_real<ctype_t>::type   type_t;
    typedef typename meta::option<typename A0::proto_child0::settings_type,nt2::tag::shape_>::type shape;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::settings(nt2::_2D)> desired_semantic;
    typedef nt2::container::table<ctype_t>  entry_type;
    typedef nt2::container::table<ctype_t,shape>  matrix_type;

    BOOST_FORCEINLINE result_type operator()( A0 const& a0, A1 const& a1, A2 const& a2  ) const
    {
      nt2::container::table<nt2_la_int> piv;
      eval(a0,a1,a2,piv,N2(),shape());
    }

    //==========================================================================
    /// INTERNAL ONLY - Shape analysis

    /// INTERNAL ONLY - X = LINSOLVE(A,B) - rectangular shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& piv
              , boost::mpl::long_<1> const&, nt2::rectangular_ const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      entry_type entry(a0);
      eval_param( a0, a1, b);

      if (issquare(entry)) nt2::gesv(boost::proto::value(entry)
                           ,boost::proto::value(piv), boost::proto::value(b));

      else {
        nt2_la_int n = nt2::width(a0);
        piv = nt2::zeros(n,1, nt2::meta::as_<nt2_la_int>());
        nt2::gelsy( boost::proto::value(entry) ,boost::proto::value(piv)
                , boost::proto::value(b) );
      }

      assign_swap (boost::proto::child_c<0>(a2), b);
    }

    //==========================================================================
    /// INTERNAL ONLY - X = LINSOLVE(A,B) - positive definite shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& /*piv*/
              , boost::mpl::long_<1> const&, nt2::positive_definite_ const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      matrix_type entry(a0);

      nt2::posv(boost::proto::value(entry), boost::proto::value(b));
      assign_swap (boost::proto::child_c<0>(a2), b);
    }

    //==========================================================================
    /// INTERNAL ONLY - X = LINSOLVE(A,B) - symmetric shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& piv
              , boost::mpl::long_<1> const&, nt2::symmetric_ const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      matrix_type entry(a0);
      piv.resize(nt2::of_size(a0.leading_size(),1));
      nt2::sysv( boost::proto::value(entry),boost::proto::value(piv)
              , boost::proto::value(b));
      assign_swap (boost::proto::child_c<0>(a2), b);

    }

    //==========================================================================
    /// INTERNAL ONLY - X = LINSOLVE(A,B) - band shape
    template<int U, int L>
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& piv
              , boost::mpl::long_<1> const&, nt2::band_diagonal_<U,L> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      matrix_type entry(a0);

      nt2::gbsv( boost::proto::value(entry),boost::proto::value(piv)
              , boost::proto::value(b));
      assign_swap (boost::proto::child_c<0>(a2), b);

    }

    //==========================================================================
    /// INTERNAL ONLY - X = LINSOLVE(A,B) -- upper triangular shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>&
              , boost::mpl::long_<1> const&, nt2::upper_triangular_ const&
              ) const
    {
      boost::proto::child_c<0>(a2) = nt2::trsolve(a0,a1);
    }

    //==========================================================================
    /// INTERNAL ONLY - X = LINSOLVE(A,B) -- lower triangular shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>&
              , boost::mpl::long_<1> const&, nt2::lower_triangular_ const&
              ) const
    {
      boost::proto::child_c<0>(a2) = nt2::trsolve(a0,a1);
    }

    //==========================================================================
    /// INTERNAL ONLY - [X,R] = LINSOLVE(A,B) -- Rectangular shape
    BOOST_FORCEINLINE
    void eval( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& piv
              , boost::mpl::long_<2> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      matrix_type entry(a0);

      eval_param( entry, a1, boost::proto::child_c<0>(a2));

      if (issquare(entry))
      {
        nt2::gesv( boost::proto::value(entry), boost::proto::value(piv)
               , boost::proto::value(b) );
        char norm = '1';

        type_t anorm = nt2::lange(boost::proto::value(entry),norm);
        boost::proto::child_c<1>(a2) = nt2::gecon(boost::proto::value(entry),norm,anorm);
      }
      else
      {
        nt2_la_int rank;
        nt2_la_int n = nt2::width(entry);
        piv = nt2::zeros(n,1, nt2::meta::as_<nt2_la_int>());

        nt2::gelsy( boost::proto::value(entry), boost::proto::value(piv)
                , boost::proto::value(b), rank);
        boost::proto::child_c<1>(a2) = static_cast<type_t>(rank);
      }
      assign_swap (boost::proto::child_c<0>(a2), b);
    }

    //==========================================================================
    /// INTERNAL ONLY - [X,R] = LINSOLVE(A,B) -- symmetric shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& piv
              , boost::mpl::long_<2> const&, nt2::symmetric_ const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      matrix_type entry(a0);
      char norm = '1';

      piv = nt2::zeros(entry.leading_size(), 1, nt2::meta::as_<nt2_la_int>() );

      typedef typename meta::hierarchy_of<nt2::symmetric_>::stripped h_;

      type_t anorm = nt2::lange(boost::proto::value(entry), norm, h_());

      nt2::sysv( boost::proto::value(entry),boost::proto::value(piv)
              , boost::proto::value(b));
      boost::proto::child_c<1>(a2) = nt2::sycon( boost::proto::value(entry)
                                               , boost::proto::value(piv) ,anorm);
      assign_swap (boost::proto::child_c<0>(a2), b);
    }

    //==========================================================================
    /// INTERNAL ONLY - [X,R] = LINSOLVE(A,B) -- positive definite shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& /*piv*/
              , boost::mpl::long_<2> const&, nt2::positive_definite_ const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      matrix_type entry(a0);

      char norm = '1';
      typedef typename meta::hierarchy_of<nt2::symmetric_>::stripped h_;
      type_t anorm = nt2::lange(boost::proto::value(entry) ,norm, h_());
      nt2::posv(boost::proto::value(entry), boost::proto::value(b));
      boost::proto::child_c<1>(a2) = nt2::pocon(boost::proto::value(entry),anorm);
      assign_swap (boost::proto::child_c<0>(a2), b);
    }

    //==========================================================================
    /// INTERNAL ONLY - [X,R] = LINSOLVE(A,B) -- general band shape
    template<int U, int L>
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& piv
              , boost::mpl::long_<2> const&, nt2::band_diagonal_<U,L> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(desired_semantic,b,a1,boost::proto::child_c<0>(a2) );
      matrix_type entry(a0);

      char norm = '1';

      type_t anorm = nt2::langb(boost::proto::value(entry),norm);
      nt2::gbsv( boost::proto::value(entry), boost::proto::value(piv)
              , boost::proto::value(b));
      boost::proto::child_c<1>(a2) = nt2::gbcon( boost::proto::value(entry)
                                               , boost::proto::value(piv),anorm);

      assign_swap (boost::proto::child_c<0>(a2), b);

    }

    /// INTERNAL ONLY - X = LINSOLVE(A,B) - default case
    template<typename N, typename sh> BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2, nt2::container::table<nt2_la_int>& piv
              , N const&, sh const&
              ) const
    {
      eval(a0,a1,a2,piv,boost::mpl::long_<1>() ,nt2::rectangular_());
    }

  };
} }

#endif


// Copyright (c) 2014-2016 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 <utilstrencodings.h>
#include <wallet/crypter.h>

#include <test/test_bitcoin.h>

#include <boost/test/unit_test.hpp>

#include <openssl/aes.h>
#include <openssl/evp.h>

#include <vector>

BOOST_FIXTURE_TEST_SUITE(wallet_crypto_tests, BasicTestingSetup)

bool OldSetKeyFromPassphrase(const SecureString &strKeyData,
                             const std::vector<uint8_t> &chSalt,
                             const unsigned int nRounds,
                             const unsigned int nDerivationMethod,
                             uint8_t *chKey, uint8_t *chIV) {
    if (nRounds < 1 || chSalt.size() != WALLET_CRYPTO_SALT_SIZE) return false;

    int i = 0;
    if (nDerivationMethod == 0)
        i = EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha512(), &chSalt[0],
                           (uint8_t *)&strKeyData[0], strKeyData.size(),
                           nRounds, chKey, chIV);

    if (i != (int)WALLET_CRYPTO_KEY_SIZE) {
        memory_cleanse(chKey, sizeof(chKey));
        memory_cleanse(chIV, sizeof(chIV));
        return false;
    }
    return true;
}

bool OldEncrypt(const CKeyingMaterial &vchPlaintext,
                std::vector<uint8_t> &vchCiphertext, const uint8_t chKey[32],
                const uint8_t chIV[16]) {
    // max ciphertext len for a n bytes of plaintext is
    // n + AES_BLOCK_SIZE - 1 bytes
    int nLen = vchPlaintext.size();
    int nCLen = nLen + AES_BLOCK_SIZE, nFLen = 0;
    vchCiphertext = std::vector<uint8_t>(nCLen);

    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();

    if (!ctx) return false;

    bool fOk = true;

    EVP_CIPHER_CTX_init(ctx);
    if (fOk)
        fOk = EVP_EncryptInit_ex(ctx, EVP_aes_256_cbc(), nullptr, chKey,
                                 chIV) != 0;
    if (fOk)
        fOk = EVP_EncryptUpdate(ctx, &vchCiphertext[0], &nCLen,
                                &vchPlaintext[0], nLen) != 0;
    if (fOk)
        fOk =
            EVP_EncryptFinal_ex(ctx, (&vchCiphertext[0]) + nCLen, &nFLen) != 0;
    EVP_CIPHER_CTX_cleanup(ctx);

    EVP_CIPHER_CTX_free(ctx);

    if (!fOk) return false;

    vchCiphertext.resize(nCLen + nFLen);
    return true;
}

bool OldDecrypt(const std::vector<uint8_t> &vchCiphertext,
                CKeyingMaterial &vchPlaintext, const uint8_t chKey[32],
                const uint8_t chIV[16]) {
    // plaintext will always be equal to or lesser than length of ciphertext
    int nLen = vchCiphertext.size();
    int nPLen = nLen, nFLen = 0;

    vchPlaintext = CKeyingMaterial(nPLen);

    EVP_CIPHER_CTX *ctx = EVP_CIPHER_CTX_new();

    if (!ctx) return false;

    bool fOk = true;

    EVP_CIPHER_CTX_init(ctx);
    if (fOk)
        fOk = EVP_DecryptInit_ex(ctx, EVP_aes_256_cbc(), nullptr, chKey,
                                 chIV) != 0;
    if (fOk)
        fOk = EVP_DecryptUpdate(ctx, &vchPlaintext[0], &nPLen,
                                &vchCiphertext[0], nLen) != 0;
    if (fOk)
        fOk = EVP_DecryptFinal_ex(ctx, (&vchPlaintext[0]) + nPLen, &nFLen) != 0;
    EVP_CIPHER_CTX_cleanup(ctx);

    EVP_CIPHER_CTX_free(ctx);

    if (!fOk) return false;

    vchPlaintext.resize(nPLen + nFLen);
    return true;
}

class TestCrypter {
public:
    static void TestPassphraseSingle(
        const std::vector<uint8_t> &vchSalt, const SecureString &passphrase,
        uint32_t rounds,
        const std::vector<uint8_t> &correctKey = std::vector<uint8_t>(),
        const std::vector<uint8_t> &correctIV = std::vector<uint8_t>()) {
        uint8_t chKey[WALLET_CRYPTO_KEY_SIZE];
        uint8_t chIV[WALLET_CRYPTO_IV_SIZE];

        CCrypter crypt;
        crypt.SetKeyFromPassphrase(passphrase, vchSalt, rounds, 0);

        OldSetKeyFromPassphrase(passphrase, vchSalt, rounds, 0, chKey, chIV);

        BOOST_CHECK_MESSAGE(
            memcmp(chKey, crypt.vchKey.data(), crypt.vchKey.size()) == 0,
            HexStr(chKey, chKey + sizeof(chKey)) + std::string(" != ") +
                HexStr(crypt.vchKey));
        BOOST_CHECK_MESSAGE(
            memcmp(chIV, crypt.vchIV.data(), crypt.vchIV.size()) == 0,
            HexStr(chIV, chIV + sizeof(chIV)) + std::string(" != ") +
                HexStr(crypt.vchIV));

        if (!correctKey.empty())
            BOOST_CHECK_MESSAGE(
                memcmp(chKey, &correctKey[0], sizeof(chKey)) == 0,
                HexStr(chKey, chKey + sizeof(chKey)) + std::string(" != ") +
                    HexStr(correctKey.begin(), correctKey.end()));
        if (!correctIV.empty())
            BOOST_CHECK_MESSAGE(memcmp(chIV, &correctIV[0], sizeof(chIV)) == 0,
                                HexStr(chIV, chIV + sizeof(chIV)) +
                                    std::string(" != ") +
                                    HexStr(correctIV.begin(), correctIV.end()));
    }

    static void TestPassphrase(
        const std::vector<uint8_t> &vchSalt, const SecureString &passphrase,
        uint32_t rounds,
        const std::vector<uint8_t> &correctKey = std::vector<uint8_t>(),
        const std::vector<uint8_t> &correctIV = std::vector<uint8_t>()) {
        TestPassphraseSingle(vchSalt, passphrase, rounds, correctKey,
                             correctIV);
        for (SecureString::const_iterator i(passphrase.begin());
             i != passphrase.end(); ++i)
            TestPassphraseSingle(vchSalt, SecureString(i, passphrase.end()),
                                 rounds);
    }

    static void TestDecrypt(
        const CCrypter &crypt, const std::vector<uint8_t> &vchCiphertext,
        const std::vector<uint8_t> &vchPlaintext = std::vector<uint8_t>()) {
        CKeyingMaterial vchDecrypted1;
        CKeyingMaterial vchDecrypted2;
        int result1, result2;
        result1 = crypt.Decrypt(vchCiphertext, vchDecrypted1);
        result2 = OldDecrypt(vchCiphertext, vchDecrypted2, crypt.vchKey.data(),
                             crypt.vchIV.data());
        BOOST_CHECK(result1 == result2);

        // These two should be equal. However, OpenSSL 1.0.1j introduced a
        // change that would zero all padding except for the last byte for
        // failed decrypts.
        // This behavior was reverted for 1.0.1k.
        if (vchDecrypted1 != vchDecrypted2 &&
            vchDecrypted1.size() >= AES_BLOCK_SIZE && SSLeay() == 0x100010afL) {
            for (CKeyingMaterial::iterator it =
                     vchDecrypted1.end() - AES_BLOCK_SIZE;
                 it != vchDecrypted1.end() - 1; it++)
                *it = 0;
        }

        BOOST_CHECK_MESSAGE(
            vchDecrypted1 == vchDecrypted2,
            HexStr(vchDecrypted1.begin(), vchDecrypted1.end()) +
                " != " + HexStr(vchDecrypted2.begin(), vchDecrypted2.end()));

        if (vchPlaintext.size())
            BOOST_CHECK(CKeyingMaterial(vchPlaintext.begin(),
                                        vchPlaintext.end()) == vchDecrypted2);
    }

    static void
    TestEncryptSingle(const CCrypter &crypt,
                      const CKeyingMaterial &vchPlaintext,
                      const std::vector<uint8_t> &vchCiphertextCorrect =
                          std::vector<uint8_t>()) {
        std::vector<uint8_t> vchCiphertext1;
        std::vector<uint8_t> vchCiphertext2;
        int result1 = crypt.Encrypt(vchPlaintext, vchCiphertext1);

        int result2 = OldEncrypt(vchPlaintext, vchCiphertext2,
                                 crypt.vchKey.data(), crypt.vchIV.data());
        BOOST_CHECK(result1 == result2);
        BOOST_CHECK(vchCiphertext1 == vchCiphertext2);

        if (!vchCiphertextCorrect.empty())
            BOOST_CHECK(vchCiphertext2 == vchCiphertextCorrect);

        const std::vector<uint8_t> vchPlaintext2(vchPlaintext.begin(),
                                                 vchPlaintext.end());

        if (vchCiphertext1 == vchCiphertext2)
            TestDecrypt(crypt, vchCiphertext1, vchPlaintext2);
    }

    static void TestEncrypt(const CCrypter &crypt,
                            const std::vector<uint8_t> &vchPlaintextIn,
                            const std::vector<uint8_t> &vchCiphertextCorrect =
                                std::vector<uint8_t>()) {
        TestEncryptSingle(
            crypt,
            CKeyingMaterial(vchPlaintextIn.begin(), vchPlaintextIn.end()),
            vchCiphertextCorrect);
        for (std::vector<uint8_t>::const_iterator i(vchPlaintextIn.begin());
             i != vchPlaintextIn.end(); ++i)
            TestEncryptSingle(crypt, CKeyingMaterial(i, vchPlaintextIn.end()));
    }
};

BOOST_AUTO_TEST_CASE(passphrase) {
    // These are expensive.

    TestCrypter::TestPassphrase(
        ParseHex("0000deadbeef0000"), "test", 25000,
        ParseHex(
            "fc7aba077ad5f4c3a0988d8daa4810d0d4a0e3bcb53af662998898f33df0556a"),
        ParseHex("cf2f2691526dd1aa220896fb8bf7c369"));

    std::string hash(GetRandHash().ToString());
    std::vector<uint8_t> vchSalt(8);
    GetRandBytes(&vchSalt[0], vchSalt.size());
    uint32_t rounds = insecure_rand();
    if (rounds > 30000) rounds = 30000;
    TestCrypter::TestPassphrase(vchSalt, SecureString(hash.begin(), hash.end()),
                                rounds);
}

BOOST_AUTO_TEST_CASE(encrypt) {
    std::vector<uint8_t> vchSalt = ParseHex("0000deadbeef0000");
    BOOST_CHECK(vchSalt.size() == WALLET_CRYPTO_SALT_SIZE);
    CCrypter crypt;
    crypt.SetKeyFromPassphrase("passphrase", vchSalt, 25000, 0);
    TestCrypter::TestEncrypt(crypt,
                             ParseHex("22bcade09ac03ff6386914359cfe885cfeb5f77f"
                                      "f0d670f102f619687453b29d"));

    for (int i = 0; i != 100; i++) {
        uint256 hash(GetRandHash());
        TestCrypter::TestEncrypt(
            crypt, std::vector<uint8_t>(hash.begin(), hash.end()));
    }
}

BOOST_AUTO_TEST_CASE(decrypt) {
    std::vector<uint8_t> vchSalt = ParseHex("0000deadbeef0000");
    BOOST_CHECK(vchSalt.size() == WALLET_CRYPTO_SALT_SIZE);
    CCrypter crypt;
    crypt.SetKeyFromPassphrase("passphrase", vchSalt, 25000, 0);

    // Some corner cases the came up while testing
    TestCrypter::TestDecrypt(crypt,
                             ParseHex("795643ce39d736088367822cdc50535ec6f10371"
                                      "5e3e48f4f3b1a60a08ef59ca"));
    TestCrypter::TestDecrypt(crypt,
                             ParseHex("de096f4a8f9bd97db012aa9d90d74de8cdea779c"
                                      "3ee8bc7633d8b5d6da703486"));
    TestCrypter::TestDecrypt(crypt,
                             ParseHex("32d0a8974e3afd9c6c3ebf4d66aa4e6419f8c173"
                                      "de25947f98cf8b7ace49449c"));
    TestCrypter::TestDecrypt(crypt,
                             ParseHex("e7c055cca2faa78cb9ac22c9357a90b4778ded9b"
                                      "2cc220a14cea49f931e596ea"));
    TestCrypter::TestDecrypt(crypt,
                             ParseHex("b88efddd668a6801d19516d6830da4ae9811988c"
                                      "cbaf40df8fbb72f3f4d335fd"));
    TestCrypter::TestDecrypt(crypt,
                             ParseHex("8cae76aa6a43694e961ebcb28c8ca8f8540b8415"
                                      "3d72865e8561ddd93fa7bfa9"));

    for (int i = 0; i != 100; i++) {
        uint256 hash(GetRandHash());
        TestCrypter::TestDecrypt(
            crypt, std::vector<uint8_t>(hash.begin(), hash.end()));
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*!
@file
Defines `boost::hana::Group::negate_mcd`.

@copyright Louis Dionne 2014
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_HANA_GROUP_NEGATE_MCD_HPP
#define BOOST_HANA_GROUP_NEGATE_MCD_HPP

#include <boost/hana/group/group.hpp>

#include <boost/hana/monoid/monoid.hpp>

namespace boost { namespace hana {
    //! Minimal complete definition: `negate`
    template <typename G1, typename G2>
    struct Group::negate_mcd {
        template <typename X, typename Y>
        static constexpr auto minus_impl(X x, Y y) {
            return plus(x, negate(y));
        }
    };
}}

#endif // !BOOST_HANA_GROUP_NEGATE_MCD_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2019 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_STREAM_CHACHA_POLICY_HPP
#define CRYPTO3_STREAM_CHACHA_POLICY_HPP

#include <boost/endian/conversion.hpp>

#include <boost/container/small_vector.hpp>

#include <nil/crypto3/detail/inline_variable.hpp>

#include <nil/crypto3/stream/detail/basic_functions.hpp>

namespace nil {
    namespace crypto3 {
        namespace stream {
            namespace detail {
                template<std::size_t Rounds, std::size_t IVBits, std::size_t KeyBits>
                struct chacha_policy : public basic_functions<32> {
                    typedef typename basic_functions<32>::byte_type byte_type;

                    constexpr static const std::size_t word_bits = basic_functions<32>::word_bits;
                    typedef typename basic_functions<32>::word_type word_type;

                    constexpr static const std::size_t rounds = Rounds;
                    BOOST_STATIC_ASSERT(Rounds % 2 == 0);

                    constexpr static const std::size_t value_bits = CHAR_BIT;
                    typedef byte_type value_type;

                    constexpr static const std::size_t block_size = 64;
                    constexpr static const std::size_t block_bits = block_size * value_bits;
                    typedef std::array<byte_type, block_size> block_type;

                    constexpr static const std::size_t min_key_bits = 16 * CHAR_BIT;
                    constexpr static const std::size_t max_key_bits = 32 * CHAR_BIT;
                    constexpr static const std::size_t key_bits = KeyBits;
                    constexpr static const std::size_t key_size = key_bits / CHAR_BIT;
                    BOOST_STATIC_ASSERT(min_key_bits <= KeyBits <= max_key_bits);
                    BOOST_STATIC_ASSERT(key_size % 16 == 0);
                    typedef std::array<byte_type, key_size> key_type;

                    constexpr static const std::size_t key_schedule_size = 16;
                    constexpr static const std::size_t key_schedule_bits = key_schedule_size * word_bits;
                    typedef std::array<word_type, key_schedule_size> key_schedule_type;

                    constexpr static const std::size_t round_constants_size = 4;
                    typedef std::array<word_type, round_constants_size> round_constants_type;

                    CRYPTO3_INLINE_VARIABLE(round_constants_type, tau,
                                            ({0x61707865, 0x3120646e, 0x79622d36, 0x6b206574}));
                    CRYPTO3_INLINE_VARIABLE(round_constants_type, sigma,
                                            ({0x61707865, 0x3320646e, 0x79622d32, 0x6b206574}));

                    constexpr static const std::size_t iv_bits = IVBits;
                    constexpr static const std::size_t iv_size = IVBits / CHAR_BIT;
                    typedef std::array<byte_type, iv_size> iv_type;
                };
            }    // namespace detail
        }        // namespace stream
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_CHACHA_POLICY_HPP


#include "fieldtrack/VelocityEstimator.h"
#include "argus_utils/utils/ParamUtils.h"
#include "argus_utils/random/MultivariateGaussian.hpp"
#include "argus_utils/utils/MathUtils.h"

#include <boost/foreach.hpp>
#include <sstream>

namespace argus
{
VelocityEstimator::VelocityEstimator() {}

void VelocityEstimator::Initialize( ros::NodeHandle& ph,
                                    ExtrinsicsInterface::Ptr extrinsics )
{
	GetParamRequired( ph, "body_frame", _bodyFrame );
	GetParam( ph, "max_entropy_threshold",
	          _maxEntropyThreshold,
	          std::numeric_limits<double>::infinity() );
	GetParam( ph, "two_dimensional", _twoDimensional, false );

	GetParamRequired( ph, "filter_order", _filterOrder );

	_initialState = VectorType::Zero( FullDim() );
	GetParam( ph, "initial_mean", _initialState );

	_initialCovariance = MatrixType( FullDim(), FullDim() );
	GetParamRequired( ph, "initial_covariance", _initialCovariance );
	_filter.Initialize( _initialState, _initialCovariance );

	_transCovRate = MatrixType( FullDim(), FullDim() );
	GetParamRequired( ph, "transition_covariance", _transCovRate );

	// Parse all update sources
	YAML::Node updateSources;
	GetParamRequired( ph, "update_sources", updateSources );
	YAML::Node::const_iterator iter;
	for( iter = updateSources.begin(); iter != updateSources.end(); iter++ )
	{
		const std::string& sourceName = iter->first.as<std::string>();
		ros::NodeHandle sh = ph.resolveName( "update_sources/" + sourceName );
		if( _sourceRegistry.count( sourceName ) > 0 )
		{
		  throw std::invalid_argument( "Source " + sourceName + " already registered!" );
		}
		_sourceRegistry[sourceName].Initialize( sh,
		                                        _twoDimensional,
		                                        _filterOrder,
		                                        _bodyFrame,
		                                        extrinsics );
	}
}

// CovarianceModel::Ptr VelocityEstimator::InitTransCovModel() const
// {
// 	TimeScaledCovariance::Ptr cov = std::make_shared<TimeScaledCovariance>();
// 	cov->Initialize( _transCovRate );
// 	cov->EnableL( false ); // TODO
// 	return cov;
// }

// std::unordered_map<std::string, CovarianceModel::Ptr>
// VelocityEstimator::InitObsCovModels() const
// {
// 	std::unordered_map<std::string, CovarianceModel::Ptr> out;
// 	typedef SourceRegistry::value_type Item;
// 	BOOST_FOREACH( const Item &item, _sourceRegistry )
// 	{
// 		const std::string& name = item.first;
// 		const VelocitySourceManager& manager = item.second;
// 		out[name] = manager.InitializeModel();
// 	}
// 	return out;
// }

// void VelocityEstimator::SetTransCovModel( const CovarianceModel& model )
// {
// 	// TODO Different transition covariance modes
// 	try
// 	{
// 		const FixedCovariance& mod = dynamic_cast<const FixedCovariance&>( model );
// 		_transCovRate = mod.GetValue();
// 		ROS_INFO_STREAM( "Transition covariance rate updated to: " << std::endl <<
// 		                 _transCovRate << std::endl <<
// 		                 "L: " << mod.GetLValue() << std::endl <<
// 		                 "D: " << mod.GetDValue().transpose() );
// 	}
// 	catch( std::bad_cast& e )
// 	{
// 		throw std::invalid_argument( "Transition cov type mismatch: " + std::string( e.what() ) );
// 	}
// }

// void VelocityEstimator::SetObsCovModel( const std::string& name,
//                                         const CovarianceModel& model )
// {
// 	if( _sourceRegistry.count( name ) == 0 )
// 	{
// 		throw std::invalid_argument( "Source " + name + " not registered!" );
// 	}
// 	try
// 	{
// 		const FixedCovariance& mod = dynamic_cast<const FixedCovariance&>( model );
// 		MatrixType R = mod.GetValue();
// 		ROS_INFO_STREAM( name << " covariance updated to: " << std::endl <<
// 		                 R << std::endl <<
// 		                 "L: " << mod.GetLValue() << std::endl <<
// 		                 "D: " << mod.GetDValue().transpose() );
// 	}
// 	catch( std::bad_cast& e )
// 	{}

// 	_sourceRegistry[name].SetModel( model );
// }

unsigned int VelocityEstimator::StateDim() const
{
	return _twoDimensional ? PoseSE2::TangentDimension :
	       PoseSE3::TangentDimension;
}

unsigned int VelocityEstimator::FullDim() const
{
	return (_filterOrder + 1) * StateDim();
}

void VelocityEstimator::GetFullVels( PoseSE3::TangentVector& vel,
                                     PoseSE3::CovarianceMatrix& cov ) const
{
	vel.setZero();
	cov.setZero();

	if( _twoDimensional )
	{
		std::vector<unsigned int> inds;
		inds = std::vector<unsigned int>( {0, 1, 5} );
		PutSubmatrix( _filter.GetState().head<3>(),
		              vel, inds );
		PutSubmatrix( _filter.GetCovariance().topLeftCorner<3, 3>(),
		              cov, inds, inds );
	}
	else
	{
		vel = _filter.GetState().head<6>();
		cov = _filter.GetCovariance().topLeftCorner<6, 6>();
	}
}

nav_msgs::Odometry VelocityEstimator::GetOdom() const
{
	PoseSE3::TangentVector vel;
	PoseSE3::CovarianceMatrix cov;
	GetFullVels( vel, cov );

	nav_msgs::Odometry msg;
	msg.header.frame_id = _bodyFrame;
	msg.header.stamp = GetFilterTime();
	msg.child_frame_id = _bodyFrame;
	msg.twist.twist = TangentToMsg( vel );
	SerializeMatrix( cov, msg.twist.covariance );
	return msg;
}

geometry_msgs::TwistStamped VelocityEstimator::GetTwist() const
{
	PoseSE3::TangentVector vel;
	PoseSE3::CovarianceMatrix cov;
	GetFullVels( vel, cov );

	geometry_msgs::TwistStamped msg;
	msg.header.frame_id = _bodyFrame;
	msg.header.stamp = GetFilterTime();
	msg.twist = TangentToMsg( vel );
	return msg;
}

geometry_msgs::TwistWithCovarianceStamped
VelocityEstimator::GetTwistWithCovariance() const
{
	PoseSE3::TangentVector vel;
	PoseSE3::CovarianceMatrix cov;
	GetFullVels( vel, cov );

	geometry_msgs::TwistWithCovarianceStamped msg;
	msg.header.frame_id = _bodyFrame;
	msg.header.stamp = GetFilterTime();
	msg.twist.twist = TangentToMsg( vel );
	SerializeMatrix( cov, msg.twist.covariance );
	return msg;
}

MatrixType VelocityEstimator::GetTransitionCov( double dt )
{
	return _transCovRate * dt;
}

void VelocityEstimator::ResetDerived( const ros::Time& time,
                                      const VectorType& state,
                                      const MatrixType& cov )
{
	VectorType initState = _initialState;
	if( state.size() == StateDim() )
	{
		initState.head( StateDim() ) = state;
	}
	else if( state.size() == FullDim() )
	{
		initState = state;
	}

	MatrixType initCov = _initialCovariance;
	if( cov.rows() == StateDim() && cov.cols() == StateDim() )
	{
		initCov.topLeftCorner( StateDim(), StateDim() ) = cov;
	}
	else if( cov.rows() == FullDim() && cov.cols() == FullDim() )
	{
		initCov = cov;
	}

	// Reset the filter state
	ROS_INFO_STREAM_THROTTLE( 1, "Resetting filter mean to: " << initState.transpose() <<
	                 " and cov: " << std::endl << initCov );
	_filter.Initialize( initState, initCov );

	// Reset all observation covariance adapters
	typedef SourceRegistry::value_type Item;
	BOOST_FOREACH( Item & item, _sourceRegistry )
	{
		item.second.Reset();
	}
}

PredictInfo VelocityEstimator::PredictUntil( const ros::Time& until )
{
	double dt = (until - GetFilterTime() ).toSec();
	MatrixType A = IntegralMatrix<double>( dt, StateDim(), _filterOrder );
	PredictInfo info = _filter.Predict( A, GetTransitionCov( dt ) );
	info.step_dt = dt;
	return info;
}

bool VelocityEstimator::ProcessMessage( const std::string& source,
                                        const ObservationMessage& msg,
                                        UpdateInfo& info )
{
	VelocitySourceManager& manager = _sourceRegistry.at( source );
	DerivObservation obs = boost::apply_visitor( manager, msg );

	// Check observation likelihood
	// TODO HACK!
	const MatrixType& C = obs.C;
	VectorType v = obs.derivatives - C * _filter.GetState();
	MatrixType V = C * _filter.GetCovariance() * C.transpose() + obs.covariance;
	double ll = GaussianLogPdf( V, v );
	if( !manager.CheckLogLikelihood( ll ) )
	{
		ROS_WARN_STREAM( "Rejecting observation from " <<
		                 source << " with log likelihood " << ll );
		return false;
	}

	info = _filter.Update( obs.derivatives, C, obs.covariance );
	info.time = GetFilterTime();
	info.frameId = source;

	manager.Update( info );
	return true;
}

void VelocityEstimator::CheckFilter()
{
	double entropy = GaussianEntropy( _filter.GetCovariance() );
	if( entropy > _maxEntropyThreshold )
	{
		ROS_WARN_STREAM_THROTTLE( 1, "Filter entropy: " << entropy << " greater than max: " <<
		                 _maxEntropyThreshold << " Resetting filter..." );
		ResetDerived( GetFilterTime() );
	}
}
}


#include <cmath>

#include <boost/timer/timer.hpp>

using namespace std;

// All code taken from the upstream documentation.
//

int
main ()
{
  boost::timer::cpu_timer t;

  for (long i (0); i < 100000000; ++i)
    static_cast<void> (sqrt (123.456L)); // burn some time

  return 0;
}


//==================================================================================================
/*!
  @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_COMMON_SIMD_FUNCTION_REM_PIO2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_REM_PIO2_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <tuple>
#include <utility>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD( rem_pio2_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      using iA0 =  bd::as_integer_t<A0>;
      using result = std::pair<iA0,A0>;
      inline result operator()(A0 const& a0) const
      {
        using stype =  bd::scalar_of_t<A0>;
        using sitype =  bd::scalar_of_t<iA0>;
        static const uint32_t size = bs::cardinal_of<A0>::value;
        alignas(iA0) std::array<sitype, size> tmp;
        alignas( A0) std::array<stype , size> txr;
        for(uint32_t i=0; i!=size; ++i)
        {
          std::tie(tmp[i],txr[i]) =  bs::rem_pio2(a0[i]);
        }
        return result(aligned_load<iA0>(&tmp[0], 0), aligned_load<A0>(&txr[0], 0));
      }
   };

} } }

#endif


#include "verlet/List.hpp"
#include "simul/Simulation.hpp"
#include "forces/NonlocalForce.hpp"
#include <Eigen/Geometry>
using namespace mdk;
using namespace mdk::vl;

#include <iostream>
using namespace std;

extern Eigen::AlignedBox3d bboxTP;
extern std::vector<int> firstTP, lastTP;
extern Pairs pairsTP;

#pragma omp threadprivate(bboxTP, firstTP, lastTP, pairsTP)

Eigen::AlignedBox3d bboxTP;
std::vector<int> firstTP, lastTP;
Pairs pairsTP;

static Eigen::Vector3i floor(Eigen::Vector3d const& v) {
    return Eigen::Vector3i {
        (int)std::floor(v.x()),
        (int)std::floor(v.y()),
        (int)std::floor(v.z())
    };
}

int List::indexOf(Eigen::Vector3i const& loc) {
    return loc.x() + grid.x() * (loc.y() + grid.y() * loc.z());
}

void List::perPair(int c1, int c2) {
    for (int pt1 = first[c1]; pt1 >= 0; pt1 = next[pt1]) {
        auto r1 = state->r[pt1];
        for (int pt2 = first[c2]; pt2 >= 0; pt2 = next[pt2]) {
            if (pt1 == pt2) continue;

            auto r2 = state->r[pt2];
            auto r12_norm2 = state->top(r2 - r1).squaredNorm();

            bool cond =
                r12_norm2 <= effCutoffSq &&
                (c1 != c2 || pt1 < pt2) &&
                chains->sepByAtLeastN(pt1, pt2, minBondSep);

            if (cond) {
                pairsTP.emplace_back(min(pt1, pt2), max(pt1, pt2));
            }
        }
    }
}

void List::perCell(int c1) {
    Eigen::Vector3i loc1 {
        c1 % grid.x(),
        (c1 / grid.x()) % grid.y(),
        (c1 / grid.x()) / grid.y()
    };

    Eigen::Vector3i d;
    for (d.x() = -1; d.x() <= 1; ++d.x()) {
        for (d.y() = -1; d.y() <= 1; ++d.y()) {
            for (d.z() = -1; d.z() <= 1; ++d.z()) {
                Eigen::Vector3i loc2 = loc1 + d;
                /* This part denotes that the space of the cells is organized
                 * in a "modular" fashion, i.e. the cells at the opposite sides
                 * are regarded as neighbours. This is done for the purposes
                 * of PBC-aware Verlet list construction.
                 */
                for (int dim = 0; dim < 3; ++dim) {
                    if (loc2[dim] >= grid[dim])
                        loc2[dim] = 0;

                    if (loc2[dim] < 0)
                        loc2[dim] = grid[dim] - 1;
                }

                auto c2 = indexOf(loc2);
                if (c1 <= c2) perPair(c1, c2);
            }
        }
    }
}

void List::updateGrid() {
    Eigen::Vector3d cell;
    int gridSize;
    Eigen::AlignedBox3d bbox;

    effCutoff = cutoff + pad;
    effCutoffSq = pow(effCutoff, 2.0);

    pairs.clear();

#pragma omp parallel
    {
        /* First, we determine the (axis-aligned) box containing all the
         * pseudoatoms.
         * TODO: If less cubic conformations are common, one could try to
         * TODO: compute a rotated box.
         */
        bboxTP = {};

#pragma omp for schedule(dynamic, 10) nowait
        for (int i = 0; i < state->n; ++i) {
            bboxTP.extend(state->top(state->r[i]));
        }

#pragma omp critical
        {
            bbox.extend(bboxTP);
        }
#pragma omp barrier

#pragma omp single
        {
            /* Next, we compute the integral size of the grid, and consequently
             * adjust the cell sizes. They will be slightly larger than
             * \p effCutoff. This is done in order to have the bounding box
             * divided into an integral number of cells along each axis, as
             * otherwise some would intuitively "stick out" which would be
             * troublesome for computing neighbors with PBC.
             */
            grid = floor(bbox.sizes() / effCutoff);
            for (int dim = 0; dim < 3; ++dim) {
                if (grid[dim] == 0) grid[dim] = 1;
            }

            cell = Eigen::Vector3d {
                bbox.sizes().x() / grid.x(),
                bbox.sizes().y() / grid.y(),
                bbox.sizes().z() / grid.z()
            };

            gridSize = grid.x() * grid.y() * grid.z();

            /* Then, we initialize the linked lists for each cell and the
             * \p next indicators.
             */
            first.resize(gridSize);
            std::fill(first.begin(), first.end(), -1);

            last.resize(gridSize);
            std::fill(last.begin(), last.end(), -1);

            next.resize(state->n);
            std::fill(next.begin(), next.end(), -1);
        }
#pragma omp barrier

        firstTP.resize(gridSize);
        std::fill(firstTP.begin(), firstTP.end(), -1);

        lastTP.resize(gridSize);
        std::fill(lastTP.begin(), lastTP.end(), -1);

#pragma omp for schedule(dynamic, 10) nowait
        for (int i = 0; i < state->n; ++i) {
            auto v = state->top(state->r[i]);

            /* For each pseudoatom, we find which cell it should belong to,
             * and add it to the appropriate linked list.
             */
            Eigen::Vector3i loc = {
                (int)std::floor((v.x() - bbox.min().x()) / cell.x()),
                (int)std::floor((v.y() - bbox.min().y()) / cell.y()),
                (int)std::floor((v.z() - bbox.min().z()) / cell.z()),
            };

            /* This is just for edge cases. */
            for (int dim = 0; dim < 3; ++dim) {
                if (loc[dim] >= grid[dim])
                    loc[dim] = grid[dim]-1;
            }

            int c = indexOf(loc);
            if (firstTP[c] < 0) {
                firstTP[c] = lastTP[c] = i;
            } else {
                next[lastTP[c]] = i;
                lastTP[c] = i;
            }
        }

        /* Here we merge the thread-private lists into a single one.
         */
#pragma omp critical
        {
            for (int c = 0; c < gridSize; ++c) {
                if (firstTP[c] < 0)
                    continue;

                if (first[c] < 0) {
                    first[c] = firstTP[c];
                } else {
                    next[last[c]] = firstTP[c];
                }
                last[c] = lastTP[c];
            }
        }
#pragma omp barrier

        pairsTP.clear();

        /* Next, we go through the cells and investigate the pairs of
         * neighbouring cells.
         */
#pragma omp for schedule(dynamic, 10) nowait
        for (int c1 = 0; c1 < gridSize; ++c1) {
            perCell(c1);
        }

        /* Finally, we merge thread-private lists into a single one.
         */
#pragma omp critical
        {
            pairs.insert(pairs.end(), pairsTP.begin(), pairsTP.end());
        }
    }

    sort(pairs.begin(), pairs.end());
    for (auto& force: forces) {
        force->vlUpdateHook();
    }
}

void List::update() {
    effCutoff = cutoff + pad;
    effCutoffSq = pow(effCutoff, 2.0);

    pairs.clear();

    #pragma omp parallel
    {
        pairsTP.clear();

        #pragma omp for schedule(dynamic, 10) nowait
        for (int pt1 = 0; pt1 < r0.size(); ++pt1) {
            auto r1 = state->r[pt1];
            for (int pt2 = pt1+1; pt2 < r0.size(); ++pt2) {
                auto r2 = state->r[pt2];
                auto r12_norm2 = state->top(r2 - r1).squaredNorm();

                bool cond = r12_norm2 <= effCutoffSq &&
                    chains->sepByAtLeastN(pt1, pt2, minBondSep);

                if (cond) pairsTP.emplace_back(pt1, pt2);
            }
        }

        #pragma omp critical
        {
            pairs.insert(pairs.end(), pairsTP.begin(), pairsTP.end());
        }
    }

    sort (pairs.begin(), pairs.end());
    for (auto& force: forces) {
        force->vlUpdateHook();
    }
}

bool List::needToReset() const {
    if (initial) return true;
    if (t0 == state->t) return false;

    auto maxMoveSq = 0.0;
    for (int i = 0; i < state->n; ++i) {
        auto moveSq = (state->r[i] - r0[i]).squaredNorm();
        maxMoveSq = std::max(maxMoveSq, moveSq);
    }

    auto maxMove = sqrt(maxMoveSq);
    auto pbcShift = (top0.cell - state->top.cell).lpNorm<1>();
    return maxMove + 2.0 * pbcShift >= pad / 2.0;
}

void List::bind(Simulation &simulation) {
    state = &simulation.var<State>();
    chains = &simulation.data<Chains>();
    initial = true;
}

void List::check() {
    if (needToReset()) {
        t0 = state->t;
        r0 = state->r;
        top0 = state->top;
        update();
    }
    initial = false;
}

void List::registerNF(NonlocalForce& force, Spec const& spec) {
    cutoff = std::max(cutoff, sqrt(spec.cutoffSq));
    if (minBondSep < 1) minBondSep = spec.minBondSep;
    else minBondSep = std::min(minBondSep, spec.minBondSep);

    forces.push_back(&force);
}


/*!
@file
Defines `boost::hana::ext::std::Ratio`.

@copyright Louis Dionne 2015
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_HANA_EXT_STD_RATIO_HPP
#define BOOST_HANA_EXT_STD_RATIO_HPP

#include <boost/hana/bool.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/constant.hpp>
#include <boost/hana/core/convert.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/detail/std/is_integral.hpp>
#include <boost/hana/group.hpp>
#include <boost/hana/integral_domain.hpp>
#include <boost/hana/monoid.hpp>
#include <boost/hana/orderable.hpp>
#include <boost/hana/ring.hpp>

#include <cstdint>
#include <ratio>

namespace boost { namespace hana {
    namespace ext { namespace std { struct Ratio; }}

    template < ::std::intmax_t num, ::std::intmax_t den>
    struct datatype< ::std::ratio<num, den>> {
        using type = ext::std::Ratio;
    };

    //////////////////////////////////////////////////////////////////////////
    // Conversion from Constants
    //////////////////////////////////////////////////////////////////////////
    template <typename C>
    struct to_impl<ext::std::Ratio, C, when<
        _models<Constant, C>{}() &&
        detail::std::is_integral<typename C::value_type>{}
    >> {
        template <typename N>
        static constexpr auto apply(N const&) {
            constexpr auto v = hana::value<N>();
            return ::std::ratio<v>{};
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<ext::std::Ratio, ext::std::Ratio> {
        template <typename R1, typename R2>
        static constexpr auto apply(R1 const&, R2 const&)
        { return bool_< ::std::ratio_equal<R1, R2>::value>; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct less_impl<ext::std::Ratio, ext::std::Ratio> {
        template <typename R1, typename R2>
        static constexpr auto apply(R1 const&, R2 const&)
        { return bool_< ::std::ratio_less<R1, R2>::value>; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct plus_impl<ext::std::Ratio, ext::std::Ratio> {
        template <typename R1, typename R2>
        static constexpr ::std::ratio_add<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct zero_impl<ext::std::Ratio> {
        static constexpr ::std::ratio<0> apply()
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Group
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct minus_impl<ext::std::Ratio, ext::std::Ratio> {
        template <typename R1, typename R2>
        static constexpr ::std::ratio_subtract<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct mult_impl<ext::std::Ratio, ext::std::Ratio> {
        template <typename R1, typename R2>
        static constexpr ::std::ratio_multiply<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct one_impl<ext::std::Ratio> {
        static constexpr ::std::ratio<1> apply()
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // IntegralDomain
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct quot_impl<ext::std::Ratio, ext::std::Ratio> {
        template <typename R1, typename R2>
        static constexpr ::std::ratio_divide<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct rem_impl<ext::std::Ratio, ext::std::Ratio> {
        template <typename R1, typename R2>
        static constexpr ::std::ratio<0> apply(R1 const&, R2 const&)
        { return {}; }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_EXT_STD_RATIO_HPP