Start KDTree implementation. Demo shows search is buggy