Implemented path trace printout (I hope). Added different heuristics. Probably broke a few things. Also, I'm running python 3 so you'll have to rename queue back to Queue. It doesn't actually work yet.