Implemented 2d Astar