Fixed and improved route finding