Adding MathUtils class and unit tests