Added the code to the I2C functions
Ok fine stopped being pedantic...
Cleaned up the i2c code.