Major refactoring - draw and math functions

Some quick hacks made just so the game still works, so things still need
to be changed