/*
	Test of the KDTree library
	https://github.com/jtsiomb/kdtree
*/

#include <kdtree.h>
#include <assert.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <math.h>

struct xyl_point {
	double x;
	double y;
	uint32_t label;
};

int kd_insert_xyl(struct kdtree *tree, struct xyl_point const * pt) {
	return kd_insert(tree, (double *) pt, (void *) (&pt->label));
}

double l2_norm(struct xyl_point * const x1, struct xyl_point * const x2) {
	return sqrt(
		(x1->x - x2->x) * (x1->x - x2->x) +
		(x1->y - x2->y) * (x1->y - x2->y)
	);
}

int main() {
	struct xyl_point points[5] = {
		{0, 0, 0},
		{0, 1, 1},
		{1, 0, 2},
		{1, 1, 3}
	};

	struct kdtree *ptree = kd_create(2);

	for(size_t i = 0; i < sizeof(points); i++) {
		assert(0 == kd_insert_xyl(ptree, points + i));
	}

	{
		struct xyl_point target = {0.75, 0.80, 0};
		struct xyl_point found;
		double dist;
		struct kdres * presults = kd_nearest(ptree, (double*) &target);

		while(!kd_res_end(presults)) {
			found.label = *(uint32_t*) kd_res_item(presults, (double *) &found);
			
			dist = l2_norm(&found, &target);

			printf("found [(%.3f, %.3f), %u] a distance of %.3f\n", found.x, found.y, found.label, dist);

			kd_res_next(presults);
		}

		kd_res_free( presults );
	}

	kd_free(ptree);
}