First pathfinding implementation