Invert order of route algorithm

[DanielSchiavini]

• Check if route is valid before adding it to the route list
• Compared in this commit with other alternatives
• It seems to make the algorithm ~3 times faster. For example:

[DanielSchiavini]

I'll merge this with #385