Pathfinding officially working.

Added toString() and updated Java Test.