More code
Start writing test code for the kdtree routine