Improved the A* code to obtain proper 'open' and 'closed' lists, improved priority queue