Improve algorithm and add more tests for step detection