A* implemented, need to test