Extracted logic into Matrix class. Extracted methods and removed duplication.