My implementation of A*