Implement better maths and fix a couple of bugs

Beta - v0.3 - lookin' good.