Working kd-tree implementation -- edge cases need to be handled