Finish math functions and unit test