Fixes problems with algorithm

[jobh]

Hi,

I really liked your approach to implementing and explaining the A* algorithm.

Now, the problem is that the algorithm as it is does not work correctly. There are two issues:

1. The heuristic is added at each step, so it ends up with really big values. This makes it hard to understand the meaning of the numbers, and can also make A* choose sub-optimal paths.
2. The loop didn't stop when end was reached, so in effect the A* cutoff was not used.

See printout from example:

70   54   60   69   81   92   98   103  113  124  
54   39   46   56   69   84   88   94   103  113  
39   25   33   44   58   71   79   86   94   103  
25   12   21   ##########82   86   96   101  109  
12   S    10   ##########89   92   99   105  112  
21   10   19   ##########95   103  108  110  116  
29   19   27   ##########107  116  111  114  119  
36   27   34   ##########112  117  113  116  120  
42   34   40   ##########123  116  E    116  119  
51   42   47   ##########123  119  116  119  123  

The cost is way too high, and all points are visited.

The root cause of the problem seems to be conflating the actual cost to reach a point with the estimated total cost of the path. This PR tries to make the difference clearer in README, and fixes the implementation by subtracting the previous heuristic before adding the new.

I understand that this repo is intended for demonstrating and teaching the principles, and should be kept as simple as possible (but correct!). If this understanding is incorrect, I can create a PR for another branch with larger changes that makes it usable for generic graphs and heuristics (not only grids).

[pablogsal]

Thanks a lot, this is a fantastic improvement!