Improved code - A* with improved implementation and binary heap F-score
list.

Signed-off-by: Nigel <Nigel@10.203.4.182>