Started basic Astar implementation