Fixed CalcDistance and added automatic stops