Refactored code and fixed some small bugs when calculating distance.