Refactored a little bit and added comments of functions in util.h