Adding initial work on Astar algorithm