KdTree - working insert and findNearest