Add solution to 041
Add utility functions