Added distance calculation with tests