Finished matrix iterator code (and tested)