void I2C_Init() {
  Wire.begin();
}

void calibrate_MPU6050(){
  float      x_accel = 0;
  float      y_accel = 0;
  float      z_accel = 0;
  float      x_gyro = 0;
  float      y_gyro = 0;
  float      z_gyro = 0;
  int        num_readings = 10;

  if (debugMode) {
    Serial.println("Starting Calibration");
  }

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Read and average the raw values from the IMU
  for (int i = 0; i < num_readings; i++) {
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    x_accel += ax;
    y_accel += ay;
    z_accel += az;
    x_gyro += gx;
    y_gyro += gy;
    z_gyro += gz;
    delay(100);
  }
  x_accel /= num_readings;
  y_accel /= num_readings;
  z_accel /= num_readings;
  x_gyro /= num_readings;
  y_gyro /= num_readings;
  z_gyro /= num_readings;

  // Store the raw calibration values globally
  base_x_accel = (int16_t)x_accel;
  base_y_accel = (int16_t)y_accel;
  base_z_accel = (int16_t)z_accel;
  base_x_gyro = (int16_t)x_gyro;
  base_y_gyro = (int16_t)y_gyro;
  base_z_gyro = (int16_t)z_gyro;

  calibrateTILT();

  set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);

  if (debugMode) {
    Serial.println("Finishing Calibration");
  }
  
}

void initSensor() {
  I2C_Init();

  delay(1000);

  // IR init
  // Read the shift bit register from the module, used in calculating range
  Wire.beginTransmission(IR_ADDRESS);
  Wire.write(IR_SHIFT);
  Wire.endTransmission();

  Wire.requestFrom(IR_ADDRESS, 1);

  if (1 <= Wire.available()) {
    ir_shift = Wire.read();
  }

  // MPU6050 Init
  if (debugMode) {
    Serial.println(F("Initializing I2C devices..."));
  }
  mpu.initialize();

  // verify connection
  if (debugMode) {
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  }

  delay(500);

  if (debugMode) {
    Serial.println(F("Initializing DMP..."));
  }
  devStatus = mpu.dmpInitialize();
  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default

  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    if (debugMode) {
      Serial.println(F("Enabling DMP..."));
    }
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    if (debugMode) {
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    }
    attachInterrupt(1, dmpDataReady, RISING); // <<------- CHANGE INTERRUPT HERE ////////////////////
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    if (debugMode) {
      Serial.println(F("DMP ready! Waiting for first interrupt..."));
    }
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  // MPU6050 Calibration
  calibrate_MPU6050();

}

void updateMPU6050() {
  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    if (debugMode) {
      Serial.println(F("FIFO overflow!"));
    }

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;

    teapotPacket[0] = fifoBuffer[0];
    teapotPacket[1] = fifoBuffer[1];
    teapotPacket[2] = fifoBuffer[4];
    teapotPacket[3] = fifoBuffer[5];
    teapotPacket[4] = fifoBuffer[8];
    teapotPacket[5] = fifoBuffer[9];
    teapotPacket[6] = fifoBuffer[12];
    teapotPacket[7] = fifoBuffer[13];

    // TESTING
    teapotPacketTest[2] = fifoBuffer[0];
    teapotPacketTest[3] = fifoBuffer[1];
    teapotPacketTest[4] = fifoBuffer[4];
    teapotPacketTest[5] = fifoBuffer[5];
    teapotPacketTest[6] = fifoBuffer[8];
    teapotPacketTest[7] = fifoBuffer[9];
    teapotPacketTest[8] = fifoBuffer[12];
    teapotPacketTest[9] = fifoBuffer[13];
    Serial.write(teapotPacketTest, 14);

  }
}