Refactoring some code, adding solution to problem 16

Extracting some methods to MathUtils, solving problem 16.