added isEmpty() method to queue
should now be correct A* implementation, implemented after the pseudocode on wikipedia