Moved print data function to it's own utilities file under namespace util::

Created a header file for main
Added a test matlab linker.