add function add_diag (tested)