rearranged some functions and added some actual tests

Changed util to internal package, to follow matlab standard. Added
default tests for both high level functions and internal functions.