Smarter path finding algorithm