A star solution maze algorithm C++