Add Node data structure
Add FindShortestPath function