Bunch of math stuff

Remind to fix and test this stuff