Refactor and added A* test