#include "MeshCuboidRelation.h"

#include "Utilities.h"

#include <cstdint>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

// Note:
// Assume that z = 0 plane is the ground plane, and +z-axis is the normal direction.
const MyMesh::Normal MeshCuboidAttributes::k_up_direction = MyMesh::Normal(0.0, 0.0, 1.0);

MeshCuboidAttributes::MeshCuboidAttributes()
	: object_name_("")
{
	initialize();
}

MeshCuboidAttributes::MeshCuboidAttributes(const std::string _object_name)
	: object_name_(_object_name)
{
	initialize();
}

MeshCuboidAttributes::MeshCuboidAttributes(const MeshCuboidAttributes& _other)
	: object_name_(_other.object_name_)
	, attributes_(_other.attributes_)
{
}

MeshCuboidAttributes::~MeshCuboidAttributes()
{
}

void MeshCuboidAttributes::initialize()
{
	// Note:
	// Initialized with NaN.
	if (!std::numeric_limits<Real>::has_quiet_NaN)
	{
		std::cerr << "Error: " << typeid(Real).name() << " does not support quiet NaN." << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	const int num_attributes = static_cast<int>(k_num_attributes);

	attributes_ = Eigen::VectorXd::Zero(num_attributes);
	for (int attribute_index = 0; attribute_index < num_attributes; ++attribute_index)
	{
		attributes_[attribute_index] = std::numeric_limits<Real>::quiet_NaN();
	}
}

void MeshCuboidAttributes::compute_attributes(const MeshCuboid *_cuboid)
{
	assert(attributes_.size() == static_cast<int>(k_num_attributes));

	for (unsigned int i = 0; i < 3; ++i)
	{
		//attributes_[k_center_index + i] = _cuboid->get_bbox_center()[i];

		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			attributes_[k_corner_index + 3 * corner_index + i] = _cuboid->get_bbox_corner(corner_index)[i];
	}
}

void MeshCuboidAttributes::get_attribute_collection_matrix(const std::list<MeshCuboidAttributes *>& _stats,
	Eigen::MatrixXd& _values)
{
	unsigned int num_objects = _stats.size();

	const int num_attributes = static_cast<int>(k_num_attributes);
	_values = Eigen::MatrixXd(num_objects, num_attributes);

	unsigned int object_index = 0;
	for (std::list<MeshCuboidAttributes *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int attribute_index = 0; attribute_index < num_attributes; ++attribute_index)
			_values(object_index, attribute_index) = (*stat_it)->attributes_[attribute_index];
	}
}

bool MeshCuboidAttributes::save_attribute_collection(const std::list<MeshCuboidAttributes *>& _stats,
	const char* _filename)
{
	const int num_attributes = static_cast<int>(k_num_attributes);

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	//unsigned int num_objects = _stats.size();

	// Write attribute values.
	unsigned int object_index = 0;
	for (std::list<MeshCuboidAttributes *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int attribute_index = 0; attribute_index < num_attributes; ++attribute_index)
		{
			Real value = (*stat_it)->attributes_[attribute_index];
			if (std::isnan(value))
			{
				// Note:
				// Undefined attributes are recorded as NaN.
				file << "NaN,";
			}
			else
			{
				file << value << ",";
			}
		}
		file << std::endl;
	}

	file.close();
	return true;
}

MeshCuboidFeatures::MeshCuboidFeatures()
	: object_name_("")
{
	initialize();
}

MeshCuboidFeatures::MeshCuboidFeatures(const std::string _object_name)
	: object_name_(_object_name)
{
	initialize();
}

MeshCuboidFeatures::MeshCuboidFeatures(const MeshCuboidFeatures& _other)
	: object_name_(_other.object_name_)
	, features_(_other.features_)
{
}

MeshCuboidFeatures::~MeshCuboidFeatures()
{
}

void MeshCuboidFeatures::initialize()
{
	// Note:
	// Initialized with NaN.
	if (!std::numeric_limits<Real>::has_quiet_NaN)
	{
		std::cerr << "Error: " << typeid(Real).name() << " does not support quiet NaN." << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	features_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	for (int attribute_index = 0; attribute_index < MeshCuboidFeatures::k_num_features; ++attribute_index)
	{
		features_[attribute_index] = std::numeric_limits<Real>::quiet_NaN();
	}
}

void MeshCuboidFeatures::compute_features(const MeshCuboid *_cuboid,
	Eigen::MatrixXd *_attributes_to_features_map)
{
	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	assert(features_.size() == static_cast<int>(MeshCuboidFeatures::k_num_features));

	// Linear map.
	Eigen::MatrixXd attributes_to_features_map =
		Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, num_attributes);

	Eigen::RowVector3d up_direction_vec;
	for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		up_direction_vec(axis_index) = MeshCuboidAttributes::k_up_direction[axis_index];

	unsigned int next_feature_index = 0;

	// Center point.
	for (unsigned int i = 0; i < 3; ++i)
	{
		features_[next_feature_index] = _cuboid->get_bbox_center()[i];

		//attributes_to_features_map.row(next_feature_index)(
		//	MeshCuboidAttributes::k_center_index + i) = 1.0;
		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
		{
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
				1.0 / MeshCuboid::k_num_corners;
		}

		++next_feature_index;
	}

	// Corner points.
	for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
	{
		MyMesh::Point corner = _cuboid->get_bbox_corner(corner_index);

		for (unsigned int i = 0; i < 3; ++i)
		{
			features_[next_feature_index] = corner[i];
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) = 1.0;
			++next_feature_index;
		}
	}

	// Center height.
	features_[next_feature_index] = dot(MeshCuboidAttributes::k_up_direction, _cuboid->get_bbox_center());
	for (unsigned int i = 0; i < 3; ++i)
	{
		//attributes_to_features_map.row(next_feature_index)(
		//	MeshCuboidAttributes::k_center_index + i) = MeshCuboidAttributes::k_up_direction[i];
		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
		{
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
				(1.0 / MeshCuboid::k_num_corners) * MeshCuboidAttributes::k_up_direction[i];
		}
	}
	++next_feature_index;

	// Corner height.
	for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
	{
		MyMesh::Point corner = _cuboid->get_bbox_corner(corner_index);
		features_[next_feature_index] = dot(MeshCuboidAttributes::k_up_direction, corner);

		for (unsigned int i = 0; i < 3; ++i)
		{
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
				MeshCuboidAttributes::k_up_direction[i];
		}
		++next_feature_index;
	}

	assert(next_feature_index == static_cast<int>(MeshCuboidFeatures::k_num_features));

#ifdef DEBUG_TEST
	MeshCuboidAttributes attributes;
	attributes.compute_attributes(_cuboid);
	Eigen::VectorXd same_features = attributes_to_features_map * attributes.get_attributes();

	assert(same_features.rows() == MeshCuboidFeatures::k_num_features);
	Real error = (same_features - features_).array().abs().sum();

	CHECK_NUMERICAL_ERROR(__FUNCTION__, error);
#endif

	// Optional.
	if (_attributes_to_features_map)
		(*_attributes_to_features_map) = attributes_to_features_map;
}

void MeshCuboidFeatures::get_feature_collection_matrix(const std::list<MeshCuboidFeatures *>& _stats,
	Eigen::MatrixXd& _values)
{
	unsigned int num_objects = _stats.size();

	const int num_features = static_cast<int>(k_num_features);
	_values = Eigen::MatrixXd(num_objects, num_features);

	unsigned int object_index = 0;
	for (std::list<MeshCuboidFeatures *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int feature_index = 0; feature_index < num_features; ++feature_index)
			_values(object_index, feature_index) = (*stat_it)->features_[feature_index];
	}
}

bool MeshCuboidFeatures::load_feature_collection(const char* _filename,
	std::list<MeshCuboidFeatures *>& _stats)
{
	for (std::list<MeshCuboidFeatures *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
		delete (*it);
	_stats.clear();

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't load file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;
	bool succeded = true;

	while (!file.eof())
	{
		std::getline(file, buffer);
		if (buffer == "") break;

		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		MeshCuboidFeatures *new_features = new MeshCuboidFeatures();
		assert(new_features);

		for (int feature_index = 0; feature_index < MeshCuboidFeatures::k_num_features; ++feature_index)
		{
			std::getline(strstr, token, ',');
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				succeded = false;
				break;
			}

			if (token == "NaN")
			{
				// Note:
				// Undefined attributes are recorded as NaN.
				new_features->features_[feature_index] = std::numeric_limits<Real>::quiet_NaN();
			}
			else
			{
				new_features->features_[feature_index] = std::stof(token);
			}
		}

		if (!succeded) break;
		_stats.push_back(new_features);
	}

	if (!succeded)
	{
		for (std::list<MeshCuboidFeatures *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
			delete (*it);
		_stats.clear();
		return false;
	}

	return true;
}

bool MeshCuboidFeatures::save_feature_collection(const char* _filename,
	const std::list<MeshCuboidFeatures *>& _stats)
{
	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	//unsigned int num_objects = _stats.size();

	// Write attribute values.
	unsigned int object_index = 0;
	for (std::list<MeshCuboidFeatures *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int feature_index = 0; feature_index < MeshCuboidFeatures::k_num_features; ++feature_index)
		{
			Real value = (*stat_it)->features_[feature_index];
			if (std::isnan(value))
			{
				// Note:
				// Undefined attributes are recorded as NaN.
				file << "NaN,";
			}
			else
			{
				file << value << ",";
			}
		}
		file << std::endl;
	}

	file.close();
	return true;
}

MeshCuboidTransformation::MeshCuboidTransformation()
: object_name_("")
{
	initialize();
}

MeshCuboidTransformation::MeshCuboidTransformation(const std::string _object_name)
: object_name_(_object_name)
{
	initialize();
}

MeshCuboidTransformation::~MeshCuboidTransformation()
{
}

void MeshCuboidTransformation::initialize()
{
	first_translation_ = Eigen::Vector3d::Zero();
	second_rotation_ = Eigen::Matrix3d::Identity();
}

void MeshCuboidTransformation::compute_transformation(const MeshCuboid *_cuboid)
{
	assert(_cuboid);

	for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
	{
		for (unsigned int i = 0; i < 3; ++i)
		{
			second_rotation_.row(axis_index)(i) = _cuboid->get_bbox_axis(axis_index)[i];
		}
		first_translation_(axis_index) = -_cuboid->get_bbox_center()[axis_index];
	};
}

Eigen::VectorXd
MeshCuboidTransformation::get_transformed_features(
const MeshCuboidFeatures& _other_features)const
{
	Eigen::VectorXd transformed_features = _other_features.get_features();

	for (unsigned int i = 0; i < MeshCuboidFeatures::k_num_local_points; ++i)
	{
		Eigen::VectorXd sub_values = transformed_features.block(3 * i, 0, 3, 1);
		sub_values = first_translation_ + sub_values;
		sub_values = second_rotation_ * sub_values;
		transformed_features.block(3 * i, 0, 3, 1) = sub_values;
	}

//#ifdef DEBUG_TEST
//	Eigen::MatrixXd rotation;
//	Eigen::MatrixXd translation;
//	get_linear_map_transformation(rotation, translation);
//	Eigen::VectorXd same_transformed_features = rotation * (translation * _other_features.get_features());
//
//	assert(same_transformed_features.rows() == MeshCuboidFeatures::k_num_features);
//	Real error = (same_transformed_features - transformed_features).array().abs().sum();
//
//	CHECK_NUMERICAL_ERROR(__FUNCTION__, error);
//#endif

	return transformed_features;
}

Eigen::VectorXd
MeshCuboidTransformation::get_transformed_features(const MeshCuboid *_other_cuboid)const
{
	assert(_other_cuboid);

	MeshCuboidFeatures other_features;
	other_features.compute_features(_other_cuboid);

	return get_transformed_features(other_features);
}

Eigen::VectorXd
MeshCuboidTransformation::get_inverse_transformed_features(
	const MeshCuboidFeatures& _other_features)const
{
	Eigen::VectorXd inverse_transformed_features = _other_features.get_features();

	for (unsigned int i = 0; i < MeshCuboidFeatures::k_num_local_points; ++i)
	{
		Eigen::VectorXd sub_values = inverse_transformed_features.block(3 * i, 0, 3, 1);
		sub_values = second_rotation_.transpose() * sub_values;
		sub_values = -first_translation_ + sub_values;
		inverse_transformed_features.block(3 * i, 0, 3, 1) = sub_values;
	}

//#ifdef DEBUG_TEST
//	Eigen::MatrixXd rotation;
//	Eigen::MatrixXd translation;
//	get_linear_map_inverse_transformation(rotation, translation);
//	Eigen::VectorXd same_transformed_features = rotation * (translation * _other_features.get_features());
//
//	assert(same_transformed_features.rows() == MeshCuboidFeatures::k_num_features);
//	Real error = (same_transformed_features - inverse_transformed_features).array().abs().sum();
//
//	CHECK_NUMERICAL_ERROR(__FUNCTION__, error);
//#endif

	return inverse_transformed_features;
}

Eigen::VectorXd
MeshCuboidTransformation::get_inverse_transformed_features(const MeshCuboid *_other_cuboid)const
{
	assert(_other_cuboid);

	MeshCuboidFeatures other_features;
	other_features.compute_features(_other_cuboid);

	return get_inverse_transformed_features(other_features);
}

void MeshCuboidTransformation::get_transformation(
	Eigen::Matrix3d &_rotation, Eigen::Vector3d &_translation) const
{
	_rotation = second_rotation_;
	_translation = first_translation_;
	_translation = _rotation * _translation;
}

void MeshCuboidTransformation::get_inverse_transformation(
	Eigen::Matrix3d &_rotation, Eigen::Vector3d &_translation) const
{
	_rotation = second_rotation_.transpose();
	_translation = -first_translation_;
}

void MeshCuboidTransformation::get_linear_map_transformation(
	Eigen::MatrixXd &_rotation, Eigen::MatrixXd &_translation) const
{
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	_rotation = Eigen::MatrixXd::Identity(num_features, num_features);
	_translation = Eigen::MatrixXd::Zero(num_features, MeshCuboidAttributes::k_num_attributes);

	for (unsigned int k = 0; k < MeshCuboidFeatures::k_num_local_points; ++k)
	{
		_rotation.block<3, 3>(3 * k, 3 * k) = second_rotation_;

		for (unsigned int i = 0; i < 3; ++i)
		{
			//_translation(3 * k + i, MeshCuboidAttributes::k_center_index + i) = -1;
			for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			{
				_translation(3 * k + i, MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
					-1.0 / MeshCuboid::k_num_corners;
			}
		}
	}

	_translation = _rotation * _translation;
}

void MeshCuboidTransformation::get_linear_map_inverse_transformation(
	Eigen::MatrixXd &_rotation, Eigen::MatrixXd &_translation) const
{
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	_rotation = Eigen::MatrixXd::Identity(num_features, num_features);
	_translation = Eigen::MatrixXd::Zero(num_features, MeshCuboidAttributes::k_num_attributes);

	for (unsigned int k = 0; k < MeshCuboidFeatures::k_num_local_points; ++k)
	{
		_rotation.block<3, 3>(3 * k, 3 * k) = second_rotation_.transpose();

		for (unsigned int i = 0; i < 3; ++i)
		{
			//_translation(3 * k + i, MeshCuboidAttributes::k_center_index + i) = 1;
			for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			{
				_translation(3 * k + i, MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
					1.0 / MeshCuboid::k_num_corners;
			}
		}
	}
}

bool MeshCuboidTransformation::load_transformation_collection(const char* _filename,
	std::list<MeshCuboidTransformation *>& _stats)
{
	for (std::list<MeshCuboidTransformation *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
		delete (*it);
	_stats.clear();

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't load file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;
	bool succeded = true;

	while (!file.eof())
	{
		std::getline(file, buffer);
		if (buffer == "") break;

		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		MeshCuboidTransformation *new_transformation = new MeshCuboidTransformation();
		assert(new_transformation);

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		{
			for (unsigned int i = 0; i < 3; ++i)
			{
				std::getline(strstr, token, ',');
				if (strstr.eof())
				{
					std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
					succeded = false;
					break;
				}

				if (!succeded) break;
				new_transformation->second_rotation_.col(axis_index)(i) = std::stof(token);
			}
		}

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		{
			std::getline(strstr, token, ',');
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				succeded = false;
				break;
			}

			new_transformation->first_translation_(axis_index) = std::stof(token);
		}

		if (!succeded) break;
		_stats.push_back(new_transformation);
	}

	if (!succeded)
	{
		for (std::list<MeshCuboidTransformation *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
			delete (*it);
		_stats.clear();
		return false;
	}

	return true;
}

bool MeshCuboidTransformation::save_transformation_collection(const char* _filename,
	const std::list<MeshCuboidTransformation *>& _stats)
{
	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	//unsigned int num_objects = _stats.size();

	// Write attribute values.
	unsigned int object_index = 0;
	for (std::list<MeshCuboidTransformation *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
			for (unsigned int i = 0; i < 3; ++i)
				file << (*stat_it)->second_rotation_.col(axis_index)(i) << ",";

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
			file << (*stat_it)->first_translation_(axis_index) << ",";

		file << std::endl;
	}

	file.close();
	return true;
}

MeshCuboidJointNormalRelations::MeshCuboidJointNormalRelations()
{
	mean_ = Eigen::VectorXd::Zero(k_mat_size);
	inv_cov_ = Eigen::MatrixXd::Zero(k_mat_size, k_mat_size);
}

MeshCuboidJointNormalRelations::~MeshCuboidJointNormalRelations()
{

}

bool MeshCuboidJointNormalRelations::load_joint_normal_csv(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ", ", "", "", "", "", "");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < k_mat_size; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');

		// Transposed.
		mean_(j) = std::stof(token);
	}

	for (int i = 0; i < k_mat_size; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < k_mat_size; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			inv_cov_(i, j) = std::stof(token);
		}
	}

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

bool MeshCuboidJointNormalRelations::save_joint_normal_csv(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	file << mean_.transpose().format(csv_format) << std::endl;
	file << inv_cov_.format(csv_format) << std::endl;

	file.close();
	return true;
}

bool MeshCuboidJointNormalRelations::load_joint_normal_dat(const char* _filename)
{
	std::ifstream file(_filename, std::ios::in | std::ios::binary);
	if (!file.good())
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	int16_t rows, cols;

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == k_mat_size);
	assert(static_cast<int>(cols) == 1);
	mean_.resize(rows);
	file.read((char *)mean_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == k_mat_size);
	assert(static_cast<int>(cols) == k_mat_size);
	inv_cov_.resize(rows, cols);
	file.read((char *)inv_cov_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

void MeshCuboidJointNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_pairwise_features_vec)
{
	assert(_cuboid_1);
	assert(_cuboid_2);

	MeshCuboidFeatures features_1;
	features_1.compute_features(_cuboid_1);

	MeshCuboidFeatures features_2;
	features_2.compute_features(_cuboid_2);

	get_pairwise_cuboid_features(features_1, features_2,
		_transformation_1, _transformation_2, _pairwise_features_vec);
}

void MeshCuboidJointNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboidFeatures &_features_1, const MeshCuboidFeatures &_features_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_pairwise_features_vec)
{
	assert(_transformation_1);
	assert(_transformation_2);

	Eigen::VectorXd transformed_features_vec_11 = _transformation_1->get_transformed_features(_features_1);
	assert(std::abs(transformed_features_vec_11[0]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[1]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[2]) < NUMERIAL_ERROR_THRESHOLD);
	assert(transformed_features_vec_11.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_12 = _transformation_1->get_transformed_features(_features_2);
	assert(transformed_features_vec_12.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_22 = _transformation_2->get_transformed_features(_features_2);
	assert(std::abs(transformed_features_vec_22[0]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_22[1]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_22[2]) < NUMERIAL_ERROR_THRESHOLD);
	assert(transformed_features_vec_22.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_21 = _transformation_2->get_transformed_features(_features_1);
	assert(transformed_features_vec_21.rows() == MeshCuboidFeatures::k_num_features);

	// NOTE:
	// Since the center point is always the origin in the local coordinates,
	// it is not used as the feature values.
	assert(k_mat_size == 2 * (2 * MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index));
	_pairwise_features_vec.resize(k_mat_size);
	_pairwise_features_vec <<
		transformed_features_vec_11.bottomRows(MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index),
		transformed_features_vec_12,
		transformed_features_vec_22.bottomRows(MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index),
		transformed_features_vec_21;
}

double MeshCuboidJointNormalRelations::compute_error(const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2) const
{
	Eigen::VectorXd pairwise_cuboid_feature;
	get_pairwise_cuboid_features(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2,
		pairwise_cuboid_feature);

	assert(mean_.rows() == pairwise_cuboid_feature.rows());
	assert(inv_cov_.rows() == pairwise_cuboid_feature.rows());
	assert(inv_cov_.cols() == pairwise_cuboid_feature.rows());

	Eigen::VectorXd diff = pairwise_cuboid_feature - mean_;

	// Mahalanobis norm.
	double error = diff.transpose() * inv_cov_ * diff;
	assert(error >= 0);

	//std::cerr << "Negative error value (error = " << error << ")" << std::endl;
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	//Real inv_cov_det = inv_cov_.determinant();
	//std::cout << "inv_cov_det = " << inv_cov_det << std::endl;

	//Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	//std::cerr << "symmetry_diff = " << symmetry_diff << std::endl;

	//std::cout << "Norm(diff) = " << diff.transpose() * diff << std::endl;

	//std::cout << "diff = " << std::endl;
	//std::cout << diff.format(csv_format) << std::endl;

	//do {
	//	std::cout << '\n' << "Press the Enter key to continue.";
	//} while (std::cin.get() != '\n');

	return error;
}

double MeshCuboidJointNormalRelations::compute_conditional_error(const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1) const
{
	Eigen::VectorXd conditional_pairwise_cuboid_feature;
	assert(_transformation_1);

	MeshCuboidFeatures features_1;
	features_1.compute_features(_cuboid_1);

	MeshCuboidFeatures features_2;
	features_2.compute_features(_cuboid_2);

	Eigen::VectorXd transformed_features_vec_11 = _transformation_1->get_transformed_features(features_1);
	assert(std::abs(transformed_features_vec_11[0]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[1]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[2]) < NUMERIAL_ERROR_THRESHOLD);
	assert(transformed_features_vec_11.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_12 = _transformation_1->get_transformed_features(features_2);
	assert(transformed_features_vec_12.rows() == MeshCuboidFeatures::k_num_features);

	// NOTE:
	// Since the center point is always the origin in the local coordinates,
	// it is not used as the feature values.
	const int num_rows = 2 * MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index;
	conditional_pairwise_cuboid_feature.resize(num_rows);
	conditional_pairwise_cuboid_feature <<
		transformed_features_vec_11.bottomRows(MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index),
		transformed_features_vec_12;

	Eigen::VectorXd conditional_mean = mean_.segment(0, num_rows);
	Eigen::MatrixXd conditional_inv_cov = inv_cov_.block(0, 0, num_rows, num_rows);
	Eigen::VectorXd diff = conditional_pairwise_cuboid_feature - conditional_mean;

	// Mahalanobis norm.
	double error = diff.transpose() * conditional_inv_cov * diff;
	assert(error >= 0);

	return error;
}

MeshCuboidCondNormalRelations::MeshCuboidCondNormalRelations()
{
	assert(MeshCuboidFeatures::k_num_global_feature_values > 0);

	mean_A_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_global_feature_values);
	mean_b_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	inv_cov_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_features);
}

MeshCuboidCondNormalRelations::~MeshCuboidCondNormalRelations()
{

}

bool MeshCuboidCondNormalRelations::load_cond_normal_csv(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ", ", "", "", "", "", "");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	for (int i = 0; i < MeshCuboidFeatures::k_num_global_feature_values; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');

			// Transposed.
			mean_A_(j, i) = std::stof(token);
		}
	}

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');

		// Transposed.
		mean_b_(j) = std::stof(token);
	}

	for (int i = 0; i < MeshCuboidFeatures::k_num_features; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			inv_cov_(i, j) = std::stof(token);
		}
	}

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

bool MeshCuboidCondNormalRelations::save_cond_normal_csv(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	// NOTE:
	// mean_A_. mean_b_: Transposed.
	file << mean_A_.transpose().format(csv_format) << std::endl;
	file << mean_b_.transpose().format(csv_format) << std::endl;
	file << inv_cov_.format(csv_format) << std::endl;

	file.close();
	return true;
}

bool MeshCuboidCondNormalRelations::load_cond_normal_dat(const char* _filename)
{
	std::ifstream file(_filename, std::ios::in | std::ios::binary);
	if (!file.good())
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	int16_t rows, cols;

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == MeshCuboidFeatures::k_num_features);
	assert(static_cast<int>(cols) == MeshCuboidFeatures::k_num_global_feature_values);
	mean_A_.resize(rows, cols);
	file.read((char *)mean_A_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == MeshCuboidFeatures::k_num_features);
	assert(static_cast<int>(cols) == 1);
	mean_b_.resize(rows);
	file.read((char *)mean_b_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == MeshCuboidFeatures::k_num_features);
	assert(static_cast<int>(cols) == MeshCuboidFeatures::k_num_features);
	inv_cov_.resize(rows, cols);
	file.read((char *)inv_cov_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

void MeshCuboidCondNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_global_features_vec_1, Eigen::VectorXd &_transformed_features_vec_12)
{
	assert(_cuboid_1);
	assert(_cuboid_2);

	MeshCuboidFeatures features_1;
	features_1.compute_features(_cuboid_1);

	MeshCuboidFeatures features_2;
	features_2.compute_features(_cuboid_2);

	get_pairwise_cuboid_features(features_1, features_2,
		_transformation_1, _transformation_2,
		_global_features_vec_1, _transformed_features_vec_12);
}

void MeshCuboidCondNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboidFeatures &_features_1, const MeshCuboidFeatures &_features_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_global_features_vec_1, Eigen::VectorXd &_transformed_features_vec_12)
{
	assert(_transformation_1);
	assert(_transformation_2);

	Eigen::VectorXd features_vec_1 = _features_1.get_features();
	assert(features_vec_1.rows() == MeshCuboidFeatures::k_num_features);

	_transformed_features_vec_12 = _transformation_1->get_transformed_features(_features_2);
	assert(_transformed_features_vec_12.rows() == MeshCuboidFeatures::k_num_features);

	_global_features_vec_1 = features_vec_1.bottomRows(MeshCuboidFeatures::k_num_global_feature_values);
}

double MeshCuboidCondNormalRelations::compute_error(const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2)const
{
	Eigen::VectorXd global_features_vec_1, transformed_features_vec_12;
	get_pairwise_cuboid_features(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2,
		global_features_vec_1, transformed_features_vec_12);

	assert(mean_A_.rows() == transformed_features_vec_12.rows());
	assert(mean_A_.cols() == global_features_vec_1.rows());
	assert(mean_b_.rows() == transformed_features_vec_12.rows());
	assert(inv_cov_.rows() == transformed_features_vec_12.rows());
	assert(inv_cov_.cols() == transformed_features_vec_12.rows());

	const Eigen::VectorXd mean_2 = mean_A_ * global_features_vec_1 + mean_b_;
	Eigen::VectorXd diff = transformed_features_vec_12 - mean_2;

	// Mahalanobis norm.
	double error = diff.transpose() * inv_cov_ * diff;
	assert(error >= 0);

	//std::cerr << "Negative error value (error = " << error << ")" << std::endl;
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	//Real inv_cov_det = inv_cov_.determinant();
	//std::cout << "inv_cov_det = " << inv_cov_det << std::endl;

	//std::cout << "Norm(diff) = " << diff.transpose() * diff << std::endl;

	//std::cout << "diff = " << std::endl;
	//std::cout << diff.format(csv_format) << std::endl;

	//do {
	//	std::cout << '\n' << "Press the Enter key to continue.";
	//} while (std::cin.get() != '\n');

	return error;
}

/*
MeshCuboidPCARelations::MeshCuboidPCARelations()
{
	mean_ = Eigen::VectorXd::Zero(k_mat_size);
	pca_bases_ = Eigen::MatrixXd::Zero(k_mat_size, k_mat_size);
}

MeshCuboidPCARelations::~MeshCuboidPCARelations()
{

}

bool MeshCuboidPCARelations::load_pca_csv(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ", ", "", "", "", "", "");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < k_mat_size; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		mean_(j) = std::stof(token);
	}

	for (int i = 0; i < k_mat_size; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < k_mat_size; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			pca_bases_(i, j) = std::stof(token);
		}
	}

	file.close();

	return true;
}

bool MeshCuboidPCARelations::save_pca_csv(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	file << mean_.transpose().format(csv_format) << std::endl;
	file << pca_bases_.format(csv_format) << std::endl;

	file.close();
	return true;
}

double MeshCuboidPCARelations::compute_error(
	const Eigen::VectorXd &_features_vec_1, const Eigen::VectorXd &_features_vec_2) const
{
	assert(_features_vec_1.rows() == MeshCuboidFeatures::k_num_features);
	assert(_features_vec_2.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd concat_features(2 * MeshCuboidFeatures::k_num_features);
	concat_features << _features_vec_1, _features_vec_2;
	assert(mean_.rows() == concat_features.rows());
	assert(pca_bases_.rows() == concat_features.rows());
	assert(pca_bases_.cols() == concat_features.rows());

	Eigen::VectorXd diff = concat_features - mean_;
	diff = diff - (pca_bases_ * diff);

	double error = diff.transpose() * diff;
	return error;
}

MeshCuboidCCARelations::MeshCuboidCCARelations()
{
	mean_1_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	mean_2_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	correlations_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	bases_12_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_features);
	bases_21_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_features);
}

MeshCuboidCCARelations::~MeshCuboidCCARelations()
{

}

bool MeshCuboidCCARelations::load_cca_bases(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		mean_1_(j) = std::stof(token);
	}

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		mean_2_(j) = std::stof(token);
	}

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		correlations_(j) = std::stof(token);
	}

	for (int i = 0; i < MeshCuboidFeatures::k_num_features; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			bases_12_(i, j) = std::stof(token);
		}
	}

	for (int i = 0; i < MeshCuboidFeatures::k_num_features; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			bases_21_(i, j) = std::stof(token);
		}
	}

	file.close();
	return true;
}

bool MeshCuboidCCARelations::save_cca_bases(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	file << mean_1_.transpose().format(csv_format) << std::endl;
	file << mean_2_.transpose().format(csv_format) << std::endl;
	file << correlations_.transpose().format(csv_format) << std::endl;
	file << bases_12_.format(csv_format) << std::endl;
	file << bases_21_.format(csv_format) << std::endl;

	file.close();
	return true;
}

double MeshCuboidCCARelations::compute_error(
	const Eigen::VectorXd &_features_vec_1, const Eigen::VectorXd &_features_vec_2) const
{
	assert(bases_12_.cols() == _features_vec_1.rows());
	assert(bases_21_.cols() == _features_vec_2.rows());

	Eigen::VectorXd transformed_attributes_1 = bases_12_ * (_features_vec_1 - mean_1_);
	Eigen::VectorXd transformed_attributes_2 = bases_21_ * (_features_vec_2 - mean_2_);
	Eigen::VectorXd diff = transformed_attributes_1 - transformed_attributes_2;

	// Mahalanobis norm.
	double error = diff.transpose() * correlations_.asDiagonal() * diff;
	return error;
}
*/


//
// Created by kobus on 2017/04/06.
//

#include <opencv2/imgproc.hpp>
#include <opencv2/photo.hpp>
#include <list>
#include <deque>
#include <iomanip>
#include <cv.hpp>
#include "cpuDsp.h"
#include <armadillo>
//#include <Eigen/Dense>
//#include <Eigen/Sparse>
//#include <Eigen/IterativeLinearSolvers>
//#include <Eigen/SparseLU>

void printMat(cv::Mat image)
{
    std::cout << "Mat:" << std::endl;
    for (int i = 0; i < image.cols; i++)
    {
        std::cout << "  " << std::setfill('0') << std::setw(5) << i << "  ";
    }
    std::cout << std::endl;

    for (int i = 0; i < image.rows; i++)
    {
        std::cout << "  " << std::setfill('0') << std::setw(5) << i << "  ";
        for (int j = 0; j < image.cols; j++)
        {
            std::cout << (image.at<int>(i, j) & 0xFF) << " ";
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
}

void printMatFloat(cv::Mat image)
{
    std::cout << "Mat:" << std::endl;
    for (int i = 0; i < image.cols; i++)
    {
        std::cout << "  " << std::setfill('0') << std::setw(5) << i << "  ";
    }
    std::cout << std::endl;

    for (int i = 0; i < image.rows; i++)
    {
        std::cout << "  " << std::setfill('0') << std::setw(5) << i << "  ";
        for (int j = 0; j < image.cols; j++)
        {
            std::cout << image.at<double>(i, j) << " ";
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
}

void printArray(double array[1280][720])
{
    std::cout << "Mat:" << std::endl;
    for (int i = 0; i < 1280; i++)
    {
        std::cout << "  " << std::setfill('0') << std::setw(5) << i << "  ";
    }
    std::cout << std::endl;

    for (int i = 0; i < 720; i++)
    {
        std::cout << "  " << std::setfill('0') << std::setw(5) << i << "  ";
        for (int j = 0; j < 1280; j++)
        {
            std::cout << ((int)(array[i][j])) << " ";
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
}

void cpuDsp::executeTiPipeline(cv::Mat& frame,
                               cv::Mat& originalFrame,
                               cv::Mat& nnFrame,
                               cv::Mat& biqFrame,
                               cv::Mat& nediFrame,
                               cv::Mat& bilinearFrame,
                               TiDspParameters params)
{
    static bool showStabilisation = false;
    static int stabFrame = 0;
    cv::Mat temp1;
    cv::Mat temp2;

    cv::cvtColor(frame, frame, CV_BGR2GRAY);
    originalFrame = frame.clone();

    if (params.outputSelect == OutputSelect::Raw)
    {
        frame.copyTo(nnFrame);
        frame.copyTo(biqFrame);
        frame.copyTo(nediFrame);
        frame.copyTo(bilinearFrame);
        return;
    }

    //    if (params.eZoomOn)
//    {
//        eZoom(frame, params);
//        if (showComparisonWindows)
//        {
//            eZoom(originalFrame, params);
//        }
//    }
//    if (params.outputSelect == OutputSelect::Ezoom)
//        frame.copyTo(outputFrame);

    if (params.downScaleOn)
    {
        if (params.ezoom == Ezoom::x2)
        {
            cv::pyrDown(frame, frame, cv::Size(frame.cols / 2, frame.rows / 2));
        }
        else if (params.ezoom == Ezoom::x4)
        {
            cv::pyrDown(frame, frame, cv::Size(frame.cols / 2, frame.rows / 2));
            cv::pyrDown(frame, frame, cv::Size(frame.cols / 2, frame.rows / 2));
        }
        else if (params.ezoom == Ezoom::x8)
        {
            cv::pyrDown(frame, frame, cv::Size(frame.cols / 2, frame.rows / 2));
            cv::pyrDown(frame, frame, cv::Size(frame.cols / 2, frame.rows / 2));
            cv::pyrDown(frame, frame, cv::Size(frame.cols / 2, frame.rows / 2));
        }
    }
    if (params.outputSelect == OutputSelect::Downscale)
    {
        frame.copyTo(nnFrame);
        frame.copyTo(biqFrame);
        frame.copyTo(nediFrame);
        frame.copyTo(bilinearFrame);
        return;
    }

    if (params.upscaleOn)
    {
        if (params.ezoom == Ezoom::NoZoom)
        {
            frame.copyTo(nnFrame);
            frame.copyTo(biqFrame);
            frame.copyTo(nediFrame);
            frame.copyTo(bilinearFrame);
        }
        else if (params.ezoom == Ezoom::x2)
        {
            cv::resize(frame, nnFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_NEAREST);

            cv::resize(frame, biqFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_CUBIC);

            cv::resize(frame, bilinearFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_LINEAR);

            nediUpscale(frame, nediFrame);
        }
        else if (params.ezoom == Ezoom::x4)
        {
            cv::resize(frame, temp1, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_NEAREST);
            cv::resize(temp1, nnFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_NEAREST);

            cv::resize(frame, temp1, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_CUBIC);
            cv::resize(temp1, biqFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_CUBIC);

            cv::resize(frame, temp1, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_LINEAR);
            cv::resize(temp1, bilinearFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_LINEAR);

            nediUpscale(frame, temp1);
            nediUpscale(temp1, nediFrame);
        }
        else if (params.ezoom == Ezoom::x8)
        {
            cv::resize(frame, temp1, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_NEAREST);
            cv::resize(temp1, temp2, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_NEAREST);
            cv::resize(temp2, nnFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_NEAREST);

            cv::resize(frame, temp1, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_CUBIC);
            cv::resize(temp1, temp2, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_CUBIC);
            cv::resize(temp2, biqFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_CUBIC);

            cv::resize(frame, temp1, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_LINEAR);
            cv::resize(temp1, temp2, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_LINEAR);
            cv::resize(temp2, bilinearFrame, cv::Size(originalFrame.cols, originalFrame.rows), 0, 0, cv::INTER_LINEAR);

            nediUpscale(frame, temp1);
            nediUpscale(temp1, temp2);
            nediUpscale(temp2, nediFrame);
        }
    }
    else
    {
        frame.copyTo(nnFrame);
        frame.copyTo(biqFrame);
        frame.copyTo(nediFrame);
        frame.copyTo(bilinearFrame);
    }

    if (params.outputSelect == OutputSelect::Upscale)
        return;

    if (params.edgeEnhancementOn)
    {
        edgeEnhance(originalFrame, params);
        edgeEnhance(nnFrame, params);
        edgeEnhance(biqFrame, params);
        edgeEnhance(nediFrame, params);
        edgeEnhance(bilinearFrame, params);
    }
    if (params.outputSelect == OutputSelect::EdgeEnhance)
        return;

    return;
}

void cpuDsp::edgeEnhance(cv::Mat& frame, TiDspParameters params)
{
    switch (params.edgeEnhanceType)
    {
        case EdgeEnhanceType::Sharpen:
        {
            cv::Mat kernel = (cv::Mat_<float>(3,3) <<
                     0,-1, 0,
                    -1, 5,-1,
                     0,-1, 0);

            filter2D(frame, frame, CV_32F, kernel);
        }

        case EdgeEnhanceType::Gaussian:
        {
            cv::Mat kernel = (cv::Mat_<float>(5,5) <<
                    -1,-1,-1,-1,-1,
                    -1, 2, 2, 2,-1,
                    -1, 2, 8, 2,-1,
                    -1, 2, 2, 2,-1,
                    -1,-1,-1,-1,-1) / 8.0;

            filter2D(frame, frame, CV_32F, kernel);
            break;
        }

        case EdgeEnhanceType::Unsharp_mask:
        {
            cv::Mat kernel = (cv::Mat_<float>(5,5) <<
                    1, 4,   6, 4, 1,
                    4,16,  24,16, 4,
                    6,24,-476,24, 6,
                    4,16,  24,16, 4,
                    1, 4,   6, 4, 1) / -256.0;

            filter2D(frame, frame, CV_32F, kernel);
            break;
        }

        default:
        {
            break;
        }
    }
    frame.convertTo(frame, CV_8U);
    convertScaleAbs(frame, frame);
}

void cpuDsp::eZoom(cv::Mat &frame, TiDspParameters params)
{
    cv::Mat dest;
    switch (params.ezoom)
    {
        case Ezoom::NoZoom:
            break;
        case Ezoom::x2:
            cv::pyrUp(frame, dest, cv::Size(frame.cols * 2, frame.rows * 2));
            moveFrame(frame, dest, params);
            break;
        case Ezoom::x4:
            cv::pyrUp(frame, dest, cv::Size(frame.cols * 2, frame.rows * 2));
            cv::pyrUp(dest, dest, cv::Size(dest.cols * 2, dest.rows * 2));
            moveFrame(frame, dest, params);
            break;
        case Ezoom::x8:
            cv::pyrUp(frame, dest, cv::Size(frame.cols * 2, frame.rows * 2));
            cv::pyrUp(dest, dest, cv::Size(dest.cols * 2, dest.rows * 2));
            cv::pyrUp(dest, dest, cv::Size(dest.cols * 2, dest.rows * 2));
            moveFrame(frame, dest, params);
            break;
    }

}

void cpuDsp::moveFrame(const cv::Mat &frame, const cv::Mat &dest, TiDspParameters params)
{
    double availableXPixels, availableYPixels;
    double xMove, yMove;

    availableXPixels = dest.cols - frame.cols;
    availableYPixels = dest.rows - frame.rows;
    xMove = params.horisontalOffset / 100.0;
    yMove = params.verticalOffset / 100.0;
    dest(cv::Rect(xMove * availableXPixels, yMove * availableYPixels, frame.cols, frame.rows)).copyTo(frame);
}

void cpuDsp::sharpen(cv::Mat& frame)
{
    cv::Mat blurred; double sigma = 1, threshold = 5, amount = 1;
    GaussianBlur(frame, blurred, cv::Size(), sigma, sigma);
    cv::Mat lowContrastMask = cv::abs(frame - blurred) < threshold;
    cv::Mat sharpened = frame*(1+amount) + blurred*(-amount);
    frame.copyTo(frame, lowContrastMask);
}

void cpuDsp::nediUpscale(const cv::Mat &frame, cv::Mat &dest) {
    const int upscaleFactor = 2;
    dest = cv::Mat::zeros(cv::Size(frame.cols * upscaleFactor, frame.rows * upscaleFactor), CV_8UC1);

    for (int y = 0; y < frame.rows; y++)
    {
        for (int x = 0; x < frame.cols; x++)
        {
            dest.ptr<unsigned char>(y * upscaleFactor)[x * upscaleFactor] = frame.ptr<unsigned char>(y)[x];
        }
    }

    arma::mat Y = arma::zeros(25, 1);
    arma::mat C = arma::zeros(25, 4);
    arma::mat CtC;
    arma::mat Cty;
    arma::mat Cinv;
    arma::mat a;
    int temp;
    //int m = 5; //todo review size of filter

    //Solve the "b" pixels
    //todo edge conditions not handled yet
    for (int i = 4; i < (dest.rows - 4) / 2; i++)
    {
        for (int j = 4; j < (dest.cols - 4) / 2; j++)
        {
            temp = 0;
            for (int ii = i - 2; ii <= i + 2; ii++)
            {
                for (int jj = j - 2; jj <= j + 2; jj++)
                {
                    Y(temp, 0) = (dest.ptr<unsigned char>(2*ii)[2*jj]);

                    C(temp, 0) = (dest.ptr<unsigned char>(2*ii - 2)[2*jj - 2]);
                    C(temp, 1) = (dest.ptr<unsigned char>(2*ii + 2)[2*jj - 2]);
                    C(temp, 2) = (dest.ptr<unsigned char>(2*ii + 2)[2*jj + 2]);
                    C(temp, 3) = (dest.ptr<unsigned char>(2*ii - 2)[2*jj + 2]);
                    temp++;
                }
            }

            Cty = C.t() * Y;
            CtC = C.t() * C;
            if (arma::solve(a, Cty, CtC))
            {
                (dest.ptr<unsigned char>(2*i + 1)[2*j + 1]) = (unsigned char) (a(0, 0) / 4 * (dest.ptr<unsigned char>(2*i)[2*j]) + \
                a(0, 1) / 4 * (dest.ptr<unsigned char>(2*i + 2)[2*j]) + \
                a(0, 2) / 4 * (dest.ptr<unsigned char>(2*i + 2)[2*j + 2]) + \
                a(0, 3) / 4  * (dest.ptr<unsigned char>(2*i)[2*j + 2]));
            }
            else
            {
                (dest.ptr<unsigned char>(2*i + 1)[2*j + 1]) = 0;//(unsigned char) (dest.ptr<unsigned char>(y - 1)[x - 1]);
            }
        }
    }

    for (int i = 4; i < (dest.rows - 4) / 2; i++)
    {
        for (int j = 4; j < (dest.cols - 4) / 2; j++)
        {
            temp = 0;
            for (int ii = i - 2; ii <= i + 2; ii++)
            {
                for (int jj = j - 2; jj <= j + 2; jj++)
                {
                    Y(temp, 0) = (dest.ptr<unsigned char>(2*ii + 1)[2*jj - 1]);

                    C(temp, 0) = (dest.ptr<unsigned char>(2*ii - 1)[2*jj - 1]);
                    C(temp, 1) = (dest.ptr<unsigned char>(2*ii + 1)[2*jj - 3]);
                    C(temp, 2) = (dest.ptr<unsigned char>(2*ii + 3)[2*jj - 1]);
                    C(temp, 3) = (dest.ptr<unsigned char>(2*ii + 1)[2*jj + 1]);
                    temp++;
                }
            }

            Cty = C.t() * Y;
            CtC = C.t() * C;
            if (arma::solve(a, Cty, CtC))
            {
                (dest.ptr<unsigned char>(2*i + 1)[2*j]) = (unsigned char) (a(0, 0) / 4 * (dest.ptr<unsigned char>(2*i)[2*j]) + \
                a(0, 1) / 4 * (dest.ptr<unsigned char>(2*i + 1)[2*j - 1]) + \
                a(0, 2) / 4 * (dest.ptr<unsigned char>(2*i + 2)[2*j]) + \
                a(0, 3) / 4  * (dest.ptr<unsigned char>(2*i + 1)[2*j + 1]));
            }
            else
            {
                (dest.ptr<unsigned char>(2*i + 1)[2*j]) = 0;//(unsigned char) (dest.ptr<unsigned char>(y - 1)[x - 1]);
            }
        }
    }

    for (int i = 4; i < (dest.rows - 4) / 2; i++)
    {
        for (int j = 4; j < (dest.cols - 4) / 2; j++)
        {
            temp = 0;
            for (int ii = i - 2; ii <= i + 2; ii++)
            {
                for (int jj = j - 2; jj <= j + 2; jj++)
                {
                    Y(temp, 0) = (dest.ptr<unsigned char>(2*ii + 1)[2*jj - 1]);

                    C(temp, 0) = (dest.ptr<unsigned char>(2*ii - 1)[2*jj - 1]);
                    C(temp, 1) = (dest.ptr<unsigned char>(2*ii + 1)[2*jj - 3]);
                    C(temp, 2) = (dest.ptr<unsigned char>(2*ii + 3)[2*jj - 1]);
                    C(temp, 3) = (dest.ptr<unsigned char>(2*ii + 1)[2*jj + 1]);
                    temp++;
                }
            }

            Cty = C.t() * Y;
            CtC = C.t() * C;
            if (arma::solve(a, Cty, CtC))
            {
                (dest.ptr<unsigned char>(2*i)[2*j + 1]) = (unsigned char) (a(0, 0) / 4 * (dest.ptr<unsigned char>(2*i - 1)[2*j + 1]) + \
                a(0, 1) / 4 * (dest.ptr<unsigned char>(2*i)[2*j]) + \
                a(0, 2) / 4 * (dest.ptr<unsigned char>(2*i + 1)[2*j + 1]) + \
                a(0, 3) / 4  * (dest.ptr<unsigned char>(2*i)[2*j + 2]));
            }
            else
            {
                (dest.ptr<unsigned char>(2*i)[2*j + 1]) = 0;//(unsigned char) (dest.ptr<unsigned char>(y - 1)[x - 1]);
            }
        }
    }
}


/* Copyright 2018 Ignacio Torroba (ignaciotb@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef EKF_LOCALIZATION_HPP
#define EKF_LOCALIZATION_HPP

#include <ros/timer.h>
#include <ros/ros.h>

#include "gazebo_msgs/GetWorldProperties.h"
#include "gazebo_msgs/GetModelState.h"

#include "utils_matrices/utils_matrices.hpp"
#include "correspondence_class/correspondence_class.hpp"
#include "noise_oneD_kf/noise_oneD_kf.hpp"

#include <queue>
#include <math.h>

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <ros/transport_hints.h>
#include <boost/scoped_ptr.hpp>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/UInt32.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseArray.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu,
        geometry_msgs::TwistWithCovarianceStamped> MsgTimingPolicy;

/**
 * @brief The EKFLocalization class
 * EKF-based localization node for LoLo
 * Inputs:
 * IMU, DVL and landmarks positions from measurements
 * Map as a collection of landmarks with respect to world frame
 * Outputs:
 * nav_msgs/Odometry with an estimate of the 6DOF pose of LoLo
 * updated tf transform odom --> base_link
 */

class EKFLocalization{

public:

    EKFLocalization(std::string node_name, ros::NodeHandle &nh);
    void ekfLocalize(const ros::TimerEvent& e);
    ~EKFLocalization();
    void init(std::vector<double> sigma_diag, std::vector<double> r_diag, std::vector<double> q_diag, double delta);

private:

    // ROS variables
    ros::NodeHandle *nh_;
    std::string node_name_;
    message_filters::Synchronizer<MsgTimingPolicy>* msg_synch_ptr_;
    message_filters::Subscriber<sensor_msgs::Imu>* imu_subs_;
    message_filters::Subscriber<geometry_msgs::TwistWithCovarianceStamped>* dvl_subs_;
    ros::Timer timer_;

    // Comms
    ros::Subscriber fast_imu_sub_;
    ros::Subscriber fast_dvl_sub_;
    ros::Subscriber tf_gt_subs_;
    ros::Subscriber observs_subs_;
    ros::Subscriber rpt_subs_;
    ros::Publisher odom_pub_;
    ros::Publisher odom_inertial_pub_;
    ros::Publisher vis_pub_;
    visualization_msgs::MarkerArray markers_;
    ros::ServiceClient gazebo_client_;
    ros::ServiceClient landmarks_client_;

    // Handlers for sensors
    std::deque<sensor_msgs::ImuPtr> imu_readings_; // TODO: add limit size to queues
    std::deque<geometry_msgs::TwistWithCovarianceStampedPtr> dvl_readings_;
    std::deque<nav_msgs::OdometryPtr> gt_readings_;
    std::deque<geometry_msgs::PoseArrayPtr> measurements_t_;
    boost::mutex msg_lock_;
    std::vector<boost::numeric::ublas::vector<double>> map_odom_;
    bool init_filter_;

    // System state variables
    boost::numeric::ublas::vector<double> mu_;
    boost::numeric::ublas::vector<double> mu_hat_;
    boost::numeric::ublas::vector<double> mu_pred_;
    boost::numeric::ublas::matrix<double> Sigma_;
    boost::numeric::ublas::matrix<double> Sigma_hat_;
    boost::numeric::ublas::matrix<double> G_t_;
    double delta_m_;
    double lambda_M_;

    // Noise models
    boost::numeric::ublas::matrix<double> R_;
    boost::numeric::ublas::matrix<double> Q_;

    // Aux
    double t_prev_;
    bool coord_;
    unsigned int size_imu_q_;
    unsigned int size_dvl_q_;

    OneDKF* dvl_x_kf;
    OneDKF* dvl_y_kf;
    OneDKF* dvl_z_kf;

    // tf
    tf::TransformBroadcaster odom_bc_;
    tf::StampedTransform transf_dvl_base_;
    tf::StampedTransform transf_world_odom_;
    tf::Transform transf_odom_world_;
    tf::StampedTransform transf_base_sssr_;
    std::string odom_frame_;
    std::string world_frame_;
    std::string base_frame_;
    std::string dvl_frame_;
    std::string sssr_frame_;
    std::string map_srv_name_;
    std::string lm_srv_name_;

    // Input callbacks
    void gtCB(const nav_msgs::OdometryPtr &pose_msg);
    void synchSensorsCB(const sensor_msgs::ImuConstPtr &imu_msg,
                        const geometry_msgs::TwistWithCovarianceStampedConstPtr &dvl_msg);
    void fastIMUCB(const sensor_msgs::ImuPtr &imu_msg);
    void fastDVLCB(const geometry_msgs::TwistWithCovarianceStampedPtr &dvl_msg);
    void observationsCB(const geometry_msgs::PoseArrayPtr &observ_msg);

    /**
     * @brief EKFLocalization::computeOdom
     * @param dvl_msg
     * @param gt_pose
     * @param q_auv
     * @param u_t
     * Integrates IMU and DVL to predict an estimate of the pose
     */
    void computeOdom(const geometry_msgs::TwistWithCovarianceStampedPtr &dvl_msg, const tf::Quaternion &q_auv,
                     boost::numeric::ublas::vector<double> &u_t);

    /**
     * @brief EKFLocalization::predictMotion
     * @param u_t
     * Prediction step for the EKF
     */
    void predictMotion(boost::numeric::ublas::vector<double> &u_t);

    /**
     * @brief predictMeasurement
     * @param landmark_j
     * @param z_i
     * @param ml_i_list
     * Measurement prediction for a given pair measurement-landmark at time t
     */
    void predictMeasurement(const boost::numeric::ublas::vector<double> &landmark_j,
                                 boost::numeric::ublas::vector<double> &z_i,
                                 std::vector<CorrespondenceClass *> &ml_i_list);

    /**
     * @brief dataAssociation
     * Maximum likelihood data association with outlier rejection
     */
    void dataAssociation();

    /**
     * @brief sequentialUpdate
     * @param c_i_j
     * Sequential update for a given match observation-landmark
     */
    void sequentialUpdate(CorrespondenceClass *c_i_j);

    /**
     * @brief createMapMarkers
     * Publishes the map as an array of markers for visualization in RVIZ
     */
    void createMapMarkers(std::vector<boost::numeric::ublas::vector<double> > map_world);

    /**
     * @brief EKFLocalization::sendOutput
     * @param t
     * @return
     * Publishes AUV odometry info and tf odom --> base_link
     */
    bool sendOutput(ros::Time t);

    /**
     * @brief EKFLocalization::interpolateDVL
     * @param t_now
     * @param dvl_msg_ptr
     * Interpolates DVL (slower) inputs through Bezier curves to synch to faster sensors
     */
    void interpolateDVL(ros::Time t_now, geometry_msgs::TwistWithCovarianceStampedPtr &dvl_msg_ptr);

};

#endif // EKF_LOCALIZATION_HPP


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
////////////////////////////////////////////////////////////////////////
// This file is generated by a script.  Do not edit directly.  Edit the
// wrapQuat.template.cpp file to make changes.

#include "pxr/base/gf/quath.h"

#include "pxr/base/tf/pyUtils.h"
#include "pxr/base/tf/wrapTypeHelpers.h"
#include "pxr/base/tf/pyContainerConversions.h"

#include <boost/python/class.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/def.hpp>
#include <boost/python/implicit.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/python/operators.hpp>
#include <boost/python/overloads.hpp>
#include <boost/python/return_arg.hpp>

#include <string>

using namespace boost::python;

using std::string;

static string __repr__(GfQuath const &self) {
    return TF_PY_REPR_PREFIX + "Quath(" +
        TfPyRepr(self.GetReal()) + ", " +
        TfPyRepr(self.GetImaginary()) + ")";
}

// Zero-initialized default ctor for python.
static GfQuath *__init__() { return new GfQuath(0); }

void wrapQuath()
{
    object getImaginary =
        make_function(&GfQuath::GetImaginary,
                      return_value_policy<return_by_value>());

    object setImaginaryVec =
        make_function((void (GfQuath::*)(const GfVec3h &))
                      &GfQuath::SetImaginary);

    object setImaginaryScl =
        make_function((void (GfQuath::*)(half, half, half))
                      &GfQuath::SetImaginary,
                      default_call_policies(),
                      (arg("i"), arg("j"), arg("k")));

    def("Slerp",
        (GfQuath (*)(double, const GfQuath&, const GfQuath&))
        GfSlerp);

    class_<GfQuath>("Quath", no_init)
        .def("__init__", make_constructor(__init__))

        .def(TfTypePythonClass())

        .def(init<half>(arg("real")))
        .def(init<half, const GfVec3h &>(
                 (arg("real"), arg("imaginary"))))
        .def(init<half, half, half, half>(
                 (arg("real"), arg("i"), arg("j"), arg("k"))))

        .def("GetIdentity", &GfQuath::GetIdentity)
        .staticmethod("GetIdentity")

        .def("GetReal", &GfQuath::GetReal)
        .def("SetReal", &GfQuath::SetReal)
        .add_property("real", &GfQuath::GetReal, &GfQuath::SetReal)

        .def("GetImaginary", getImaginary)
        .def("SetImaginary", setImaginaryVec)
        .def("SetImaginary", setImaginaryScl)
        .add_property("imaginary", getImaginary, setImaginaryVec)

        .def("GetLength", &GfQuath::GetLength)

        .def("GetNormalized", &GfQuath::GetNormalized,
             (arg("eps")=GF_MIN_VECTOR_LENGTH))
        .def("Normalize", &GfQuath::Normalize,
             (arg("eps")=GF_MIN_VECTOR_LENGTH), return_self<>())

        .def("GetConjugate", &GfQuath::GetConjugate)
        .def("GetInverse", &GfQuath::GetInverse)

        .def(str(self))
        .def(-self)
        .def(self == self)
        .def(self != self)
        .def(self *= self)
        .def(self *= half())
        .def(self /= half())
        .def(self += self)
        .def(self -= self)
        .def(self + self)
        .def(self - self)
        .def(self * self)
        .def(self * half())
        .def(half() * self)
        .def(self / half())

        .def("__repr__", __repr__)

        ;

    to_python_converter<std::vector<GfQuath>,
        TfPySequenceToPython<std::vector<GfQuath> > >();

}


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/scope.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/enum.hpp>

#include <cctbx/sgtbx/space_group.h>
#include <rstbx/dps_core/dps_core.h>
#include <rstbx/dps_core/direction.h>
#include <rstbx/diffraction/ewald_sphere.h>
#include <rstbx/diffraction/partial_spot_position_partial_H.h>

#include <scitbx/array_family/flex_types.h>
#include <vector>
#include <rstbx/backplane.h>

using namespace boost::python;

namespace rstbx {

/* SimpleSamplerTool samples the unit sphere; outputting a list of surface
   points that are spaced apart a specified number of radians.
*/

class SimpleSamplerTool {
 typedef scitbx::af::shared<Direction > flex_Direction;

 public:
  double incr;  //initial directional spacing in radians
  flex_Direction angles;

  SimpleSamplerTool(const double& characteristic_grid):
    // The maximum allowable characteristic grid should be about 0.029 radians,
    // corresponding to the grid sampling used in the Rossman DPS paper;
    // approx 7900 directions; 0.03 seconds in C++ code.
    // But the characteristic grid sampling should be reflective of the problem at hand =
    // approximately: the observed resolution limit /
    //                most conservative (largest) cell estimate

    incr(characteristic_grid) {
    //construct_hemisphere_grid(incr);
  }

  flex_Direction construct_hemisphere_grid(const double& sampling){
    // psi is the equivalent of latitude, measured as an angle from the North pole
    // rounding:
    int psi_index_range = int (0.5 + scitbx::constants::pi_2/sampling);
    // adjust for integral number
    double adjusted_psi_incr = scitbx::constants::pi_2/psi_index_range;
    angles = flex_Direction();
    angles.reserve(4*psi_index_range*psi_index_range);

    for (int x = 0; x <= psi_index_range; ++x){
      double psi = x * adjusted_psi_incr;
      if (psi > scitbx::constants::pi){
        double eps = 1E-4; psi=scitbx::constants::pi-eps;
      }

      // phi is the equivalent of longitude
      if (psi==0){
        double phi=0.;
        angles.push_back(Direction(psi,phi));
      } else {
        int phi_index_range = int (0.5 + 2.*scitbx::constants::pi*std::sin(psi)/sampling);
        double adjusted_phi_incr = 2.*scitbx::constants::pi/phi_index_range;
        for (int y =0; y < phi_index_range; ++y) {
          double phi = y * adjusted_phi_incr;
          angles.push_back(Direction(psi,phi));
        }
      }
    }
    return angles;
  }
};

static boost::python::tuple
observed_indices_and_angles_from_rotation_angles_range(rotation_angles& ra,
  double const& phi_start_rad,double const& phi_end_rad,
  const scitbx::af::shared<cctbx::miller::index<> >& indices){
    // This is going to require some revision to assure it works in an arbitrary
    // principle value region for phi_start_rad and phi_end_rad

    scitbx::af::shared<scitbx::vec3<double> > return_indices;
    scitbx::af::shared<double> return_angles_rad;

    for (int ihkl = 0; ihkl < indices.size(); ++ihkl) {
       scitbx::vec3<double> test_index( // convert integer Miller index to double type
         indices[ihkl][0],indices[ihkl][1],indices[ihkl][2]);
       if (ra( test_index )) {
         scitbx::vec2<Angle> intersection_angles = ra.get_intersection_angles();
         for (int iangle = 0; iangle < 2; ++iangle){
           if (intersection_angles[iangle] >= phi_start_rad &&
               intersection_angles[iangle] <= phi_end_rad){
                 return_indices.push_back(test_index);
                 return_angles_rad.push_back(intersection_angles[iangle]);
           }
         }
       }
     }
     return make_tuple(return_indices,return_angles_rad);
}

static boost::python::tuple
rp_predict(reflection_prediction& rp,
  scitbx::af::shared<scitbx::vec3<double> > const& observed_indices,
  scitbx::af::shared<double> const& observed_angles){

    scitbx::af::shared<scitbx::vec3<double> > return_indices;
    scitbx::af::shared<double> return_fast_px;
    scitbx::af::shared<double> return_slow_px;
    scitbx::af::shared<double> return_angle_rad;
    scitbx::af::shared<double> return_angle_full_width_rad;
    scitbx::af::shared<double> return_lorentz_factor;
    scitbx::af::shared<scitbx::vec3<double> > return_s;

    if (rp.use_gh1982a){
      for (int ihkl = 0; ihkl < observed_indices.size(); ++ihkl) {
        if (rp( observed_indices[ihkl], observed_angles[ihkl] )) {
           scitbx::vec2<double> xy = rp.get_prediction();
           scitbx::vec3<double> s = rp.get_s();
           return_indices.push_back(observed_indices[ihkl]);
           return_angle_rad.push_back(observed_angles[ihkl]);
           return_fast_px.push_back(xy[0]);
           return_slow_px.push_back(xy[1]);
           return_lorentz_factor.push_back(rp.lorentz_factor());
           return_angle_full_width_rad.push_back(rp.get_full_width());
           return_s.push_back(s);
         }
       }
       return make_tuple(return_indices,return_fast_px,return_slow_px,return_angle_rad,
                         return_lorentz_factor, return_angle_full_width_rad, return_s);
    }
    for (int ihkl = 0; ihkl < observed_indices.size(); ++ihkl) {
       if (rp( observed_indices[ihkl], observed_angles[ihkl] )) {
         scitbx::vec2<double> xy = rp.get_prediction();
         scitbx::vec3<double> s = rp.get_s();

                 return_indices.push_back(observed_indices[ihkl]);
                 return_angle_rad.push_back(observed_angles[ihkl]);
                 return_fast_px.push_back(xy[0]);
                 return_slow_px.push_back(xy[1]);
           return_s.push_back(s);
       }
     }
      return make_tuple(return_indices,return_fast_px,return_slow_px,return_angle_rad, return_s);
}

static af::shared<cctbx::miller::index<> >
full_sphere_indices(cctbx::uctbx::unit_cell const& uc,
                    double const& resolution_limit,
                    cctbx::sgtbx::space_group const& sg){

  cctbx::miller::index<> maxhkl = uc.max_miller_indices(resolution_limit);

  af::shared<cctbx::miller::index<> > present;

  for (int h = -maxhkl[0]; h <= maxhkl[0]; ++h){
    for (int k = -maxhkl[1]; k <= maxhkl[1]; ++k){
      for (int l = -maxhkl[2]; l <= maxhkl[2]; ++l){

        if (h == 0 && k == 0 && l == 0) { continue; }

        if (uc.d(cctbx::miller::index<>(h, k, l)) < resolution_limit){
                    continue;}

        if (sg.is_sys_absent(cctbx::miller::index<>(h, k, l))){
                    continue;}

        present.push_back(cctbx::miller::index<>(h, k, l));
      }
    }
  }
  return present;
}

namespace boost_python { namespace {

  boost::python::tuple
  foo()
  {
    return boost::python::make_tuple(1,2,3,4);
  }

  Direction
  fft_result(dps_core& ai,Direction& angle){
      fftptr dfft( ai.fft_factory(angle) );
      angle.extract_directional_properties(dfft,true);
      return angle;
  }

  void
  init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;

    def("foo", &foo);

    class_<corrected_backplane>("corrected_backplane",init<const int&, const int&>())
      .def("accumulate",&corrected_backplane::accumulate)
      .def("finish",&corrected_backplane::finish)
      .def("localmean",&corrected_backplane::localmean)
      .def_readonly("rmsd",&corrected_backplane::boxstd)
    ;

    class_<dps_core>("dps_core",init<>())
      .def("setMaxcell",&dps_core::setMaxcell)
      .def("setXyzData",&dps_core::setXyzData)
      .def("getXyzSize",&dps_core::getXyzSize)
      .def("getXyzData",&dps_core::getXyzData)
      .def("fft_result",fft_result)
      .def("setSolutions",&dps_core::setSolutions)
      .def("set_presorted_solutions",&dps_core::set_presorted_solutions)
      .def("getSolutions",&dps_core::getSolutions)
      .def("n_candidates",&dps_core::n_candidates)
      .def("__getitem__",&dps_core::candidate)
      .def("setOrientation",&dps_core::setOrientation)
      .def("set_orientation_direct_matrix",
          &dps_core::set_orientation_direct_matrix)
      .def("set_orientation_reciprocal_matrix",
          &dps_core::set_orientation_reciprocal_matrix)
      .def("getOrientation",&dps_core::getOrientation)
      .def("rmsdev",&dps_core::rmsdev)
      .def("hklobserved",(hkllistmm(dps_core::*)()const) &dps_core::hklobserved)
      .def("hklobserved",
(hkllistmm (dps_core::*)(const pointlistmm&)const) &dps_core::hklobserved)
      .def("observed",&dps_core::observed)
      .def_readonly("granularity",&dps_core::granularity)
      .def_readonly("amax",&dps_core::amax)

    ;

    class_<Direction>("Direction", init<const point &>())
      .def(init<const double &, const double &>((arg("psi"),arg("phi"))))
      .def_readonly("kmax",&Direction::kmax)
      .add_property("kval",make_getter(&Direction::kval, rbv()),
                           make_setter(&Direction::kval, dcp()))
      .def_readonly("kval0",&Direction::kval0)
      .def_readonly("kval2",&Direction::kval2)
      .def_readonly("kval3",&Direction::kval3)
      .def_readonly("psi",&Direction::psi)
      .def_readonly("phi",&Direction::phi)
      .def_readonly("m",&Direction::m)
      .def_readonly("delta_p",&Direction::delta_p)
      .def("bvec",&Direction::bvec)
      .def("getff",&Direction::getff)
      .add_property("dvec",make_getter(&Direction::dvec, rbv()))
      .add_property("real",make_getter(&Direction::uc_length, rbv()))
      .def("is_nearly_collinear",&Direction::is_nearly_collinear)
   ;
    class_<Directional_FFT>("Directional_FFT", no_init)
      .def(init<const Direction&, const af::shared<scitbx::vec3<double> >&,
                const double&, const double&,
                const sztype&>((arg("angle"),arg("xyzdata"),
                                arg("granularity"),arg("amax"),
                                arg("F0_cutoff"))))
      .def_readonly("pmin",&Directional_FFT::pmin)
      .def_readonly("delta_p",&Directional_FFT::delta_p)
      .def_readonly("fft_result",&Directional_FFT::fft_result)
      .def("kval",&Directional_FFT::kval)
      .def("kmax",&Directional_FFT::kmax)
   ;

    class_<SimpleSamplerTool >("SimpleSamplerTool", init<const double &>())
      .add_property("incr",make_getter(&SimpleSamplerTool::incr, rbv()),
                           make_setter(&SimpleSamplerTool::incr, dcp()))
      .add_property("angles",make_getter(&SimpleSamplerTool::angles, rbv()),
                             make_setter(&SimpleSamplerTool::angles, dcp()))
      .def("construct_hemisphere_grid",&SimpleSamplerTool::construct_hemisphere_grid)
   ;

    class_<ewald_sphere_base_model>("ewald_sphere_base_model",
      init<const double&, const ewald_sphere_base_model::matrix&, const double&,
           const ewald_sphere_base_model::point&>(
           (arg("limiting_resolution"),arg("orientation"),
            arg("wavelength"),arg("axial_direction"))))
      .def("setH",(void(ewald_sphere_base_model::*)
           (const ewald_sphere_base_model::point&)) &ewald_sphere_base_model::setH)
      .def("setH",(void(ewald_sphere_base_model::*)
           (const cctbx::miller::index<>&)) &ewald_sphere_base_model::setH)
      .add_property("H",make_getter(&ewald_sphere_base_model::H, rbv()))
    ;

    class_<rotation_angles, bases<ewald_sphere_base_model> >("rotation_angles",
      init<const double&, const ewald_sphere_base_model::matrix&, const double&,
           const ewald_sphere_base_model::point&>(
           (arg("limiting_resolution"),arg("orientation"),
            arg("wavelength"),arg("axial_direction"))))
      .def(init<const ewald_sphere_base_model&>())
      .def("__call__", &rotation_angles::operator())
      .def("axis", &rotation_angles::axis)
      .def("offsetdot", &rotation_angles::offsetdot)
      .def("get_intersection_angles", &rotation_angles::get_intersection_angles)
      .def("observed_indices_and_angles_from_angle_range",
            &observed_indices_and_angles_from_rotation_angles_range,
           (arg("phi_start_rad"),arg("phi_end_rad"),arg("indices")))
    ;

    class_<reflection_prediction>("reflection_prediction",
                                  init<const scitbx::vec3<double> &,
                                  const scitbx::vec3<double> &,
                                  const scitbx::mat3<double> &,
                                  const reflection_prediction::sensor_type &>
      ((arg("axis"), arg("s0"), arg("ub"), arg("sensor"))))
      .def("__call__", & reflection_prediction::operator())
      .def("get_prediction", & reflection_prediction::get_prediction)
      .def("get_s", & reflection_prediction::get_s)
      .def("set_rocking_curve", & reflection_prediction::set_rocking_curve)
      .def("set_mosaicity", & reflection_prediction::set_mosaicity,
            (arg("mos"),arg("degrees")))
      .def("predict",
            &rp_predict,
           (arg("observed_indices"),arg("observed_angles")));

    def("full_sphere_indices",&full_sphere_indices,
      (arg("unit_cell"), arg("resolution_limit"),
       arg("space_group")));

    class_<partial_spot_position_partial_H, bases<rotation_angles> >(
      "partial_spot_position_partial_H",
      init<const double&, const ewald_sphere_base_model::matrix&, const double&,
           const ewald_sphere_base_model::point&>((arg("limiting_resolution"),
             arg("orientation"),
             arg("wavelength"),arg("axial_direction")
      )))
      .def("__call__", &partial_spot_position_partial_H::operator())
      .def("dangle_", &partial_spot_position_partial_H::dangle_)
    ;

    class_<scattering_list >("scattering_list",
      init<scitbx::af::shared<cctbx::miller::index<> >,
                           const cctbx::crystal_orientation&,
                           scitbx::vec3<double>,
                           scitbx::vec2<double>,
                           const double&,
                           const double&>())
      .def("mm_coord", &scattering_list::mm_coord)
      .def("reflections", &scattering_list::reflections)

      .def("vec3", &scattering_list::mm_coord) // when used as a spot positions container
      .def("hkl", &scattering_list::reflections) // when used as a spot positions container
    ;
}
}}} // namespace omptbx::boost_python::<anonymous>

BOOST_PYTHON_MODULE(rstbx_ext)
{
  rstbx::boost_python::init_module();

  // Expose SpotClass to Python
  enum_<rstbx::SpotClass>("SpotClass")
    .value("GOOD",rstbx::GOOD)
    .value("OVERLAP",rstbx::OVERLAP)
    .value("SPINDLE",rstbx::SPINDLE)
    .value("ICE",rstbx::ICE)
    .value("OTHERIMAGE",rstbx::OTHERIMAGE)
    .value("FULL_ENTER",rstbx::FULL_ENTER)
    .value("FULL_EXIT",rstbx::FULL_EXIT)
    .value("ENTER1",rstbx::ENTER1)
    .value("ENTER2",rstbx::ENTER2)
    .value("EXIT3",rstbx::EXIT3)
    .value("EXIT4",rstbx::EXIT4)
    .value("NONE",rstbx::NONE)
    .value("OUTLIER",rstbx::OUTLIER)
    .export_values()
    ;
}