Add distance and test code