Calculating distances first phase
Seems like the step function works fine