/*
  VDD ---------- 3.3V
  SDA ---------- A4
  SCL ---------- A5
  GND ---------- GND
*/


#include "MadgwickAHRS.h" // github.com/arduino-libraries/MadgwickAHRS
#include "MPU9250.h" // github.com/bolderflight/MPU9250

Madgwick MadgwickFilter; // initialise Madgwick object

MPU9250 IMU(Wire, 0x68);
int status;

int count = 0;

float heading;
unsigned long timer;

// https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml?#declination
float declinationAngle = 7.0 * (57.0 / 60.0) * PI / 180.0;

float ax, ay, az;
float gx, gy, gz;
float mx, my, mz;
float yaw, pitch, roll;

void setup() {
  Serial.begin(115200);
  while (!Serial) {}

  // start communication with IMU
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while (1) {}
  }

  bool calibrate = true;
  if (calibrate) {
    Serial.println("calibrate IMU accelerometer");
    IMU.calibrateAccel();
    Serial.println("calibrate IMU gyro");
    IMU.calibrateGyro();
    Serial.println("calibrate IMU magnetometer");
    IMU.calibrateMag();
    Serial.println("calibrate completed");

    Serial.println("accelerometer: bias, scale factor");
    Serial.print("\t");
    Serial.print(IMU.getAccelBiasX_mss());
    Serial.print(", ");
    Serial.println(IMU.getAccelScaleFactorX());
    Serial.print("\t");
    Serial.print(IMU.getAccelBiasY_mss());
    Serial.print(", ");
    Serial.println(IMU.getAccelScaleFactorY());
    Serial.print("\t");
    Serial.print(IMU.getAccelBiasZ_mss());
    Serial.print(", ");
    Serial.println(IMU.getAccelScaleFactorZ());

    Serial.println("gyro: bias");
    Serial.print("\t");
    Serial.println(IMU.getGyroBiasX_rads());
    Serial.print("\t");
    Serial.println(IMU.getGyroBiasY_rads());
    Serial.print("\t");
    Serial.println(IMU.getGyroBiasZ_rads());

    Serial.println("magnetometer: bias, scale factor");
    Serial.print("\t");
    Serial.print(IMU.getMagBiasX_uT());
    Serial.print(", ");
    Serial.println(IMU.getMagScaleFactorX());
    Serial.print("\t");
    Serial.print(IMU.getMagBiasY_uT());
    Serial.print(", ");
    Serial.println(IMU.getMagScaleFactorY());
    Serial.print("\t");
    Serial.print(IMU.getMagBiasZ_uT());
    Serial.print(", ");
    Serial.println(IMU.getMagScaleFactorZ());
  } else {
    IMU.setAccelCalX(0.0, 1.0);
    IMU.setAccelCalY(0.0, 1.0);
    IMU.setAccelCalZ(0.0, 1.0);
    IMU.setGyroBiasX_rads(0.00);
    IMU.setGyroBiasY_rads(0.00);
    IMU.setGyroBiasZ_rads(0.00);
    IMU.setMagCalX(18.28, 0.99);
    IMU.setMagCalY(-5.25, 0.99);
    IMU.setMagCalZ(-36.37, 1.02);
  }

  IMU.setAccelRange(MPU9250::ACCEL_RANGE_4G);
  IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_92HZ);
  IMU.setSrd(8); // 1000Hz / (1 + SRD)

  MadgwickFilter.begin(111); // Hz
}

void loop() {
  IMU.readSensor();

  ax = IMU.getAccelX_mss();
  ay = IMU.getAccelY_mss();
  az = IMU.getAccelZ_mss();
  gx = IMU.getGyroX_rads();
  gy = IMU.getGyroY_rads();
  gz = IMU.getGyroZ_rads();
  mx = IMU.getMagX_uT();
  my = IMU.getMagY_uT();
  mz = IMU.getMagZ_uT();

  MadgwickFilter.update(gx * 57.29578f, gy * 57.29578f, gz * 57.29578f, ax, ay, az, mx, my, mz);

  heading = atan2(-my, -mx); + declinationAngle;
  if (heading < 0) {
    heading += 2 * PI;
  }
  if (heading > 2 * PI) {
    heading -= 2 * PI;
  }

  yaw = MadgwickFilter.getYaw();
  roll = MadgwickFilter.getRoll();
  pitch = MadgwickFilter.getPitch();

  unsigned long currentTimer = millis();

  count++;
  if (currentTimer - timer >= 100) {
    Serial.print((int)(heading * 180.0 / PI));
    Serial.print(";");
    Serial.print(yaw);
    Serial.print(";");
    Serial.print(pitch);
    Serial.print(";");
    Serial.print(roll);
    Serial.print(";");
    Serial.print(((float)count * 1000) / (float)(currentTimer - timer));
    Serial.println("Hz");
    timer = currentTimer;
    count = 0;
  }
}