Add queue functions to util & test