#include "ndt_cpu/VoxelGrid.h"
#include "ndt_cpu/debug.h"
#include <math.h>
#include <limits>
#include <inttypes.h>

#include <vector>
#include <cmath>

#include <stdio.h>
#include <sys/time.h>

#include "ndt_cpu/SymmetricEigenSolver.h"

namespace cpu {

template <typename PointSourceType>
VoxelGrid<PointSourceType>::VoxelGrid():
	voxel_num_(0),
	max_x_(FLT_MIN),
	max_y_(FLT_MIN),
	max_z_(FLT_MIN),
	min_x_(FLT_MAX),
	min_y_(FLT_MAX),
	min_z_(FLT_MAX),
	voxel_x_(0),
	voxel_y_(0),
	voxel_z_(0),
	max_b_x_(0),
	max_b_y_(0),
	max_b_z_(0),
	min_b_x_(0),
	min_b_y_(0),
	min_b_z_(0),
	vgrid_x_(0),
	vgrid_y_(0),
	vgrid_z_(0),
	min_points_per_voxel_(6),
	real_max_bx_(INT_MIN),
	real_max_by_(INT_MIN),
	real_max_bz_(INT_MIN),
	real_min_bx_(INT_MAX),
	real_min_by_(INT_MAX),
	real_min_bz_(INT_MAX)
{
	centroid_.reset();
	icovariance_.reset();
	points_id_.reset();
	points_per_voxel_.reset();
	tmp_centroid_.reset();
	tmp_cov_.reset();
};

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::roundUp(int input, int factor)
{
	return (input < 0) ? -((-input) / factor) * factor : ((input + factor - 1) / factor) * factor;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::roundDown(int input, int factor)
{
	return (input < 0) ? -((-input + factor - 1) / factor) * factor : (input / factor) * factor;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::div(int input, int divisor)
{
	return (input < 0) ? -((-input + divisor - 1) / divisor) : input / divisor;
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::initialize()
{
	centroid_.reset();
	centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num_);

	icovariance_.reset();
	icovariance_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num_);

	points_id_.reset();
	points_id_ = boost::make_shared<std::vector<std::vector<int> > >(voxel_num_);

	points_per_voxel_.reset();
	points_per_voxel_ = boost::make_shared<std::vector<int> >(voxel_num_, 0);

	tmp_centroid_.reset();
	tmp_centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num_);

	tmp_cov_.reset();
	tmp_cov_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num_);
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getVoxelNum() const
{
	return voxel_num_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getMaxX() const
{
	return max_x_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getMaxY() const
{
	return max_y_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getMaxZ() const
{
	return max_z_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getMinX() const
{
	return min_x_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getMinY() const
{
	return min_y_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getMinZ() const
{
	return min_z_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getVoxelX() const
{
	return voxel_x_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getVoxelY() const
{
	return voxel_y_;
}

template <typename PointSourceType>
float VoxelGrid<PointSourceType>::getVoxelZ() const
{
	return voxel_z_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getMaxBX() const
{
	return max_b_x_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getMaxBY() const
{
	return max_b_y_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getMaxBZ() const
{
	return max_b_z_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getMinBX() const
{
	return min_b_x_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getMinBY() const
{
	return min_b_y_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getMinBZ() const
{
	return min_b_z_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getVgridX() const
{
	return vgrid_x_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getVgridY() const
{
	return vgrid_y_;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::getVgridZ() const
{
	return vgrid_z_;
}

template <typename PointSourceType>
Eigen::Vector3d VoxelGrid<PointSourceType>::getCentroid(int voxel_id) const
{
	return (*centroid_)[voxel_id];
}

template <typename PointSourceType>
Eigen::Matrix3d VoxelGrid<PointSourceType>::getInverseCovariance(int voxel_id) const
{
	return (*icovariance_)[voxel_id];
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::setLeafSize(float voxel_x, float voxel_y, float voxel_z)
{
	voxel_x_ = voxel_x;
	voxel_y_ = voxel_y;
	voxel_z_ = voxel_z;
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::voxelId(PointSourceType p)
{
	int idx = static_cast<int>(floor(p.x / voxel_x_)) - min_b_x_;
	int idy = static_cast<int>(floor(p.y / voxel_y_)) - min_b_y_;
	int idz = static_cast<int>(floor(p.z / voxel_z_)) - min_b_z_;

	return (idx + idy * vgrid_x_ + idz * vgrid_x_ * vgrid_y_);
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::voxelId(PointSourceType p,
											float voxel_x, float voxel_y, float voxel_z,
											int min_b_x, int min_b_y, int min_b_z,
											int vgrid_x, int vgrid_y, int vgrid_z)
{
	int idx = static_cast<int>(floor(p.x / voxel_x)) - min_b_x;
	int idy = static_cast<int>(floor(p.y / voxel_y)) - min_b_y;
	int idz = static_cast<int>(floor(p.z / voxel_z)) - min_b_z;

	return (idx + idy * vgrid_x + idz * vgrid_x * vgrid_y);
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::voxelId(int idx, int idy, int idz,
										int min_b_x, int min_b_y, int min_b_z,
										int size_x, int size_y, int size_z)
{
	return (idx - min_b_x) + (idy - min_b_y) * size_x + (idz - min_b_z) * size_x * size_y;
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::computeCentroidAndCovariance()
{
	for (int idx = real_min_bx_; idx <= real_max_bx_; idx++)
		for (int idy = real_min_by_; idy <= real_max_by_; idy++)
			for (int idz = real_min_bz_; idz <= real_max_bz_; idz++) {
				int i = voxelId(idx, idy, idz, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
				int ipoint_num = (*points_id_)[i].size();
				double point_num = static_cast<double>(ipoint_num);
				Eigen::Vector3d pt_sum = (*tmp_centroid_)[i];

				if (ipoint_num > 0) {
					(*centroid_)[i] = pt_sum / point_num;
				}

				Eigen::Matrix3d covariance;

				if (ipoint_num >= min_points_per_voxel_) {
					covariance = ((*tmp_cov_)[i] - 2.0 * (pt_sum * (*centroid_)[i].transpose())) / point_num + (*centroid_)[i] * (*centroid_)[i].transpose();
					covariance *= (point_num - 1.0) / point_num;

					SymmetricEigensolver3x3 sv(covariance);

					sv.compute();
					Eigen::Matrix3d evecs = sv.eigenvectors();
					Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();

					if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
						(*points_per_voxel_)[i] = -1;
						continue;
					}

					double min_cov_eigvalue = evals(2, 2) * 0.01;

					if (evals(0, 0) < min_cov_eigvalue) {
						evals(0, 0) = min_cov_eigvalue;

						if (evals(1, 1) < min_cov_eigvalue) {
							evals(1, 1) = min_cov_eigvalue;
						}

						covariance = evecs * evals * evecs.inverse();
					}

					(*icovariance_)[i] = covariance.inverse();
				}
			}
}

//Input are supposed to be in device memory
template <typename PointSourceType>
void VoxelGrid<PointSourceType>::setInput(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud)
{
	if (input_cloud->points.size() > 0) {
		/* If no voxel grid was created, then
		 * build the initial voxel grid and octree
		 */
		source_cloud_ = input_cloud;

		findBoundaries();

		std::vector<Eigen::Vector3i> voxel_ids(input_cloud->points.size());

		for (int i = 0; i < input_cloud->points.size(); i++) {
			Eigen::Vector3i &vid = voxel_ids[i];
			PointSourceType p = input_cloud->points[i];

			vid(0) = static_cast<int>(floor(p.x / voxel_x_));
			vid(1) = static_cast<int>(floor(p.y / voxel_y_));
			vid(2) = static_cast<int>(floor(p.z / voxel_z_));
		}

		octree_.setInput(voxel_ids, input_cloud);

		voxel_ids.clear();

		initialize();

		scatterPointsToVoxelGrid();

		computeCentroidAndCovariance();
	}
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::findBoundaries()
{

	findBoundaries(source_cloud_, max_x_, max_y_, max_z_, min_x_, min_y_, min_z_);

	real_max_bx_ = max_b_x_ = static_cast<int> (floor(max_x_ / voxel_x_));
	real_max_by_ = max_b_y_ = static_cast<int> (floor(max_y_ / voxel_y_));
	real_max_bz_ = max_b_z_ = static_cast<int> (floor(max_z_ / voxel_z_));

	real_min_bx_ = min_b_x_ = static_cast<int> (floor(min_x_ / voxel_x_));
	real_min_by_ = min_b_y_ = static_cast<int> (floor(min_y_ / voxel_y_));
	real_min_bz_ = min_b_z_ = static_cast<int> (floor(min_z_ / voxel_z_));

	/* Allocate a poll of memory that is larger than the requested memory so
	 * we do not have to reallocate buffers when the target cloud is set
	 */
	/* Max bounds round toward plus infinity */
	max_b_x_ = roundUp(max_b_x_, MAX_BX_);
	max_b_y_ = roundUp(max_b_y_, MAX_BY_);
	max_b_z_ = roundUp(max_b_z_, MAX_BZ_);

	/* Min bounds round toward minus infinity */
	min_b_x_ = roundDown(min_b_x_, MAX_BX_);
	min_b_y_ = roundDown(min_b_y_, MAX_BY_);
	min_b_z_ = roundDown(min_b_z_, MAX_BZ_);

	vgrid_x_ = max_b_x_ - min_b_x_ + 1;
	vgrid_y_ = max_b_y_ - min_b_y_ + 1;
	vgrid_z_ = max_b_z_ - min_b_z_ + 1;

	if (vgrid_x_ > 0 && vgrid_y_ > 0 && vgrid_z_ > 0) {
		voxel_num_ = vgrid_x_ * vgrid_y_ * vgrid_z_;
	} else {
		voxel_num_ = 0;
	}
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::findBoundaries(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud,
													float &max_x, float &max_y, float &max_z,
													float &min_x, float &min_y, float &min_z)
{

	max_x = max_y = max_z = -FLT_MAX;
	min_x = min_y = min_z = FLT_MAX;

	for (int i = 0; i < input_cloud->points.size(); i++) {
		float x = input_cloud->points[i].x;
		float y = input_cloud->points[i].y;
		float z = input_cloud->points[i].z;

		max_x = (max_x > x) ? max_x : x;
		max_y = (max_y > y) ? max_y : y;
		max_z = (max_z > z) ? max_z : z;

		min_x = (min_x < x) ? min_x : x;
		min_y = (min_y < y) ? min_y : y;
		min_z = (min_z < z) ? min_z : z;
	}
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::radiusSearch(PointSourceType p, float radius, std::vector<int> &voxel_ids, int max_nn)
{
	float t_x = p.x;
	float t_y = p.y;
	float t_z = p.z;

	int max_id_x = static_cast<int>(floor((t_x + radius) / voxel_x_));
	int max_id_y = static_cast<int>(floor((t_y + radius) / voxel_y_));
	int max_id_z = static_cast<int>(floor((t_z + radius) / voxel_z_));

	int min_id_x = static_cast<int>(floor((t_x - radius) / voxel_x_));
	int min_id_y = static_cast<int>(floor((t_y - radius) / voxel_y_));
	int min_id_z = static_cast<int>(floor((t_z - radius) / voxel_z_));

	/* Find intersection of the cube containing
	 * the NN sphere of the point and the voxel grid
	 */
	max_id_x = (max_id_x > real_max_bx_) ? real_max_bx_ : max_id_x;
	max_id_y = (max_id_y > real_max_by_) ? real_max_by_ : max_id_y;
	max_id_z = (max_id_z > real_max_bz_) ? real_max_bz_ : max_id_z;

	min_id_x = (min_id_x < real_min_bx_) ? real_min_bx_ : min_id_x;
	min_id_y = (min_id_y < real_min_by_) ? real_min_by_ : min_id_y;
	min_id_z = (min_id_z < real_min_bz_) ? real_min_bz_ : min_id_z;
	int nn = 0;

	for (int idx = min_id_x; idx <= max_id_x && nn < max_nn; idx++) {
		for (int idy = min_id_y; idy <= max_id_y && nn < max_nn; idy++) {
			for (int idz = min_id_z; idz <= max_id_z && nn < max_nn; idz++) {
				int vid = voxelId(idx, idy, idz,
									min_b_x_, min_b_y_, min_b_z_,
									vgrid_x_, vgrid_y_, vgrid_z_);

				if ((*points_per_voxel_)[vid] >= min_points_per_voxel_) {
					double cx = (*centroid_)[vid](0) - static_cast<double>(t_x);
					double cy = (*centroid_)[vid](1) - static_cast<double>(t_y);
					double cz = (*centroid_)[vid](2) - static_cast<double>(t_z);

					double distance = sqrt(cx * cx + cy * cy + cz * cz);

					if (distance < radius) {
						nn++;
						voxel_ids.push_back(vid);
					}
				}
			}
		}
	}
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::scatterPointsToVoxelGrid()
{

	for (int pid = 0; pid < source_cloud_->points.size(); pid++) {
		int vid = voxelId(source_cloud_->points[pid]);
		PointSourceType p = source_cloud_->points[pid];

		Eigen::Vector3d p3d(p.x, p.y, p.z);

		if ((*points_id_)[vid].size() == 0) {
			(*centroid_)[vid].setZero();
			(*points_per_voxel_)[vid] = 0;
			(*tmp_centroid_)[vid].setZero();
			(*tmp_cov_)[vid].setIdentity();
		}

		(*tmp_centroid_)[vid] += p3d;
		(*tmp_cov_)[vid] += p3d * p3d.transpose();
		(*points_id_)[vid].push_back(pid);
		(*points_per_voxel_)[vid]++;
	}
}

template <typename PointSourceType>
int VoxelGrid<PointSourceType>::nearestVoxel(PointSourceType query_point, Eigen::Matrix<float, 6, 1> boundaries, float max_range)
{
	// Index of the origin of the circle (query point)
	float qx = query_point.x;
	float qy = query_point.y;
	float qz = query_point.z;

	int lower_x = static_cast<int>(floor(boundaries(0) / voxel_x_));
	int lower_y = static_cast<int>(floor(boundaries(1) / voxel_y_));
	int lower_z = static_cast<int>(floor(boundaries(2) / voxel_z_));

	int upper_x = static_cast<int>(floor(boundaries(3) / voxel_x_));
	int upper_y = static_cast<int>(floor(boundaries(4) / voxel_y_));
	int upper_z = static_cast<int>(floor(boundaries(5) / voxel_z_));

	double min_dist = DBL_MAX;
	int nn_vid = -1;

	for (int i = lower_x; i <= upper_x; i++) {
		for (int j = lower_y; j <= upper_y; j++) {
			for (int k = lower_z; k <= upper_z; k++) {
				int vid = voxelId(i, j, k, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
				Eigen::Vector3d c = (*centroid_)[vid];

				if ((*points_id_)[vid].size() > 0) {
					double cur_dist = sqrt((qx - c(0)) * (qx - c(0)) + (qy - c(1)) * (qy - c(1)) + (qz - c(2)) * (qz - c(2)));

					if (cur_dist < min_dist) {
						min_dist = cur_dist;
						nn_vid = vid;
					}
				}
			}
		}
	}

	return nn_vid;
}

template <typename PointSourceType>
double VoxelGrid<PointSourceType>::nearestNeighborDistance(PointSourceType q, float max_range)
{
	Eigen::Matrix<float, 6, 1> nn_node_bounds;

	nn_node_bounds = octree_.nearestOctreeNode(q);

	int nn_vid = nearestVoxel(q, nn_node_bounds, max_range);

	Eigen::Vector3d c = (*centroid_)[nn_vid];
	double min_dist = sqrt((q.x - c(0)) * (q.x - c(0)) + (q.y - c(1)) * (q.y - c(1)) + (q.z - c(2)) * (q.z - c(2)));

	if (min_dist >= max_range) {
		return DBL_MAX;
	}

	return min_dist;

}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::updateBoundaries(float max_x, float max_y, float max_z,
													float min_x, float min_y, float min_z)
{

	float new_max_x, new_max_y, new_max_z;
	float new_min_x, new_min_y, new_min_z;

	new_max_x = (max_x_ >= max_x) ? max_x_ : max_x;
	new_max_y = (max_y_ >= max_y) ? max_y_ : max_y;
	new_max_z = (max_z_ >= max_z) ? max_z_ : max_z;

	new_min_x = (min_x_ <= min_x) ? min_x_ : min_x;
	new_min_y = (min_y_ <= min_y) ? min_y_ : min_y;
	new_min_z = (min_z_ <= min_z) ? min_z_ : min_z;

	/* If the boundaries change, then we need to extend the list of voxels */
	if (new_max_x > max_x_ || new_max_y > max_y_ || new_max_z > max_z_ ||
			new_min_x < min_x_ || new_min_y < min_y_ || new_min_z < min_z_) {

		int max_b_x = static_cast<int> (floor(new_max_x / voxel_x_));
		int max_b_y = static_cast<int> (floor(new_max_y / voxel_y_));
		int max_b_z = static_cast<int> (floor(new_max_z / voxel_z_));

		int min_b_x = static_cast<int> (floor(new_min_x / voxel_x_));
		int min_b_y = static_cast<int> (floor(new_min_y / voxel_y_));
		int min_b_z = static_cast<int> (floor(new_min_z / voxel_z_));

		int real_max_bx = max_b_x;
		int real_max_by = max_b_y;
		int real_max_bz = max_b_z;

		int real_min_bx = min_b_x;
		int real_min_by = min_b_y;
		int real_min_bz = min_b_z;

		/* Max bounds round toward plus infinity */
		max_b_x = roundUp(max_b_x, MAX_BX_);
		max_b_y = roundUp(max_b_y, MAX_BY_);
		max_b_z = roundUp(max_b_z, MAX_BZ_);

		/* Min bounds round toward minus infinity */
		min_b_x = roundDown(min_b_x, MAX_BX_);
		min_b_y = roundDown(min_b_y, MAX_BY_);
		min_b_z = roundDown(min_b_z, MAX_BZ_);

		if (max_b_x > max_b_x_ || max_b_y > max_b_y_ || max_b_z > max_b_z_ ||
				min_b_x < min_b_x_ || min_b_y < min_b_y_ || min_b_z < min_b_z_) {
			int vgrid_x = max_b_x - min_b_x + 1;
			int vgrid_y = max_b_y - min_b_y + 1;
			int vgrid_z = max_b_z - min_b_z + 1;

			int voxel_num = vgrid_x * vgrid_y * vgrid_z;

			boost::shared_ptr<std::vector<Eigen::Vector3d> > old_centroid = centroid_;
			boost::shared_ptr<std::vector<Eigen::Matrix3d> > old_icovariance = icovariance_;
			boost::shared_ptr<std::vector<std::vector<int> > > old_points_id = points_id_;
			boost::shared_ptr<std::vector<int> > old_points_per_voxel = points_per_voxel_;

			boost::shared_ptr<std::vector<Eigen::Vector3d> > old_tmp_centroid = tmp_centroid_;
			boost::shared_ptr<std::vector<Eigen::Matrix3d> > old_tmp_cov = tmp_cov_;

			centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num);
			icovariance_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num);
			points_id_ = boost::make_shared<std::vector<std::vector<int> > >(voxel_num);
			points_per_voxel_ = boost::make_shared<std::vector<int> >(voxel_num, 0);
			tmp_centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num);
			tmp_cov_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num);

			// Move the old non-empty voxels to the new list of voxels

			int idx, idy, idz;
			int old_id, new_id;

			for (int idx = real_min_bx_; idx <= real_max_bx_; idx++) {
				for (int idy = real_min_by_; idy <= real_max_by_; idy++) {
					for (int idz = real_min_bz_; idz <= real_max_bz_; idz++) {
						old_id = voxelId(idx, idy, idz,
											min_b_x_, min_b_y_, min_b_z_,
											vgrid_x_, vgrid_y_, vgrid_z_);
						new_id = voxelId(idx, idy, idz,
											min_b_x, min_b_y, min_b_z,
											vgrid_x, vgrid_y, vgrid_z);

						if ((*old_points_id)[old_id].size() > 0) {
							(*points_per_voxel_)[new_id] = (*old_points_per_voxel)[old_id];
							(*centroid_)[new_id] = (*old_centroid)[old_id];
							(*icovariance_)[new_id] = (*old_icovariance)[old_id];
							(*points_id_)[new_id] = (*old_points_id)[old_id];

							(*tmp_centroid_)[new_id] = (*old_tmp_centroid)[old_id];
							(*tmp_cov_)[new_id] = (*old_tmp_cov)[old_id];
						}
					}
				}
			}

			// Update boundaries of voxels
			max_b_x_ = max_b_x;
			max_b_y_ = max_b_y;
			max_b_z_ = max_b_z;

			min_b_x_ = min_b_x;
			min_b_y_ = min_b_y;
			min_b_z_ = min_b_z;

			vgrid_x_ = vgrid_x;
			vgrid_y_ = vgrid_y;
			vgrid_z_ = vgrid_z;

			voxel_num_ = voxel_num;

		}
		// Update actual voxel boundaries
		real_min_bx_ = real_min_bx;
		real_min_by_ = real_min_by;
		real_min_bz_ = real_min_bz;

		real_max_bx_ = real_max_bx;
		real_max_by_ = real_max_by;
		real_max_bz_ = real_max_bz;


		//Update boundaries of points
		max_x_ = new_max_x;
		max_y_ = new_max_y;
		max_z_ = new_max_z;

		min_x_ = new_min_x;
		min_y_ = new_min_y;
		min_z_ = new_min_z;

	}
}


template <typename PointSourceType>
void VoxelGrid<PointSourceType>::update(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud)
{
	if (new_cloud->points.size() <= 0) {
		return;
	}

	float new_max_x, new_max_y, new_max_z;
	float new_min_x, new_min_y, new_min_z;

	// Find boundaries of the new point cloud
	findBoundaries(new_cloud, new_max_x, new_max_y, new_max_z, new_min_x, new_min_y, new_min_z);

	/* Update current boundaries of the voxel grid
	 * Also allocate buffer for new voxel grid and
	 * octree and move the current voxel grid and
	 * octree to the new buffer if necessary
	 */
	updateBoundaries(new_max_x, new_max_y, new_max_z, new_min_x, new_min_y, new_min_z);

	/* Update changed voxels (voxels that contains new points).
	 * Update centroids of voxels and their covariance matrixes
	 * as well as inverse covariance matrixes */
	updateVoxelContent(new_cloud);

	/* Update octree */
	std::vector<Eigen::Vector3i> new_voxel_id(new_cloud->points.size());

	for (int i = 0; i < new_cloud->points.size(); i++) {
		Eigen::Vector3i &vid = new_voxel_id[i];
		PointSourceType p = new_cloud->points[i];

		vid(0) = static_cast<int>(floor(p.x / voxel_x_));
		vid(1) = static_cast<int>(floor(p.y / voxel_y_));
		vid(2) = static_cast<int>(floor(p.z / voxel_z_));
	}

	octree_.update(new_voxel_id, new_cloud);

	*source_cloud_ += *new_cloud;
}

template <typename PointSourceType>
void VoxelGrid<PointSourceType>::updateVoxelContent(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud)
{
	int total_points_num = source_cloud_->points.size();

	for (int i = 0; i < new_cloud->points.size(); i++) {
		PointSourceType p = new_cloud->points[i];
		Eigen::Vector3d p3d(p.x, p.y, p.z);
		int vx = static_cast<int>(floor(p.x / voxel_x_));
		int vy = static_cast<int>(floor(p.y / voxel_y_));
		int vz = static_cast<int>(floor(p.z / voxel_z_));
		int vid = voxelId(vx, vy, vz, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
		std::vector<int> &tmp_pid = (*points_id_)[vid];

		if (tmp_pid.size() == 0) {
			(*centroid_)[vid].setZero();
			(*tmp_cov_)[vid].setIdentity();
			(*tmp_centroid_)[vid].setZero();
		}

		(*tmp_centroid_)[vid] += p3d;
		(*tmp_cov_)[vid] += p3d * p3d.transpose();
		(*points_id_)[vid].push_back(total_points_num + i);

		// Update centroids
		int ipoint_num = (*points_id_)[vid].size();
		(*centroid_)[vid] = (*tmp_centroid_)[vid] / static_cast<double>(ipoint_num);
		(*points_per_voxel_)[vid] = ipoint_num;


		// Update covariance
		double point_num = static_cast<double>(ipoint_num);
		Eigen::Vector3d pt_sum = (*tmp_centroid_)[vid];

		// Update  centroids
		(*centroid_)[vid] = (*tmp_centroid_)[vid] / point_num;

		if (ipoint_num >= min_points_per_voxel_) {
			Eigen::Matrix3d covariance;
			covariance = ((*tmp_cov_)[vid] - 2.0 * (pt_sum * (*centroid_)[vid].transpose())) / point_num + (*centroid_)[vid] * (*centroid_)[vid].transpose();
			covariance *= (point_num - 1.0) / point_num;

			SymmetricEigensolver3x3 sv(covariance);

			sv.compute();
			Eigen::Matrix3d evecs = sv.eigenvectors();
			Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();

			if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
				(*points_per_voxel_)[vid] = -1;
				continue;
			}

			double min_cov_eigvalue = evals(2, 2) * 0.01;

			if (evals(0, 0) < min_cov_eigvalue) {
				evals(0, 0) = min_cov_eigvalue;

				if (evals(1, 1) < min_cov_eigvalue) {
					evals(1, 1) = min_cov_eigvalue;
				}

				covariance = evecs * evals * evecs.inverse();
			}

			(*icovariance_)[vid] = covariance.inverse();
		}
	}
}

template class VoxelGrid<pcl::PointXYZI>;
template class VoxelGrid<pcl::PointXYZ>;

}