Some bug fixing. I'm not sure if it's taking shortest path?

A* might not be taking shortest path. Revisit node.cost part of
algorithm