working implemented pathfinding