#include <MPU9250_WE.h>
#include <Adafruit_BME280.h>
#include <Wire.h>

#define MPU9250_ADDR 0x68

MPU9250_WE MPU9250 = MPU9250_WE(MPU9250_ADDR);

#define SEALEVELPRESSURE_HPA (1019.7)  // Change to current reported reading

Adafruit_BME280 altimeter;

float net_gravity_accel;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  if (!MPU9250.init()) {
    Serial.println("MPU9520 is not responding");
  } else {
    Serial.println("MPU9520 is connected");
  }

  if (!MPU9250.initMagnetometer()) {
    Serial.println("Magnetometer is not responding");
  } else {
    Serial.println("Magnetometer is connected");
  }
  
/*******************************************************************************************************/
/*MPU9250 General Settings*/
  //MPU9250.autoOffsets();  // Lay the sensor on a level surface and call this function
  
  /*  Sample rate divider divides the output rate of the gyroscope and accelerometer.
   *  Sample rate = Internal sample rate / (1 + divider) 
   *  It can only be applied if the corresponding DLPF is enabled and 0<DLPF<7!
   *  Divider is a number 0...255
   */
  MPU9250.setSampleRateDivider(5);
/*End MPU9250 General Settings*/
/*******************************************************************************************************/
/*Accelerometer Settings*/
  MPU9250.enableAccDLPF(true);  // Enable the digital low pass filter (DLPF) for the accelerometer
  /*  Digital low pass filter (DLPF) for the accelerometer, if enabled 
   *  MPU9250_DPLF_0, MPU9250_DPLF_2, ...... MPU9250_DPLF_7 
   *   DLPF     Bandwidth [Hz]      Delay [ms]    Output rate [kHz]
   *     0           460               1.94           1
   *     1           184               5.80           1
   *     2            92               7.80           1
   *     3            41              11.80           1
   *     4            20              19.80           1
   *     5            10              35.70           1
   *     6             5              66.96           1
   *     7           460               1.94           1
   */
  MPU9250.setAccDLPF(MPU9250_DLPF_6);  // Set the DLPF for the accelerometer
  /*Accelerometer Range
   *  MPU9250_ACC_RANGE_2G      2 g   (default)
   *  MPU9250_ACC_RANGE_4G      4 g
   *  MPU9250_ACC_RANGE_8G      8 g   
   *  MPU9250_ACC_RANGE_16G    16 g
   */
  MPU9250.setAccRange(MPU9250_ACC_RANGE_4G);
  /* You can enable or disable the axes for gyroscope and/or accelerometer measurements.
   * By default all axes are enabled. Parameters are:  
   * MPU9250_ENABLE_XYZ  //all axes are enabled (default)
   * MPU9250_ENABLE_XY0  // X, Y enabled, Z disabled
   * MPU9250_ENABLE_X0Z   
   * MPU9250_ENABLE_X00
   * MPU9250_ENABLE_0YZ
   * MPU9250_ENABLE_0Y0
   * MPU9250_ENABLE_00Z
   * MPU9250_ENABLE_000  // all axes disabled
   */
  //MPU9250.enableAccAxes(MPU9250_ENABLE_XYZ);
/*End Accelerometer Settings*/
/*******************************************************************************************************/
/*Gyroscope Settings*/
  MPU9250.enableGyrDLPF(); // Enable the digital low pass filter (DLPF) for the gyroscope
  /*  Digital Low Pass Filter for the gyroscope must be enabled to choose the level. 
   *  MPU9250_DPLF_0, MPU9250_DPLF_2, ...... MPU9250_DPLF_7 
   *  
   *  DLPF    Bandwidth [Hz]   Delay [ms]   Output Rate [kHz]
   *    0         250            0.97             8
   *    1         184            2.9              1
   *    2          92            3.9              1
   *    3          41            5.9              1
   *    4          20            9.9              1
   *    5          10           17.85             1
   *    6           5           33.48             1
   *    7        3600            0.17             8
   *    
   *    You achieve lowest noise using level 6  
   */
  MPU9250.setGyrDLPF(MPU9250_DLPF_6);  // Set the DLPF for the gyroscope
  /*Gyroscope Range
   *  MPU9250_GYRO_RANGE_250       250 degrees per second (default)
   *  MPU9250_GYRO_RANGE_500       500 degrees per second
   *  MPU9250_GYRO_RANGE_1000     1000 degrees per second
   *  MPU9250_GYRO_RANGE_2000     2000 degrees per second
   */
  MPU9250.setGyrRange(MPU9250_GYRO_RANGE_250);
  /* You can enable or disable the axes for gyroscope and/or accelerometer measurements.
   * By default all axes are enabled. Parameters are:  
   * MPU9250_ENABLE_XYZ  //all axes are enabled (default)
   * MPU9250_ENABLE_XY0  // X, Y enabled, Z disabled
   * MPU9250_ENABLE_X0Z   
   * MPU9250_ENABLE_X00
   * MPU9250_ENABLE_0YZ
   * MPU9250_ENABLE_0Y0
   * MPU9250_ENABLE_00Z
   * MPU9250_ENABLE_000  // all axes disabled
   */
  //MPU9250.enableGyrAxes(MPU9250_ENABLE_XYZ);
/*End Gyroscope Settings*/
/*******************************************************************************************************/
/*Magnetometer Settings*/
  /*
   * AK8963_PWR_DOWN       
   * AK8963_CONT_MODE_8HZ         default
   * AK8963_CONT_MODE_100HZ
   * AK8963_FUSE_ROM_ACC_MODE 
   */
  MPU9250.setMagOpMode(AK8963_CONT_MODE_100HZ);
/*End Magnetometer Settings*/

  altimeter.begin(0x76);

  //Serial.println("Getting the Net Gravitational Acceleration");
  //Serial.println("Keep the Accelerometer Still!");
  //delay(1000);
  net_gravity_accel = get_net_gravity_accel();
  //Serial.println(net_gravity_accel);
}

struct IMUFloat {
  xyzFloat accel;
  xyzFloat gyro;
  xyzFloat mag;
  xyzFloat angles;
};

void loop() {
  IMUFloat imu_data;
  imu_data.accel = get_median_accel(10, 2);  // Get the median of 10 measurements with 2ms between them
  imu_data.gyro = MPU9250.getGyrValues();  // Automatically applyies the autoOffsets() calculated earlier and relates raw values to degreees per second
  imu_data.mag = MPU9250.getMagValues();
  imu_data.angles = MPU9250.getAngles();

  Serial.println("Acceleration in m/s/s (x,y,z):");
  Serial.print(imu_data.accel.x); Serial.print("    ");
  Serial.print(imu_data.accel.y); Serial.print("    ");
  Serial.print(imu_data.accel.z); Serial.print("    ");
  Serial.println("");

  get_accel_without_gravity(imu_data.accel);

  Serial.println("Rotational Velocity in deg/s (x,y,z):");
  Serial.print(imu_data.gyro.x); Serial.print("    ");
  Serial.print(imu_data.gyro.y); Serial.print("    ");
  Serial.print(imu_data.gyro.z); Serial.print("    ");
  Serial.println("");

  Serial.println("Magnetometer data in µTesla (x,y,z):");
  Serial.print(imu_data.mag.x); Serial.print("    ");
  Serial.print(imu_data.mag.y); Serial.print("    ");
  Serial.print(imu_data.mag.z); Serial.print("    ");
  Serial.println();
  Serial.print("  Heading: "); Serial.println(get_mag_heading(imu_data.mag));
  Serial.println("");

  Serial.println("Angles in deg (x,y,z):");
  Serial.print(imu_data.angles.x); Serial.print("    ");
  Serial.print(imu_data.angles.y); Serial.print("    ");
  Serial.print(imu_data.angles.z); Serial.print("    ");
  Serial.println("");

  Serial.println("Angles from pitch/roll in deg (x,y):");
  Serial.print(MPU9250.getPitch()); Serial.print("    ");
  Serial.print(MPU9250.getRoll()); Serial.print("    ");
  Serial.println("");
  
  Serial.println("Altimeter Data:");
  Serial.print("  Pressure Alt: "); Serial.println(altimeter.readAltitude(1013.25)*3.28084);
  Serial.print("  Indicated Alt: "); Serial.println(altimeter.readAltitude(SEALEVELPRESSURE_HPA)*3.28084);
  Serial.print("  Pressure: "); Serial.print(altimeter.readPressure() / 100.0F);
  Serial.println("");

  Serial.println("************************************");
  delay(1000);
}


xyzFloat get_accel_without_gravity(xyzFloat uncorrected_accel) {
  IMUFloat uncorrected_data;

  uncorrected_data.accel = uncorrected_accel;
  uncorrected_data.angles = MPU9250.getAngles();

  /*if (uncorrected_data.angles.x > 60) {
    uncorrected_data.angles.x = MPU9250.getPitch();
  }
  if (uncorrected_data.angles.y > 60) {
    uncorrected_data.angles.y = MPU9250.getRoll();
  }*/
  
  //float net_gravity_accel = sqrt(uncorrected_data.accel.x*uncorrected_data.accel.x + uncorrected_data.accel.y*uncorrected_data.accel.y + uncorrected_data.accel.z*uncorrected_data.accel.z);

  xyzFloat corrected_accel;
  corrected_accel.x = uncorrected_data.accel.x - (net_gravity_accel*sin(uncorrected_data.angles.x*0.0174532));
  corrected_accel.x = round_decimals(corrected_accel.x, 2);
  corrected_accel.y = uncorrected_data.accel.y - (net_gravity_accel*sin(uncorrected_data.angles.y*0.0174532));
  corrected_accel.y = round_decimals(corrected_accel.y, 2);
  corrected_accel.z = uncorrected_data.accel.z - (net_gravity_accel*sin(uncorrected_data.angles.z*0.0174532));
  corrected_accel.z = round_decimals(corrected_accel.z, 2);

  Serial.println("Acceleration without Gravity in m/s/s (x,y,z):");
  Serial.print(corrected_accel.x); Serial.print("    ");
  Serial.print(corrected_accel.y); Serial.print("    ");
  Serial.print(corrected_accel.z); Serial.print("    ");
  Serial.println("");

  return corrected_accel;
}


xyzFloat get_average_accel(int num_samples, int millis_delay) {
  /*
   * This function helps reduced the inaccuracy of the accelerometer by averaging readings
   * It returns an xyzFloat struct as defined in the MPU9250 library
   * num_samples is the number of readings from the accelerometer to take
   * millis_delay is the delay in between readings
   */

  xyzFloat reading;  // Temporary struct to store each reading
  float x_sum, y_sum, z_sum;  // Floats to tally up the total value of the readings to then divide by the total number of readings
  
  for (int i = 0; i < num_samples; i++) {
    reading = MPU9250.getAccValues();
    x_sum += reading.x;
    y_sum += reading.y;
    z_sum += reading.z;
    
    delay(millis_delay);
  }

  xyzFloat average_readings;  // xyzFloat struct to store and return the averages
  
  average_readings.x = x_sum / num_samples;
  average_readings.y = y_sum / num_samples;
  average_readings.z = z_sum / num_samples;
  
  return average_readings;
}


float get_net_gravity_accel() {
  xyzFloat accel = get_median_accel(50, 20);  // Get the median of 50 measurements with 20ms between them

  return sqrt(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
}


float get_mag_heading(xyzFloat mag_readings) {
  return atan2(mag_readings.y*0.01, mag_readings.x*0.01)*180/3.14169;
}


float round_decimals(float num, int places) {
// num is the original number to be rounded
// places is the number of figures after the decimal place to keep

  float new_num = num * pow(10, places);
  new_num += (0.5 * pow(10, -1*(places-1)));
  
  return float(int(new_num) / pow(10, places));
}


void round_to_zero(float *num, float *margin) {
// num is the original number to be rounded to zero if the number is within an absolute distance "margin" away from zero

  if (*num >= -1*(*margin) && *num <= *margin) {
    *num = 0.0;
  }

  /*How to call it
  float number = 0.07;
  float margin = 0.05;
  round_to_zero(&number, &margin);
  Serial.println(number);
  */
}