Cleanup
Cleanup, added rest of simple math functions