Working pathfinder, it sucks though not closing #2