Fixed walking algo but he is still oscilating in loop... Maybe map can help