#pragma once
#include <PointCloud.hpp>
#include <nanoflann.hpp>
#include <Eigen/Core>

namespace meshless {
	class KDTree {
	private:
		typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, PointCloud>, PointCloud, 3, int> kd_tree_t;
		PointCloud points_;
		kd_tree_t tree;
	public:
		explicit KDTree(const std::vector<Eigen::Vector3d>& points) :points_(points), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {
			tree.buildIndex();
		}

		KDTree() :points_(), tree(3, points_, nanoflann::KDTreeSingleIndexAdaptorParams(20)) {}

		void reset(const std::vector<Eigen::Vector3d>& points) {
			points_.setPts(points);
			tree.buildIndex();
		}
		std::pair<std::vector<int>, std::vector<double>> query(const Eigen::Vector3d& point, int k = 1)const;

		std::pair<std::vector<int>, std::vector<double>> query(const Eigen::Vector3d& point, const double& radius_squared) const;

		Eigen::Vector3d get(int index) const {
			return points_.get(index);
		}

		std::vector<Eigen::Vector3d> get(const std::vector<int>& indexes) const {
			const int n = indexes.size();
			std::vector<Eigen::Vector3d> result(n);
			for(int i = 0; i < n; i++) {
				result[i] = points_.get(indexes[i]);
			}
			return result;
		}

		int size() const {
			return points_.kdtree_get_point_count();
		}


	};



};