Working on astar implementation