KdTree implemented. 75% passed