Add distance calculator
Fix failing unit test