Pathfinding class is working, now remains to do the integration :)