Improve steps algorithm implementation

Update a few tests