Move some functionality to util.rs, add test