Add implementation for find_shortest_path function