Add initial astar code
Added some documentation