Completed the diagonal function and added tests.

I changed the code to use ```cond``` and the function returns an empty vector for values of k that are out of range