Shortest Path in a Rooting Problem Code C/C++