Added DODiagonal (and general refactoring)