The master and slave code look remarkably similar. Maybe you would be better off using a library for I2C: