Refactoring, preparations to add new features

Refactoring - separating to functions
Adding firmware detection
Some work with error handling