We finalzed the function that returns the diagonial from the sparse matrix. We are not sure if it is completetly right