Fixed algorithm, implemented simple goto(L,D) when idle