Thats really not trivial and more complicated than I thought because of edge cases (not covered in your sample code)

So I can fully recommend using an already tested and optimized graph module because all solutions I had so far need to be rewritten for performance reasons...