A* implemented and tested