#include "quaternionFilters.h"
#include "MPU9250.h"
#include <U8g2lib.h>

#define I2Cclock 400000
#define I2Cport Wire
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0

MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);
U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);

//This line contain direction logo/bitmap
#define direction_width 9
#define direction_height 13
static const unsigned char direction_bits[] U8X8_PROGMEM = {
  0x10, 0xfe, 0x10, 0xfe, 0x10, 0xfe, 0x10, 0xfe, 0x38, 0xfe, 0x38, 0xfe,
  0x38, 0xfe, 0x7c, 0xfe, 0x7c, 0xfe, 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe,
  0xfe, 0xfe
};

const int centreX = 32;
const int centreY = 32;
const int radius = 23;
int xn, yn, xe, ye, xs, ys, xw, yw;

//offset setting for Display N-E-S-W
int offsetRadius = 5;
int offsetCentreX = -3;
int offsetCentreY = 1;

//offset setting for North
float offsetCalibration = -0.5;

void setup() {
  Wire.begin();
  u8g2.begin();
  myIMU.initMPU9250();

  byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);

  u8g2.firstPage();
  do {
    u8g2.setFont(u8g2_font_5x7_tr);
    u8g2.setCursor(0, 10);
    if (d == 0x48) {
      u8g2.print("Magnetometer Connected!");
    } else {
      u8g2.print("Magnetometer Failed!");
    }
  } while (u8g2.nextPage());

  delay(2000);

  myIMU.initAK8963(myIMU.factoryMagCalibration);
  myIMU.getMres();

  for (int i = 8; i > 0; i--) {
    u8g2.firstPage();
    do {
      u8g2.setFont(u8g2_font_5x7_tr);
      u8g2.setCursor(0, 10);
      u8g2.print("Need to calibrate ...");
      u8g2.setCursor(0, 22);
      u8g2.print("Please Wave device in ");
      u8g2.setCursor(0, 32);
      u8g2.print("a figure 8 until done!");

      u8g2.setCursor(0, 42);
      u8g2.print("Calibration will start in");
      u8g2.setFont(u8g2_font_victoriabold8_8r);
      u8g2.setCursor(60, 52);
      u8g2.print(i);
    } while (u8g2.nextPage());
    delay(1000);
  }

  u8g2.firstPage();
  do {
    u8g2.setFont(u8g2_font_5x7_tr);
    u8g2.setCursor(0, 10);
    u8g2.print("Wave now !");
    u8g2.setCursor(0, 22);
    u8g2.print("Data is being collected...");
  } while (u8g2.nextPage());

  // The next call delays for 4 seconds, and then records about 15 seconds of
  // data to calculate bias and scale.
  myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale);

  u8g2.firstPage();
  do {
     u8g2.setFont(u8g2_font_victoriabold8_8r);
    u8g2.setCursor(0, 32);
    u8g2.print("Calibration Done!");
  } while (u8g2.nextPage());

  delay(5000);
}
void loop() {
  myIMU.readMagData(myIMU.magCount);

  myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes * myIMU.factoryMagCalibration[0] - myIMU.magBias[0];
  myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes * myIMU.factoryMagCalibration[1] - myIMU.magBias[1];
  myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes * myIMU.factoryMagCalibration[2] - myIMU.magBias[2];

  myIMU.updateTime();

  // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of
  // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis
  // (+ up) of accelerometer and gyro! We have to make some allowance for this
  // orientationmismatch in feeding the output to the quaternion filter. For the
  // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward
  // along the x-axis just like in the LSM9DS0 sensor. This rotation can be
  // modified to allow any convenient orientation convention. This is ok by
  // aircraft orientation standards! Pass gyro rate as rad/s
  MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD, myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my, myIMU.mx, myIMU.mz, myIMU.deltat);


  float heading = atan2(myIMU.mx, myIMU.my) + offsetCalibration;

  // How to get the magnetic declination angle
  // is available in the article https://bit.ly/3Zd6e2z
  float declination = -0.7;

  heading += declination;

  // Correct for when signs are reversed.
  if (heading < 0)
    heading += 2 * PI;

  // Check for wrap due to addition of declination.
  if (heading > 2 * PI)
    heading -= 2 * PI;

  float angle = (heading * 180 / M_PI);
  //Serial.println(angle);


  u8g2.firstPage();
  do {
    //calculate X, Y position of West
    xw = ((radius + offsetRadius) * -cos((angle - 0) * 3.14 / 180)) + centreX + offsetCentreX;
    yw = ((radius + offsetRadius) * sin((angle - 0) * 3.14 / 180)) + centreY + offsetCentreY;

    //calculate X, Y position of North
    xn = ((radius + offsetRadius) * -cos((angle - 90) * 3.14 / 180)) + centreX + offsetCentreX;
    yn = ((radius + offsetRadius) * sin((angle - 90) * 3.14 / 180)) + centreY + offsetCentreY;

    //calculate X, Y position of East
    xe = ((radius + offsetRadius) * -cos((angle - 180) * 3.14 / 180)) + centreX + offsetCentreX;
    ye = ((radius + offsetRadius) * sin((angle - 180) * 3.14 / 180)) + centreY + offsetCentreY;

    //calculate X, Y position of South
    xs = ((radius + offsetRadius) * -cos((angle - 270) * 3.14 / 180)) + centreX + offsetCentreX;
    ys = ((radius + offsetRadius) * sin((angle - 270) * 3.14 / 180)) + centreY + offsetCentreY;

    //Draw Circle
    u8g2.drawCircle(centreX, centreY, radius, U8G2_DRAW_ALL);

    //Show Direction Icon, by tutorial here https://bit.ly/3VAOlrm
    u8g2.drawXBMP(28, 16, direction_width, direction_height, direction_bits);

    //Show angle value on OLED
    u8g2.setFont(u8g2_font_victoriabold8_8r);
    u8g2.setCursor(73, 32);

    u8g2.print((String(angle, 0) + " Deg"));

    //Draw points of the compass every angle
    u8g2.setFontPosCenter();
    u8g2.setDrawColor(1);
    u8g2.setBitmapMode(0);
    u8g2.drawStr(xn, yn, "N");
    u8g2.drawStr(xe, ye, "E");
    u8g2.drawStr(xs, ys, "S");
    u8g2.drawStr(xw, yw, "W");
  } while (u8g2.nextPage());

  delay(150);
}