#include <Wire.h>

const int MPU_addr=0x68;

// For calibration
int16_t GyroX, GyroY, GyroZ;
float offsetX = 0, offsetY=0, offsetZ = 0;

// FIND CHIP REGISTER MAP TO GET REGISTER VALUES AND BIT DESIGNATIONS

void setup() {
  Serial.begin(9600);
  Wire.begin();

  // Power Management Register Settings
  Wire.beginTransmission(MPU_addr); // Must be done before every write() command
  Wire.write(0x6B); // Access PWR_MGMT register (0x6B)
  Wire.write(0);  // Set SLEEP register to 0, does not trigger sleep mode. 
  Wire.endTransmission(); // Must be done after every write() command

  // Gyroscope Congifiguration Register Settings
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B); // Access gyro config register (0x1B) (Section 4.4)
  Wire.write(0x00000000);    // Sets gyro to full scale +/-250deg/s, 500deg/s
    // 0x00 for +/-250deg/s
    // 0x08 for +/-500deg/s
    // 0x10 for +/-1000deg/s
    // 0x11 for +/-2000deg/s
  Wire.endTransmission();

  // Accelerometer Configuration Register Settings
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C); // Access accel config register (0x1C) (Section 4.5)
  Wire.write(0b00000000); // Set accel to +/-2g
  Wire.endTransmission();
}

void recordAccelRegisters() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B) // Starting register for accel readings
  Wire.endTransmission(); 
  Wire.requestFrom(MPU_addr,6); // request accel registers (3B - 40)
  while(Wire.available() < 6);  
  accelX = Wire.read()<<8|Wire.read();  // Stores first 2 bytes into accelX
  accelY = Wire.read()<<8|Wire.read();  // Stores middle 2 bytes into accelY
  accelZ = Wire.read()<<8|Wire.read();  // Sores last two bytes into accelZ
}

void recordGyroRegisters() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(MPU_addr,6);
  while(Wire.available() <6);
  gyroX = Wire.read()<<8|Wire.read();  // Stores first 2 bytes into accelX
  gyroY = Wire.read()<<8|Wire.read();  // Stores middle 2 bytes into accelY
  gyroZ = Wire.read()<<8|Wire.read();  // Sores last two bytes into accelZ
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
}

void printData() {
  Serial.print("Acceleration [g] ");
  Serial.print("X= ");
  Serial.print(gForceX);

  Serial.print("Acceleration [g] ");
  Serial.print("Y= ");
  Serial.print(gForceY);

  Serial.print("Acceleration [g] ");
  Serial.print("Z= ");
  Serial.print(gForceZ);
}

void loop() {
  recordAccelRegisters();
  recordGyroRegisters();
  printData();
  delay(100);    
}