Halfway through implementing A*