#include <Wire.h>
#include <MPU6050_tockn.h>

MPU6050 mpu6050(Wire);

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu6050.begin();

  // Check the MPU-6050's WHO_AM_I register
  uint8_t mpu6050Address = 0x68; // Default I2C address of the MPU-6050
  uint8_t whoAmIRegister = 0x75; // WHO_AM_I register address

  Wire.beginTransmission(mpu6050Address);
  Wire.write(whoAmIRegister);
  Wire.endTransmission(false);

  Wire.requestFrom(mpu6050Address, 1, true);
  uint8_t whoAmIValue = Wire.read();

  if (whoAmIValue != 0x68) {
    Serial.print("MPU6050 not found. WHO_AM_I register returned: 0x");
    Serial.println(whoAmIValue, HEX);
    while (1);
  } else {
    Serial.println("MPU6050 found!");
  }
}

void loop() {
  mpu6050.update();
  float accelX = mpu6050.getAccX();
  float accelY = mpu6050.getAccY();
  float accelZ = mpu6050.getAccZ();
  float gyroX = mpu6050.getGyroX();
  float gyroY = mpu6050.getGyroY();
  float gyroZ = mpu6050.getGyroZ();
  
  // Read the raw temperature value
  int16_t rawTemperature = mpu6050.getTemp();

  // Convert the raw temperature value to degrees Celsius
  float temperature = (rawTemperature / 340.0) + 36.53;

  Serial.print("Accel [X,Y,Z]: ");
  Serial.print(accelX);
  Serial.print(", ");
  Serial.print(accelY);
  Serial.print(", ");
  Serial.print(accelZ);
  Serial.print(" | Gyro [X,Y,Z]: ");
  Serial.print(gyroX);
  Serial.print(", ");
  Serial.print(gyroY);
  Serial.print(", ");
  Serial.print(gyroZ);
  Serial.print(" | Temperature: ");
  Serial.print(temperature);
  Serial.println(" °C");

  delay(1000); // Adjust the delay as needed
}