A* implemented
A* and default heuristic fixed