Add non-square and big matrices for diagonal tests
Fix and refactor diagonal mapper.
Merge pull request #165 from rgardner/add-diagonal

Add diagonal