Updates to Matlab code.
Continuing refactoring..
Commenting code