Fix distance measurement bug, refactor code.