Moved around print statements to make things nicer.
Altered the heuristic cost some more. Should be nicer now :D