Added A* search, need to find logic error