Bug fix and add MathUtil