Linear system solve routine implemented, refactoring