I2C Write/Read single/multi works.

Added some code for demo in main.