Implement kdtree build and nearest neighbour search
Add unit test for nearest neighbour search