Made hacky fix to findOptimalRoute algo