Updated version with comments and tested with different size matrix