Added A* code
A_star stuff