Fix up stupid bugs (priority queue ordered the wrong way...), now sort of almost works some times!