Was not passing properly and getting stuck sometimes, so modify the checking and route creation a bit to prevent it. Seems to work.