Fixed issue with endless pathfining
Self-sorting priority queue was implemented