initial A-star implementation (heuristic not working at the moment)