Fixed nasty non-initialized integer bug.
Cleaned diag() and shift()