Add step tolerance. Move optional step behavior to its own method.