Added distance calculations and some debug/test methods