Completed a-star search
Fixed a problem with a-star where nodes were being appended incorrectly
Issue: cannot print a-star path. Check cSpace, looks like the parent node of each node is not being correctly added.