Add better maths and tests