Write code KdTree (without testing)