Reorganize code and add new matrix function