Doing some basic a-star in c++