Implementing AStarCpp on square grid