Reorganized some code and got ready to implement MOVEM.
merged and tested