Pathfinding Fix + Clean

made to resort the old key, if we update it's cost