Squash the bug that caused the infinite while loop
Remove redundant distance checking