Optimized shortest path finding is now working