Refactoring step1, step2, step3, so they are all together in one file, and addings test

Trying to improve efficiency of step1, step2 and step3 scripts, this is still work in progress.