Refactor and add mathcheck

Remove commented code.