Change to more generic matrix solver (rather than inverse) and add unit tests