///////////////////////////////////////////////////////
//
// Luca Amore
// https://www.lucaamore.com
//
///////////////////////////////////////////////////////

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#define LED_STATUS 2
#define LED_STATUS_INTERVAL 300

#define CALIBRATE_ENABLE
#define CALIBRATE_NUM_READINGS 200

// Globals
Adafruit_MPU6050 mpu;

float ax_offset = 0, ay_offset = 0, az_offset = 0;
float gx_offset = 0, gy_offset = 0, gz_offset = 0;

void setupMPU6050(void){

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 is ready!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+/-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+/-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+/-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+/-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+/- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+/- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+/- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+/- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

#ifdef CALIBRATE_ENABLE
  // Calibrate the MPU6050
  Serial.println("MPU6050 calibrating enabled");
  calibratingMPU6050();
#else
  Serial.println("MPU6050 calibrating disabled");
#endif

}

void calibratingMPU6050(void){

  float sum_ax = 0, sum_ay = 0, sum_az = 0;
  float sum_gx = 0, sum_gy = 0, sum_gz = 0;
  
  Serial.println("MPU6050 Calibrating. Please keep the device still...");

  // Take a number of readings to calculate the average
  for (int i = 0; i < CALIBRATE_NUM_READINGS; i++) {

    sensors_event_t a, g, t;
    mpu.getEvent(&a, &g, &t);

    sum_ax += a.acceleration.x;
    sum_ay += a.acceleration.y;
    sum_az += a.acceleration.z;
    sum_gx += g.gyro.x;
    sum_gy += g.gyro.y;
    sum_gz += g.gyro.z;

    delay(10);
  }

  // calculating gyro offsets
  ax_offset = sum_ax / CALIBRATE_NUM_READINGS;
  ay_offset = sum_ay / CALIBRATE_NUM_READINGS;
  az_offset = sum_az / CALIBRATE_NUM_READINGS;
  gx_offset = sum_gx / CALIBRATE_NUM_READINGS;
  gy_offset = sum_gy / CALIBRATE_NUM_READINGS;
  gz_offset = sum_gz / CALIBRATE_NUM_READINGS;

  Serial.println("MPU6050 Calibration completed.");

  Serial.println("MPU6050 Calibration offsets:");
  Serial.println(
    "MPU6050 dax: " + String(ax_offset) + 
      " day: " + String(ay_offset) + 
      " daz: " + String(az_offset) + 
      " dgx: " + String(gx_offset) + 
      " dgy: " + String(gy_offset) + 
      " dgz: " + String(gz_offset)
  );

   delay(3000);
}

void getMPU(sensors_event_t &a, sensors_event_t &g, sensors_event_t &t) {

  mpu.getEvent(&a, &g, &t);

#ifdef CALIBRATE_ENABLE 
      a.acceleration.x -= ax_offset;
      a.acceleration.y -= ay_offset;
      a.acceleration.z -= az_offset;
      g.gyro.x -= gx_offset;
      g.gyro.y -= gy_offset;
      g.gyro.z -= gz_offset;
#endif

}

void setup(void) {
  Serial.begin(115200);

  pinMode(LED_STATUS, OUTPUT);
  digitalWrite(LED_STATUS, HIGH);

  setupMPU6050();
}


void loop() {

  sensors_event_t a, g, t;
  getMPU(a, g, t);

  Serial.println(
    "ax: " + String(a.acceleration.x) + 
      " ay: " + String(a.acceleration.y) + 
      " az: " + String(a.acceleration.z) + 
      " gx: " + String(g.gyro.x) + 
      " gy: " + String(g.gyro.y) + 
      " gz: " + String(g.gyro.z) +
      " tp: " + String(t.temperature)
  );


  digitalWrite(LED_STATUS, (millis() / LED_STATUS_INTERVAL) % 2);

  delay(100);

}