Adding working A*2 w/ tests