Added A*

-Needs to fix out of bounds error
-Need to add travel cost to the herusiticcs