Commenting and cleaning up code
Moved utility functions into header