Written unit tests for math part.
Updated to push