Finished implementing KD-Tree