#pragma once
#include <KDTree_fwd.hpp>
#include <assert.hpp>

namespace meshless {
	std::pair<std::vector<int>, std::vector<double>> KDTree::query(const Eigen::Vector3d& point, int k)const {

		assert_msg(point.array().isFinite().prod() == 1, "Invalid point.");
		std::vector<int> ret_index(k);
		std::vector<double> out_dist_sqr(k);
		int actual_k = tree.knnSearch(point.data(), k, &ret_index[0], &out_dist_sqr[0]);
		assert_msg(actual_k == k, "There were not enough points in the tree, you requested %d "
			"points, the tree only contains %d points.", k, actual_k);
		return { ret_index, out_dist_sqr };
	}

	std::pair<std::vector<int>, std::vector<double>> KDTree::query(const Eigen::Vector3d& point, const double& radius_squared)const {
		assert_msg(point.array().isFinite().prod() == 1, "Invalid point.");
		std::vector<std::pair<int, double>> idx_dist;
		int k = tree.radiusSearch(point.data(), radius_squared, idx_dist, nanoflann::SearchParams());
		std::vector<int> idx(k); std::vector<double> dists(k);
		for(int i = 0; i < k; ++i) {
			std::tie(idx[i], dists[i]) = idx_dist[i];
		}
		return { idx, dists };
	}



};