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

This reverts commit bf7ca395938bff952bb975685e960fd221dd178e.