MathUtil finished
Tests added