Started to implement A*

Fixed tests.py to be better.