Fix distance calculations

Also added tests to check if the distance is correctly calculated.