Implement diag in C