Shortest Path Algorithm Leetcode Two