Refactored helper functions
Add boilerplate for Mat2x2
Add post step callback