//This code is for Lab 3 Distinction Task

//include the necessary libraries
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050.h>

  MPU6050 mpu; //Initialize MPU6050 object
  Adafruit_SSD1306 display(128, 64, &Wire, -1); //Initialise the SSD1306 OLED Display
  const int16_t Scale_F = 16384; // set the constant value of the acceleration scale factor for the MPU6050 sensor.

void setup() {

  //Initialise the Wire and Accel & Gyro Sensor
   Wire.begin();
   mpu.initialize();

  //Start and clear the display, set the text size and colour
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
    display.clearDisplay();
    display.setTextColor(WHITE);
    display.setTextSize(2);

  pinMode(0, OUTPUT); // Set DC pin to output mode

          }       

//Function to get acceleration readings from MPU6050.
void getAcceleration(float* x, float* y, float* z)

  {
      //Save the raw acceleration data to the x, y, and z variables as floating point values
     int16_t ax, ay, az;
     mpu.getAcceleration(&ax, &ay, &az);
     *x = (float)ax * (1.0 / Scale_F);
     *y = (float)ay * (1.0 / Scale_F);
     *z = (float)az * (1.0 / Scale_F);

  }

void loop() {

  float X, Y, Z;
  float Pitch, Roll; //X, Y, and Z axis acceleration, as well as calculated Pitch and Roll angles, are stored in variables

     getAcceleration(&X, &Y, &Z); 
     Pitch = atan2(Y, sqrt(X * X + Z * Z));
     Roll = atan2(-X, Z);  //Calculate Pitch and Roll values from accelerometer data


  Pitch = Z * 180 / PI; //Pitch and Roll values are converted from radians to degrees
  Roll = Roll * 180 / PI;

//Prints the Pitch and Roll value 
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("Pitch- ");
  display.println(Pitch);
  display.print("Roll- ");
  display.println(Roll);

  // Draw spirit level
     int x, y;
     x = 64 + (int)(Roll * 32 / 90);
     y = 32 + (int)(Pitch * 32 / 90);

  display.drawCircle(64, 32, 31, WHITE);
  display.drawLine(64, 1, 64, 7, WHITE);
  display.drawLine(64, 57, 64, 63, WHITE);
  display.drawLine(1, 32, 7, 32, WHITE);
  display.drawLine(121, 32, 127, 32, WHITE);
  display.drawLine(x-2, y, x+2, y, WHITE);
  display.drawLine(x, y-2, x, y+2, WHITE);
  display.display();
  
      delay(100);

  }