Solve KD algorithm bug