Refactored the math to be more object oriented. Fixes #2