Tidy up
submatrix and cscToArray functions
Tidy up