Finish KD tree implementation and tests
Clean up more of the older code. rework the "reference" system