First test; test code works.
Implemented my own contains method for visited.
Works for now.
Equals was added to Node.