Refactor and add mathcheck

Unnoticed commented line? If it's not needed I'd remove it.