Math helpers update

inlined Math helper methods and added unit tests for them