#9 Added initial pathfinding