Initial version of pathfinding