#include "PatchData.h"
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <set>

PatchData::PatchData(TriMesh &mesh, std::vector<std::vector<TriMesh::FaceHandle>> &all_face_neighbor,
	std::vector<double> &face_area, std::vector<TriMesh::Point> &face_centroid,
	TriMesh::FaceIter &iter_face, shen::Geometry::EasyFlann &kd_tree, glm::dvec3& gt_normal, int num_ring, int radius)
{
	std::vector<std::vector<TriMesh::FaceHandle>> face_neighbor_ring(num_ring + 1);
	std::vector<std::vector<bool>> flag(num_ring + 1, std::vector<bool>(mesh.n_faces(), false));

	face_neighbor_ring[0].push_back(*iter_face);
	if (num_ring >= 1)
	{
		face_neighbor_ring[1] = all_face_neighbor[iter_face->idx()];
	}
	flag[0][iter_face->idx()] = true;

	if (num_ring >= 1)
	{
		for (int i = 0; i < (int)face_neighbor_ring[1].size(); i++)
		{
			flag[1][face_neighbor_ring[1][i].idx()] = true;
		}
	}

	for (int ring = 1; ring < num_ring; ring++)
	{
		for (int i = 0; i < (int)face_neighbor_ring[ring].size(); i++)
		{
			std::vector<TriMesh::FaceHandle> temp_neighbor = all_face_neighbor[face_neighbor_ring[ring][i].idx()];
			for (int t = 0; t < (int)temp_neighbor.size(); t++)
			{
				if ((!flag[ring - 1][temp_neighbor[t].idx()]) && (!flag[ring][temp_neighbor[t].idx()]) && (!flag[ring + 1][temp_neighbor[t].idx()]))
				{
					face_neighbor_ring[ring + 1].push_back(temp_neighbor[t]);
					flag[ring + 1][temp_neighbor[t].idx()] = true;
				}
			}
		}
	}

	// for center point position
	TriMesh::Point centroidc = face_centroid[iter_face->idx()];
	double areac = face_area[iter_face->idx()];
	glm::dvec4 center_face_centorid(areac, centroidc[0], centroidc[1], centroidc[2]);
	m_patch_faces_centers.push_back(center_face_centorid);

	glm::dvec3 normalc(mesh.normal(*iter_face)[0], mesh.normal(*iter_face)[1], mesh.normal(*iter_face)[2]);
	m_patch_faces_normals.push_back(normalc);
	for (TriMesh::FaceVertexIter fv_it = mesh.fv_iter(*iter_face); fv_it.is_valid(); fv_it++)
	{
		glm::dvec3 positionc(mesh.point(*fv_it)[0], mesh.point(*fv_it)[1], mesh.point(*fv_it)[2]);
		m_patch_points_positions.push_back(positionc);
	}

	int num_faces = 1;
	for (int ring = 1; ring <= num_ring; ring++)
	{
		for (int i = 0; i < (int)face_neighbor_ring[ring].size(); i++)
		{
			TriMesh::Point centroid = face_centroid[face_neighbor_ring[ring][i].idx()];

			glm::dvec4 neighbor_faces_centorid(areac, centroid[0], centroid[1], centroid[2]);

			// for kinect fussion calculate the mean area of 2-ring neighbors
			// glm::dvec4 neighbor_faces_centorid(face_area[face_neighbor_ring[ring][i].idx()], centroid[0], centroid[1], centroid[2]);

			m_patch_faces_centers.push_back(neighbor_faces_centorid);

			glm::dvec3 normal(mesh.normal(face_neighbor_ring[ring][i])[0], mesh.normal(face_neighbor_ring[ring][i])[1], mesh.normal(face_neighbor_ring[ring][i])[2]);
			m_patch_faces_normals.push_back(normal);
			for (TriMesh::FaceVertexIter fv_it = mesh.fv_iter(face_neighbor_ring[ring][i]); fv_it.is_valid(); fv_it++)
			{
				glm::dvec3 position(mesh.point(*fv_it)[0], mesh.point(*fv_it)[1], mesh.point(*fv_it)[2]);
				m_patch_points_positions.push_back(position);
			}
			num_faces++;
		}
	}
	m_patch_num_faces = num_faces;

	// no face-face neighbors
	if (m_patch_num_faces == 1)
	{
		return;
	}

	m_radius_region = sqrt(areac * double(radius));

	//// for kinect fussion calculate the mean area of 2-ring neighbors
	//double ring_2_mean_area = areac;
	//double ring_2_sum_area = 0;
	//for (int i_ring_area = 0; i_ring_area < m_patch_num_faces; i_ring_area++)
	//{
	//	ring_2_sum_area += m_patch_faces_centers[i_ring_area][0];
	//}
	//ring_2_mean_area = ring_2_sum_area / double(m_patch_num_faces);
	//m_radius_region = sqrt(ring_2_mean_area * double(radius));

	glm::vec3 query_point(m_patch_faces_centers[0][1], m_patch_faces_centers[0][2], m_patch_faces_centers[0][3]);
	std::vector<float> points_distances;
	std::vector<int> points_indices;
	kd_tree.radiusSearchNoLimit(query_point, points_distances, points_indices, m_radius_region * m_radius_region);

	int num_region_points = points_indices.size();
	std::vector<TriMesh::VertexIter> vertex_iters;

	for (int i = 0; i < num_region_points; i++)
	{
		TriMesh::VertexIter temp_iter = mesh.vertices_begin();
		for (int i_iter = 0; i_iter < points_indices[i]; i_iter++)
		{
			temp_iter++;
		}
		vertex_iters.push_back(temp_iter);
	}
	points_indices.clear();
	points_distances.clear();

	std::set<int> face_indices_set;		// faces in the fixed region
	std::vector<int> face_indices;
	std::vector<TriMesh::VertexFaceIter> patch_faces_iter;
	for (int i = 0; i < num_region_points; i++)
	{
		for (TriMesh::VertexFaceIter vf_it = mesh.vf_iter(vertex_iters[i]); vf_it.is_valid(); vf_it++)
		{
			if (face_indices_set.find(vf_it->idx()) == face_indices_set.end())
			{
				face_indices_set.insert(vf_it->idx());
				face_indices.push_back(vf_it->idx());
				patch_faces_iter.push_back(vf_it);
				if (vf_it->idx() == iter_face->idx())
				{
					m_center_index = face_indices.size() - 1;
				}
				glm::dvec3 normal(mesh.normal(*vf_it)[0], mesh.normal(*vf_it)[1], mesh.normal(*vf_it)[2]);
				m_aligned_patch_faces_normals.push_back(normal);
				for (TriMesh::FaceVertexIter fv_it = mesh.fv_iter(vf_it); fv_it.is_valid(); fv_it++)
				{
					glm::dvec3 position(mesh.point(*fv_it)[0], mesh.point(*fv_it)[1], mesh.point(*fv_it)[2]);
					m_aligned_patch_points_positions.push_back(position);
				}
				m_aligned_patch_num_faces++;
			}
		}
	}

	/*std::cout << m_aligned_patch_num_faces << std::endl;*/
	if (m_aligned_patch_num_faces <= 1)
	{
		// std::cout << m_aligned_patch_num_faces << std::endl;
		return;
	}

	// initialize the patch graph
	m_temp_graph_matrix = new unsigned char[m_aligned_patch_num_faces * m_aligned_patch_num_faces];
	m_patch_graph_matrix = new unsigned char[m_aligned_patch_num_faces * m_aligned_patch_num_faces];
	m_temp_network_input = new double[m_aligned_patch_num_faces * (17 + 3)];
	m_patch_node_features = new double[m_aligned_patch_num_faces * 17];	// 17 kinds of features, face center(3),  face normal (3), area(1), num of neighbors(1), points position (9),
	m_matrix_indices = new int[m_aligned_patch_num_faces];
	for (int i = 0; i < m_aligned_patch_num_faces * m_aligned_patch_num_faces; i++)
	{
		m_temp_graph_matrix[i] = 0;
		m_patch_graph_matrix[i] = 0;
	}

	for (int i_f = 0; i_f < m_aligned_patch_num_faces; i_f++)
	{
		int neighbor_count = 0;
		for (TriMesh::FaceFaceIter ff_it = mesh.ff_iter(patch_faces_iter[i_f]); ff_it.is_valid(); ff_it++)
		{
			int temp_neighbor_index = ff_it->idx();
			for (int j_f = 0; j_f < m_aligned_patch_num_faces; j_f++)
			{
				if (temp_neighbor_index == face_indices[j_f])
				{
					m_temp_graph_matrix[i_f * m_aligned_patch_num_faces + j_f] = 1;
					if (neighbor_count < 3)
					{
						if (j_f < 64)
						{
							m_temp_network_input[i_f * 20 + 17 + neighbor_count] = (double)j_f;
						}
						else
						{
							/*m_temp_network_input[i_f * 20 + 17 + neighbor_count] = 63.;*/
							neighbor_count--;
						}
					}
					neighbor_count++;
					break;
				}
			}
		}

		// padding of neighbors
		if (neighbor_count == 2)
		{
			m_temp_network_input[i_f * 20 + 17 + 2] = m_temp_network_input[i_f * 20 + 17 + 1];
		}
		else if (neighbor_count == 1)
		{
			m_temp_network_input[i_f * 20 + 17 + 1] = m_temp_network_input[i_f * 20 + 17 + 0];
			m_temp_network_input[i_f * 20 + 17 + 2] = m_temp_network_input[i_f * 20 + 17 + 1];
		}
		else if (neighbor_count == 0)
		{
			m_temp_network_input[i_f * 20 + 17 + 0] = (double)i_f;
			m_temp_network_input[i_f * 20 + 17 + 1] = (double)i_f;
			m_temp_network_input[i_f * 20 + 17 + 2] = (double)i_f;
		}

		// area
		m_patch_node_features[i_f * 17 + 6] = face_area[face_indices[i_f]];

		// num of neighbors
		int num_neighbors = all_face_neighbor[face_indices[i_f]].size();
		m_patch_node_features[i_f * 17 + 7] = ((((double)num_neighbors - 12.) / 6.) + 1.) / 2.;
		m_temp_network_input[i_f * 20 + 7] = m_patch_node_features[i_f * 17 + 7];
	}

	vertex_iters.clear();
	face_indices_set.clear();
	patchAlignment(gt_normal);
}

PatchData::~PatchData()
{
	m_patch_faces_centers.clear();
	m_patch_points_positions.clear();
	m_patch_faces_normals.clear();
	m_aligned_patch_points_positions.clear();
	m_aligned_patch_faces_normals.clear();

	if (m_temp_graph_matrix != NULL)
	{
		delete m_temp_graph_matrix;
		delete m_patch_graph_matrix;
		delete  m_patch_node_features;
		delete m_temp_network_input;
		delete m_matrix_indices;
	}
}

void PatchData::patchAlignment(glm::dvec3& gt_normal)
{
	glm::dvec4 area_center = m_patch_faces_centers[0];
	double area_max = 0;
	for (int i = 0; i < m_patch_num_faces; i++)
	{
		if (area_max < m_patch_faces_centers[i][0])
		{
			area_max = m_patch_faces_centers[i][0];
		}
	}

	// std::cout << "	\tThe max area of the patch is: " << area_max << std::endl;

	Eigen::Matrix3d tensor_feature_mat;
	Eigen::Matrix3d temp_mat;
	tensor_feature_mat = Eigen::Matrix3d::Zero();

	for (int i = 1; i < m_patch_num_faces; i++)
	{
		temp_mat = Eigen::Matrix3d::Zero();
		glm::dvec3 temp_center(m_patch_faces_centers[i][1] - m_patch_faces_centers[0][1],
			m_patch_faces_centers[i][2] - m_patch_faces_centers[0][2],
			m_patch_faces_centers[i][3] - m_patch_faces_centers[0][3]);

		double temp_area = m_patch_faces_centers[i][0] / area_max;
		double  temp_center_norm_para = exp(-3 * glm::length(temp_center));

		glm::dvec3 temp_weight = glm::cross(glm::cross(temp_center, m_patch_faces_normals[i]), temp_center);
		temp_weight = glm::normalize(temp_weight);
		glm::dvec3 temp_normal = 2. * glm::dot(m_patch_faces_normals[i], temp_weight) * temp_weight - m_patch_faces_normals[i];

		for (int r = 0; r < 3; r++)
		{
			for (int c = 0; c < 3; c++)
			{
				temp_mat(r, c) = temp_normal[r] * temp_normal[c];
			}
		}
		temp_mat *= temp_area * temp_center_norm_para;
		// temp_mat *= temp_center_norm_para;
		tensor_feature_mat += temp_mat;
	}

	Eigen::EigenSolver<Eigen::Matrix3d> eigen_solver(tensor_feature_mat);

	double max_eigen_value = eigen_solver.eigenvalues()[0].real();
	double min_eigen_value = eigen_solver.eigenvalues()[0].real();
	int max_ev_idx = 0;
	int min_ev_idx = 0;
	for (int i = 0; i < 3; i++)
	{
		if (max_eigen_value < eigen_solver.eigenvalues()[i].real())
		{
			max_eigen_value = eigen_solver.eigenvalues()[i].real();
			max_ev_idx = i;
		}
		if (min_eigen_value > eigen_solver.eigenvalues()[i].real())
		{
			min_eigen_value = eigen_solver.eigenvalues()[i].real();
			min_ev_idx = i;
		}
	}
	int middle_ev_idx = 3 - max_ev_idx - min_ev_idx;

	// only one face
	if (middle_ev_idx < 0 || middle_ev_idx > 2)
	{
		min_ev_idx = 0;
		middle_ev_idx = 1;
		max_ev_idx = 2;
		/*std::cout << "!!!____Sort ERROR____!!!" << std::endl;
		exit(0);*/
	}

	glm::dvec3 sorted_eigen_value(eigen_solver.eigenvalues()[max_ev_idx].real(),
		eigen_solver.eigenvalues()[middle_ev_idx].real(),
		eigen_solver.eigenvalues()[min_ev_idx].real());

	glm::dvec3 max_eigen_vec(eigen_solver.eigenvectors().col(max_ev_idx).row(0).value().real(),
		eigen_solver.eigenvectors().col(max_ev_idx).row(1).value().real(),
		eigen_solver.eigenvectors().col(max_ev_idx).row(2).value().real());
	glm::dvec3 middel_eigen_vec(eigen_solver.eigenvectors().col(middle_ev_idx).row(0).value().real(),
		eigen_solver.eigenvectors().col(middle_ev_idx).row(1).value().real(),
		eigen_solver.eigenvectors().col(middle_ev_idx).row(2).value().real());
	glm::dvec3 min_eigen_vec(eigen_solver.eigenvectors().col(min_ev_idx).row(0).value().real(),
		eigen_solver.eigenvectors().col(min_ev_idx).row(1).value().real(),
		eigen_solver.eigenvectors().col(min_ev_idx).row(2).value().real());

	glm::dmat3x3 sorted_eigen_vec(max_eigen_vec, middel_eigen_vec, min_eigen_vec);

	if (sorted_eigen_vec[0][0] * m_patch_faces_normals[0][0]
		+ sorted_eigen_vec[0][1] * m_patch_faces_normals[0][1]
		+ sorted_eigen_vec[0][2] * m_patch_faces_normals[0][2] < 0.)
	{
		sorted_eigen_vec[0] *= -1.;
		sorted_eigen_vec[1] *= -1.;
		sorted_eigen_vec[2] *= -1.;
	}

	m_matrix = sorted_eigen_vec;

	sorted_eigen_value = sorted_eigen_value / sqrt(pow(sorted_eigen_value[0], 2) + pow(sorted_eigen_value[1], 2) + pow(sorted_eigen_value[2], 2));

	// std::cout << sorted_eigen_value[0] << " " << sorted_eigen_value[1] << " " << sorted_eigen_value[2] << std::endl;

	// for normal base change
	glm::dmat3x3 inversed_sorted_vec = glm::inverse(sorted_eigen_vec);

	/* std::cout << inversed_sorted_vec[0][0] << " " << inversed_sorted_vec[1][0] << " " << inversed_sorted_vec[2][0] << std::endl <<
		 inversed_sorted_vec[0][1] << " " << inversed_sorted_vec[1][1] << " " << inversed_sorted_vec[2][1] << std::endl <<
		 inversed_sorted_vec[0][2] << " " << inversed_sorted_vec[1][2] << " " << inversed_sorted_vec[2][2] << std::endl;*/

	for (int i = 0; i < m_aligned_patch_num_faces; i++)
	{
		m_aligned_patch_faces_normals[i] = inversed_sorted_vec * m_aligned_patch_faces_normals[i];
	}

	gt_normal = inversed_sorted_vec * gt_normal;

	// for position base change
	glm::dmat4x4 trans_mat(0);
	for (int i = 0; i < 4; i++)
	{
		trans_mat[i][i] = 1;
		if (i < 3)
		{
			trans_mat[3][i] = -1 * m_patch_faces_centers[0][i + 1];
		}
	}

	glm::dmat4x4 rot_mat(0);
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			if (i < 3 && j < 3)
			{
				rot_mat[i][j] = inversed_sorted_vec[i][j];
			}
		}
	}
	rot_mat[3][3] = 1.;

	double determinant = glm::determinant(inversed_sorted_vec);

	double coord_max = 0.;
	std::vector<glm::dvec3> temp_positions;
	for (int i_face = 0; i_face < m_aligned_patch_num_faces; i_face++)
	{
		for (int i_vertex = 0; i_vertex < 3; i_vertex++)
		{
			glm::dvec4 temp_position;
			if (determinant < 0)
			{
				temp_position = glm::dvec4(m_aligned_patch_points_positions[i_face * 3 + 2 - i_vertex][0],
					m_aligned_patch_points_positions[i_face * 3 + 2 - i_vertex][1],
					m_aligned_patch_points_positions[i_face * 3 + 2 - i_vertex][2], 1.);
			}
			else if (determinant > 0)
			{
				temp_position = glm::dvec4(m_aligned_patch_points_positions[i_face * 3 + i_vertex][0],
					m_aligned_patch_points_positions[i_face * 3 + i_vertex][1],
					m_aligned_patch_points_positions[i_face * 3 + i_vertex][2], 1.);
			}
			else
			{
				/* std::cout << "!!!____Transform matrix Error, Not positive definite matrix____!!!" << std::endl;
				 exit(0);*/
				temp_position = glm::dvec4(m_aligned_patch_points_positions[i_face * 3 + i_vertex][0],
					m_aligned_patch_points_positions[i_face * 3 + i_vertex][1],
					m_aligned_patch_points_positions[i_face * 3 + i_vertex][2], 1.);
			}

			temp_position = trans_mat * temp_position;
			temp_position = rot_mat * temp_position;

			for (int i_v = 0; i_v < 3; i_v++)
			{
				if (coord_max < abs(temp_position[i_v]))
				{
					coord_max = abs(temp_position[i_v]);
				}
			}

			temp_positions.push_back(glm::dvec3(temp_position[0],
				temp_position[1],
				temp_position[2]));
		}
	}

	for (int i_v = 0; i_v < m_aligned_patch_num_faces * 3; i_v++)
	{
		m_aligned_patch_points_positions[i_v] = temp_positions[i_v] / m_radius_region;
	}

	for (int i_f = 0; i_f < m_aligned_patch_num_faces; i_f++)
	{
		m_patch_node_features[i_f * 17 + 6] /= m_radius_region * m_radius_region;
		m_temp_network_input[i_f * 20 + 6] = m_patch_node_features[i_f * 17 + 6];
		// face center and normal
		for (int i_v = 0; i_v < 3; i_v++)
		{
			m_patch_node_features[i_f * 17 + i_v] = ((m_aligned_patch_points_positions[i_f * 3][i_v]
				+ m_aligned_patch_points_positions[i_f * 3 + 1][i_v]
				+ m_aligned_patch_points_positions[i_f * 3 + 2][i_v]) / 3 + 1) / 2.;
			m_temp_network_input[i_f * 20 + i_v] = m_patch_node_features[i_f * 17 + i_v];

			m_patch_node_features[i_f * 17 + i_v + 3] = (m_aligned_patch_faces_normals[i_f][i_v] + 1) / 2.;
			m_temp_network_input[i_f * 20 + i_v + 3] = m_patch_node_features[i_f * 17 + i_v + 3];
		}
		// face vertices
		for (int i_p = 0; i_p < 3; i_p++)
		{
			m_patch_node_features[i_f * 17 + i_p * 3 + 8] = (m_aligned_patch_points_positions[i_f * 3][0] + 1) / 2.;
			m_patch_node_features[i_f * 17 + i_p * 3 + 8 + 1] = (m_aligned_patch_points_positions[i_f * 3][1] + 1) / 2.;
			m_patch_node_features[i_f * 17 + i_p * 3 + 8 + 2] = (m_aligned_patch_points_positions[i_f * 3][2] + 1) / 2.;

			m_temp_network_input[i_f * 20 + i_p * 3 + 8] = m_patch_node_features[i_f * 17 + i_p * 3 + 8];
			m_temp_network_input[i_f * 20 + i_p * 3 + 8 + 1] = m_patch_node_features[i_f * 17 + i_p * 3 + 8 + 1];
			m_temp_network_input[i_f * 20 + i_p * 3 + 8 + 2] = m_patch_node_features[i_f * 17 + i_p * 3 + 8 + 2];
		}
	}

	m_center_normal = glm::dvec3(0., 0., 0.);
	for (int i_n = 0; i_n < 3; i_n++)
	{
		m_center_normal[i_n] = m_aligned_patch_faces_normals[m_center_index][i_n];
	}
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Pavel Kharitonov <ipavrus@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_HASH_TIGER_HPP
#define CRYPTO3_HASH_TIGER_HPP

#include <boost/crypto3/hash/detail/tiger/tiger_policy.hpp>

#include <boost/crypto3/hash/detail/merkle_damgard_construction.hpp>
#include <boost/crypto3/hash/detail/tiger/tiger_padding.hpp>
#include <boost/crypto3/hash/detail/block_stream_processor.hpp>

namespace boost {
    namespace crypto3 {
        namespace hashes {

            template<std::size_t DigestBits = 192, std::size_t Passes = 3>
            struct tiger_compressor {
                typedef detail::tiger_policy<DigestBits, Passes> policy_type;

                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t state_bits = policy_type::state_bits;
                constexpr static const std::size_t state_words = policy_type::state_words;
                typedef typename policy_type::state_type state_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                static inline void process_block(state_type &state, const block_type &block) {

                    word_type A = state[0], B = state[1], C = state[2];
                    block_type input = block;
                    policy_type::pass(A, B, C, input, 5);
                    policy_type::mix(input);
                    policy_type::pass(C, A, B, input, 7);
                    policy_type::mix(input);
                    policy_type::pass(B, C, A, input, 9);

                    for (size_t j = 3; j != policy_type::passes; ++j) {
                        policy_type::mix(input);
                        policy_type::pass(A, B, C, input, 9);
                        word_type T = A;
                        A = C;
                        C = B;
                        B = T;
                    }

                    state[0] ^= A;
                    state[1] = B - state[1];
                    state[2] += C;
                }
            };

            /*!
             * @brief Tiger. An older 192-bit hashes function, optimized for 64-bit
             * systems. Possibly vulnerable to side channels due to its use of table
             * lookups. Prefer Skein-512 or BLAKE2b in new code.
             *
             * @ingroup hashes
             */
            template<std::size_t DigestBits = 192, std::size_t Passes = 3>
            class tiger {
                typedef detail::tiger_policy<DigestBits, Passes> policy_type;

            public:
                struct construction {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t length_bits = policy_type::word_bits;
                        constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                    };

                    typedef merkle_damgard_construction<params_type, typename policy_type::iv_generator,
                                                        tiger_compressor<DigestBits, Passes>,
                                                        detail::tiger_padding<policy_type>>
                        type;
                };

                template<typename StateAccumulator, std::size_t ValueBits>
                struct stream_processor {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t value_bits = ValueBits;
                    };

                    typedef block_stream_processor<construction, StateAccumulator, params_type> type;
                };

                constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                typedef typename policy_type::digest_type digest_type;
            };
        }    // namespace hashes
    }        // namespace crypto3
}    // namespace boost

#endif


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_NARROWPHASE_HPP
#define MCL_NARROWPHASE_HPP 1

#include <Eigen/Core>

namespace mcl
{

template <typename T, int DIM>
class NarrowPhase
{
public:
    typedef Eigen::Matrix<T,DIM,1> VecType;
    using Vec3 = Eigen::Matrix<T,3,1>;
    using Vec2 = Eigen::Matrix<T,2,1>;

    static bool hit_wrong_side_vf(const VecType *v0, const VecType *v1, T t);

    // Used in vertex-facet queries,
    // tests vertex as a ray against the box.
    static bool query_ray_box(
        const VecType &p_x0, const VecType &p_x1,
        const VecType &bmin, const VecType &bmax);

    // Discrete tests for face-face and edge-edge
    // only defined for DIM=3 and DIM=2 respectively

    // f0 and f1 are arrays of three vertices
    static bool discrete_tri_tri(
        const Vec3 *f0, const Vec3 *f1);

    // e0 and e1 are arrays of two vertices
    static bool discrete_edge_edge(
        const Vec2 *e0, const Vec2 *e1);
};

// Even though float is allowed as a template, everything is casted
// to double before evaluating. Kernels for floats are future work.
template <typename T, int DIM>
class NarrowPhaseCTCD
{
public:
    typedef Eigen::Matrix<T,DIM,1> VecType;

    // Returns:
    // 0 = not colliding
    // 1 = is colliding
    // -1 = colliding wrong side
    // Can only return -1 if test_wrong_side=true

    static int query_ccd_vf(
        const VecType *v0, const VecType *v1,
        const T &eta, // gap
        bool test_wrong_side,
        T &t_impact);

    static int query_ccd_ee(
        const VecType *v0, const VecType *v1,
        const T &eta, // gap
        bool test_vv_and_ve,
        T &t_impact);
};

// ACCD from https://doi.org/10.1145/3450626.3459767
// which is a variant of conservative advancement.
template <typename T, int DIM>
class NarrowPhaseACCD
{
public:
    typedef Eigen::Matrix<T,DIM,1> VecType;

    // Returns:
    // 0 = not colliding
    // 1 = is colliding

    static int query_ccd_vf(
        const VecType *v0, const VecType *v1,
        const T &eta, // gap
        T &t_impact);

    static int query_ccd_ee(
        const VecType *v0, const VecType *v1,
        const T &eta, // gap
        T &t_impact);

    // Alg 1. from doi: 10.1145/3450626.3459767
    static bool additive_ccd(
        const VecType *v0, const VecType *v1,
        const T &eta, // gap
        bool is_vf,
        T &t_impact);

    // Returns distance between pairs (VF or EE)
    // If error, returns -1
    static T pair_distance(
        const VecType *v,
        bool is_vf);
};

} // ns mcl

#endif


/*
Copyright 2016-2017 Robotics and Biology Lab, TU Berlin. All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    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.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.

The views and conclusions contained in the software and documentation are those of the authors and should not be interpreted as representing official policies, either expressed or implied, of the FreeBSD Project.
*/

#include <ecto_rbo_grasping/PoseSet.h>

#include <tf_conversions/tf_eigen.h>
#include <Eigen/Geometry>

#include <CGAL/Gmpz.h>
#include <CGAL/Extended_homogeneous.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Nef_polyhedron_3.h>

#include "Wm5IntrSegment3Box3.h"
#include "Wm5IntrBox3Box3.h"
#include "Wm5ContMinBox3.h"

#include <ecto_rbo_grasping/gdiam.hpp>

//typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
//typedef CGAL::Extended_homogeneous<CGAL::Gmpz>  Kernel;
typedef CGAL::Extended_cartesian< CGAL::Lazy_exact_nt<CGAL::Gmpq> > Kernel;
typedef CGAL::Nef_polyhedron_3<Kernel>  Nef_polyhedron;
typedef Nef_polyhedron::Vertex_const_iterator Vertex_const_iterator;
typedef Nef_polyhedron::Plane_3  Plane_3;
typedef Nef_polyhedron::Point_3  Point_3;
typedef Nef_polyhedron::Vector_3  Vector_3;

namespace posesets
{

PoseSet::PoseSet(const tf::Transform& origin) :
    origin(origin)
{
}

PoseSet::~PoseSet()
{
}

void PoseSet::updateBoxVertices(Wm5::Vector3d box_vertices[8], Wm5::Segment3d edges[12]) const
{
    Wm5::Vector3d origin_a(this->origin.getOrigin().x(), this->origin.getOrigin().y(), this->origin.getOrigin().z());
    //    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(a.getBasis().getRow(0).x(), a.getBasis().getRow(0).y(), a.getBasis().getRow(0).z()),
    //                               Wm5::Vector3d(a.getBasis().getRow(1).x(), a.getBasis().getRow(1).y(), a.getBasis().getRow(1).z()),
    //                               Wm5::Vector3d(a.getBasis().getRow(2).x(), a.getBasis().getRow(2).y(), a.getBasis().getRow(2).z()) };
    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(this->origin.getBasis().getColumn(0).x(), this->origin.getBasis().getColumn(0).y(), this->origin.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(this->origin.getBasis().getColumn(1).x(), this->origin.getBasis().getColumn(1).y(), this->origin.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(this->origin.getBasis().getColumn(2).x(), this->origin.getBasis().getColumn(2).y(), this->origin.getBasis().getColumn(2).z()) };
    double size_a[3] = {0.5 * this->getPositions()[0], 0.5 * this->getPositions()[1], 0.5 * this->getPositions()[2]};

    Wm5::Box3d box_a(origin_a, dim_a, size_a);
    box_a.ComputeVertices(box_vertices);

    edges[0] = Wm5::Segment3d(box_vertices[0], box_vertices[1]);
    edges[1] = Wm5::Segment3d(box_vertices[0], box_vertices[3]);
    edges[2] = Wm5::Segment3d(box_vertices[0], box_vertices[4]);
    edges[3] = Wm5::Segment3d(box_vertices[1], box_vertices[2]);
    edges[4] = Wm5::Segment3d(box_vertices[1], box_vertices[5]);
    edges[5] = Wm5::Segment3d(box_vertices[2], box_vertices[6]);
    edges[6] = Wm5::Segment3d(box_vertices[2], box_vertices[3]);
    edges[7] = Wm5::Segment3d(box_vertices[3], box_vertices[7]);
    edges[8] = Wm5::Segment3d(box_vertices[4], box_vertices[7]);
    edges[9] = Wm5::Segment3d(box_vertices[4], box_vertices[5]);
    edges[10] = Wm5::Segment3d(box_vertices[5], box_vertices[6]);
    edges[11] = Wm5::Segment3d(box_vertices[6], box_vertices[7]);
}

void PoseSet::setOrigin(const tf::Transform& origin)
{
    this->origin = origin;
}

void PoseSet::setOrientations(const OrientationSet& orientations)
{
    this->orientations = orientations;
}

void PoseSet::setPositions(const tf::Vector3& positions)
{
    this->positions = positions;
}

const tf::Transform& PoseSet::getOrigin() const
{
    return origin;
}

OrientationSet& PoseSet::getOrientations()
{
    return orientations;
}

const OrientationSet& PoseSet::getOrientations() const
{
    return orientations;
}

const tf::Vector3& PoseSet::getPositions() const
{
    return positions;
}

Wm5::Box3d PoseSet::getWm5Box3d() const
{
    Wm5::Vector3d origin_a(origin.getOrigin().x(), origin.getOrigin().y(), origin.getOrigin().z());
    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(origin.getBasis().getColumn(0).x(), origin.getBasis().getColumn(0).y(), origin.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(origin.getBasis().getColumn(1).x(), origin.getBasis().getColumn(1).y(), origin.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(origin.getBasis().getColumn(2).x(), origin.getBasis().getColumn(2).y(), origin.getBasis().getColumn(2).z()) };
    double size_a[3] = {0.5 * positions[0], 0.5 * positions[1], 0.5 * positions[2]};

    return Wm5::Box3d(origin_a, dim_a, size_a);
}

bool PoseSet::isIntersecting(const PoseSet& other) const
{
//    if (this->orientations.getIntersection(other.getOrientations()).empty())
    if (!this->orientations.hasIntersection(other.getOrientations()))
    {
//        std::cout << "orientation false " << std::endl;
        return false;
    }

    Wm5::IntrBox3Box3d intersector(this->getWm5Box3d(), other.getWm5Box3d());

//    if (!intersector.Test())
//        std::cout << "position false " << std::endl;

    return intersector.Test();
}

bool PoseSet::intersect(const PoseSet& other)
{
    this->orientations.intersect(other.getOrientations());

    if (this->orientations.empty())
        return false;

    return intersectBoxesGeometricTools(this->origin, this->positions, other.getOrigin(), other.getPositions(), this->origin, this->positions);

    /*
    if (intersectBoxes(this->origin, this->positions, other.getOrigin(), other.getPositions()))
    {
        ROS_INFO("Intersection! (maybe)");
//        this->origin.setOrigin(origin.getOrigin().lerp(other.getOrigin().getOrigin(), 0.5));
//        this->positions.setMin(other.getPositions());
    }
    else {
        ROS_INFO("They DONT intersect!");
        this->origin.setIdentity();
        this->positions.setZero();
    }
    */
}

bool PoseSet::intersectBoxesGeometricTools(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b, tf::Transform& n, tf::Vector3& extents_n)
{
    Wm5::Vector3d origin_a(a.getOrigin().x(), a.getOrigin().y(), a.getOrigin().z());
//    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(a.getBasis().getRow(0).x(), a.getBasis().getRow(0).y(), a.getBasis().getRow(0).z()),
//                               Wm5::Vector3d(a.getBasis().getRow(1).x(), a.getBasis().getRow(1).y(), a.getBasis().getRow(1).z()),
//                               Wm5::Vector3d(a.getBasis().getRow(2).x(), a.getBasis().getRow(2).y(), a.getBasis().getRow(2).z()) };
    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(a.getBasis().getColumn(0).x(), a.getBasis().getColumn(0).y(), a.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(a.getBasis().getColumn(1).x(), a.getBasis().getColumn(1).y(), a.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(a.getBasis().getColumn(2).x(), a.getBasis().getColumn(2).y(), a.getBasis().getColumn(2).z()) };
    double size_a[3] = {0.5 * extents_a[0], 0.5 * extents_a[1], 0.5 * extents_a[2]};

    Wm5::Vector3d origin_b(b.getOrigin().x(), b.getOrigin().y(), b.getOrigin().z());
//    Wm5::Vector3d dim_b[3] = { Wm5::Vector3d(b.getBasis().getRow(0).x(), b.getBasis().getRow(0).y(), b.getBasis().getRow(0).z()),
//                               Wm5::Vector3d(b.getBasis().getRow(1).x(), b.getBasis().getRow(1).y(), b.getBasis().getRow(1).z()),
//                               Wm5::Vector3d(b.getBasis().getRow(2).x(), b.getBasis().getRow(2).y(), b.getBasis().getRow(2).z()) };
    Wm5::Vector3d dim_b[3] = { Wm5::Vector3d(b.getBasis().getColumn(0).x(), b.getBasis().getColumn(0).y(), b.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(b.getBasis().getColumn(1).x(), b.getBasis().getColumn(1).y(), b.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(b.getBasis().getColumn(2).x(), b.getBasis().getColumn(2).y(), b.getBasis().getColumn(2).z()) };
    double size_b[3] = {0.5 * extents_b[0], 0.5 * extents_b[1], 0.5 * extents_b[2]};

    Wm5::Box3d box_a(origin_a, dim_a, size_a);
    Wm5::Box3d box_b(origin_b, dim_b, size_b);
    Wm5::IntrBox3Box3d intersector(box_a, box_b);

    if (!intersector.Test())
    {
        //ROS_INFO("NO intersection.");
        n.setIdentity();
        extents_n.setZero();
        return false;
    }

    // do static check differently:
    // check intersections between 12 edges (segments) of one box with the other box
    intersection_points.clear();
//    std::vector<Wm5::Vector3d> intersection_points;
    for (int l = 0; l < 2; ++l)
    {
        const Wm5::Box3d& box = (l == 0) ? box_a : box_b;
        const Wm5::Box3d& other_box = (l == 1) ? box_a : box_b;

        Wm5::Vector3d vertices[8];
        box.ComputeVertices(vertices);

//        if (l == 0)
//            for (int k = 0; k < 8; ++k)
//                this->box_vertices[k] = vertices[k];

        Wm5::Segment3d edges[12] = {
            Wm5::Segment3d(vertices[0], vertices[1]),
            Wm5::Segment3d(vertices[0], vertices[3]),
            Wm5::Segment3d(vertices[0], vertices[4]),
            Wm5::Segment3d(vertices[1], vertices[2]),
            Wm5::Segment3d(vertices[1], vertices[5]),
            Wm5::Segment3d(vertices[2], vertices[6]),
            Wm5::Segment3d(vertices[2], vertices[3]),
            Wm5::Segment3d(vertices[3], vertices[7]),
            Wm5::Segment3d(vertices[4], vertices[7]),
            Wm5::Segment3d(vertices[4], vertices[5]),
            Wm5::Segment3d(vertices[5], vertices[6]),
            Wm5::Segment3d(vertices[6], vertices[7])
        };

        for (int i = 0; i < 12; ++i)
        {
            Wm5::IntrSegment3Box3d intersector2(edges[i], other_box, true);
            if (!intersector2.Find())
                continue;

            int no_intersections = intersector2.GetQuantity();

    //        std::cout << "Found: " << no_intersections << std::endl;
            for (int j = 0; j < no_intersections; ++j)
            {
//                bool already_in_there = false;
                const Wm5::Vector3d& candidate = intersector2.GetPoint(j);
//                // check if for some reasons this point is already in there
//                for (int k = 0; k < intersection_points.size(); ++k)
//                {
//                    if (fabs(candidate.X() - intersection_points[k].X()) < 0.001f ||
//                        fabs(candidate.Y() - intersection_points[k].Y()) < 0.001f ||
//                        fabs(candidate.Z() - intersection_points[k].Z()) < 0.001f)
//                    {
//                        already_in_there = true;
//                        break;
//                    }
//                }
//                if (!already_in_there)
                    intersection_points.push_back(intersector2.GetPoint(j));
            }
        }
    }

    /*
    std::cout << "In Total: " << intersection_points.size() << std::endl;
    for (std::vector<Wm5::Vector3d>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
    {
        std::cout << " (" << it->X() << " " << it->Y() << " " << it->Z() << ")";
    }
    std::cout << std::endl;
    */

    if (intersection_points.size() < 4)
    {
        n.setIdentity();
        extents_n.setZero();
        return false;
    }

//    Wm5::Box3d minBox = Wm5::MinBox3<double>((int) intersection_points.size(), &intersection_points[0], 0.001, Wm5::Query::QT_REAL);

//    n.setBasis(tf::Matrix3x3(minBox.Axis[0].X(), minBox.Axis[0].Y(), minBox.Axis[0].Z(),
//                             minBox.Axis[1].X(), minBox.Axis[1].Y(), minBox.Axis[1].Z(),
//                             minBox.Axis[2].X(), minBox.Axis[2].Y(), minBox.Axis[2].Z()));

//    n.setOrigin(tf::Vector3(minBox.Center.X(), minBox.Center.Y(), minBox.Center.Z()));

//    std::cout << "New Box extents: " << extents_n[0] << " " << extents_n[1] << " " << extents_n[2] << std::endl;

//    extents_n.setX(minBox.Extent[0]);
//    extents_n.setY(minBox.Extent[1]);
//    extents_n.setZ(minBox.Extent[2]);

//    if (extents_n[0] == 0 || extents_n[1] == 0 || extents_n[2] == 0)
//    {
//        n.setIdentity();
//        extents_n.setZero();
//        return false;
//    }

//    intersector.Find(0.0, Wm5::Vector3d::ZERO, Wm5::Vector3d::ZERO);
//    intersector.Find(0.1, Wm5::Vector3d(0.1, 0.1, 0.1), Wm5::Vector3d(0.1, 0.1, 0.1));
//    int number_of_intersection_points = intersector.GetQuantity();

//    std::cout << "Number of intersection points: " << number_of_intersection_points << std::endl;
//    if (number_of_intersection_points == 0)
//    {
//        n.setIdentity();
//        extents_n.setZero();
//        return false;
//    }

    gdiam_real* points;
    points = (gdiam_point)malloc( sizeof( gdiam_point_t ) * intersection_points.size());

    for (int i = 0; i < intersection_points.size(); ++i)
    {
        const Wm5::Vector3d& p = intersection_points[i];
        points[i * 3 + 0] = p.X();
        points[i * 3 + 1] = p.Y();
        points[i * 3 + 2] = p.Z();
//        std::cout << "HA: " << p.X() << " " << p.Y() << " " << p.Z() << std::endl;
    }

    gdiam_point* pnt_arr;
    gdiam_bbox bb;
    pnt_arr = gdiam_convert( (gdiam_real *)points, intersection_points.size());

//    printf( "Computing a tight-fitting bounding box of the point-set\n" );
//    bb = gdiam_approx_mvbb_grid_sample(pnt_arr, nefI.number_of_vertices(), 5, 400 );
    bb = gdiam_approx_mvbb(pnt_arr, intersection_points.size(), 0.01f);

    //printf( "Resulting bounding box:\n" );
    bb.dump();

    gdiam_point bb_dir0 = bb.get_dir(0);
    gdiam_point bb_dir1 = bb.get_dir(1);
    gdiam_point bb_dir2 = bb.get_dir(2);

    n.setBasis(tf::Matrix3x3(bb_dir0[0], bb_dir1[0], bb_dir2[0],
                             bb_dir0[1], bb_dir1[1], bb_dir2[1],
                             bb_dir0[2], bb_dir1[2], bb_dir2[2]));

    double x, y, z;
    bb.get_vertex(0.5, 0.5, 0.5, &x, &y, &z);
    n.setOrigin(tf::Vector3(x, y, z));

    extents_n.setX(bb.get_len(0) * 1.f);
    extents_n.setY(bb.get_len(1) * 1.f);
    extents_n.setZ(bb.get_len(2) * 1.f);

//    if (extents_n[0] == 0 || extents_n[1] == 0 || extents_n[2] == 0)
    if (bb.volume() == 0)
    {
        //ROS_INFO("Too Small!");
        n.setIdentity();
        extents_n.setZero();
        return false;
    }

    return true;
}

void PoseSet::intersectBoxesCGAL(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b, tf::Transform& n, tf::Vector3& extents_n)
{
    // generate a Nef Polyhedron for box A
    Point_3 origin_a(a.getOrigin().x(), a.getOrigin().x(), a.getOrigin().x());
    Vector_3 dim1_a(a.getBasis().getRow(0).x(), a.getBasis().getRow(0).y(), a.getBasis().getRow(0).z());
    Vector_3 dim2_a(a.getBasis().getRow(1).x(), a.getBasis().getRow(1).y(), a.getBasis().getRow(1).z());
    Vector_3 dim3_a(a.getBasis().getRow(2).x(), a.getBasis().getRow(2).y(), a.getBasis().getRow(2).z());
    Nef_polyhedron NA1(Plane_3(origin_a + dim1_a * extents_a[0], dim1_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA2(Plane_3(origin_a - dim1_a * extents_a[0], -dim1_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA3(Plane_3(origin_a + dim2_a * extents_a[1], dim2_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA4(Plane_3(origin_a - dim2_a * extents_a[1], -dim2_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA5(Plane_3(origin_a + dim3_a * extents_a[2], dim3_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA6(Plane_3(origin_a - dim3_a * extents_a[2], -dim3_a), Nef_polyhedron::INCLUDED);

//    Nef_polyhedron nefA = NA1 * NA2 * NA3 * NA4 * NA5 * NA6;

    // generate a Nef Polyhedron for box B
    Point_3 origin_b(b.getOrigin().x(), b.getOrigin().x(), b.getOrigin().x());
    Vector_3 dim1_b(b.getBasis().getRow(0).x(), b.getBasis().getRow(0).y(), b.getBasis().getRow(0).z());
    Vector_3 dim2_b(b.getBasis().getRow(1).x(), b.getBasis().getRow(1).y(), b.getBasis().getRow(1).z());
    Vector_3 dim3_b(b.getBasis().getRow(2).x(), b.getBasis().getRow(2).y(), b.getBasis().getRow(2).z());
    Nef_polyhedron NB1(Plane_3(origin_b + dim1_b * extents_b[0], dim1_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB2(Plane_3(origin_b - dim1_b * extents_b[0], -dim1_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB3(Plane_3(origin_b + dim2_b * extents_b[1], dim2_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB4(Plane_3(origin_b - dim2_b * extents_b[1], -dim2_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB5(Plane_3(origin_b + dim3_b * extents_b[2], dim3_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB6(Plane_3(origin_b - dim3_b * extents_b[2], -dim3_b), Nef_polyhedron::INCLUDED);

//    Nef_polyhedron nefB = NB1 * NB2 * NB3 * NB4 * NB5 * NB6;

//    Nef_polyhedron nefI = nefA.intersection(nefB);
    Nef_polyhedron nefI = NA1 * NA2 * NA3 * NA4 * NA5 * NA6 * NB1 * NB2 * NB3 * NB4 * NB5 * NB6;

    /*
    std::cout << "properties: " << std::endl;
    std::cout << "is_simple: " << nefI.is_simple() << std::endl;
    std::cout << "is_bounded: " << nefI.is_bounded() << std::endl;
//    std::cout << "is_convex: " << nefI.is_convex() << std::endl;
    std::cout << "vertices: " << nefI.number_of_vertices() << std::endl;
    std::cout << "is_empty: " << nefI.is_empty() << std::endl;
    */

    if (nefI.is_empty())
        return;

    Vertex_const_iterator it;
//    gdiam_point* pnt_arr;// = new gdiam_point[nefI.number_of_vertices()];
//    pnt_arr = (gdiam_point *)malloc( sizeof( gdiam_point ) * nefI.number_of_vertices());
    gdiam_real* points;
    points = (gdiam_point)malloc( sizeof( gdiam_point_t ) * nefI.number_of_vertices());

    size_t i = 0;
    CGAL_forall_vertices(it, nefI)
    {
        //std::cout << CGAL::to_double(it->point().x()) << " HAH " << CGAL::to_double(it->point().y()) << " " << CGAL::to_double(it->point().z()) << std::endl;
//        pnt_init((gdiam_point)(pnt_arr[i]), 0, 0, 0);
        points[i * 3 + 0] = CGAL::to_double(it->point().x());
        points[i * 3 + 1] = CGAL::to_double(it->point().y());
        points[i * 3 + 2] = CGAL::to_double(it->point().z());
//        pnt_init(pnt_arr[i], CGAL::to_double(it->point().x()), CGAL::to_double(it->point().y()), CGAL::to_double(it->point().z()));
        ++i;
    }

    gdiam_point* pnt_arr;
    gdiam_bbox bb;
    pnt_arr = gdiam_convert( (gdiam_real *)points, nefI.number_of_vertices());

//    printf( "Computing a tight-fitting bounding box of the point-set\n" );
//    bb = gdiam_approx_mvbb_grid_sample(pnt_arr, nefI.number_of_vertices(), 5, 400 );
    bb = gdiam_approx_mvbb(pnt_arr, nefI.number_of_vertices(), 0.1f);

    //printf( "Resulting bounding box:\n" );
    bb.dump();

    gdiam_point bb_dir0 = bb.get_dir(0);
    gdiam_point bb_dir1 = bb.get_dir(1);
    gdiam_point bb_dir2 = bb.get_dir(2);

    n.setBasis(tf::Matrix3x3(bb_dir0[0], bb_dir0[1], bb_dir0[2],
                             bb_dir1[0], bb_dir1[1], bb_dir1[2],
                             bb_dir2[0], bb_dir2[1], bb_dir2[2]));

    double x, y, z;
    bb.get_vertex(0.5, 0.5, 0.5, &x, &y, &z);
    n.setOrigin(tf::Vector3(x, y, z));

    extents_n.setX(bb.get_len(0) * 0.5f);
    extents_n.setY(bb.get_len(1) * 0.5f);
    extents_n.setZ(bb.get_len(2) * 0.5f);
}

void PoseSet::intersectBoxes(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b, tf::Transform& n, tf::Vector3& extents_n)
{
//    tf::Transform b_a = a.inverseTimes(b);
//    tf::Transform a_b = b.inverseTimes(a);

//    // do aabb-intersection along axes of a
//    tf::Matrix3x3 all_dot_products = a.getBasis().transposeTimes(b_a.getBasis());
//    tf::Vector3 extents_b_a(all_dot_products.getRow(0)[all_dot_products.getRow(0).maxAxis()],
//                            all_dot_products.getRow(1)[all_dot_products.getRow(1).maxAxis()],
//                            all_dot_products.getRow(2)[all_dot_products.getRow(2).maxAxis()]);
//    extents_b_a *= extents_b;

//    // do aabb-intersection along axes of b
////    all_dot_products = b.getBasis().transposeTimes(a_b.getBasis());
////    tf::Vector3 extents_a_b(all_dot_products.getRow(0)[all_dot_products.getRow(0).maxAxis()],
////                            all_dot_products.getRow(1)[all_dot_products.getRow(1).maxAxis()],
////                            all_dot_products.getRow(2)[all_dot_products.getRow(2).maxAxis()]);
////    extents_a_b *= extents_a;

////    if (extents_b_a.length2() < extents_a_b.length2())
////    {
//        // a is the reference frame
//        Eigen::Vector3d extents, origin;
//        tf::vectorTFToEigen(extents_a, extents);
//        tf::vectorTFToEigen(a.getOrigin(), origin);
//        Eigen::AlignedBox<float, 3> box_a(origin + extents, origin - extents);

////        Eigen::AlignedBox<float, 3> box_b(v, v);

////        Eigen::AlignedBox<float, 3> box_i = box_a.intersection(box_b);

////        tf::vectorEigenToTF(box_i.center(), n.getOrigin());
////        tf::vectorEigenToTF(box_i.max(), extents_n);
////        n.setRotation(a.getRotation());
////    }
////    else {
////        extents_n = (extents_b - extents_a_b).absolute();
////        n.setOrigin(b.getOrigin() + extents_b - extents_n * 0.5);
////        n.setRotation(b.getRotation());
////    }
}

bool PoseSet::intersectBoxes(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b)
{
    tf::Matrix3x3 A = a.getBasis();
    tf::Matrix3x3 B = b.getBasis();

    //translation, in parent frame
    tf::Vector3 v = b.getOrigin() - a.getOrigin();
    //translation, in A's frame
    tf::Vector3 T(v.dot(A.getColumn(0)), v.dot(A.getColumn(1)), v.dot(A.getColumn(2)));

    //B's basis with respect to A's local frame
    tf::Matrix3x3 R = A.transposeTimes(B);
    float ra, rb, t;
    long i, k;

    //calculate rotation matrix
//        for( i=0 ; i<3 ; i++ )
//            for( k=0 ; k<3 ; k++ )
//                R[i][k] = A.getColumn(i).dot(B.getColumn(k));
    /*ALGORITHM: Use the separating axis test for all 15 potential
separating axes. If a separating axis could not be found, the two
boxes overlap. */

    //A's basis vectors
    for( i=0 ; i<3 ; i++ )
    {
        ra = extents_a[i];
        rb = extents_b[0]*fabs(R[i][0]) + extents_b[1]*fabs(R[i][1]) + extents_b[2]*fabs(R[i][2]);

        t = fabs( T[i] );

        if( t > ra + rb )
            return false;
    }

    //B's basis vectors
    for( k=0 ; k<3 ; k++ )
    {
        ra = extents_a[0]*fabs(R[0][k]) + extents_a[1]*fabs(R[1][k]) + extents_a[2]*fabs(R[2][k]);
        rb = extents_b[k];

        t = fabs( T[0]*R[0][k] + T[1]*R[1][k] + T[2]*R[2][k] );

        if( t > ra + rb )
            return false;
    }

    //9 cross products

    //L = A0 x B0
    ra = extents_a[1]*fabs(R[2][0]) + extents_a[2]*fabs(R[1][0]);

    rb = extents_b[1]*fabs(R[0][2]) + extents_b[2]*fabs(R[0][1]);

    t = fabs( T[2]*R[1][0] - T[1]*R[2][0] );

    if( t > ra + rb )
        return false;

    //L = A0 x B1
    ra = extents_a[1]*fabs(R[2][1]) + extents_a[2]*fabs(R[1][1]);

    rb = extents_b[0]*fabs(R[0][2]) + extents_b[2]*fabs(R[0][0]);

    t = fabs( T[2]*R[1][1] - T[1]*R[2][1] );

    if( t > ra + rb )
        return false;

    //L = A0 x B2
    ra = extents_a[1]*fabs(R[2][2]) + extents_a[2]*fabs(R[1][2]);

    rb = extents_b[0]*fabs(R[0][1]) + extents_b[1]*fabs(R[0][0]);

    t = fabs( T[2]*R[1][2] - T[1]*R[2][2] );

    if( t > ra + rb )
        return false;

    //L = A1 x B0
    ra = extents_a[0]*fabs(R[2][0]) + extents_a[2]*fabs(R[0][0]);

    rb = extents_b[1]*fabs(R[1][2]) + extents_b[2]*fabs(R[1][1]);

    t = fabs( T[0]*R[2][0] - T[2]*R[0][0] );

    if( t > ra + rb )
        return false;

    //L = A1 x B1
    ra = extents_a[0]*fabs(R[2][1]) + extents_a[2]*fabs(R[0][1]);

    rb = extents_b[0]*fabs(R[1][2]) + extents_b[2]*fabs(R[1][0]);

    t = fabs( T[0]*R[2][1] - T[2]*R[0][1] );

    if( t > ra + rb )
        return false;

    //L = A1 x B2
    ra = extents_a[0]*fabs(R[2][2]) + extents_a[2]*fabs(R[0][2]);

    rb = extents_b[0]*fabs(R[1][1]) + extents_b[1]*fabs(R[1][0]);

    t = fabs( T[0]*R[2][2] - T[2]*R[0][2] );

    if( t > ra + rb )
        return false;

    //L = A2 x B0
    ra = extents_a[0]*fabs(R[1][0]) + extents_a[1]*fabs(R[0][0]);

    rb = extents_b[1]*fabs(R[2][2]) + extents_b[2]*fabs(R[2][1]);

    t = fabs( T[1]*R[0][0] - T[0]*R[1][0] );

    if( t > ra + rb )
        return false;

    //L = A2 x B1
    ra = extents_a[0]*fabs(R[1][1]) + extents_a[1]*fabs(R[0][1]);

    rb = extents_b[0] *fabs(R[2][2]) + extents_b[2]*fabs(R[2][0]);

    t = fabs( T[1]*R[0][1] - T[0]*R[1][1] );

    if( t > ra + rb )
        return false;

    //L = A2 x B2
    ra = extents_a[0]*fabs(R[1][2]) + extents_a[1]*fabs(R[0][2]);

    rb = extents_b[0]*fabs(R[2][1]) + extents_b[1]*fabs(R[2][0]);

    t = fabs( T[1]*R[0][2] - T[0]*R[1][2] );

    if( t > ra + rb )
        return false;

    /*no separating axis found, the two boxes overlap */

    return true;
}

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 BOOST_SIMD_TOOLBOX_CONSTANT_INCLUDE_CONSTANTS_REAL_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_INCLUDE_CONSTANTS_REAL_HPP_INCLUDED

#include <boost/simd/toolbox/constant/include/constants/pi.hpp>
#include <boost/simd/toolbox/constant/include/constants/pio_2.hpp>
#include <boost/simd/toolbox/constant/include/constants/inf.hpp>
#include <boost/simd/toolbox/constant/include/constants/nan.hpp>
#include <boost/simd/toolbox/constant/include/constants/half.hpp>
#include <boost/simd/toolbox/constant/include/constants/minf.hpp>
#include <boost/simd/toolbox/constant/include/constants/gold.hpp>
#include <boost/simd/toolbox/constant/include/constants/sqrt_2.hpp>
#include <boost/simd/toolbox/constant/include/constants/cgold.hpp>
#include <boost/simd/toolbox/constant/include/constants/mhalf.hpp>
#include <boost/simd/toolbox/constant/include/constants/mzero.hpp>
#include <boost/simd/toolbox/constant/include/constants/third.hpp>
#include <boost/simd/toolbox/constant/include/constants/sqrt_2o_2.hpp>
#include <boost/simd/toolbox/constant/include/constants/quarter.hpp>
#include <boost/simd/toolbox/constant/include/constants/twotom10.hpp>
#include <boost/simd/toolbox/constant/include/constants/twotonmb.hpp>
#include <boost/simd/toolbox/constant/include/constants/real_splat.hpp>
#include <boost/simd/toolbox/constant/include/constants/splitfactor.hpp>

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>
#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/algorithms/detail/course.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a Great Circle segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_cross_track
{

public :
    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                typename coordinate_type<P1>::type,
                                typename coordinate_type<P2>::type
                            >::type,
                        typename coordinate_type<P>::type
                    >::type,
                CalculationType
            >::type coordinate_type;

        boost::ignore_unused<coordinate_type>();

        double d1 = 0.001; // m_strategy.apply(sp1, p);
        double crs_AD = geometry::detail::course<double>(p1, p);
        double crs_AB = geometry::detail::course<double>(p1, p2);
        double XTD = asin(sin(d1) * sin(crs_AD - crs_AB));

        return math::equals(XTD, 0) ? 0 : XTD < 0 ? 1 : -1;
    }
};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include <thread>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/simulation/simulation.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_gauss_propagator )

// Test Gauss propagator for point mass central body.
BOOST_AUTO_TEST_CASE( testGaussPopagatorForPointMassCentralBodies )
{
    for( int propagatorType = 0; propagatorType < 2; propagatorType++ )
    {
        // Test simulation for different central body cases
        //    for( unsigned int simulationCase = 1; simulationCase < 2; simulationCase++ )
        //    {
        //Using declarations.
        using namespace tudat::interpolators;
        using namespace tudat::numerical_integrators;
        using namespace tudat::spice_interface;
        using namespace tudat::simulation_setup;
        using namespace tudat::basic_astrodynamics;
        using namespace tudat::orbital_element_conversions;
        using namespace tudat::propagators;

        //Load spice kernels.
        spice_interface::loadStandardSpiceKernels( );

        // Define bodies in simulation.
        unsigned int totalNumberOfBodies = 7;
        std::vector< std::string > bodyNames;
        bodyNames.resize( totalNumberOfBodies );
        bodyNames[ 0 ] = "Earth";
        bodyNames[ 1 ] = "Mars";
        bodyNames[ 2 ] = "Sun";
        bodyNames[ 3 ] = "Venus";
        bodyNames[ 4 ] = "Moon";
        bodyNames[ 5 ] = "Mercury";
        bodyNames[ 6 ] = "Jupiter";

        double initialEphemerisTime = 1.0E7;
        double finalEphemerisTime = 2.0E7;
        double maximumTimeStep = 250.0;
        double buffer = 5.0 * maximumTimeStep;

        // Create bodies needed in simulation
        BodyListSettings bodySettings =
                getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer , finalEphemerisTime + buffer,
                                         "SSB", "J2000" );

        SystemOfBodies bodies = createSystemOfBodies( bodySettings );

        // Set accelerations between bodies that are to be taken into account.
        SelectedAccelerationMap accelerationMap;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfEarth;
        accelerationsOfEarth[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationsOfEarth[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationsOfEarth[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationMap[ "Earth" ] = accelerationsOfEarth;

        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMars;
        accelerationsOfMars[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationsOfMars[ "Venus" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationsOfMars[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationMap[ "Mars" ] = accelerationsOfMars;

        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVenus;
        accelerationsOfVenus[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationsOfVenus[ "Mars" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationsOfVenus[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
        accelerationMap[ "Venus" ] = accelerationsOfVenus;

        // Propagate Earth, Mars and Moon
        std::vector< std::string > bodiesToPropagate;
        bodiesToPropagate.push_back( "Earth" );
        bodiesToPropagate.push_back( "Mars" );
        bodiesToPropagate.push_back( "Venus" );

        unsigned int numberOfNumericalBodies = bodiesToPropagate.size( );

        // Define central bodies: all Sun for simulationCase = 0, Earth and Mars: Sun, Moon: Earth for simulationCase = 1
        std::vector< std::string > centralBodies;
        std::map< std::string, std::string > centralBodyMap;
        centralBodies.resize( numberOfNumericalBodies );
        for( unsigned int i = 0; i < numberOfNumericalBodies; i++ )
        {
            centralBodies[ i ] = "Sun";
            //        if( i == 2 )
            //        {
            //            centralBodies[ i ] = "Earth";
            //        }
            //        else
            {
                centralBodies[ i ] = "Sun";
            }
            centralBodyMap[ bodiesToPropagate[ i ] ] = centralBodies[ i ];
        }

        // Get initial states for bodies.
        Eigen::VectorXd systemInitialState = Eigen::VectorXd( bodiesToPropagate.size( ) * 6 );
        for( unsigned int i = 0; i < numberOfNumericalBodies ; i++ )
        {
            systemInitialState.segment( i * 6 , 6 ) =
                    bodies.at( bodiesToPropagate[ i ] )->getStateInBaseFrameFromEphemeris( initialEphemerisTime ) -
                    bodies.at( centralBodies[ i ] )->getStateInBaseFrameFromEphemeris( initialEphemerisTime );
        }

        // Avoid degradation of performance in Kepler element conversions
        Eigen::Vector6d earthInitialKeplerElements =
                convertCartesianToKeplerianElements(
                    Eigen::Vector6d( systemInitialState.segment( 0, 6 ) ), bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) +
                    bodies.at( "Sun" )->getGravityFieldModel( )->getGravitationalParameter( ) );

        earthInitialKeplerElements( 2 ) = mathematical_constants::PI - earthInitialKeplerElements( 2 );
        earthInitialKeplerElements( 4 ) = earthInitialKeplerElements( 4 ) - 0.4;
        systemInitialState.segment( 0, 6 ) =
                convertKeplerianToCartesianElements(
                    earthInitialKeplerElements, bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) +
                    bodies.at( "Sun" )->getGravityFieldModel( )->getGravitationalParameter( ) );

        // Create acceleratiuon models.
        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodies, accelerationMap, centralBodyMap );

        // Create integrator settings.
        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< IntegratorSettings< > >
                ( rungeKutta4,
                  initialEphemerisTime, 250.0 );

        // Create propagation settings (Cowell)
        std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, finalEphemerisTime );

        // Propagate orbit with Cowell method
        SingleArcDynamicsSimulator< double > dynamicsSimulator2(
                    bodies, integratorSettings, propagatorSettings, true, false, true );

        // Define ephemeris interrogation settings.
        double initialTestTime = initialEphemerisTime;
        double finalTestTime = finalEphemerisTime;
        double testTimeStep = 1000.0;

        // Get resutls of Cowell integration at given times.
        double currentTestTime = initialTestTime;
        std::map< double, Eigen::Matrix< double, 18, 1 > > cowellIntegrationResults;
        bodies.at( "Earth" )->recomputeStateOnNextCall( );
        bodies.at( "Mars" )->recomputeStateOnNextCall( );
        bodies.at( "Venus" )->recomputeStateOnNextCall( );

        while( currentTestTime < finalTestTime )
        {
            cowellIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                    bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( currentTestTime );
            cowellIntegrationResults[ currentTestTime ].segment( 6, 6 ) =
                    bodies.at( "Mars" )->getStateInBaseFrameFromEphemeris( currentTestTime );
            cowellIntegrationResults[ currentTestTime ].segment( 12, 6 ) =
                    bodies.at( "Venus" )->getStateInBaseFrameFromEphemeris( currentTestTime );

            currentTestTime += testTimeStep;
        }

        TranslationalPropagatorType translationalPropagatorType = undefined_translational_propagator;
        if( propagatorType == 0 )
        {
            translationalPropagatorType = gauss_modified_equinoctial;
        }
        else
        {
            translationalPropagatorType = gauss_keplerian;
        }

        // Create propagation settings (Gauss)
        propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, finalEphemerisTime, translationalPropagatorType );

        // Propagate orbit with Gauss method
        SingleArcDynamicsSimulator< double > dynamicsSimulator(
                    bodies, integratorSettings, propagatorSettings, true, false, true );

        // Get resutls of Gauss integration at given times.
        currentTestTime = initialTestTime;
        std::map< double, Eigen::Matrix< double, 18, 1 > > gaussIntegrationResults;
        while( currentTestTime < finalTestTime )
        {
            gaussIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                    bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( currentTestTime );
            gaussIntegrationResults[ currentTestTime ].segment( 6, 6 ) =
                    bodies.at( "Mars" )->getStateInBaseFrameFromEphemeris( currentTestTime );
            gaussIntegrationResults[ currentTestTime ].segment( 12, 6 ) =
                    bodies.at( "Venus" )->getStateInBaseFrameFromEphemeris( currentTestTime );
            currentTestTime += testTimeStep;
        }

        // Compare results of Cowell and Gauss propagations
        std::map< double, Eigen::Matrix< double, 18, 1 > >::iterator gaussIterator = gaussIntegrationResults.begin( );
        Eigen::Vector6d currentCartesianState, currentKeplerianStateDifference;
        std::map< double, Eigen::Matrix< double, 18, 1 > >::iterator cowellIterator = cowellIntegrationResults.begin( );
        for( unsigned int i = 0; i < gaussIntegrationResults.size( ); i++ )
        {

            for( int j= 0; j< 3; j++ )
            {
                BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.05 );
            }

            for( int j = 6; j < 9; j++ )
            {
                BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.05 );
            }

            for( int j = 12; j < 15; j++ )
            {
                BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.075 );
            }

            for( int j = 3; j < 6; j++ )
            {
                BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-8 );
            }

            for( int j = 9; j < 12; j++ )
            {
                BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-8 );

            }

            for( int j = 15; j < 18; j++ )
            {
                BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-6 );
            }
            gaussIterator++;
            cowellIterator++;
        }
    }
}

// Test Gauss propagator for point mass, and spherical harmonics central body.
BOOST_AUTO_TEST_CASE( testGaussPopagatorForSphericalHarmonicCentralBodies )
{
    using namespace tudat;
    using namespace simulation_setup;
    using namespace propagators;
    using namespace numerical_integrators;
    using namespace orbital_element_conversions;
    using namespace basic_mathematics;
    using namespace gravitation;
    using namespace numerical_integrators;

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation time settings.
    const double simulationStartEpoch = 0.0;
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    for( int propagatorType = 0; propagatorType < 2; propagatorType++ )
    {
        TranslationalPropagatorType translationalPropagatorType;
        if( propagatorType == 0 )
        {
            translationalPropagatorType = gauss_modified_equinoctial;
        }
        else
        {
            translationalPropagatorType = gauss_keplerian;
        }
        for( unsigned int simulationCase = 0; simulationCase < 4; simulationCase++ )
        {
            std::cout << "Simulation case : " << simulationCase << std::endl << std::endl;

            // Define body settings for simulation.
            std::vector< std::string > bodiesToCreate;
            bodiesToCreate.push_back( "Sun" );
            bodiesToCreate.push_back( "Earth" );
            bodiesToCreate.push_back( "Moon" );
            bodiesToCreate.push_back( "Mars" );
            bodiesToCreate.push_back( "Venus" );

            // Create body objects.
            BodyListSettings bodySettings =
                    getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0,
                                            "SSB", "J2000" );
            for( unsigned int i = 0; i < bodiesToCreate.size( ); i++ )
            {
                bodySettings.at( bodiesToCreate.at( i ) )->ephemerisSettings->resetFrameOrientation( "J2000" );
                bodySettings.at( bodiesToCreate.at( i ) )->rotationModelSettings->resetOriginalFrame( "J2000" );
            }
            SystemOfBodies bodies = createSystemOfBodies( bodySettings );

            // Create spacecraft object.
            bodies.createEmptyBody( "Vehicle" );
            bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 );
            std::shared_ptr< RadiationPressureInterfaceSettings > vehicleRadiationPressureSettings =
                    std::make_shared< CannonBallRadiationPressureInterfaceSettings >(
                        "Sun", 4.0, 1.2, std::vector< std::string >{ "Earth" } );
            bodies.at( "Vehicle" )->setRadiationPressureInterface(
                        "Sun", createRadiationPressureInterface(
                            vehicleRadiationPressureSettings, "Vehicle", bodies ) );

            // Define propagator settings variables.
            SelectedAccelerationMap accelerationMap;
            std::vector< std::string > bodiesToPropagate;
            std::vector< std::string > centralBodies;

            // Define propagation settings.
            std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;

            // Use only central gravity for Earth
            if( simulationCase < 2 )
            {
                accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                                 basic_astrodynamics::point_mass_gravity ) );
            }
            // Use spherical harmonics for Earth
            else
            {
                accelerationsOfVehicle[ "Earth" ].push_back(
                            std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ) );

            }

            // Use perturbations other than Earth gravity
            if( simulationCase % 2 == 0 )
            {
                accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                               basic_astrodynamics::point_mass_gravity ) );
                accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                                basic_astrodynamics::point_mass_gravity ) );
                accelerationsOfVehicle[ "Mars" ].push_back( std::make_shared< AccelerationSettings >(
                                                                basic_astrodynamics::point_mass_gravity ) );
                accelerationsOfVehicle[ "Venus" ].push_back( std::make_shared< AccelerationSettings >(
                                                                 basic_astrodynamics::point_mass_gravity ) );
                accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                               basic_astrodynamics::cannon_ball_radiation_pressure ) );
            }
            accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;
            bodiesToPropagate.push_back( "Vehicle" );
            centralBodies.push_back( "Earth" );
            basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                        bodies, accelerationMap, bodiesToPropagate, centralBodies );

            // Set Keplerian elements for Vehicle.
            Eigen::Vector6d vehicleInitialStateInKeplerianElements;
            vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = 8000.0E3;
            vehicleInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
            vehicleInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
            vehicleInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
                    = unit_conversions::convertDegreesToRadians( 235.7 );
            vehicleInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                    = unit_conversions::convertDegreesToRadians( 23.4 );
            vehicleInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

            double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
            const Eigen::Vector6d vehicleInitialState = convertKeplerianToCartesianElements(
                        vehicleInitialStateInKeplerianElements, earthGravitationalParameter );

            // Define propagator settings (Cowell)
            std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                    std::make_shared< TranslationalStatePropagatorSettings< double > >
                    ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, simulationEndEpoch );

            // Define integrator settings.
            const double fixedStepSize = 5.0;
            std::shared_ptr< IntegratorSettings< > > integratorSettings =
                    std::make_shared< IntegratorSettings< > >
                    ( rungeKutta4, 0.0, fixedStepSize );

            // Propagate orbit with Cowell method
            SingleArcDynamicsSimulator< double > dynamicsSimulator2(
                        bodies, integratorSettings, propagatorSettings, true, false, true );

            // Define ephemeris interrogation settings.
            double initialTestTime = simulationStartEpoch;
            double finalTestTime = simulationEndEpoch - 10.0 * fixedStepSize;
            double testTimeStep = 1.0E4;

            // Get resutls of Cowell integration at given times.
            double currentTestTime = initialTestTime;
            std::map< double, Eigen::Matrix< double, 6, 1 > > cowellIntegrationResults;
            while( currentTestTime < finalTestTime )
            {
                cowellIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                        bodies.at( "Vehicle" )->getEphemeris( )->getCartesianState( currentTestTime );

                currentTestTime += testTimeStep;
            }

            // Create propagation settings (Gauss)
            propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > >
                    ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, simulationEndEpoch, translationalPropagatorType );

            // Propagate orbit with Gauss method
            SingleArcDynamicsSimulator< double > dynamicsSimulator(
                        bodies, integratorSettings, propagatorSettings, true, false, true );

            // Get resutls of Gauss integration at given times.
            currentTestTime = initialTestTime;
            std::map< double, Eigen::Matrix< double, 6, 1 > > gaussIntegrationResults;
            while( currentTestTime < finalTestTime )
            {
                gaussIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                        bodies.at( "Vehicle" )->getEphemeris( )->getCartesianState( currentTestTime );
                currentTestTime += testTimeStep;
            }

            // Compare results of Cowell and Gauss propagations
            std::map< double, Eigen::Matrix< double, 6, 1 > >::iterator gaussIterator = gaussIntegrationResults.begin( );
            std::map< double, Eigen::Matrix< double, 6, 1 > >::iterator cowellIterator = cowellIntegrationResults.begin( );
            for( unsigned int i = 0; i < gaussIntegrationResults.size( ); i++ )
            {
                for( int j= 0; j< 3; j++ )
                {
                    BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second )( j ), 0.02 );
                }

                for( int j = 3; j < 6; j++ )
                {
                    BOOST_CHECK_SMALL( ( gaussIterator->second - cowellIterator->second )( j ), 1.0E-5 );

                }
                gaussIterator++;
                cowellIterator++;
            }
        }
    }
}
BOOST_AUTO_TEST_SUITE_END( )

}

}