Added priority queues, start of LPA* logic.