#pragma once
#include "nanoflann.hpp"
#include <array>
#include <iosfwd>
#include "PointCloud.hpp"

namespace meshless {
	class KDTreeMutable {

	private:
		typedef nanoflann::KDTreeSingleIndexDynamicAdaptor<nanoflann::L2_Simple_Adaptor<double, PointCloud>, PointCloud, 3, int> kd_tree_t;
		int size_;
		PointCloud points_;
		kd_tree_t tree;

	public:
		explicit KDTreeMutable(const std::vector<Eigen::Vector3d>& points) :points_(points), size_(points.size()), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {}

		KDTreeMutable() :points_(), size_(0), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {}

		void reset(const std::vector<Eigen::Vector3d>& points) {
			points_.setPts(points);
			size_ = points.size();
			tree.reset();

		}

		void insert(const Eigen::Vector3d& point);
		void insert(const std::vector<Eigen::Vector3d>& points);

		/// Check if any point exists in sphere centered at `p` with radius `r`.
		bool existsPointInSphere(const Eigen::Vector3d& p, double r) {
			if(size_ == 0)
				return false;
			return query(p).second[0] <= r * r;
		}

		/**
		 * Removes a point with given index from the tree. The indexes of the points are given
		 * sequentially at insertion and do not change. The removal is lazy and point still takes
		 * up memory.
		 */
		void remove(int index) {
			size_ -= tree.removePoint(index);
		}

		/// Returns number of points in the tree.
		int size() const {
			return size_;
		}

		/**
		 * Find `k` nearest neighbors to given point.
		 * @param point Find closest points to this point.
		 * @param k How many nearest points to find.
		 *
		 * @return A pair of two vectors of size `k` containing
		 * indices of nearest neighbors and squared distances to said neighbors.
		 * @throw Assertion fails if there are not enough points in the tree.
		 */
		std::pair<std::vector<int>, std::vector<double>> query(const Eigen::Vector3d& point, int k = 1);



	};



};