Add unit tests for math