static bool NunchuckInitialize(TwoWire& interface)
{
  interface.begin(); // join i2c bus as master

  interface.beginTransmission(0x52);// transmit to device 0x52
    interface.write((uint8_t)0xF0);// sends a zero.
    interface.write((uint8_t)0x55);// sends a zero.
    interface.write((uint8_t)0xFB);// sends a zero.
    interface.write((uint8_t)0x00);// sends a zero.
  interface.endTransmission();// stop transmitting

  return 1;
}

    // Initialise the device
    void Init_Dev(TwoWire *I2C_Ptr) {

      I2C = I2C_Ptr;       // Pointer to the "Wire" I2C object 
    
      // Setup the ITG3200 Gyroscope
      Serial.print("Setting up ITG3200 Gyroscope...");

      //Set the sample rate to 100 hz
      I2C->beginTransmission(GyroAddress); 
      I2C->write(0x15);  // point to Sample Rate Divider register address
      // I2C->write(0x09);  // (internal sample rate of 8 kHz) / (*9* + 1) = 800 per second (800Hz)
      I2C->write(0b01001111);  // (internal sample rate of 8 kHz) / (*79* + 1) = 100 per second (100Hz)
      I2C->endTransmission();

      //Set the gyroscope scale for +/-2000 degrees per second
      I2C->beginTransmission(GyroAddress); 
      I2C->write(0x16);  // point to DLPF and FS register address (0x16)
      I2C->write(0b00011000);  // set Full Scale range (+/-2000deg/sec) and DLPF (LPF=256Hz and Internal Sample Rate = 8kHz)
      I2C->endTransmission();

      Serial.println("Done");
      delay(100);
    }

int PCF8574::i2cRead(uint8_t  value){
  Wire1.requestFrom((uint8_t )PCFaddress, (uint8_t )1);
  if (Wire1.available())  PCFPORTA = (int) Wire1.read();
  else PCFPORTA = (int)value; //error condition
  //return value;
   return PCFPORTA;

}

/** Read multiple bytes from an 8-bit device register.
 * @param useSPI  true : use SPI 
 * @param devAddr I2C slave device address
 * @param regAddr First register regAddr to read from
 * @param length Number of bytes to read
 * @param data Buffer to store read data in
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Number of bytes read (0 indicates failure)
 */
int8_t I2Cdev::readBytes(bool useSPI, uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print(useSPI ? "SPI (0x" : "I2C 0x");
        Serial.print(devAddr, HEX);
        Serial.print(") reading ");
        Serial.print(length, DEC);
        Serial.print(" bytes from 0x");
        Serial.print(regAddr, HEX);
        Serial.print("...");
    #endif
    int8_t count = 0;
	// I2C
	if (!useSPI) {
		Wire.beginTransmission(devAddr);
		#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
			Wire.send(regAddr);
		#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
			Wire.write(regAddr);
		#endif
		Wire.endTransmission();
		Wire.beginTransmission(devAddr);
		Wire.requestFrom(devAddr, length);

		uint32_t t1 = millis();
		for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
			#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
				data[count] = Wire.receive();
			#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
				data[count] = Wire.read();
			#endif
			#ifdef I2CDEV_SERIAL_DEBUG
				Serial.print(data[count], HEX);
				if (count + 1 < length) Serial.print(" ");
			#endif
		}
		if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
		Wire.endTransmission();
	} else {
	    digitalWrite(devAddr, LOW);
		byte Addr = regAddr | 0x80;
		SPI.transfer(Addr);
		for (uint8_t cnt=0; cnt < length; cnt++) {
			data[cnt] = SPI.transfer(0);
			count++;
		}
		digitalWrite(devAddr, HIGH);
	}
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print(". Done (");
        Serial.print(count, DEC);
        Serial.println(" read).");
    #endif
    return count;
}

unsigned int MCP342X::readADC()
{
    delay(80);
    unsigned int t;
  	Wire1.requestFrom(I2C_ADDRESS, (byte) 3);
	byte h = Wire1.read();
  	byte l = Wire1.read();
  	byte r = Wire1.read();
    t = (h << 8) |  l;
    return t;
}

void MCP23018_WRITE(uint8_t device, uint8_t reg, uint8_t data)
{
	uint8_t addr;

	if(DEV_MISC == device){ addr = MISC_ADDR; }
	else{ addr = VEH_IO_ADDR; }

	i2c.beginTransmission(addr);
	i2c.write(reg);
	i2c.write(data);
	i2c.endTransmission();
}

void MCP342X::selectChannel(byte channel, byte gain)
{

    //configuration register, assuming 16 bit resolution
    // Initiate one shot conversion
    // 16 bits, 66.6 ms later the data should be available.
	byte reg = 1 << BIT_RDY |
			channel << BIT_C0 |
			0 << BIT_OC |
			1 << BIT_S1 |
			gain;
	Wire1.beginTransmission(I2C_ADDRESS);
	Wire1.write(reg);
	Wire1.endTransmission();
}

/** Read multiple bytes from an 8-bit device register.
 * @param devAddr I2C slave device address
 * @param regAddr First register regAddr to read from
 * @param length Number of bytes to read
 * @param data Buffer to store read data in
 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
 * @return Number of bytes read (0 indicates failure)
 */
int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print("I2C (0x");
        Serial.print(devAddr, HEX);
        Serial.print(") reading ");
        Serial.print(length, DEC);
        Serial.print(" bytes from 0x");
        Serial.print(regAddr, HEX);
        Serial.print("...");
    #endif

    int8_t count = 0;

    Wire.beginTransmission(devAddr);
    #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
        Wire.send(regAddr);
    #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
        Wire.write(regAddr);
    #endif
    Wire.endTransmission();

    Wire.beginTransmission(devAddr);
    Wire.requestFrom(devAddr, length);

    uint32_t t1 = millis();
    for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
        #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
            data[count] = Wire.receive();
        #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
            data[count] = Wire.read();
        #endif
        #ifdef I2CDEV_SERIAL_DEBUG
            Serial.print(data[count], HEX);
            if (count + 1 < length) Serial.print(" ");
        #endif
    }
    if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout

    Wire.endTransmission();

    #ifdef I2CDEV_SERIAL_DEBUG
        Serial.print(". Done (");
        Serial.print(count, DEC);
        Serial.println(" read).");
    #endif

    return count;
}

static void read16(byte reg, uint16_t *value)
{
  Wire1.beginTransmission((uint8_t)BMP085_ADDRESS);
  #if ARDUINO >= 100
    Wire1.write((uint8_t)reg);
  #else
    Wire1.send(reg);
  #endif
  Wire1.endTransmission();
  Wire1.requestFrom((uint8_t)BMP085_ADDRESS, (byte)2);
  #if ARDUINO >= 100
    *value = (Wire1.read() << 8) | Wire1.read();
  #else
    *value = (Wire1.receive() << 8) | Wire1.receive();
  #endif
  Wire1.endTransmission();
}

void setupTone(int pin) {
  if (!globalToneSetupDone) {   
    Wire.begin();
    globalToneSetupDone = 1;
  }
  
  if (!toneSetupDone[pin]) {
    pinMode(pin, OUTPUT);
    analogWrite(pin, 1);
    toneSetupDone[pin] = 1;
  }  
}

void MCP23018_Init_IOCON(void)
{
	delayMicroseconds(1000u);

	i2c.beginTransmission((uint8_t)MISC_ADDR);
	i2c.write(0x0Au);  // select IOCON
    i2c.write(0xA0u);  // BANK=1, ADDR pointer NOT incremented
	i2c.endTransmission();

	delayMicroseconds(1000u);

	i2c.beginTransmission((uint8_t)VEH_IO_ADDR);
	i2c.write(0x0Au);  // select IOCON
    i2c.write(0xA0u);  // BANK=1, ADDR pointer NOT incremented
	i2c.endTransmission();

}

void MCP23018_Init(void)
{
	pinMode(MISC_INT_PIN, INPUT);	// MISC_INT configured as INPUT
	pinMode(MISC_RST_PIN, INPUT);	// MISC_RST configured as INPUT (OUTPUT LOW to RESET)
	pinMode(VEH_IO_INT_PIN, INPUT);
	pinMode(VEH_IO_RST_PIN, INPUT);

	MCP23018_Reset(DEV_MISC);
	MCP23018_Reset(DEV_VEH_IO);

	i2c.begin();

	MCP23018_Init_IOCON();

	// VEH IO INPUT pins (MISC_IN1:MISC_IN8) always inputs
	MCP23018_WRITE(DEV_VEH_IO, (uint8_t)IODIRA, (uint8_t)0xFFu);
}

static void writeCommand(byte reg, byte value)
{
  Wire1.beginTransmission((uint8_t)BMP085_ADDRESS);
  #if ARDUINO >= 100
    Wire1.write((uint8_t)reg);
    Wire1.write((uint8_t)value);
  #else
    Wire1.send(reg);
    Wire1.send(value);
  #endif
  Wire1.endTransmission();
}

uint8_t MCP23018_READ(uint8_t device, uint8_t reg)
{
	uint8_t addr;
	uint8_t result;

	if(DEV_MISC == device){ addr = MISC_ADDR; }
	else{ addr = VEH_IO_ADDR; }

	i2c.beginTransmission(addr);
	i2c.write(reg);  				// IODIRA.BANK1 Address
	i2c.endTransmission();

	i2c.requestFrom(addr,(uint8_t)1u);
	while (i2c.available() < 1u);
	result = i2c.receive();

	return result;
}

   // Read the ITG3200 Gyroscope
   void Read_Gyro() {
     // Point to the first data register
     I2C->beginTransmission(GyroAddress); 
     I2C->write(0x1B);  // point to the first data register
     I2C->endTransmission();
   
     // read 8 byte, from address 4 (Data Registers)
     I2C->beginTransmission(GyroAddress); 
     I2C->requestFrom(GyroAddress, 8);
     if (I2C->available() >= 8) {
       //Just confirming that the data order is Temp, X, Y, Z
       Gyro_T = (I2C->read() * 256) + I2C->read();  // Temp MSB * 256 + Temp LSB
       Gyro_X = (I2C->read() * 256) + I2C->read();  // X axis MSB * 256 + X axis LSB
       Gyro_Y = (I2C->read() * 256) + I2C->read();  // Y axis MSB * 256 + Y axis LSB
       Gyro_Z = (I2C->read() * 256) + I2C->read();  // Z axis MSB * 256 + Z axis LSB
       }
 
     // Incorrent number of returned bytes
     else {
       Serial.println("Recieving incorrect amount of bytes from Gyroscope");
       while(I2C->available()) {
         Serial.print("data byte = ");
         Serial.println(I2C->read(), DEC);    //print the returned number as a decimal
       }
     }
     I2C->endTransmission();
   }

void I2C1_IRQHandler(void)
{
    Wire.onService();
}

void WIRE_ISR_HANDLER(void) {
	Wire.onService();
}

 void WIRE4_IT_HANDLER(void) {
   Wire4.onService();
 }

    // Read the HMC5883L Compass
    void Read_Compass()  {
      // Point to the first data register
      I2C->beginTransmission(CompassAddress); 
      I2C->write(0x03);  // point to the first data register
      I2C->endTransmission();
    
      // read 6 byte, from address 3 (Data Registers)
      I2C->beginTransmission(CompassAddress); 
      I2C->requestFrom(CompassAddress, 6);
      if (I2C->available() >= 6) {
        //Just confirming that the data register order is X Z Y (ie NOT X Y Z) 
        Compass_Raw_X = (I2C->read() * 256) + I2C->read();  // X axis MSB * 256 + X axis LSB
        Compass_Raw_Z = (I2C->read() * 256) + I2C->read();  // Z axis MSB * 256 + Z axis LSB
        Compass_Raw_Y = (I2C->read() * 256) + I2C->read();  // Y axis MSB * 256 + Y axis LSB
      }

  
      // Incorrent number of returned bytes
      else {
        Serial.println("Recieving incorrect amount of bytes from Compass");
        while(I2C->available()) {
          Serial.print("data byte = ");
          Serial.println(I2C->read(), DEC);    //print the returned number as a decimal
        }
      }
      I2C->endTransmission();
  
      // Scale the Raw readings based on the sensor scale (Gauss = 1.3 & Scale = 0.92)
      Compass_Raw_X *= 0.92;
      Compass_Raw_Y *= 0.92;
      Compass_Raw_Z *= 0.92;
    
      // Update the Max Min limit readings
      if (Compass_Raw_X > Compass_Max_X) Compass_Max_X = Compass_Raw_X;
      if (Compass_Raw_X < Compass_Min_X) Compass_Min_X = Compass_Raw_X;
      if (Compass_Raw_Y > Compass_Max_Y) Compass_Max_Y = Compass_Raw_Y;
      if (Compass_Raw_Y < Compass_Min_Y) Compass_Min_Y = Compass_Raw_Y;
      if (Compass_Raw_Z > Compass_Max_Z) Compass_Max_Z = Compass_Raw_Z;
      if (Compass_Raw_Z < Compass_Min_Z) Compass_Min_Z = Compass_Raw_Z;
  
      // Update the offset
      Compass_Offset_X = (Compass_Max_X + Compass_Min_X) / 2;
      Compass_Offset_Y = (Compass_Max_Y + Compass_Min_Y) / 2;
      Compass_Offset_Z = (Compass_Max_Z + Compass_Min_Z) / 2;
  
      // Calculate calibrated readings
      Compass_Calib_X = Compass_Raw_X - Compass_Offset_X;
      Compass_Calib_Y = Compass_Raw_Y - Compass_Offset_Y;
      Compass_Calib_Z = Compass_Raw_Z - Compass_Offset_Z;
  
      //--- Calculate the X Y plane heading ---
      // Calculate heading (radians) using the X, Y plane - Assuming magnetometer is level
      Compass_Heading = atan2(Compass_Calib_Y, Compass_Calib_X);
      // Adjust with Declination error (magnetic north vs true north)
      Compass_Heading += Compass_Declination_Angle;
      // Correct for when signs are reversed.
      if(Compass_Heading < 0) Compass_Heading += 2*PI;
      // Check for wrap due to addition of declination.
      if(Compass_Heading > 2*PI) Compass_Heading -= 2*PI;  
      // Convert radians to degrees for readability.
      Compass_Heading *= 180/M_PI;
  
      //--- Calculate the X Z plane heading ---
      // Calculate heading (radians) using the X, Z plane
      Compass_Heading_XZ = atan2(Compass_Calib_Z, Compass_Calib_X);
      // Adjust with Declination error (magnetic north vs true north)
      Compass_Heading_XZ += Compass_Declination_Angle;
      // Correct for when signs are reversed.
      if(Compass_Heading_XZ < 0) Compass_Heading_XZ += 2*PI;
      // Check for wrap due to addition of declination.
      if(Compass_Heading_XZ > 2*PI) Compass_Heading_XZ -= 2*PI;  
      // Convert radians to degrees for readability.
      Compass_Heading_XZ *= 180/M_PI;
  
      //--- Calculate the Y Z plane heading ---
      // Calculate heading (radians) using the Y, Z plane
      Compass_Heading_YZ = atan2(Compass_Calib_Z, Compass_Calib_Y);
      // Adjust with Declination error (magnetic north vs true north)
      Compass_Heading_YZ += Compass_Declination_Angle;
      // Correct for when signs are reversed.
      if(Compass_Heading_YZ < 0) Compass_Heading_YZ += 2*PI;
      // Check for wrap due to addition of declination.
      if(Compass_Heading_YZ > 2*PI) Compass_Heading_YZ -= 2*PI;  
      // Convert radians to degrees for readability.
      Compass_Heading_YZ *= 180/M_PI;
    }

    // Read the ADXL345 Accelerometer
    void Read_Accel() {
      I2C->beginTransmission(AccelAddress);  // start transmission to device 
      I2C->write(0x32);                       // point to the first data register DATAX0
      I2C->endTransmission();                // end transmission

      // read 6 byte, from address 32 (Data Registers)
      I2C->beginTransmission(AccelAddress);  // start transmission to device 
      I2C->requestFrom(AccelAddress, 6);
      if (I2C->available() >= 6) {
        Accel_X = I2C->read() + (I2C->read() * 256);  // X axis LSB + X axis MSB * 256
        Accel_Y = I2C->read() + (I2C->read() * 256);  // Y axis LSB + Y axis MSB * 256
        Accel_Z = I2C->read() + (I2C->read() * 256);  // Z axis LSB + Z axis MSB * 256
      }

      // Incorrent number of returned bytes
      else {
        Serial.println("Recieving incorrect amount of bytes from Accelerometer");
        while(I2C->available()) {
          Serial.print("data byte = ");
          Serial.println(I2C->read(), DEC);    //print the returned number as a decimal
        }
      }
      I2C->endTransmission();
    }