Reworked walking algo