Astar implementation

Astar implementation and tests