Tidied, uses heapq
Split algorithm from printouts. No longer passes the tutorial but is a better search
Added various heuristics