#include <Wire.h>

const byte MPU_ADDRESS = 0x68;

int RED_LED=2; //We will run the local LED from digital pin 2

bool isSensorVALID=true; //boolean variable to track sensor state

//Function for startup I2C scan
void scanI2C() {
  byte error, address;
  int nDevices = 0;

  Serial.println(F("Scanning I2C bus..."));

  for(address = 0; address < 128; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();//If the device does not return the transmission, then an error

    if (error == 0) //there exists an I2C device at this address.
    {
      Serial.print(F("I2C device found at address 0x"));
      if (address < 16) Serial.print("0");
      Serial.print(address, HEX);
      Serial.println(F("  !")); // F tells compiler to keep this in Flash memory and save RAM.
      nDevices++;
    }
  }
  if (nDevices == 0) Serial.println(F("No I2C devices found\n"));
  else Serial.println(F("I2C Scan done.\n"));
}


// the I2C startup scan finds Address as follows
// MPU6050: 0x68

void setup() {

  pinMode(RED_LED,OUTPUT);

  Serial.begin(9600); // Start serial communication for output
  while(!Serial);//waiting for serial window to open..

  //I2C startup scan.
  Wire.begin(); // Join the I2C bus (Arduino acts as master so no address here)
  scanI2C();

  Wire.setWireTimeout(25000,true); //setting timeout time to 25 ms and reset condition to true.

  // Wake up the MPU6050
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x6B); // Power management register
  Wire.write(0x00); // Write 0 to wake it up
  Wire.endTransmission();

  Serial.println("System Initialized. Starting Telemetry Acquisition...");
  delay(2000); //Putting a delay of 2 seconds to allow sensor to settle and I2C connection to establish
}

void loop() {
  //REQUESTING DATA

  // Point to the starting register for Accelerometer X (0x3B) [ACCEL_XOUT_H]
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x3B); 
  Wire.endTransmission(false); // 'false' keeps the connection active for the read

  // Request 6 bytes (2 bytes each for X, Y, Z axes)
  Wire.requestFrom(MPU_ADDRESS, 6);

  //FAULT DETECTION

  if(Wire.getWireTimeoutFlag())
  {
    digitalWrite(RED_LED,HIGH); //turning on fault LED
    isSensorVALID=false;
    Serial.print("WARNING!! MPU6050 sensor NOT VALID \n");

    Wire.clearWireTimeoutFlag(); //Clearing flag for retry on next loop
  }

  else //Everything is fine!
  {
    digitalWrite(RED_LED,LOW); //turning off fault LED
    isSensorVALID=true;

    // Ensure we actually received the 6 bytes we asked for
    if (Wire.available() == 6) {
      
      // Reading and combining the HIGH and LOW 8 bit values to 16 bit integers
      //HIGH value is read first so shifting by 8 bits left then taking OR with the LOW 8 bit value
      int16_t accelX = (Wire.read() << 8) | Wire.read();
      int16_t accelY = (Wire.read() << 8) | Wire.read();
      int16_t accelZ = (Wire.read() << 8) | Wire.read();
      
      float accelX_ms2=9.81*(accelX/16384.0);
      float accelY_ms2=9.81*(accelY/16384.0);
      float accelZ_ms2=9.81*(accelZ/16384.0);
      //adjusting to readable values of acceleration in m/s^2



      // Generate and print the telemetry data packet
      Serial.print("|| X: ");
      Serial.print(accelX_ms2);
      Serial.print(" m/s^2");
      Serial.print(" | Y: ");
      Serial.print(accelY_ms2);
      Serial.print(" m/s^2");
      Serial.print(" | Z: ");
      Serial.print(accelZ_ms2);
      Serial.print(" m/s^2");
      Serial.println("\n");
    }
  }
  
  // Small delay to prevent flooding the serial monitor and allow bus to settle
  delay(250);

  }