Allow setting diagonal of block matrix
Add tests setting diagonal of block matrix