A - star completed and correct (but it needs some refactor)