KDTree compiles but needs debugging