Implemented unfinished function except for run. Added checks if p == null to avoid null pointer exceptions.