Refactored code

* Main steps split into functions
* Add error handling

Change-Id: I373f95172a121ea3564e787529f1159936fe505a