Adding some unittests, working. Still need to fix some stuff and more complex tests
Fix the problems when using different sices for partition the matrix. Check issue #5