Fix bug with closed set adding to first position (breaks algorithm)