Added I2C and other changes

I2C testbench works but still having problems with hardware