Refactor and add mathcheck

Nit: I'd put the 'then' keyword at the same level as 'if', like in all other 'if' statements.