add A* algorithm

Add A* path finding algorithm