#include <Adafruit_SSD1306.h>

#include "MPU9250.h"

// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

int status;
int i;
float u,ux,uy,temp;
float k;

void setup() {
  // serial to display data
  Serial.begin(115200);
   display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  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) {}
  }
  // setting the accelerometer full scale range to +/-8G 
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  IMU.setSrd(19);
  display.setTextColor(WHITE);
  display.setTextSize(2);
  
}

void loop() {
  temp=0;
  
  for(i=0;i<10;i++)
  {
  IMU.readSensor();

  // display the data
  Serial.print(ux=IMU.getMagX_uT()-36+1.5);
  Serial.print("  ");
  Serial.println(uy=IMU.getMagY_uT()-48+7.5);
  ux=-ux;
  u=(ux*ux+uy*uy);
  u=pow(u,.5);
  
  
  k=asin(uy/u)/3.141*180;
  if(ux<0)
  {
    if(uy>0)
    {
      k=180-k;
    }
    else
    {
      k=-180-k;
    }
  }
  //Serial.println(k);
  temp=temp+k;
  delay(30);
  
  }
  k=temp/10.0;
  display.clearDisplay();
  display.setCursor(0,0);
  display.println(k);
  display.print(" w.r.t E");
  display.display();
  
}