A* pathfinding now works. Fixed a few issues with the PriorityQueue and altered GridNode to make immutable class