Update find_path function to optimize path