#pragma once
#include "assert.hpp"
#include "KDTreeMutable_fwd.hpp"

namespace meshless {

	void KDTreeMutable::insert(const Eigen::Vector3d& point) {
		assert_msg(point.array().isFinite().prod() == 1, "Invalid point.");
		auto n = points_.kdtree_get_point_count();
		points_.add(point);
		tree.addPoints(n, n);
		++size_;

	}

	void KDTreeMutable::insert(const std::vector<Eigen::Vector3d>& points) {
		auto n = points_.kdtree_get_point_count();
		for(const auto& p : points) {
			assert_msg(p.array().isFinite().prod() == 1, "One of the points is invalid.");
			points_.add(p);
		}
		size_ += points.size();
		tree.addPoints(n, n + points.size() - 1);

	}

	std::pair<std::vector<int>, std::vector<double>> KDTreeMutable::query(const Eigen::Vector3d& point, int k) {
		assert_msg(point.array().isFinite().prod() == 1, "Invalid query point.");
		nanoflann::KNNResultSet<double, int> resultSet(k);
		std::vector<int> ret_index(k);
		std::vector<double> out_dist_sqr(k);
		resultSet.init(&ret_index[0], &out_dist_sqr[0]);
		tree.findNeighbors(resultSet, point.data(), nanoflann::SearchParams(k));
		assert_msg(resultSet.full(), "Not enough points in the tree, you requested %d points, "
			"but the tree contains only %d points.",
			k, size());
		return { ret_index, out_dist_sqr };
	}

};