Refactoring and improved I2C implementation
Refactor: modularize all the things