Implement matrices as templates (and add tests)