Revert "Revert "STEP: code optimization + some fixes""

This reverts commit bf7ca395938bff952bb975685e960fd221dd178e.
Merge pull request #2 from ByPort/dev

Revert "Revert "STEP: code optimization + some fixes""