Fixed AStar and optimised it a bit.