/*

Name: ECE_342_Accelerometer_V5
Author: Thomas Pinon
Date: 3/8/24
Description:

This is the fifth and final version of the main ESP-32 code for the OSU Junior Design II AIAA Accelerometer project (group EJ02).
Designed to run on a NodeMCU 32-S, this code integrates most of the features tested in earlier versions. This version has been
streamlined and more heavily annotated for user readability. Features include:

- Live capture of 3-axis acceleration data using an MPU-6050
- Live and maximum magnitude of acceleration (in Gs) are displayed using a 128x32 OLED screen
- Maximum recorded magnitude of acceleration can be reset using a button
- Live acceleration magnitude data is constantly broadcast over bluetooth
- A buzzer can be activated at a configurable acceleration magnitude threshold
- An LED can be activated at a configurable acceleration magnitude threshold

*/

// include libraries
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#include <BluetoothSerial.h>

// initialize MPU object
Adafruit_MPU6050 mpu;

// initialize Bluetooth Serial object
BluetoothSerial SerialBT;

// initialize acceleration value storage variables
float acc_x;
float acc_y;
float acc_z;
float acc_mag;
float acc_max = 0;
sensors_event_t a, g, temp;

// initialize error correction variables
// these are used to correct against intrinsic sensor error
float x_error;
float y_error;
float z_error;

// OLED initialization parameters
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C

// initialize OLED object
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// define peripheral pins
#define rst_sw 14
#define LED 12
#define buzzer 23

// define LED and buzzer activation thresholds in Gs
float LED_th = 2.0;
float buzzer_th = 3.0;

// this will be called once on startup to calibrate the MPU
void calibrate() {

  // initialize sum variables for later averaging
  float x_sum = 0;
  float y_sum = 0;
  float z_sum = 0;

  // display instructions for calibration on the OLED screen
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.println("Calibrating. Leave");
  display.println("motionless on a");
  display.println("flat and stable");
  display.println("surface until ready.");

  display.display();

  delay(5000);

  // samples acceleration data every 100ms for 10s
  // sum of all data points are held in their correct sum variable by axis
  for (int i = 0; i < 100; i++){

      mpu.getEvent(&a, &g, &temp);

      acc_x = a.acceleration.x;
      acc_y = a.acceleration.y;
      acc_z = a.acceleration.z;

      x_sum += acc_x;
      y_sum += acc_y;
      z_sum += acc_z;

      delay(100);
    }

  // error values are calculated by averaging the all collected data points and comparing them to ideal values
  // x and y acceleration should be 0 and z acceleration should be 9.81 (not -9.81 due to physical orientation of the MPU)
  x_error = 0 - (x_sum / 100);
  y_error = 0 - (y_sum / 100);
  z_error = 9.81 - (z_sum / 100);

}

// runs once on startup
void setup() {

  // begins OLED control and clears display
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();

  // OLED display setup
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);

  // set peripheral pinmodes
  pinMode(rst_sw, INPUT);
  pinMode(LED, OUTPUT);
  pinMode(buzzer, OUTPUT);

  // starts bluetooth serial connection
  SerialBT.begin("Accelerometer");

  // delays until MPU is online
  while (!mpu.begin()){
    delay(10);
  }

  // initializes MPU parameters
  // even though we are not using the gyro, it must be initialized for the library to function correctly
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);

  //calibrate the MPU
  calibrate();

}

// runs infinitely
void loop() {
  
  // reads values from MPU
  mpu.getEvent(&a, &g, &temp);

  // sets temporary acceleration variables
  // takes into account sensor error (including flipping the z sign)
  acc_x = a.acceleration.x + x_error;
  acc_y = a.acceleration.y + y_error;
  acc_z = -1 * (a.acceleration.z + z_error);

  // acceleration is converted to Gs
  // acceleration is in m/s^2 by default
  acc_x = acc_x / 9.81;
  acc_y = acc_y / 9.81;
  acc_z = acc_z / 9.81;

  // computes the magnitude of acceleration
  acc_mag = sqrt(pow(acc_x, 2) + pow(acc_y, 2) + pow(acc_z, 2));
  
  // if the current acceleration is the largest, it becomes the new max acceleration
  if (digitalRead(rst_sw) == HIGH) {
    acc_max = 1.00;
  } else if (acc_mag > acc_max) {
    acc_max = acc_mag;
  }

  // flash LED if acceleration magnitude exceeds defined threshold
  if (acc_mag >= LED_th) {
    digitalWrite(LED, HIGH);
  } else {
    digitalWrite(LED, LOW);
  }

  // beep buzzer if acceleration magnitude exceeds defined threshold
  if (acc_mag >= buzzer_th) {
    digitalWrite(buzzer, HIGH);
  } else {
    digitalWrite(buzzer, LOW);
  }

  // configures OLED as needed and displays acceleration magnitude values
  // as well as necessary labels
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(2);
  display.print(String(acc_mag));
  display.print(" ");
  display.print(acc_max);

  display.setTextSize(1);

  display.println("");
  display.println("                   Gs");
  display.println("");
  display.print("  LIVE      MAX");

  display.display();
  
  // sends the acceleration magnitude as a string over bluetooth serial connection
  SerialBT.println(String(acc_mag));

  // waits 50 milliseconds before sampling and displaying data again
  delay(50);
  
}