It's kinda working... Still some problem with infinite heuristics in SBPL