A*Star Pathfinding now finds correct path

- Forgot the last step of the algorithm by going through each node's
parent node.
- Added the extra calculation needed to see if a node on the open list
could lead to a better path.
- Simplified some of the code to make it easier for people to read.