Found safety overrun issue: duplicates were being added to open_heap. Added open_set check to prevent this yet still try to minimize additional performance issues.