Implemented astar fathfinding