addDiagonal function completed