Currently I2C-related code (I2C.h)) is spread between several functions, this should be packed into a class and reduced if possible. Also the number of functions could be reduced, if the code has only slight differences and can be replaced by parameters/arguments.