Rewrite kdtree

Required for O(n*log n) building. But it is broken for now.
Print some more info
Mess around with indexing

Still broken
Fix indexing