Algorithms: improved A* and added unit tests