Sergeanur Move a bunch of math to cpp files + small fixes