A* Finish

Implemented A* using heuristics