path following algorithm
new unit test + some refactoring