Fix bug in generation of SNF and simplify code

The generation of Smith Normal Form matrices seems to be broken, which was revealed by this bug report. I simplified and code and fixed the bug (I think) but I will write some tests before merge to make sure this is actually correct.