//MPU6050 Code
/*
  Reference PDF (MPU6050): https://cdn.sparkfun.com/datasheets/Sensors/Accelerometers/RM-MPU-6000A.pdf
  Reference PDF (MPU9250): https://invensense.tdk.com/wp-content/uploads/2015/02/RM-MPU-9250A-00-v1.6.pdf
  For precision tracking of both fast and slow motions, the MPU-60X0 features a user-programmable
  gyroscope full-scale range of ±250, ±500, ±1000, and ±2000°/sec (dps).
  The parts also have a user programmable
  accelerometer full-scale range of ±2g, ±4g, ±8g, and ±16g.
************************************************************************
  Important Note:
  Wire.read() will read incoming 1byte of data
  But all the readings are 2bytes in length
  So, we define a 16bit variable
  Then, we read first 8bytes which in this case is ACCEL_XOUT_H
  So, we then left shift the whole value by 1 byte
  Then, we read the next 1byte i.e ACCEL_XOUT_L
  Then, we perform logical OR operation in order to obtain the exact 16-bit data
*/
// I2C Library
#include <Wire.h>
// ************************************************************************
// I2C Address for MPU6050
#define MPU 0x68  // Given that AD0 is not set
// Real gyro data sensitivity (Page 32)
#define gySens 65.5
// Real accelerometer data sensitivity (Page 30)
#define accSens 8192
// Number of readings for caliberation
// #define calNum 500
// Defining choice
int choice = -1;
// ************************************************************************
// Globally defining variables
int16_t rawaccX = 0, rawaccY = 0, rawaccZ = 0, rawgyX = 0, rawgyY = 0, rawgyZ = 0, rawtemp = 0;
float calaccX = 0, calaccY = 0, calaccZ = 0, calgyX = 0, calgyY = 0, calgyZ = 0;
float accX = 0, accY = 0, accZ = 0, gyX = 0, gyY = 0, gyZ = 0, temp = 0;
// ************************************************************************
// Defining Class for MPU6050
class MPU6050 {
  public:
    // ************************************************************************
    void mpuConfig() {
      // Start I2C Protocol on Slave 0x68
      Wire.beginTransmission(MPU);
      // Accessing Register 0x1A (Low Pass Filter Configuration) {Refer Pg 13}
      Wire.write(0x1A);
      // DLPF:- Mode: 2; Acc BW(Hz): 94, Delay(ms): 3.0;  Gyroscope BW(Hz): 98, Delay(ms): 2.8; Fs(kHz): 1
      Wire.write(0x02);
      // Completed with setup
      Wire.endTransmission();
    }
    // ************************************************************************
    void gyroConfig() {
      // Start I2C Protocol on Slave 0x68
      Wire.beginTransmission(MPU);
      // Accessing Register 0x1B (Full Scale Range Select) {Refer Pg 14}
      Wire.write(0x1B);
      // Range Selection:- FS_SEL: 1; Full Scale Range: +-500deg/s
      Wire.write(0x08);
      // Completed with setup
      Wire.endTransmission();
    }
    // ************************************************************************
    void accConfig() {
      // Start I2C Protocol on Slave 0x68
      Wire.beginTransmission(MPU);
      // Accessing Register 0x1C (Full Scale Range Select) {Refer Pg 15}
      Wire.write(0x1C);
      // Range Selection:- FS_SEL: 1; Full Scale Range: +-4g
      Wire.write(0x08);
      // Completed with setup
      Wire.endTransmission();
    }
    // ************************************************************************
    // Refer Page 30
    // True => Caliberated Data && False => Caliberation
    void rawAcc(bool a) {
      Wire.beginTransmission(MPU);
      // Accesing Regsiter 0x3B
      Wire.write(0x3B);
      // Bus not idle
      Wire.endTransmission();
      // Requesting 6bytes of data (i.e 6 continous registers) from address 0x68
      Wire.requestFrom(MPU, 6);
      // Accelerometer Raw Readings
      rawaccX = Wire.read() << 8 | Wire.read();
      rawaccY = Wire.read() << 8 | Wire.read();
      rawaccZ = Wire.read() << 8 | Wire.read();
      if (a) {
        realAcc(accSens);
      }
      else {
        calAcc(accSens);
      }
    }

    void realAcc(float s) {
      accX = (rawaccX / s) - calaccX;
      accY = (rawaccY / s) - calaccY;
      accZ = (rawaccZ / s) - calaccZ;
    }
    // ************************************************************************
    // Refer Page 32
    // True => Caliberated Data && False => Caliberation
    void rawGyro(bool a) {
      Wire.beginTransmission(MPU);
      // Accesing Regsiter 0x43
      Wire.write(0x43);
      // Bus not idle
      Wire.endTransmission();
      // Requesting 6bytes of data (i.e 6 continous registers) from address 0x68
      Wire.requestFrom(MPU, 6);
      // Gyro Raw Readings
      rawgyX = Wire.read() << 8 | Wire.read();
      rawgyY = Wire.read() << 8 | Wire.read();
      rawgyZ = Wire.read() << 8 | Wire.read();
      if (a) {
        realGyro(gySens);
      }
      else {
        calGyro(gySens);
      }
    }

    void realGyro(float s) {
      gyX = (rawgyX / s) - calgyX;
      gyY = (rawgyY / s) - calgyY;
      gyZ = (rawgyZ / s) - calgyZ;
    }
    // ************************************************************************
    // Refer Page 31
    void rawTemp() {
      Wire.beginTransmission(MPU);
      // Accesing Regsiter 0x41
      Wire.write(0x41);
      // Bus not idle
      Wire.endTransmission();
      // Requesting 2bytes of data (i.e 6 continous registers) from address 0x68
      Wire.requestFrom(MPU, 2);
      // Temperature Raw Readings
      rawtemp = Wire.read() << 8 | Wire.read();
      // Refer Page 31
      temp = (rawtemp / 340.0) + 36.5;
    }
    // ************************************************************************
    // Raw Data Display
    void rawDisp() {
      // Gyroscope
      Serial.print("GyroX = "); Serial.print (rawgyX);
      Serial.print(" GyroY = "); Serial.print (rawgyY);
      Serial.print(" GyroZ = "); Serial.print (rawgyZ);
      // Accelerometer
      Serial.print("| AccX = "); Serial.print (rawaccX);
      Serial.print(" AccY = "); Serial.print (rawaccY);
      Serial.print(" AccZ = "); Serial.print (rawaccZ);
      // Temperature
      Serial.print("| Temp = "); Serial.println(rawtemp);
    }
    // ************************************************************************
    // Real Data Display
    void realDisp() {
      // Gyroscope
      Serial.print("GyroX = "); Serial.print (gyX);
      Serial.print("; GyroY = "); Serial.print (gyY);
      Serial.print("; GyroZ = "); Serial.print (gyZ);
      // Accelerometer
      Serial.print("| AccX = "); Serial.print (accX);
      Serial.print("; AccY = "); Serial.print (accY);
      Serial.print("; AccZ = "); Serial.print (accZ);
      // Temperature
      Serial.print("| Temp = "); Serial.println(temp);
    }
    // ************************************************************************
    // Fetching Data
    void getData() {
      rawGyro(true);
      rawAcc(true);
      rawTemp();
    }
    // ************************************************************************
    // Caliberation Function
    void calGyro(float s) {
      calgyX += (rawgyX / s);
      calgyY += (rawgyY / s);
      calgyZ += (rawgyZ / s);
    }

    void calAcc(float s) {
      calaccX += (rawaccX / s);
      calaccY += (rawaccY / s);
      calaccZ += (rawaccZ / s);
    }

    void calData(int stopTime) {
      int timeNow = millis(),c=0;
      do {
        rawGyro(false);
        rawAcc(false);
        c++;
      }
      while (millis() - timeNow <= (stopTime*1000));
      // Taking average for each value of gyro
      calgyX /= c; calgyY /= c; calgyZ /= c;
      // Taking average for each value of acc
      calaccX /= c; calaccY /= c; calaccZ /= c;
    }
};
// Object for accesing the MPU6050 class
MPU6050 myMPU6050;
// ************************************************************************
class MPU9250 {
  public:

    // FCHOICE = !FCHOICE_B (Complimented Values are stored in FCHOICE)
    // FCHOICE_B present in Register 0x1B (27)

    // Setup for MPU9250 {Refer Page 12-13}
    void mpuConfig() {
      Wire.beginTransmission(MPU);
      // Accesing Register 0x1A (26)
      Wire.write(0x1A); 
      // DLPF:- FCHOICE: 0B11; Mode: 2; Gyroscope BW(Hz): 92, Delay(ms): 3.9;  Temperature BW(Hz): 98, Delay(ms): 2.8; Fs(kHz): 1  
      Wire.write(0x02);
      Wire.endTransmission();
    }

  // Refer Page Number 13-14
    void gyroConfig(){
      Wire.beginTransmission(MPU);
      // Accesing Register 0x1B (27)
      Wire.write(0x1B); 
      // DLPF:- FCHOICE: 0B00; Mode: 0B01 (+- 500deg/s)
      Wire.write(0x08); //Value = 0B00001000
      Wire.endTransmission();
    }

  // Refer Page Number 14-15
    void accConfig(){
      Wire.beginTransmission(MPU);
      // Accesing Register 0x1C (28)
      Wire.write(0x1C); 
      // Mode: 0B01 (+- 4g)
      Wire.write(0x08); //Value = 0B00001000
      Wire.endTransmission();
      
      Wire.beginTransmission(MPU);
      // Accesing Register 0x1D (29)
      Wire.write(0x1D); 
      // ACCEL_FCHOICE: 1; DLPF Mode: 2; 3dB BW (Hz): 99; Delay(ms): 2.88
      Wire.write(0x02); //Value = 0B00000010
      Wire.endTransmission();
    }

    void rawAcc(bool a){
      Wire.beginTransmission(MPU);
      // Accesing Register 0x3B (59)
      Wire.write(0x3B); 
      // Stop the I2C Bus
      Wire.endTransmission();
      // Request 6bytes of successive data
      Wire.requestFrom(MPU,6);
      rawaccX = (Wire.read()<<8)|(Wire.read());
      rawaccY = (Wire.read()<<8)|(Wire.read());
      rawaccZ = (Wire.read()<<8)|(Wire.read());
      if(a){
        // realAcc(accSens);
      }
      else{
        // calAcc();
      }
    }

    void realAcc(){

    }
};

// Object for accessing the MPU9250 class
MPU9250 myMPU9250;
// ************************************************************************
void look() {
  for (int i = 0; i < 70; i++) {
    Serial.print("*");
  }
  Serial.println("");
}
// ************************************************************************
void setup() {
  // Serial Monitor begin
  Serial.begin(9600);
  // I2C begin
  Wire.begin();
  Serial.println("Enter 1 for MPU6050 or Enter 2 for MPU9250");
  Serial.println("Any other value will be considered as MPU6050!");
  Serial.print("Enter your choice: ");
  // Wait until the value has been entered on the serial monitor
  while (!Serial.available());
  choice = Serial.read();
  Serial.println((char)choice);
  if (choice == '2') {
    myMPU9250.mpuConfig();
  }
  else {
    // Setup MPU6050
    myMPU6050.mpuConfig();
    // Setup gyroscope
    myMPU6050.gyroConfig();
    // Setup accelerometer
    myMPU6050.accConfig();
    // Caliberation Process
    Serial.print("Enter Number of Seconds: ");
    while(!Serial.available());
    int calTime = Serial.parseInt();
    Serial.println(calTime);
    Serial.println("Please do not move. Caliberating!");
    myMPU6050.calData(calTime);
    Serial.println("Caliberation Done!");
  }
}
// ************************************************************************
void loop() {
  if (choice == '2') {
    Serial.println("Not yet defined!");
  }
  else {
    myMPU6050.getData();
    myMPU6050.realDisp();
  }
  look();
  delay(1000);
}
// ************************************************************************
// This is how you read a character in a loop
/*
void checkChoice() {
  int temp = Serial.read();
  if (temp != -1 || temp != 10) {
    choice = temp;
  }
}
*/
// ************************************************************************