Working version of A* implemented.