Update util.h

// [MR] added some bugfixes, mainly to make the Code more readable, function wrappers to avoid typos, some less copy overhead.