(WIP) Cleanup, new kd-tree design