Update kdtree.c with untested findNearest function