small change to pathfind and added test