Added some math helper stuff and infastructure beginning