Shortest path works. Fixed heroPos X,Y bug