CS 182: Assignment 4, Implement Graph plan algorithm