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

#define MPU9250_ADDR 0x68

MPU9250_WE MPU9250 = MPU9250_WE(MPU9250_ADDR);


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*/

  //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 gravity_corrected_accel;
  float absolute_z_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();
  imu_data.gravity_corrected_accel = get_accel_without_gravity(imu_data.accel, imu_data.angles);

  /*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();

  Serial.println("Acceleration without Gravity in m/s/s (x,y,z):");
  Serial.print(imu_data.gravity_corrected_accel.x); Serial.print("    ");
  Serial.print(imu_data.gravity_corrected_accel.y); Serial.print("    ");
  Serial.print(imu_data.gravity_corrected_accel.z); Serial.print("    ");
  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();
  Serial.println("************************************");
  */

  //Serial.print(imu_data.gravity_corrected_accel.x); Serial.print(", ");
  //Serial.print(imu_data.gravity_corrected_accel.y); Serial.print(", ");
  Serial.print(imu_data.angles.z); Serial.print(", ");
  Serial.print(imu_data.accel.z); Serial.print(", ");
  Serial.print(imu_data.gravity_corrected_accel.z); Serial.println(", ");
  //delay(1000);
}


float get_absolute_z_accel(xyzFloat accel_without_gravity) {
  
}


xyzFloat get_angles_correct() {
  
}


xyzFloat get_accel_without_gravity(xyzFloat uncorrected_accel, xyzFloat angles) {
  IMUFloat uncorrected_data;

  uncorrected_data.accel = uncorrected_accel;
  uncorrected_data.angles = angles;
  
  //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_to_zero(round_decimals(corrected_accel.x, 2), 0.1);
  corrected_accel.y = uncorrected_data.accel.y - (net_gravity_accel*sin(uncorrected_data.angles.y*0.0174532));
  corrected_accel.y = round_to_zero(round_decimals(corrected_accel.y, 2), 0.1);
  corrected_accel.z = uncorrected_data.accel.z - (net_gravity_accel*sin(uncorrected_data.angles.z*0.0174532));
  corrected_accel.z = round_to_zero(round_decimals(corrected_accel.z, 2), 0.1);

  return corrected_accel;
}

xyzFloat get_median_accel(int num_samples, int millis_delay) {
// num_samples is the total samples to take with a delay of "millis_delay" milliseconds between them

  xyzFloat array_values[num_samples];

  for (int i = 0; i < num_samples; i++) {
    array_values[i] = MPU9250.getAccValues();
    delay(millis_delay);
  }
  return array_values[num_samples/2];
}

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 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));
}

float 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) {
    return 0.0;
  }
  return num;
}